Add new methods for b3RobotSimulatorClientAPI, thanks to John F. Canny for the contribution!
This commit is contained in:
erwincoumans
@@ -20,6 +20,22 @@
|
||||
#include "../SharedMemory/SharedMemoryPublic.h"
|
||||
#include "Bullet3Common/b3Logging.h"
|
||||
|
||||
static void scalarToDouble3(b3Scalar a[3], double b[3])
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
b[i] = a[i];
|
||||
}
|
||||
}
|
||||
|
||||
static void scalarToDouble4(b3Scalar a[4], double b[4])
|
||||
{
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
b[i] = a[i];
|
||||
}
|
||||
}
|
||||
|
||||
struct b3RobotSimulatorClientAPI_InternalData
|
||||
{
|
||||
b3PhysicsClientHandle m_physicsClientHandle;
|
||||
@@ -82,7 +98,7 @@ bool b3RobotSimulatorClientAPI::mouseMoveCallback(float x,float y)
|
||||
}
|
||||
if (m_data->m_guiHelper)
|
||||
{
|
||||
return b3InProcessMouseMoveCallback(m_data->m_physicsClientHandle, x,y);
|
||||
return b3InProcessMouseMoveCallback(m_data->m_physicsClientHandle, x,y)!=0;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
@@ -95,7 +111,7 @@ bool b3RobotSimulatorClientAPI::mouseButtonCallback(int button, int state, float
|
||||
}
|
||||
if (m_data->m_guiHelper)
|
||||
{
|
||||
return b3InProcessMouseButtonCallback(m_data->m_physicsClientHandle, button,state,x,y);
|
||||
return b3InProcessMouseButtonCallback(m_data->m_physicsClientHandle, button,state,x,y)!=0;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
@@ -995,18 +1011,26 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex,
|
||||
return false;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState)
|
||||
{
|
||||
if (!isConnected())
|
||||
bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState)
|
||||
{
|
||||
if (!isConnected()) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
|
||||
if (computeLinkVelocity) {
|
||||
b3RequestActualStateCommandComputeLinkVelocity(command, computeLinkVelocity);
|
||||
}
|
||||
|
||||
if (computeForwardKinematics) {
|
||||
b3RequestActualStateCommandComputeForwardKinematics(command, computeForwardKinematics);
|
||||
}
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
|
||||
if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
{
|
||||
|
||||
if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
|
||||
b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
|
||||
return true;
|
||||
}
|
||||
@@ -1157,3 +1181,896 @@ void b3RobotSimulatorClientAPI::loadSoftBody(const std::string& fileName, double
|
||||
b3LoadSoftBodySetCollisionMargin(command, collisionMargin);
|
||||
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::getMouseEvents(b3MouseEventsData* mouseEventsData)
|
||||
{
|
||||
mouseEventsData->m_numMouseEvents = 0;
|
||||
mouseEventsData->m_mouseEvents = 0;
|
||||
if (!isConnected()) {
|
||||
b3Warning("Not connected");
|
||||
return;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3RequestMouseEventsCommandInit(m_data->m_physicsClientHandle);
|
||||
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
b3GetMouseEventsData(m_data->m_physicsClientHandle, mouseEventsData);
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities,
|
||||
double *jointAccelerations, double *jointForcesOutput)
|
||||
{
|
||||
if (!isConnected()) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
|
||||
int numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, jointPositions,
|
||||
jointVelocities, jointAccelerations);
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) {
|
||||
int bodyUniqueId;
|
||||
int dofCount;
|
||||
|
||||
b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, &dofCount, 0);
|
||||
|
||||
if (dofCount) {
|
||||
b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0, jointForcesOutput);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI::getNumBodies() const
|
||||
{
|
||||
if (!isConnected()) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
return b3GetNumBodies(m_data->m_physicsClientHandle);
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI::getBodyUniqueId(int bodyId) const
|
||||
{
|
||||
if (!isConnected()) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
return b3GetBodyUniqueId(m_data->m_physicsClientHandle, bodyId);
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::removeBody(int bodyUniqueId)
|
||||
{
|
||||
if (!isConnected()) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) {
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitRemoveBodyCommand(m_data->m_physicsClientHandle, bodyUniqueId));
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_REMOVE_BODY_COMPLETED) {
|
||||
return true;
|
||||
} else {
|
||||
b3Warning("getDynamicsInfo did not complete");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
b3Warning("removeBody could not submit command");
|
||||
return false;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo) {
|
||||
if (!isConnected()) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
int status_type = 0;
|
||||
b3SharedMemoryCommandHandle cmd_handle;
|
||||
b3SharedMemoryStatusHandle status_handle;
|
||||
// struct b3DynamicsInfo info;
|
||||
|
||||
if (bodyUniqueId < 0) {
|
||||
b3Warning("getDynamicsInfo failed; invalid bodyUniqueId");
|
||||
return false;
|
||||
}
|
||||
if (linkIndex < -1) {
|
||||
b3Warning("getDynamicsInfo failed; invalid linkIndex");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) {
|
||||
cmd_handle = b3GetDynamicsInfoCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex);
|
||||
status_handle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle);
|
||||
status_type = b3GetStatusType(status_handle);
|
||||
if (status_type == CMD_GET_DYNAMICS_INFO_COMPLETED) {
|
||||
return true;
|
||||
} else {
|
||||
b3Warning("getDynamicsInfo did not complete");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
b3Warning("getDynamicsInfo could not submit command");
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected to physics server.");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
|
||||
if (args.m_mass >= 0) {
|
||||
b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, args.m_mass);
|
||||
}
|
||||
|
||||
if (args.m_lateralFriction >= 0) {
|
||||
b3ChangeDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, args.m_lateralFriction);
|
||||
}
|
||||
|
||||
if (args.m_spinningFriction>=0) {
|
||||
b3ChangeDynamicsInfoSetSpinningFriction(command, bodyUniqueId, linkIndex, args.m_spinningFriction);
|
||||
}
|
||||
|
||||
if (args.m_rollingFriction>=0) {
|
||||
b3ChangeDynamicsInfoSetRollingFriction(command, bodyUniqueId, linkIndex, args.m_rollingFriction);
|
||||
}
|
||||
|
||||
if (args.m_linearDamping>=0) {
|
||||
b3ChangeDynamicsInfoSetLinearDamping(command, bodyUniqueId, args.m_linearDamping);
|
||||
}
|
||||
|
||||
if (args.m_angularDamping>=0) {
|
||||
b3ChangeDynamicsInfoSetAngularDamping(command, bodyUniqueId, args.m_angularDamping);
|
||||
}
|
||||
|
||||
if (args.m_restitution>=0) {
|
||||
b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, args.m_restitution);
|
||||
}
|
||||
|
||||
if (args.m_contactStiffness>=0 && args.m_contactDamping >=0) {
|
||||
b3ChangeDynamicsInfoSetContactStiffnessAndDamping(command, bodyUniqueId, linkIndex, args.m_contactStiffness, args.m_contactDamping);
|
||||
}
|
||||
|
||||
if (args.m_frictionAnchor>=0) {
|
||||
b3ChangeDynamicsInfoSetFrictionAnchor(command, bodyUniqueId,linkIndex, args.m_frictionAnchor);
|
||||
}
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
return true;
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI::addUserDebugParameter(char * paramName, double rangeMin, double rangeMax, double startValue) {
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected to physics server.");
|
||||
return -1;
|
||||
}
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
commandHandle = b3InitUserDebugAddParameter(sm, paramName, rangeMin, rangeMax, startValue);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) {
|
||||
int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
|
||||
return debugItemUniqueId;
|
||||
}
|
||||
b3Warning("addUserDebugParameter failed.");
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
double b3RobotSimulatorClientAPI::readUserDebugParameter(int itemUniqueId) {
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected to physics server.");
|
||||
return 0;
|
||||
}
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
commandHandle = b3InitUserDebugReadParameter(sm, itemUniqueId);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED) {
|
||||
double paramValue = 0.f;
|
||||
int ok = b3GetStatusDebugParameterValue(statusHandle, ¶mValue);
|
||||
if (ok) {
|
||||
return paramValue;
|
||||
}
|
||||
}
|
||||
b3Warning("readUserDebugParameter failed.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::removeUserDebugItem(int itemUniqueId) {
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected to physics server.");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
commandHandle = b3InitUserDebugDrawRemove(sm, itemUniqueId);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
int b3RobotSimulatorClientAPI::addUserDebugText(char *text, double *posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected to physics server.");
|
||||
return -1;
|
||||
}
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, &args.m_colorRGB[0], args.m_size, args.m_lifeTime);
|
||||
|
||||
if (args.m_parentObjectUniqueId>=0) {
|
||||
b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex);
|
||||
}
|
||||
|
||||
if (args.m_flags & DEBUG_TEXT_HAS_ORIENTATION) {
|
||||
b3UserDebugTextSetOrientation(commandHandle, &args.m_textOrientation[0]);
|
||||
}
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) {
|
||||
int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
|
||||
return debugItemUniqueId;
|
||||
}
|
||||
b3Warning("addUserDebugText3D failed.");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI::addUserDebugText(char *text, b3Vector3 &posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args)
|
||||
{
|
||||
double dposXYZ[3];
|
||||
dposXYZ[0] = posXYZ.x;
|
||||
dposXYZ[1] = posXYZ.y;
|
||||
dposXYZ[2] = posXYZ.z;
|
||||
|
||||
return addUserDebugText(text, &dposXYZ[0], args);
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI::addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected to physics server.");
|
||||
return -1;
|
||||
}
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, &args.m_colorRGB[0], args.m_lineWidth, args.m_lifeTime);
|
||||
|
||||
if (args.m_parentObjectUniqueId>=0) {
|
||||
b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex);
|
||||
}
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) {
|
||||
int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
|
||||
return debugItemUniqueId;
|
||||
}
|
||||
b3Warning("addUserDebugLine failed.");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI::addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args)
|
||||
{
|
||||
double dfromXYZ[3];
|
||||
double dtoXYZ[3];
|
||||
dfromXYZ[0] = fromXYZ.x;
|
||||
dfromXYZ[1] = fromXYZ.y;
|
||||
dfromXYZ[2] = fromXYZ.z;
|
||||
|
||||
dtoXYZ[0] = toXYZ.x;
|
||||
dtoXYZ[1] = toXYZ.y;
|
||||
dtoXYZ[2] = toXYZ.z;
|
||||
return addUserDebugLine(&dfromXYZ[0], &dtoXYZ[0], args);
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected to physics server.");
|
||||
return false;
|
||||
}
|
||||
int numJoints = b3GetNumJoints(sm, bodyUniqueId);
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
// int statusType;
|
||||
struct b3JointInfo info;
|
||||
|
||||
commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, args.m_controlMode);
|
||||
|
||||
for (int i=0;i<args.m_numControlledDofs;i++) {
|
||||
double targetVelocity = 0.0;
|
||||
double targetPosition = 0.0;
|
||||
double force = 100000.0;
|
||||
double kp = 0.1;
|
||||
double kd = 1.0;
|
||||
int jointIndex;
|
||||
|
||||
if (args.m_jointIndices) {
|
||||
jointIndex = args.m_jointIndices[i];
|
||||
} else {
|
||||
jointIndex = i;
|
||||
}
|
||||
|
||||
if (args.m_targetVelocities) {
|
||||
targetVelocity = args.m_targetVelocities[i];
|
||||
}
|
||||
|
||||
if (args.m_targetPositions) {
|
||||
targetPosition = args.m_targetPositions[i];
|
||||
}
|
||||
|
||||
if (args.m_forces) {
|
||||
force = args.m_forces[i];
|
||||
}
|
||||
|
||||
if (args.m_kps) {
|
||||
kp = args.m_kps[i];
|
||||
}
|
||||
|
||||
if (args.m_kds) {
|
||||
kd = args.m_kds[i];
|
||||
}
|
||||
|
||||
b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info);
|
||||
|
||||
switch (args.m_controlMode) {
|
||||
case CONTROL_MODE_VELOCITY: {
|
||||
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
|
||||
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
|
||||
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
|
||||
break;
|
||||
}
|
||||
|
||||
case CONTROL_MODE_TORQUE: {
|
||||
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, force);
|
||||
break;
|
||||
}
|
||||
|
||||
case CONTROL_MODE_POSITION_VELOCITY_PD: {
|
||||
b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
|
||||
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
|
||||
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
|
||||
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
|
||||
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
|
||||
break;
|
||||
}
|
||||
|
||||
default: {}
|
||||
};
|
||||
}
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
|
||||
if (args.m_numSolverIterations >= 0) {
|
||||
b3PhysicsParamSetNumSolverIterations(command, args.m_numSolverIterations);
|
||||
}
|
||||
|
||||
if (args.m_collisionFilterMode >= 0) {
|
||||
b3PhysicsParamSetCollisionFilterMode(command, args.m_collisionFilterMode);
|
||||
}
|
||||
|
||||
if (args.m_numSubSteps >= 0) {
|
||||
b3PhysicsParamSetNumSubSteps(command, args.m_numSubSteps);
|
||||
}
|
||||
|
||||
if (args.m_fixedTimeStep >= 0) {
|
||||
b3PhysicsParamSetTimeStep(command, args.m_fixedTimeStep);
|
||||
}
|
||||
|
||||
if (args.m_useSplitImpulse >= 0) {
|
||||
b3PhysicsParamSetUseSplitImpulse(command, args.m_useSplitImpulse);
|
||||
}
|
||||
|
||||
if (args.m_splitImpulsePenetrationThreshold >= 0) {
|
||||
b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, args.m_splitImpulsePenetrationThreshold);
|
||||
}
|
||||
|
||||
if (args.m_contactBreakingThreshold >= 0) {
|
||||
b3PhysicsParamSetContactBreakingThreshold(command, args.m_contactBreakingThreshold);
|
||||
}
|
||||
|
||||
if (args.m_maxNumCmdPer1ms >= -1) {
|
||||
b3PhysicsParamSetMaxNumCommandsPer1ms(command, args.m_maxNumCmdPer1ms);
|
||||
}
|
||||
|
||||
if (args.m_restitutionVelocityThreshold>=0) {
|
||||
b3PhysicsParamSetRestitutionVelocityThreshold(command, args.m_restitutionVelocityThreshold);
|
||||
}
|
||||
|
||||
if (args.m_enableFileCaching>=0) {
|
||||
b3PhysicsParamSetEnableFileCaching(command, args.m_enableFileCaching);
|
||||
}
|
||||
|
||||
if (args.m_erp>=0) {
|
||||
b3PhysicsParamSetDefaultNonContactERP(command,args.m_erp);
|
||||
}
|
||||
|
||||
if (args.m_contactERP>=0) {
|
||||
b3PhysicsParamSetDefaultContactERP(command,args.m_contactERP);
|
||||
}
|
||||
|
||||
if (args.m_frictionERP >=0) {
|
||||
b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP);
|
||||
}
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
|
||||
command = b3ApplyExternalForceCommandInit(sm);
|
||||
b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position, flags);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags)
|
||||
{
|
||||
double dforce[3];
|
||||
double dposition[3];
|
||||
|
||||
dforce[0] = force.x;
|
||||
dforce[1] = force.y;
|
||||
dforce[2] = force.z;
|
||||
|
||||
dposition[0] = position.x;
|
||||
dposition[1] = position.y;
|
||||
dposition[2] = position.z;
|
||||
|
||||
return applyExternalForce(objectUniqueId, linkIndex, &dforce[0], &dposition[0], flags);
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
|
||||
command = b3ApplyExternalForceCommandInit(sm);
|
||||
b3ApplyExternalTorque(command, objectUniqueId, linkIndex, torque, flags);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags)
|
||||
{
|
||||
double dtorque[3];
|
||||
|
||||
dtorque[0] = torque.x;
|
||||
dtorque[1] = torque.y;
|
||||
dtorque[2] = torque.z;
|
||||
|
||||
return applyExternalTorque(objectUniqueId, linkIndex, &dtorque[0], flags);
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::enableJointForceTorqueSensor(int bodyUniqueId, int jointIndex, bool enable)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
int numJoints = b3GetNumJoints(sm, bodyUniqueId);
|
||||
if ((jointIndex < 0) || (jointIndex >= numJoints)) {
|
||||
b3Warning("Error: invalid jointIndex.");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
command = b3CreateSensorCommandInit(sm, bodyUniqueId);
|
||||
b3CreateSensorEnable6DofJointForceTorqueSensor(command, jointIndex, enable);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_CLIENT_COMMAND_COMPLETED) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
command = b3InitRequestOpenGLVisualizerCameraCommand(sm);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusOpenGLVisualizerCamera(statusHandle, cameraInfo);
|
||||
|
||||
if (statusType) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
command = b3InitRequestContactPointInformation(sm);
|
||||
|
||||
if (args.m_bodyUniqueIdA>=0) {
|
||||
b3SetContactFilterBodyA(command, args.m_bodyUniqueIdA);
|
||||
}
|
||||
if (args.m_bodyUniqueIdB>=0) {
|
||||
b3SetContactFilterBodyB(command, args.m_bodyUniqueIdB);
|
||||
}
|
||||
if (args.m_linkIndexA>=-1) {
|
||||
b3SetContactFilterLinkA(command, args.m_linkIndexA);
|
||||
}
|
||||
if (args.m_linkIndexB >=-1) {
|
||||
b3SetContactFilterLinkB(command, args.m_linkIndexB);
|
||||
}
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
|
||||
b3GetContactPointInformation(sm, contactInfo);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
command = b3InitClosestDistanceQuery(sm);
|
||||
|
||||
b3SetClosestDistanceFilterBodyA(command, args.m_bodyUniqueIdA);
|
||||
b3SetClosestDistanceFilterBodyB(command, args.m_bodyUniqueIdB);
|
||||
b3SetClosestDistanceThreshold(command, distance);
|
||||
|
||||
if (args.m_linkIndexA>=-1) {
|
||||
b3SetClosestDistanceFilterLinkA(command, args.m_linkIndexA);
|
||||
}
|
||||
if (args.m_linkIndexB >=-1) {
|
||||
b3SetClosestDistanceFilterLinkB(command, args.m_linkIndexB);
|
||||
}
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
|
||||
b3GetContactPointInformation(sm, contactInfo);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
// int statusType;
|
||||
|
||||
command = b3InitAABBOverlapQuery(sm, aabbMin, aabbMax);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
b3GetAABBOverlapResults(sm, overlapData);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData)
|
||||
{
|
||||
double daabbMin[3];
|
||||
double daabbMax[3];
|
||||
|
||||
daabbMin[0] = aabbMin.x;
|
||||
daabbMin[1] = aabbMin.y;
|
||||
daabbMin[2] = aabbMin.z;
|
||||
|
||||
daabbMax[0] = aabbMax.x;
|
||||
daabbMax[1] = aabbMax.y;
|
||||
daabbMax[2] = aabbMax.z;
|
||||
|
||||
return getOverlappingObjects(&daabbMin[0], &daabbMax[0], overlapData);
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
if (bodyUniqueId < 0) {
|
||||
b3Warning("Invalid bodyUniqueId");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (linkIndex < -1) {
|
||||
b3Warning("Invalid linkIndex");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (aabbMin == NULL || aabbMax == NULL) {
|
||||
b3Warning("Output AABB matrix is NULL");
|
||||
return false;
|
||||
}
|
||||
|
||||
command = b3RequestCollisionInfoCommandInit(sm, bodyUniqueId);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType != CMD_REQUEST_COLLISION_INFO_COMPLETED) {
|
||||
return false;
|
||||
}
|
||||
if (b3GetStatusAABB(statusHandle, linkIndex, aabbMin, aabbMax)) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax)
|
||||
{
|
||||
double daabbMin[3];
|
||||
double daabbMax[3];
|
||||
|
||||
bool status = getAABB(bodyUniqueId, linkIndex, &daabbMin[0], &daabbMax[0]);
|
||||
|
||||
aabbMin.x = (float)daabbMin[0];
|
||||
aabbMin.y = (float)daabbMin[1];
|
||||
aabbMin.z = (float)daabbMin[2];
|
||||
|
||||
aabbMax.x = (float)daabbMax[0];
|
||||
aabbMax.y = (float)daabbMax[1];
|
||||
aabbMax.z = (float)daabbMax[2];
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int b3RobotSimulatorClientAPI::createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
int shapeIndex = -1;
|
||||
|
||||
command = b3CreateCollisionShapeCommandInit(sm);
|
||||
|
||||
if (shapeType==GEOM_SPHERE && args.m_radius>0) {
|
||||
shapeIndex = b3CreateCollisionShapeAddSphere(command, args.m_radius);
|
||||
}
|
||||
if (shapeType==GEOM_BOX) {
|
||||
double halfExtents[3];
|
||||
scalarToDouble3(args.m_halfExtents, halfExtents);
|
||||
shapeIndex = b3CreateCollisionShapeAddBox(command, halfExtents);
|
||||
}
|
||||
if (shapeType==GEOM_CAPSULE && args.m_radius>0 && args.m_height>=0) {
|
||||
shapeIndex = b3CreateCollisionShapeAddCapsule(command, args.m_radius, args.m_height);
|
||||
}
|
||||
if (shapeType==GEOM_CYLINDER && args.m_radius>0 && args.m_height>=0) {
|
||||
shapeIndex = b3CreateCollisionShapeAddCylinder(command, args.m_radius, args.m_height);
|
||||
}
|
||||
if (shapeType==GEOM_MESH && args.m_fileName) {
|
||||
double meshScale[3];
|
||||
scalarToDouble3(args.m_meshScale, meshScale);
|
||||
shapeIndex = b3CreateCollisionShapeAddMesh(command, args.m_fileName, meshScale);
|
||||
}
|
||||
if (shapeType==GEOM_PLANE) {
|
||||
double planeConstant=0;
|
||||
double planeNormal[3];
|
||||
scalarToDouble3(args.m_planeNormal, planeNormal);
|
||||
shapeIndex = b3CreateCollisionShapeAddPlane(command, planeNormal, planeConstant);
|
||||
}
|
||||
if (shapeIndex>=0 && args.m_flags) {
|
||||
b3CreateCollisionSetFlag(command, shapeIndex, args.m_flags);
|
||||
}
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED) {
|
||||
int uid = b3GetStatusCollisionShapeUniqueId(statusHandle);
|
||||
return uid;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI::createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType, baseIndex;
|
||||
|
||||
double doubleBasePosition[3];
|
||||
double doubleBaseInertialFramePosition[3];
|
||||
scalarToDouble3(args.m_basePosition.m_floats, doubleBasePosition);
|
||||
scalarToDouble3(args.m_baseInertialFramePosition.m_floats, doubleBaseInertialFramePosition);
|
||||
|
||||
double doubleBaseOrientation[4];
|
||||
double doubleBaseInertialFrameOrientation[4];
|
||||
scalarToDouble4(args.m_baseOrientation.m_floats, doubleBaseOrientation);
|
||||
scalarToDouble4(args.m_baseInertialFrameOrientation.m_floats, doubleBaseInertialFrameOrientation);
|
||||
|
||||
command = b3CreateMultiBodyCommandInit(sm);
|
||||
|
||||
baseIndex = b3CreateMultiBodyBase(command, args.m_baseMass, args.m_baseCollisionShapeIndex, args.m_baseVisualShapeIndex,
|
||||
doubleBasePosition, doubleBaseOrientation, doubleBaseInertialFramePosition, doubleBaseInertialFrameOrientation);
|
||||
|
||||
for (int i = 0; i < args.m_numLinks; i++) {
|
||||
double linkMass = args.m_linkMasses[i];
|
||||
int linkCollisionShapeIndex = args.m_linkCollisionShapeIndices[i];
|
||||
int linkVisualShapeIndex = args.m_linkVisualShapeIndices[i];
|
||||
b3Vector3 linkPosition = args.m_linkPositions[i];
|
||||
b3Quaternion linkOrientation = args.m_linkOrientations[i];
|
||||
b3Vector3 linkInertialFramePosition = args.m_linkInertialFramePositions[i];
|
||||
b3Quaternion linkInertialFrameOrientation = args.m_linkInertialFrameOrientations[i];
|
||||
int linkParentIndex = args.m_linkParentIndices[i];
|
||||
int linkJointType = args.m_linkJointTypes[i];
|
||||
b3Vector3 linkJointAxis = args.m_linkJointAxes[i];
|
||||
|
||||
double doubleLinkPosition[3];
|
||||
double doubleLinkInertialFramePosition[3];
|
||||
double doubleLinkJointAxis[3];
|
||||
scalarToDouble3(linkPosition.m_floats, doubleLinkPosition);
|
||||
scalarToDouble3(linkInertialFramePosition.m_floats, doubleLinkInertialFramePosition);
|
||||
scalarToDouble3(linkJointAxis.m_floats, doubleLinkJointAxis);
|
||||
|
||||
double doubleLinkOrientation[4];
|
||||
double doubleLinkInertialFrameOrientation[4];
|
||||
scalarToDouble4(linkOrientation.m_floats, doubleLinkOrientation);
|
||||
scalarToDouble4(linkInertialFrameOrientation.m_floats, doubleLinkInertialFrameOrientation);
|
||||
|
||||
b3CreateMultiBodyLink(command,
|
||||
linkMass,
|
||||
linkCollisionShapeIndex,
|
||||
linkVisualShapeIndex,
|
||||
doubleLinkPosition,
|
||||
doubleLinkOrientation,
|
||||
doubleLinkInertialFramePosition,
|
||||
doubleLinkInertialFrameOrientation,
|
||||
linkParentIndex,
|
||||
linkJointType,
|
||||
doubleLinkJointAxis
|
||||
);
|
||||
}
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) {
|
||||
int uid = b3GetStatusBodyIndex(statusHandle);
|
||||
return uid;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI::getNumConstraints() const
|
||||
{
|
||||
if (!isConnected()) {
|
||||
b3Warning("Not connected");
|
||||
return -1;
|
||||
}
|
||||
return b3GetNumUserConstraints(m_data->m_physicsClientHandle);
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI::getConstraintUniqueId(int serialIndex)
|
||||
{
|
||||
if (!isConnected()) {
|
||||
b3Warning("Not connected");
|
||||
return -1;
|
||||
}
|
||||
int userConstraintId = -1;
|
||||
userConstraintId = b3GetUserConstraintId(m_data->m_physicsClientHandle, serialIndex);
|
||||
return userConstraintId;
|
||||
}
|
||||
@@ -135,6 +135,265 @@ struct b3JointStates2
|
||||
b3AlignedObjectArray<double> m_jointReactionForces;
|
||||
};
|
||||
|
||||
|
||||
struct b3RobotSimulatorJointMotorArrayArgs
|
||||
{
|
||||
int m_controlMode;
|
||||
int m_numControlledDofs;
|
||||
|
||||
int *m_jointIndices;
|
||||
|
||||
double *m_targetPositions;
|
||||
double *m_kps;
|
||||
|
||||
double *m_targetVelocities;
|
||||
double *m_kds;
|
||||
|
||||
double *m_forces;
|
||||
|
||||
b3RobotSimulatorJointMotorArrayArgs(int controlMode, int numControlledDofs)
|
||||
: m_controlMode(controlMode), m_numControlledDofs(numControlledDofs)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct b3RobotSimulatorGetCameraImageArgs
|
||||
{
|
||||
int m_width;
|
||||
int m_height;
|
||||
float *m_viewMatrix;
|
||||
float *m_projectionMatrix;
|
||||
float *m_lightDirection;
|
||||
float *m_lightColor;
|
||||
float m_lightDistance;
|
||||
int m_hasShadow;
|
||||
float m_lightAmbientCoeff;
|
||||
float m_lightDiffuseCoeff;
|
||||
float m_lightSpecularCoeff;
|
||||
int m_renderer;
|
||||
|
||||
b3RobotSimulatorGetCameraImageArgs(int width, int height)
|
||||
: m_width(width),
|
||||
m_height(height),
|
||||
m_viewMatrix(NULL),
|
||||
m_projectionMatrix(NULL),
|
||||
m_lightDirection(NULL),
|
||||
m_lightColor(NULL),
|
||||
m_lightDistance(-1),
|
||||
m_hasShadow(-1),
|
||||
m_lightAmbientCoeff(-1),
|
||||
m_lightDiffuseCoeff(-1),
|
||||
m_lightSpecularCoeff(-1),
|
||||
m_renderer(-1)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct b3RobotSimulatorSetPhysicsEngineParameters
|
||||
{
|
||||
double m_fixedTimeStep;
|
||||
int m_numSolverIterations;
|
||||
int m_useSplitImpulse;
|
||||
double m_splitImpulsePenetrationThreshold;
|
||||
int m_numSubSteps;
|
||||
int m_collisionFilterMode;
|
||||
double m_contactBreakingThreshold;
|
||||
int m_maxNumCmdPer1ms;
|
||||
int m_enableFileCaching;
|
||||
double m_restitutionVelocityThreshold;
|
||||
double m_erp;
|
||||
double m_contactERP;
|
||||
double m_frictionERP;
|
||||
|
||||
b3RobotSimulatorSetPhysicsEngineParameters()
|
||||
: m_fixedTimeStep(-1),
|
||||
m_numSolverIterations(-1),
|
||||
m_useSplitImpulse(-1),
|
||||
m_splitImpulsePenetrationThreshold(-1),
|
||||
m_numSubSteps(-1),
|
||||
m_collisionFilterMode(-1),
|
||||
m_contactBreakingThreshold(-1),
|
||||
m_maxNumCmdPer1ms(-1),
|
||||
m_enableFileCaching(-1),
|
||||
m_restitutionVelocityThreshold(-1),
|
||||
m_erp(-1),
|
||||
m_contactERP(-1),
|
||||
m_frictionERP(-1)
|
||||
{}
|
||||
};
|
||||
|
||||
struct b3RobotSimulatorChangeDynamicsArgs
|
||||
{
|
||||
double m_mass;
|
||||
double m_lateralFriction;
|
||||
double m_spinningFriction;
|
||||
double m_rollingFriction;
|
||||
double m_restitution;
|
||||
double m_linearDamping;
|
||||
double m_angularDamping;
|
||||
double m_contactStiffness;
|
||||
double m_contactDamping;
|
||||
int m_frictionAnchor;
|
||||
|
||||
b3RobotSimulatorChangeDynamicsArgs()
|
||||
: m_mass(-1),
|
||||
m_lateralFriction(-1),
|
||||
m_spinningFriction(-1),
|
||||
m_rollingFriction(-1),
|
||||
m_restitution(-1),
|
||||
m_linearDamping(-1),
|
||||
m_angularDamping(-1),
|
||||
m_contactStiffness(-1),
|
||||
m_contactDamping(-1),
|
||||
m_frictionAnchor(-1)
|
||||
{}
|
||||
};
|
||||
|
||||
struct b3RobotSimulatorAddUserDebugLineArgs
|
||||
{
|
||||
double m_colorRGB[3];
|
||||
double m_lineWidth;
|
||||
double m_lifeTime;
|
||||
int m_parentObjectUniqueId;
|
||||
int m_parentLinkIndex;
|
||||
|
||||
b3RobotSimulatorAddUserDebugLineArgs()
|
||||
:
|
||||
m_lineWidth(1),
|
||||
m_lifeTime(0),
|
||||
m_parentObjectUniqueId(-1),
|
||||
m_parentLinkIndex(-1)
|
||||
{
|
||||
m_colorRGB[0] = 1;
|
||||
m_colorRGB[1] = 1;
|
||||
m_colorRGB[2] = 1;
|
||||
}
|
||||
};
|
||||
|
||||
enum b3AddUserDebugTextFlags {
|
||||
DEBUG_TEXT_HAS_ORIENTATION = 1
|
||||
};
|
||||
|
||||
struct b3RobotSimulatorAddUserDebugTextArgs
|
||||
{
|
||||
double m_colorRGB[3];
|
||||
double m_size;
|
||||
double m_lifeTime;
|
||||
double m_textOrientation[4];
|
||||
int m_parentObjectUniqueId;
|
||||
int m_parentLinkIndex;
|
||||
int m_flags;
|
||||
|
||||
b3RobotSimulatorAddUserDebugTextArgs()
|
||||
: m_size(1),
|
||||
m_lifeTime(0),
|
||||
m_parentObjectUniqueId(-1),
|
||||
m_parentLinkIndex(-1),
|
||||
m_flags(0)
|
||||
{
|
||||
m_colorRGB[0] = 1;
|
||||
m_colorRGB[1] = 1;
|
||||
m_colorRGB[2] = 1;
|
||||
|
||||
m_textOrientation[0] = 0;
|
||||
m_textOrientation[1] = 0;
|
||||
m_textOrientation[2] = 0;
|
||||
m_textOrientation[3] = 1;
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
struct b3RobotSimulatorGetContactPointsArgs
|
||||
{
|
||||
int m_bodyUniqueIdA;
|
||||
int m_bodyUniqueIdB;
|
||||
int m_linkIndexA;
|
||||
int m_linkIndexB;
|
||||
|
||||
b3RobotSimulatorGetContactPointsArgs()
|
||||
: m_bodyUniqueIdA(-1),
|
||||
m_bodyUniqueIdB(-1),
|
||||
m_linkIndexA(-2),
|
||||
m_linkIndexB(-2)
|
||||
{}
|
||||
};
|
||||
|
||||
struct b3RobotSimulatorCreateCollisionShapeArgs
|
||||
{
|
||||
int m_shapeType;
|
||||
double m_radius;
|
||||
b3Vector3 m_halfExtents;
|
||||
double m_height;
|
||||
char* m_fileName;
|
||||
b3Vector3 m_meshScale;
|
||||
b3Vector3 m_planeNormal;
|
||||
int m_flags;
|
||||
b3RobotSimulatorCreateCollisionShapeArgs()
|
||||
: m_shapeType(-1),
|
||||
m_radius(0.5),
|
||||
m_height(1),
|
||||
m_fileName(NULL),
|
||||
m_flags(0)
|
||||
{
|
||||
m_halfExtents.m_floats[0] = 1;
|
||||
m_halfExtents.m_floats[1] = 1;
|
||||
m_halfExtents.m_floats[2] = 1;
|
||||
|
||||
m_meshScale.m_floats[0] = 1;
|
||||
m_meshScale.m_floats[1] = 1;
|
||||
m_meshScale.m_floats[2] = 1;
|
||||
|
||||
m_planeNormal.m_floats[0] = 0;
|
||||
m_planeNormal.m_floats[1] = 0;
|
||||
m_planeNormal.m_floats[2] = 1;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
struct b3RobotSimulatorCreateMultiBodyArgs
|
||||
{
|
||||
double m_baseMass;
|
||||
int m_baseCollisionShapeIndex;
|
||||
int m_baseVisualShapeIndex;
|
||||
b3Vector3 m_basePosition;
|
||||
b3Quaternion m_baseOrientation;
|
||||
b3Vector3 m_baseInertialFramePosition;
|
||||
b3Quaternion m_baseInertialFrameOrientation;
|
||||
|
||||
int m_numLinks;
|
||||
double *m_linkMasses;
|
||||
int *m_linkCollisionShapeIndices;
|
||||
int *m_linkVisualShapeIndices;
|
||||
b3Vector3 *m_linkPositions;
|
||||
b3Quaternion *m_linkOrientations;
|
||||
b3Vector3 *m_linkInertialFramePositions;
|
||||
b3Quaternion *m_linkInertialFrameOrientations;
|
||||
int *m_linkParentIndices;
|
||||
int *m_linkJointTypes;
|
||||
b3Vector3 *m_linkJointAxes;
|
||||
|
||||
int m_useMaximalCoordinates;
|
||||
|
||||
b3RobotSimulatorCreateMultiBodyArgs()
|
||||
: m_numLinks(0), m_baseMass(0), m_baseCollisionShapeIndex(-1), m_baseVisualShapeIndex(-1), m_useMaximalCoordinates(0),
|
||||
m_linkMasses(NULL),
|
||||
m_linkCollisionShapeIndices(NULL),
|
||||
m_linkVisualShapeIndices(NULL),
|
||||
m_linkPositions(NULL),
|
||||
m_linkOrientations(NULL),
|
||||
m_linkInertialFramePositions(NULL),
|
||||
m_linkInertialFrameOrientations(NULL),
|
||||
m_linkParentIndices(NULL),
|
||||
m_linkJointTypes(NULL),
|
||||
m_linkJointAxes(NULL)
|
||||
{
|
||||
m_basePosition.setValue(0,0,0);
|
||||
m_baseOrientation.setValue(0,0,0,1);
|
||||
m_baseInertialFramePosition.setValue(0,0,0);
|
||||
m_baseInertialFrameOrientation.setValue(0,0,0,1);
|
||||
}
|
||||
};
|
||||
|
||||
///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
|
||||
///as documented in the pybullet Quickstart Guide
|
||||
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
|
||||
@@ -194,6 +453,10 @@ public:
|
||||
|
||||
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args);
|
||||
|
||||
bool setJointMotorControlArray(int bodyUniqueId, int controlMode, int numControlledDofs,
|
||||
int *jointIndices, double *targetVelocities, double *targetPositions,
|
||||
double *forces, double *kps, double *kds);
|
||||
|
||||
void stepSimulation();
|
||||
|
||||
bool canSubmitCommand() const;
|
||||
@@ -226,6 +489,75 @@ public:
|
||||
|
||||
void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1);
|
||||
|
||||
// JFC: added these 24 methods
|
||||
|
||||
void getMouseEvents(b3MouseEventsData* mouseEventsData);
|
||||
|
||||
bool getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeInverseKinematics, b3LinkState* linkState);
|
||||
|
||||
bool getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, b3CameraImageData &imageData);
|
||||
|
||||
bool calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities, double *jointAccelerations, double *jointForcesOutput);
|
||||
|
||||
int getNumBodies() const;
|
||||
|
||||
int getBodyUniqueId(int bodyId) const;
|
||||
|
||||
bool removeBody(int bodyUniqueId);
|
||||
|
||||
bool getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo);
|
||||
|
||||
bool changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args);
|
||||
|
||||
int addUserDebugParameter(char *paramName, double rangeMin, double rangeMax, double startValue);
|
||||
|
||||
double readUserDebugParameter(int itemUniqueId);
|
||||
|
||||
bool removeUserDebugItem(int itemUniqueId);
|
||||
|
||||
int addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args);
|
||||
|
||||
int addUserDebugText(char *text, b3Vector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args);
|
||||
|
||||
int addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args);
|
||||
|
||||
int addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args);
|
||||
|
||||
bool setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args);
|
||||
|
||||
bool setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args);
|
||||
|
||||
bool applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags);
|
||||
|
||||
bool applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags);
|
||||
|
||||
bool applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags);
|
||||
|
||||
bool applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags);
|
||||
|
||||
bool enableJointForceTorqueSensor(int bodyUniqueId, int linkIndex, bool enable);
|
||||
|
||||
bool getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo);
|
||||
|
||||
bool getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo);
|
||||
|
||||
bool getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo);
|
||||
|
||||
bool getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData);
|
||||
|
||||
bool getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData);
|
||||
|
||||
bool getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax);
|
||||
|
||||
bool getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax);
|
||||
|
||||
int createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args);
|
||||
|
||||
int createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args);
|
||||
|
||||
int getNumConstraints() const;
|
||||
|
||||
int getConstraintUniqueId(int serialIndex);
|
||||
|
||||
//////////////// INTERNAL
|
||||
|
||||
|
||||
Reference in New Issue
Block a user