AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
erwincoumans 2018-06-16 06:58:10 -07:00
commit 23f9250e04
6 changed files with 95 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -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;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)

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -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

16
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@ -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");

1
examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
View File

@ -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++)
{

12
examples/pybullet/examples/pdControl.py
View File

@ -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.)

6
examples/pybullet/pybullet.c
View File

@ -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);