diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 1a7bf5b77..9bc1f91c0 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -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; } @@ -118,13 +134,13 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i switch (mode) { - case eCONNECT_EXISTING_EXAMPLE_BROWSER: + case eCONNECT_EXISTING_EXAMPLE_BROWSER: { sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(m_data->m_guiHelper); break; } - case eCONNECT_GUI: + case eCONNECT_GUI: { int argc = 0; char* argv[1] = {0}; @@ -135,7 +151,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i #endif break; } - case eCONNECT_GUI_SERVER: + case eCONNECT_GUI_SERVER: { int argc = 0; char* argv[1] = {0}; @@ -146,12 +162,12 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i #endif break; } - case eCONNECT_DIRECT: + case eCONNECT_DIRECT: { sm = b3ConnectPhysicsDirect(); break; } - case eCONNECT_SHARED_MEMORY: + case eCONNECT_SHARED_MEMORY: { if (portOrKey >= 0) { @@ -160,7 +176,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i sm = b3ConnectSharedMemory(key); break; } - case eCONNECT_UDP: + case eCONNECT_UDP: { if (portOrKey >= 0) { @@ -175,7 +191,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i break; } - case eCONNECT_TCP: + case eCONNECT_TCP: { if (portOrKey >= 0) { @@ -190,7 +206,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i break; } - default: + default: { b3Warning("connectPhysicsServer unexpected argument"); } @@ -307,7 +323,7 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration) b3SharedMemoryStatusHandle statusHandle; b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); -// b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); + // b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw) @@ -344,8 +360,8 @@ int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struc b3LoadUrdfCommandSetFlags(command,args.m_flags); b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0], - args.m_startPosition[1], - args.m_startPosition[2]); + args.m_startPosition[1], + args.m_startPosition[2]); b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]); if (args.m_forceOverrideFixedBase) { @@ -520,7 +536,7 @@ bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]); b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1], - baseOrientation[2], baseOrientation[3]); + baseOrientation[2], baseOrientation[3]); b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); @@ -548,9 +564,9 @@ bool b3RobotSimulatorClientAPI::getBaseVelocity(int bodyUniqueId, b3Vector3& bas } b3GetStatusActualState(statusHandle, 0 /* body_unique_id */, - 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, - 0 /*root_local_inertial_frame*/, 0, - &actualStateQdot, 0 /* joint_reaction_forces */); + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, 0, + &actualStateQdot, 0 /* joint_reaction_forces */); baseLinearVelocity[0] = actualStateQdot[0]; baseLinearVelocity[1] = actualStateQdot[1]; @@ -734,54 +750,54 @@ bool b3RobotSimulatorClientAPI::getJointStates(int bodyUniqueId, b3JointStates2& } b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - if (statusHandle) - { - // double rootInertialFrame[7]; - const double* rootLocalInertialFrame; - const double* actualStateQ; - const double* actualStateQdot; - const double* jointReactionForces; - - int stat = b3GetStatusActualState(statusHandle, - &state.m_bodyUniqueId, - &state.m_numDegreeOfFreedomQ, - &state.m_numDegreeOfFreedomU, - &rootLocalInertialFrame, - &actualStateQ, - &actualStateQdot, - &jointReactionForces); - if (stat) - { - state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ); - state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - for (int i=0;im_physicsClientHandle, bodyUniqueId); b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex, - targetValue); + targetValue); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); return false; @@ -820,7 +836,7 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint b3SharedMemoryStatusHandle statusHandle; switch (args.m_controlMode) { - case CONTROL_MODE_VELOCITY: + case CONTROL_MODE_VELOCITY: { b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY); b3JointInfo jointInfo; @@ -832,7 +848,7 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); break; } - case CONTROL_MODE_POSITION_VELOCITY_PD: + case CONTROL_MODE_POSITION_VELOCITY_PD: { b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD); b3JointInfo jointInfo; @@ -848,7 +864,7 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); break; } - case CONTROL_MODE_TORQUE: + case CONTROL_MODE_TORQUE: { b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE); b3JointInfo jointInfo; @@ -858,7 +874,7 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); break; } - default: + default: { b3Error("Unknown control command in b3RobotSimulationClientAPI::setJointMotorControl"); } @@ -962,16 +978,16 @@ bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotS int numPos = 0; bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - 0) != 0; + &results.m_bodyUniqueId, + &numPos, + 0) != 0; if (result && numPos) { results.m_calculatedJointPositions.resize(numPos); result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - &results.m_calculatedJointPositions[0]) != 0; + &results.m_bodyUniqueId, + &numPos, + &results.m_calculatedJointPositions[0]) != 0; } return result; } @@ -995,18 +1011,26 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, return false; } -bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState) +bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState) { - if (!isConnected()) - { + 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; } @@ -1117,13 +1141,13 @@ void b3RobotSimulatorClientAPI::resetDebugVisualizerCamera(double cameraDistance b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle); if (commandHandle) { - if ((cameraDistance >= 0)) - { - b3Vector3FloatData camTargetPos; - targetPos.serializeFloat(camTargetPos); - b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats); - } - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + if ((cameraDistance >= 0)) + { + b3Vector3FloatData camTargetPos; + targetPos.serializeFloat(camTargetPos); + b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats); + } + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); } } @@ -1151,9 +1175,902 @@ void b3RobotSimulatorClientAPI::loadSoftBody(const std::string& fileName, double return; } - b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); - b3LoadSoftBodySetScale(command, scale); - b3LoadSoftBodySetMass(command, mass); - b3LoadSoftBodySetCollisionMargin(command, collisionMargin); - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + b3LoadSoftBodySetScale(command, scale); + b3LoadSoftBodySetMass(command, mass); + 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;im_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; +} \ No newline at end of file diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index d2bee5338..37e9c0e9a 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -19,19 +19,19 @@ struct b3RobotSimulatorLoadUrdfFileArgs b3RobotSimulatorLoadUrdfFileArgs(const b3Vector3& startPos, const b3Quaternion& startOrn) : m_startPosition(startPos), - m_startOrientation(startOrn), - m_forceOverrideFixedBase(false), - m_useMultiBody(true), - m_flags(0) + m_startOrientation(startOrn), + m_forceOverrideFixedBase(false), + m_useMultiBody(true), + m_flags(0) { } b3RobotSimulatorLoadUrdfFileArgs() : m_startPosition(b3MakeVector3(0, 0, 0)), - m_startOrientation(b3Quaternion(0, 0, 0, 1)), - m_forceOverrideFixedBase(false), - m_useMultiBody(true), - m_flags(0) + m_startOrientation(b3Quaternion(0, 0, 0, 1)), + m_forceOverrideFixedBase(false), + m_useMultiBody(true), + m_flags(0) { } }; @@ -43,7 +43,7 @@ struct b3RobotSimulatorLoadSdfFileArgs b3RobotSimulatorLoadSdfFileArgs() : m_forceOverrideFixedBase(false), - m_useMultiBody(true) + m_useMultiBody(true) { } }; @@ -70,11 +70,11 @@ struct b3RobotSimulatorJointMotorArgs b3RobotSimulatorJointMotorArgs(int controlMode) : m_controlMode(controlMode), - m_targetPosition(0), - m_kp(0.1), - m_targetVelocity(0), - m_kd(0.9), - m_maxTorqueValue(1000) + m_targetPosition(0), + m_kp(0.1), + m_targetVelocity(0), + m_kd(0.9), + m_maxTorqueValue(1000) { } }; @@ -104,8 +104,8 @@ struct b3RobotSimulatorInverseKinematicArgs b3RobotSimulatorInverseKinematicArgs() : m_bodyUniqueId(-1), - m_endEffectorLinkIndex(-1), - m_flags(0) + m_endEffectorLinkIndex(-1), + m_flags(0) { m_endEffectorTargetPosition[0] = 0; m_endEffectorTargetPosition[1] = 0; @@ -126,13 +126,272 @@ struct b3RobotSimulatorInverseKinematicsResults struct b3JointStates2 { - int m_bodyUniqueId; - int m_numDegreeOfFreedomQ; - int m_numDegreeOfFreedomU; - b3Transform m_rootLocalInertialFrame; - b3AlignedObjectArray m_actualStateQ; - b3AlignedObjectArray m_actualStateQdot; - b3AlignedObjectArray m_jointReactionForces; + int m_bodyUniqueId; + int m_numDegreeOfFreedomQ; + int m_numDegreeOfFreedomU; + b3Transform m_rootLocalInertialFrame; + b3AlignedObjectArray m_actualStateQ; + b3AlignedObjectArray m_actualStateQdot; + b3AlignedObjectArray 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 @@ -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; @@ -224,8 +487,77 @@ public: void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter); void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData); - void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1); + 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