From 3eebcd40ca851d536dcfd2733647f5e8167a8e5e Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 9 Jun 2018 19:40:12 -0700 Subject: [PATCH] exposed a few more methods in the C++ b3RobotSimulatorClientAPI (_NoDirect base class) --- .../RobotSimulator/HelloBulletRobotics.cpp | 23 ++- .../PhysicsServerCommandProcessor.cpp | 23 ++- .../b3RobotSimulatorClientAPI_NoDirect.cpp | 118 +++++++++++++-- .../b3RobotSimulatorClientAPI_NoDirect.h | 134 +++++++++++++----- 4 files changed, 246 insertions(+), 52 deletions(-) diff --git a/examples/RobotSimulator/HelloBulletRobotics.cpp b/examples/RobotSimulator/HelloBulletRobotics.cpp index 3b131d17f..da8f58e4e 100644 --- a/examples/RobotSimulator/HelloBulletRobotics.cpp +++ b/examples/RobotSimulator/HelloBulletRobotics.cpp @@ -6,6 +6,8 @@ int main(int argc, char* argv[]) b3RobotSimulatorClientAPI_NoGUI* sim = new b3RobotSimulatorClientAPI_NoGUI(); bool isConnected = sim->connect(eCONNECT_SHARED_MEMORY); + + if (!isConnected) { printf("Using Direct mode\n"); @@ -14,23 +16,36 @@ int main(int argc, char* argv[]) { printf("Using shared memory\n"); } + + //remove all existing objects (if any) sim->resetSimulation(); + sim->setGravity(btVector3(0,0,-9.8)); + sim->setNumSolverIterations(100); + b3RobotSimulatorSetPhysicsEngineParameters args; + sim->getPhysicsEngineParameters(args); + int planeUid = sim->loadURDF("plane.urdf"); printf("planeUid = %d\n", planeUid); int r2d2Uid = sim->loadURDF("r2d2.urdf"); printf("r2d2 #joints = %d\n", sim->getNumJoints(r2d2Uid)); - - btVector3 basePosition = btVector3(0,0,1); + + + btVector3 basePosition = btVector3(0,0,0.5); btQuaternion baseOrientation = btQuaternion(0,0,0,1); - sim->resetBasePositionAndOrientation(r2d2Uid, basePosition, baseOrientation); - sim->setGravity(btVector3(0,0,-10)); + sim->resetBasePositionAndOrientation(r2d2Uid, basePosition, baseOrientation); + while (sim->isConnected()) { + btVector3 basePos; + btQuaternion baseOrn; + sim->getBasePositionAndOrientation(r2d2Uid,basePos,baseOrn); + printf("r2d2 basePosition = [%f,%f,%f]\n", basePos[0],basePos[1],basePos[2]); + sim->stepSimulation(); } delete sim; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index ec82fe1a6..b7ebc132f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6859,24 +6859,45 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom bool hasStatus = true; SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED; + + serverCmd.m_simulationParameterResultArgs.m_allowedCcdPenetration = m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration; serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode; + serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime; serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold; + serverCmd.m_simulationParameterResultArgs.m_contactSlop = m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop; + serverCmd.m_simulationParameterResultArgs.m_defaultGlobalCFM = m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm; serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2; serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp; serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime; + serverCmd.m_simulationParameterResultArgs.m_deterministicOverlappingPairs = m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs; + serverCmd.m_simulationParameterResultArgs.m_enableConeFriction = (m_data->m_dynamicsWorld->getSolverInfo().m_solverMode &SOLVER_DISABLE_IMPLICIT_CONE_FRICTION)? 0 : 1; serverCmd.m_simulationParameterResultArgs.m_enableFileCaching = b3IsFileCachingEnabled(); + serverCmd.m_simulationParameterResultArgs.m_frictionCFM = m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM; serverCmd.m_simulationParameterResultArgs.m_frictionERP = m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP; btVector3 grav = m_data->m_dynamicsWorld->getGravity(); serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav[0]; serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav[1]; serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2]; serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags; + serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode = 0; + if (gJointFeedbackInWorldSpace) + { + serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_WORLD_SPACE; + } + if (gJointFeedbackInJointFrame ) + { + serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_JOINT_FRAME; + } + serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = m_data->m_numSimulationSubSteps; serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numIterations; serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold; + + serverCmd.m_simulationParameterResultArgs.m_solverResidualThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold; serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold; serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = m_data->m_useRealTimeSimulation; serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse; + return hasStatus; } @@ -7010,7 +7031,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM) { - m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionCFM; + m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM = clientCmd.m_physSimParamArgs.m_frictionCFM; } if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD) diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 52ee8faf9..fda38058d 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -224,6 +224,63 @@ bool b3RobotSimulatorClientAPI_NoDirect::loadMJCF(const std::string& fileName, b return true; } +bool b3RobotSimulatorClientAPI_NoDirect::savePythonWorld(const std::string& fileName) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command; + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + + if (fileName.length()==0) + { + return false; + } + + command = b3SaveWorldCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_SAVE_WORLD_COMPLETED) + { + return false; + } + return true; +} + +bool b3RobotSimulatorClientAPI_NoDirect::saveBullet(const std::string& fileName) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command; + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + + if (fileName.length()==0) + { + return false; + } + + command = b3SaveBulletCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_BULLET_SAVING_COMPLETED) + { + return false; + } + return true; +} + + bool b3RobotSimulatorClientAPI_NoDirect::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results) { if (!isConnected()) @@ -534,6 +591,19 @@ void b3RobotSimulatorClientAPI_NoDirect::removeConstraint(int constraintId) b3GetStatusType(statusHandle); } +bool b3RobotSimulatorClientAPI_NoDirect::getConstraintInfo(int constraintUniqueId, struct b3UserConstraint& constraintInfo) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + if (b3GetUserConstraintInfo(m_data->m_physicsClientHandle, constraintUniqueId, &constraintInfo)) + { + return true; + } + return false; +} bool b3RobotSimulatorClientAPI_NoDirect::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state) { @@ -1478,8 +1548,34 @@ bool b3RobotSimulatorClientAPI_NoDirect::setJointMotorControlArray(int bodyUniqu return true; } + +bool b3RobotSimulatorClientAPI_NoDirect::getPhysicsEngineParameters(struct b3RobotSimulatorSetPhysicsEngineParameters &args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } -bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args) + { + b3SharedMemoryCommandHandle command = b3InitRequestPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType!=CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED) + { + return false; + } + b3GetStatusPhysicsSimulationParameters(statusHandle,&args); + } + return true; +} + + + +bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(const struct b3RobotSimulatorSetPhysicsEngineParameters &args) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1497,12 +1593,12 @@ bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3Robo b3PhysicsParamSetCollisionFilterMode(command, args.m_collisionFilterMode); } - if (args.m_numSubSteps >= 0) { - b3PhysicsParamSetNumSubSteps(command, args.m_numSubSteps); + if (args.m_numSimulationSubSteps >= 0) { + b3PhysicsParamSetNumSubSteps(command, args.m_numSimulationSubSteps); } - if (args.m_fixedTimeStep >= 0) { - b3PhysicsParamSetTimeStep(command, args.m_fixedTimeStep); + if (args.m_deltaTime >= 0) { + b3PhysicsParamSetTimeStep(command, args.m_deltaTime); } if (args.m_useSplitImpulse >= 0) { @@ -1517,10 +1613,6 @@ bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3Robo 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); } @@ -1529,12 +1621,12 @@ bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3Robo b3PhysicsParamSetEnableFileCaching(command, args.m_enableFileCaching); } - if (args.m_erp>=0) { - b3PhysicsParamSetDefaultNonContactERP(command,args.m_erp); + if (args.m_defaultNonContactERP>=0) { + b3PhysicsParamSetDefaultNonContactERP(command,args.m_defaultNonContactERP); } - if (args.m_contactERP>=0) { - b3PhysicsParamSetDefaultContactERP(command,args.m_contactERP); + if (args.m_defaultContactERP>=0) { + b3PhysicsParamSetDefaultContactERP(command,args.m_defaultContactERP); } if (args.m_frictionERP >=0) { diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index 3714d17f2..b71d4fad8 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -200,39 +200,50 @@ struct b3RobotSimulatorGetCameraImageArgs } }; -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; - double m_solverResidualThreshold; + + +struct b3RobotSimulatorSetPhysicsEngineParameters : b3PhysicsSimulationParameters +{ + 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), - m_solverResidualThreshold(-1) - {} + { + + m_deltaTime=-1; + m_gravityAcceleration[3]; + m_numSimulationSubSteps; + m_numSolverIterations=-1; + m_useRealTimeSimulation = -1; + m_useSplitImpulse = -1; + m_splitImpulsePenetrationThreshold =-1; + m_contactBreakingThreshold = -1; + m_internalSimFlags = -1; + m_defaultContactERP = -1; + m_collisionFilterMode = -1; + m_enableFileCaching = -1; + m_restitutionVelocityThreshold = -1; + m_defaultNonContactERP = -1; + m_frictionERP = -1; + m_defaultGlobalCFM = -1; + m_frictionCFM = -1; + m_enableConeFriction = -1; + m_deterministicOverlappingPairs = -1; + m_allowedCcdPenetration = -1; + m_jointFeedbackMode = -1; + m_solverResidualThreshold = -1; + m_contactSlop = -1; + + m_collisionFilterMode=-1; + m_contactBreakingThreshold=-1; + + m_enableFileCaching=-1; + m_restitutionVelocityThreshold=-1; + + m_frictionERP=-1; + m_solverResidualThreshold=-1; + + + } }; struct b3RobotSimulatorChangeDynamicsArgs @@ -410,6 +421,54 @@ struct b3RobotSimulatorCreateMultiBodyArgs }; +struct b3RobotJointInfo : public b3JointInfo +{ + b3RobotJointInfo() + { + m_linkName[0] = 0; + m_jointName[0] = 0; + m_jointType = eFixedType; + m_qIndex = -1; + m_uIndex = -1; + m_jointIndex = -1; + m_flags = 0; + m_jointDamping = 0; + m_jointFriction = 0; + m_jointLowerLimit = 1; + m_jointUpperLimit = -1; + m_jointMaxForce = 500; + m_jointMaxVelocity = 100; + m_parentIndex = -1; + + //position + m_parentFrame[0] = 0; + m_parentFrame[1] = 0; + m_parentFrame[2] = 0; + //orientation quaternion [x,y,z,w] + m_parentFrame[3] = 0; + m_parentFrame[4] = 0; + m_parentFrame[5] = 0; + m_parentFrame[6] = 1; + + //position + m_childFrame[0] = 0; + m_childFrame[1] = 0; + m_childFrame[2] = 0; + //orientation quaternion [x,y,z,w] + m_childFrame[3] = 0; + m_childFrame[4] = 0; + m_childFrame[5] = 0; + m_childFrame[6] = 1; + + + m_jointAxis[0] = 0; + m_jointAxis[1] = 0; + m_jointAxis[2] = 1; + } +}; + + + class b3RobotSimulatorClientAPI_NoDirect { protected: @@ -441,7 +500,10 @@ public: bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs()); bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); - + bool saveBullet(const std::string& fileName); + + bool savePythonWorld(const std::string& fileName); + bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo); bool getBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) const; @@ -457,9 +519,11 @@ public: int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); int changeConstraint(int constraintId, b3JointInfo* jointInfo); - + void removeConstraint(int constraintId); + bool getConstraintInfo(int constraintUniqueId, struct b3UserConstraint& constraintInfo); + bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state); bool getJointStates(int bodyUniqueId, b3JointStates2& state); @@ -540,7 +604,9 @@ public: bool setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args); - bool setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args); + bool setPhysicsEngineParameter(const struct b3RobotSimulatorSetPhysicsEngineParameters&args); + + bool getPhysicsEngineParameters(struct b3RobotSimulatorSetPhysicsEngineParameters&args); bool applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags);