exposed a few more methods in the C++ b3RobotSimulatorClientAPI (_NoDirect base class)

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->resetBasePositionAndOrientation(r2d2Uid, basePosition, baseOrientation);
-	sim->setGravity(btVector3(0,0,-10));
+	
 	
 	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;

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)

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)	{
+	if (args.m_numSimulationSubSteps >= 0)	{
-		b3PhysicsParamSetNumSubSteps(command, args.m_numSubSteps);
+		b3PhysicsParamSetNumSubSteps(command, args.m_numSimulationSubSteps);
 	}
 
-	if (args.m_fixedTimeStep >= 0) {
+	if (args.m_deltaTime >= 0) {
-		b3PhysicsParamSetTimeStep(command, args.m_fixedTimeStep);
+		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) {
+	if (args.m_defaultNonContactERP>=0) {
-		b3PhysicsParamSetDefaultNonContactERP(command,args.m_erp);
+		b3PhysicsParamSetDefaultNonContactERP(command,args.m_defaultNonContactERP);
 	}
 
-	if (args.m_contactERP>=0) {
+	if (args.m_defaultContactERP>=0) {
-		b3PhysicsParamSetDefaultContactERP(command,args.m_contactERP);
+		b3PhysicsParamSetDefaultContactERP(command,args.m_defaultContactERP);
 	}
 
 	if (args.m_frictionERP >=0) {

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_deltaTime=-1;
-		m_splitImpulsePenetrationThreshold(-1),
+		m_gravityAcceleration[3];
-		m_numSubSteps(-1),
+		m_numSimulationSubSteps;
-		m_collisionFilterMode(-1),
+		m_numSolverIterations=-1;
-		m_contactBreakingThreshold(-1),
+		m_useRealTimeSimulation = -1;
-		m_maxNumCmdPer1ms(-1),
+		m_useSplitImpulse = -1;
-		m_enableFileCaching(-1),
+		m_splitImpulsePenetrationThreshold =-1;
-		m_restitutionVelocityThreshold(-1),
+		m_contactBreakingThreshold = -1;
-		m_erp(-1),
+		m_internalSimFlags = -1;
-		m_contactERP(-1),
+		m_defaultContactERP = -1;
-		m_frictionERP(-1),
+		m_collisionFilterMode = -1;
-	  	m_solverResidualThreshold(-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);