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

move PD control from PhysicsServerCommandProcessor into btMultiBodyJointMotor

Browse Source 
improvements/changes in pybullet API
This commit is contained in:
Erwin Coumans
2016-06-24 11:06:56 -07:00
parent 6d1948e79e
commit c17c39c2c9
5 changed files with 146 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1434,7 +1434,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0)
{
desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
motor->setVelocityTarget(desiredVelocity);
btScalar kd = 0.1f;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0)
{
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
}
motor->setVelocityTarget(desiredVelocity,kd);
btScalar kp = 0.f;
motor->setPositionTarget(0,kp);
hasDesiredVelocity = true;
}
if (hasDesiredVelocity)
@@ -1511,16 +1520,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
}
int dof1 = 0;
btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1];
btScalar currentVelocity = mb->getJointVelMultiDof(link)[dof1];
btScalar positionStabiliationTerm = (desiredPosition-currentPosition)/m_data->m_physicsDeltaTime;
btScalar velocityError = (desiredVelocity - currentVelocity);
desiredVelocity = kp * positionStabiliationTerm +
kd * velocityError;
motor->setVelocityTarget(desiredVelocity);
motor->setVelocityTarget(desiredVelocity,kd);
motor->setPositionTarget(desiredPosition,kp);
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)

151
examples/pybullet/pybullet.c
View File

@@ -290,7 +290,13 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
//todo(erwincoumans): set max forces, kp, kd
int size;
int bodyIndex, jointIndex, controlMode;
double targetValue=0;
double maxForce=100000;
double gains=0.1;
int valid = 0;
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
@@ -301,69 +307,96 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
if (size==4)
{
int bodyIndex, jointIndex, controlMode;
double targetValue;
if (PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
{
int numJoints;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
struct b3JointInfo info;
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
}
if (size==5)
{
if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
}
if (size==6)
{
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gains))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
}
if (valid)
{
int numJoints;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
struct b3JointInfo info;
numJoints = b3GetNumJoints(sm,bodyIndex);
if ((jointIndex >= numJoints) || (jointIndex < 0))
{
PyErr_SetString(SpamError, "Joint index out-of-range.");
return NULL;
}
if ((controlMode != CONTROL_MODE_VELOCITY) &&
(controlMode != CONTROL_MODE_TORQUE) &&
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD))
{
PyErr_SetString(SpamError, "Illegral control mode.");
return NULL;
}
commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode);
b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
switch (controlMode)
{
case CONTROL_MODE_VELOCITY:
{
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
b3JointControlSetKd(commandHandle,info.m_uIndex,1);
b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,10000);
break;
}
case CONTROL_MODE_TORQUE:
{
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue);
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue);
b3JointControlSetKp(commandHandle,info.m_uIndex,1);
b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,10000);
break;
}
default:
{
}
};
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_INCREF(Py_None);
return Py_None;
numJoints = b3GetNumJoints(sm,bodyIndex);
if ((jointIndex >= numJoints) || (jointIndex < 0))
{
PyErr_SetString(SpamError, "Joint index out-of-range.");
return NULL;
}
if ((controlMode != CONTROL_MODE_VELOCITY) &&
(controlMode != CONTROL_MODE_TORQUE) &&
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD))
{
PyErr_SetString(SpamError, "Illegral control mode.");
return NULL;
}
commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode);
b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
switch (controlMode)
{
case CONTROL_MODE_VELOCITY:
{
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
double kd = gains;
b3JointControlSetKd(commandHandle,info.m_uIndex,kd);
b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce);
break;
}
case CONTROL_MODE_TORQUE:
{
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue);
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue);
double kp = gains;
b3JointControlSetKp(commandHandle,info.m_uIndex,kp);
b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce);
break;
}
default:
{
}
};
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_INCREF(Py_None);
return Py_None;
}
PyErr_SetString(SpamError, "error in setJointControl.");
return NULL;