Merge remote-tracking branch 'bp/master'

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -5104,13 +5104,76 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
 
 		switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
 		{
-/*		case CONTROL_MODE_PD:
+        case CONTROL_MODE_PD:
 		{
+            if (m_data->m_verboseOutput)
+            {
+                b3Printf("Using CONTROL_MODE_PD");
+            }
+            
 			b3PluginArguments args;
-			m_data->m_pluginManager.executePluginCommand(pdControlPlugin, args);
+            args.m_ints[0] = eSetPDControl;
-			//#p.executePluginCommand(plugin ,"r2d2.urdf", [1,2,3],[50.0,3.3])
+            args.m_ints[1] = bodyUniqueId;
+            //find the joint motors and apply the desired velocity and maximum force/torque
+            {
+                int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base
+                int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
+                for (int link=0;link<mb->getNumLinks();link++)
+                {
+                    if (supportsJointMotor(mb,link))
+                    {
+                        bool hasDesiredPosOrVel = false;
+                        btScalar desiredVelocity = 0.f;
+                        if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0)
+                        {
+                            hasDesiredPosOrVel = true;
+                            desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
+                            args.m_floats[2] = 0.1; // kd
+                        }
+                        btScalar desiredPosition = 0.f;
+                        if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0)
+                        {
+                            hasDesiredPosOrVel = true;
+                            desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
+                            args.m_floats[3] = 0.1; // kp
+                        }
+                        
+                        if (hasDesiredPosOrVel)
+                        {
+                            if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP)!=0)
+                            {
+                                args.m_floats[3] = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
+                            }
+                            if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD)!=0)
+                            {
+                                args.m_floats[2] = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
+                            }
+                            
+                            args.m_floats[1] = desiredVelocity;
+                            //clamp position
+                            if (mb->getLink(link).m_jointLowerLimit <= mb->getLink(link).m_jointUpperLimit)
+                            {
+                                btClamp(desiredPosition, mb->getLink(link).m_jointLowerLimit, mb->getLink(link).m_jointUpperLimit);
+                            }
+                            args.m_floats[0] = desiredPosition;
+                            
+                            btScalar maxImp = 1000000.f;
+                            if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
+                                maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex];
+                            args.m_floats[4] = maxImp;
+                            
+                            args.m_ints[2] = link;
+                            args.m_numInts = 3;
+                            args.m_numFloats = 5;
+                            m_data->m_pluginManager.executePluginCommand(m_data->m_pdControlPlugin, &args);
+                        }
+                    }
+                    velIndex += mb->getLink(link).m_dofCount;
+                    posIndex += mb->getLink(link).m_posVarCount;
+                }
+            }
+            break;
 		}
-		*/
 		case CONTROL_MODE_TORQUE:
 			{
 				if (m_data->m_verboseOutput)

examples/SharedMemory/SharedMemoryPublic.h

@@ -659,6 +659,7 @@ enum {
     CONTROL_MODE_VELOCITY=0,
     CONTROL_MODE_TORQUE,
     CONTROL_MODE_POSITION_VELOCITY_PD,
+    CONTROL_MODE_PD, // The standard PD control implemented as soft constraint.
 };
 
 ///flags for b3ApplyExternalTorque and b3ApplyExternalForce

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

@@ -758,6 +758,22 @@ void b3RobotSimulatorClientAPI_NoDirect::setJointMotorControl(int bodyUniqueId,
 			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
 			break;
 		}
+    case CONTROL_MODE_PD:
+    {
+        b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_PD);
+        b3JointInfo jointInfo;
+        b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
+        int uIndex = jointInfo.m_uIndex;
+        int qIndex = jointInfo.m_qIndex;
+        
+        b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition);
+        b3JointControlSetKp(command, uIndex, args.m_kp);
+        b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
+        b3JointControlSetKd(command, uIndex, args.m_kd);
+        b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
+        statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+        break;
+    }
 	default:
 		{
 			b3Error("Unknown control command in b3RobotSimulationClientAPI::setJointMotorControl");

examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp

@@ -122,7 +122,6 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
 		controller.m_maxForce = arguments->m_floats[4];
 		controller.m_objectUniqueId = arguments->m_ints[1];
 		controller.m_linkIndex = arguments->m_ints[2];
-
 		int foundIndex = -1;
 		for (int i = 0; i < obj->m_controllers.size(); i++)
 		{

examples/pybullet/examples/pdControl.py

@@ -14,20 +14,20 @@ for i in range (p.getNumJoints(pole)):
 desiredPosCartId = p.addUserDebugParameter("desiredPosCart",-10,10,2)
 desiredVelCartId = p.addUserDebugParameter("desiredVelCart",-10,10,0)
 kpCartId = p.addUserDebugParameter("kpCart",0,500,300)
-kdCartId = p.addUserDebugParameter("kpCart",0,300,150)
+kdCartId = p.addUserDebugParameter("kdCart",0,300,150)
 maxForceCartId = p.addUserDebugParameter("maxForceCart",0,5000,1000)
 
 
 desiredPosPoleId = p.addUserDebugParameter("desiredPosPole",-10,10,0)
 desiredVelPoleId = p.addUserDebugParameter("desiredVelPole",-10,10,0)
 kpPoleId = p.addUserDebugParameter("kpPole",0,500,200)
-kdPoleId = p.addUserDebugParameter("kpPole",0,300,100)
+kdPoleId = p.addUserDebugParameter("kdPole",0,300,100)
 maxForcePoleId = p.addUserDebugParameter("maxForcePole",0,5000,1000)
 
 pd = p.loadPlugin("pdControlPlugin")
 
 	
-p.setGravity(0,0,0)
+p.setGravity(0,0,-10)
 
 useRealTimeSim = True
 
@@ -44,7 +44,7 @@ while p.isConnected():
 		kdCart = p.readUserDebugParameter(kdCartId)
 		maxForceCart = p.readUserDebugParameter(maxForceCartId)
 		link = 0
-		p.executePluginCommand(pd,"test",[1, pole, link], [desiredPosCart, desiredVelCart, kdCart, kpCart, maxForceCart])
+		p.setJointMotorControl2(bodyUniqueId=pole,jointIndex=link,controlMode=p.PD_CONTROL,targetPosition=desiredPosCart,targetVelocity=desiredVelCart,force=maxForceCart, positionGain=kpCart, velocityGain=kdCart)
 		
 		desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
 		desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
@@ -52,11 +52,11 @@ while p.isConnected():
 		kdPole = p.readUserDebugParameter(kdPoleId)
 		maxForcePole = p.readUserDebugParameter(maxForcePoleId)
 		link = 1
-		p.executePluginCommand(pd,"test",[1, pole, link], [desiredPosPole, desiredVelPole, kdPole, kpPole, maxForcePole])
+		p.setJointMotorControl2(bodyUniqueId=pole,jointIndex=link,controlMode=p.PD_CONTROL,targetPosition=desiredPosPole,targetVelocity=desiredVelPole,force=maxForcePole, positionGain=kpPole, velocityGain=kdPole)
 		
 
 	if (not useRealTimeSim):
 		p.stepSimulation()
 		time.sleep(1./240.)
 	
-	
+	

examples/pybullet/pybullet.c

@@ -2417,7 +2417,8 @@ static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args,
 
 		if ((controlMode != CONTROL_MODE_VELOCITY) &&
 			(controlMode != CONTROL_MODE_TORQUE) &&
-			(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD))
+			(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD) &&
+            (controlMode != CONTROL_MODE_PD))
 		{
 			PyErr_SetString(SpamError, "Illegral control mode.");
 			return NULL;
@@ -2446,6 +2447,7 @@ static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args,
 			}
 
 			case CONTROL_MODE_POSITION_VELOCITY_PD:
+            case CONTROL_MODE_PD:
 			{
 				if (maxVelocity>0)
 				{
@@ -9561,6 +9563,8 @@ initpybullet(void)
 							CONTROL_MODE_VELOCITY);  // user read
 	PyModule_AddIntConstant(m, "POSITION_CONTROL",
 							CONTROL_MODE_POSITION_VELOCITY_PD);  // user read
+    PyModule_AddIntConstant(m, "PD_CONTROL",
+                            CONTROL_MODE_PD);  // user read
 
 	PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME);
 	PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_FRAME);