Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests 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
parent 75ec8f3fd9 d660042d76
commit 23f9250e04
6 changed files with 95 additions and 12 deletions
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)