PyBullet: avoid calling syncBodies for each DOF in pdControl.

Allow PD_CONTROL in setJointMotorControlArray.

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -5581,6 +5581,11 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
 				args.m_ints[1] = bodyUniqueId;
 				//find the joint motors and apply the desired velocity and maximum force/torque
 				{
+					args.m_numInts = 0;
+					args.m_numFloats = 0;
+					//syncBodies is expensive/slow, use it only once
+					m_data->m_pluginManager.executePluginCommand(m_data->m_pdControlPlugin, &args);
+
 					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++)

examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp

@@ -89,7 +89,11 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
 {
 	MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
 
-	obj->m_api.syncBodies();
+	if (arguments->m_numInts == 0)
+	{
+		obj->m_api.syncBodies();
+		return -1;
+	}
 
 	int numObj = obj->m_api.getNumBodies();
 	//printf("numObj = %d\n", numObj);

examples/pybullet/pybullet.c

@@ -2039,7 +2039,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
 			(controlMode != CONTROL_MODE_TORQUE) &&
 			(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD))
 		{
-			PyErr_SetString(SpamError, "Illegral control mode.");
+			PyErr_SetString(SpamError, "Illegal control mode.");
 			return NULL;
 		}
 
@@ -2140,9 +2140,10 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
 
 		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.");
+			PyErr_SetString(SpamError, "Illegal control mode.");
 			return NULL;
 		}
 
@@ -2340,7 +2341,7 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
 					break;
 				}
 
-				case CONTROL_MODE_POSITION_VELOCITY_PD:
+				default:
 				{
 					b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex,
 													 targetPosition);
@@ -2351,9 +2352,6 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
 					b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
 					break;
 				}
-				default:
-				{
-				}
 			};
 		}
 
@@ -2440,7 +2438,7 @@ static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args,
 			(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD) &&
 			(controlMode != CONTROL_MODE_PD))
 		{
-			PyErr_SetString(SpamError, "Illegral control mode.");
+			PyErr_SetString(SpamError, "Illegal control mode.");
 			return NULL;
 		}