Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1763 from YunfeiBai/master

Browse Source 
Add the pdControlPlugin to the joint control C API, and add the PD co…
This commit is contained in:
erwincoumans
2018-06-16 06:57:50 -07:00
committed by GitHub
parent 5b3e43d4d8 1c0de3c4cb
commit d660042d76
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

@@ -5102,13 +5102,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

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