From 7ac3e263ab6cb9141b4e4a1e84c0925b8180bafa Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 9 Jun 2018 13:54:22 -0700 Subject: [PATCH 1/5] add saveStateToMemory/restoreStateFromMemory/setAdditionalSearchPath/getAPIVersion to Bullet Robotics API. --- .../rigidbody/GpuRigidBodyDemoInternalData.h | 2 +- .../b3RobotSimulatorClientAPI_NoDirect.cpp | 76 +++++++++++++++++++ .../b3RobotSimulatorClientAPI_NoDirect.h | 9 ++- 3 files changed, 85 insertions(+), 2 deletions(-) diff --git a/examples/OpenCL/rigidbody/GpuRigidBodyDemoInternalData.h b/examples/OpenCL/rigidbody/GpuRigidBodyDemoInternalData.h index 8c7b5f5bb..770fb017a 100644 --- a/examples/OpenCL/rigidbody/GpuRigidBodyDemoInternalData.h +++ b/examples/OpenCL/rigidbody/GpuRigidBodyDemoInternalData.h @@ -17,7 +17,7 @@ struct GpuRigidBodyDemoInternalData class b3GpuNarrowPhase* m_np; class b3GpuBroadphaseInterface* m_bp; - class b3DynamicBvhBroadphase* m_broadphaseDbvt; + struct b3DynamicBvhBroadphase* m_broadphaseDbvt; b3Vector3 m_pickPivotInA; b3Vector3 m_pickPivotInB; diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 3d9e4be22..52ee8faf9 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -2013,6 +2013,65 @@ bool b3RobotSimulatorClientAPI_NoDirect::getCollisionShapeData(int bodyUniqueId, return true; } +int b3RobotSimulatorClientAPI_NoDirect::saveStateToMemory() +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command; + + int stateId = -1; + + if (sm == 0) + { + return -1; + } + + command = b3SaveStateCommandInit(sm); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + + if (statusType != CMD_SAVE_STATE_COMPLETED) + { + return -1; + } + + stateId = b3GetStatusGetStateId(statusHandle); + return stateId; +} + +void b3RobotSimulatorClientAPI_NoDirect::restoreStateFromMemory(int stateId) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return; + } + + int statusType; + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int physicsClientId = 0; + + command = b3LoadStateCommandInit(sm); + if (stateId >= 0) + { + b3LoadStateSetStateId(command, stateId); + } +// if (fileName) +// { +// b3LoadStateSetFileName(command, fileName); +// } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); +} + bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; @@ -2036,3 +2095,20 @@ bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3 return true; } } + +void b3RobotSimulatorClientAPI_NoDirect::setAdditionalSearchPath(std::string path) +{ + int physicsClientId = 0; + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return; + } + if (path.length()) + { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + commandHandle = b3SetAdditionalSearchPath(sm, path.c_str()); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + } +} diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index 0e80d3df6..3714d17f2 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -582,8 +582,15 @@ public: bool getCollisionShapeData(int bodyUniqueId, int linkIndex, b3CollisionShapeInformation &collisionShapeInfo); bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo); - + int saveStateToMemory(); + void restoreStateFromMemory(int stateId); + + int getAPIVersion() const + { + return SHARED_MEMORY_MAGIC_NUMBER; + } + void setAdditionalSearchPath(std::string path); }; From 3eebcd40ca851d536dcfd2733647f5e8167a8e5e Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 9 Jun 2018 19:40:12 -0700 Subject: [PATCH 2/5] 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); From 38469deec78cc4d980e32f5cd0d4df98f2ad5145 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 11 Jun 2018 11:41:33 -0700 Subject: [PATCH 3/5] getNumLinks -> getNumDofs, to make sure fixed objects don't transfer activation state --- examples/Importers/ImportURDFDemo/URDF2Bullet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 6eee15418..171619666 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -595,7 +595,7 @@ void ConvertURDF2BulletInternal( } } else { - if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumLinks()==0) + if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumDofs()==0) { //col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT); col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT); From ad0c7ecaa2b0756e4ad34893a4cf882de6b01815 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 11 Jun 2018 14:42:26 -0700 Subject: [PATCH 4/5] different way of sorting pairs (using indexing), don't 'clear' all pairs. --- .../btOverlappingPairCache.cpp | 64 +++++++++++++++++++ .../btOverlappingPairCache.h | 6 ++ .../btCollisionDispatcher.cpp | 10 +-- 3 files changed, 73 insertions(+), 7 deletions(-) diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index 55ebf06f1..da1335702 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -395,6 +395,70 @@ void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* } } +struct MyPairIndex +{ + int m_orgIndex; + int m_uidA0; + int m_uidA1; +}; + +class MyPairIndeSortPredicate +{ +public: + + bool operator() ( const MyPairIndex& a, const MyPairIndex& b ) const + { + const int uidA0 = a.m_uidA0; + const int uidB0 = b.m_uidA0; + const int uidA1 = a.m_uidA1; + const int uidB1 = b.m_uidA1; + return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1); + } +}; + +void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo) +{ + if (dispatchInfo.m_deterministicOverlappingPairs) + { + btBroadphasePairArray& pa = getOverlappingPairArray(); + btAlignedObjectArray indices; + { + BT_PROFILE("sortOverlappingPairs"); + indices.resize(pa.size()); + for (int i=0;im_uniqueId : -1; + const int uidA1 = p.m_pProxy1 ? p.m_pProxy1->m_uniqueId : -1; + + indices[i].m_uidA0 = uidA0; + indices[i].m_uidA1 = uidA1; + indices[i].m_orgIndex = i; + } + indices.quickSort(MyPairIndeSortPredicate()); + } + { + BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs"); + int i; + for (i=0;iprocessOverlap(*pair)) + { + removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher); + } else + { + i++; + } + } + } + } else + { + processAllOverlappingPairs(callback, dispatcher); + } +} + + void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) { ///need to keep hashmap in sync with pair address, so rebuild all diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index f7be7d45b..202606d23 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -78,6 +78,10 @@ public: virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0; + virtual void processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo) + { + processAllOverlappingPairs(callback, dispatcher, dispatchInfo); + } virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0; virtual bool hasDeferredRemoval() = 0; @@ -144,6 +148,8 @@ public: virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); + virtual void processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo); + virtual btBroadphasePair* getOverlappingPairArrayPtr() { return &m_overlappingPairArray[0]; diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index af521ea9c..9889f0de7 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -246,20 +246,16 @@ public: + void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) { //m_blockedForChanges = true; btCollisionPairCallback collisionCallback(dispatchInfo,this); - if (dispatchInfo.m_deterministicOverlappingPairs) - { - BT_PROFILE("sortOverlappingPairs"); - pairCache->sortOverlappingPairs(this); - } - { + { BT_PROFILE("processAllOverlappingPairs"); - pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); + pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher, dispatchInfo); } //m_blockedForChanges = false; From 62485abafe0134bf2f8685bd3d2a2057e1c61440 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 11 Jun 2018 17:32:44 -0700 Subject: [PATCH 5/5] PyBullet: disable deterministic pairs by default PyBullet: Only set static when sleeping is enables (todo: figure out issue with determinism) --- examples/Importers/ImportURDFDemo/URDF2Bullet.cpp | 14 +++++++++----- .../BroadphaseCollision/btDispatcher.h | 2 +- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 171619666..b336659f2 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -333,11 +333,12 @@ void ConvertURDF2BulletInternal( btRigidBody* linkRigidBody = 0; btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame; + bool canSleep = (flags & CUF_ENABLE_SLEEPING)!=0; if (!createMultiBody) { btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape); - bool canSleep = (flags & CUF_ENABLE_SLEEPING)!=0; + if (!canSleep) { body->forceActivationState(DISABLE_DEACTIVATION); @@ -365,7 +366,7 @@ void ConvertURDF2BulletInternal( if (cache.m_bulletMultiBody==0) { - bool canSleep = (flags & CUF_ENABLE_SLEEPING)!=0; + bool isFixedBase = (mass==0);//todo: figure out when base is fixed int totalNumJoints = cache.m_totalNumJoints1; cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep); @@ -595,10 +596,13 @@ void ConvertURDF2BulletInternal( } } else { - if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumDofs()==0) + if (canSleep) { - //col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT); - col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT); + if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumDofs()==0) + { + //col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT); + col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT); + } } diff --git a/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/src/BulletCollision/BroadphaseCollision/btDispatcher.h index d7a97e8e0..a0e4c1892 100644 --- a/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -47,7 +47,7 @@ struct btDispatcherInfo m_allowedCcdPenetration(btScalar(0.04)), m_useConvexConservativeDistanceUtil(false), m_convexConservativeDistanceThreshold(0.0f), - m_deterministicOverlappingPairs(true) + m_deterministicOverlappingPairs(false) { }