diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b39baeed1..56a083a61 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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); - //#p.executePluginCommand(plugin ,"r2d2.urdf", [1,2,3],[50.0,3.3]) + args.m_ints[0] = eSetPDControl; + 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;linkgetNumLinks();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) diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7a434ec48..5a57d69e5 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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 diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 67091cb46..496faea40 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/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"); diff --git a/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp b/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp index 285b3e40e..e9d23fbd2 100644 --- a/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp +++ b/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++) { diff --git a/examples/pybullet/examples/pdControl.py b/examples/pybullet/examples/pdControl.py index 501a84dc0..505ee3109 100644 --- a/examples/pybullet/examples/pdControl.py +++ b/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.) - \ No newline at end of file + diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b74140751..250f43df3 100644 --- a/examples/pybullet/pybullet.c +++ b/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);