AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 13:20:07 +00:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
build_and_run_cmake.sh
View File

@ -2,6 +2,6 @@
rm CMakeCache.txt
mkdir build_cmake
cd build_cmake
cmake ..
cmake -DBUILD_PYTHON=OFF -CMAKE_BUILD_TYPE=Release ..
make -j12
examples/ExampleBrowser/App_ExampleBrowser

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;

29
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@ -23,7 +23,10 @@ subject to the following restrictions:
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity)
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(0.1),
m_kp(0)
{
m_maxAppliedImpulse = maxMotorImpulse;
@ -51,7 +54,10 @@ void btMultiBodyJointMotor::finalizeMultiDof()
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
//:btMultiBodyConstraint(body,0,link,-1,1,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity)
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(0.1),
m_kp(0)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
@ -113,9 +119,22 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
for (int row=0;row<getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + m_kd * velocityError;
printf("m_kd = %f\n", m_kd);
printf("m_kp = %f\n", m_kp);
printf("m_desiredVelocity = %f\n", m_desiredVelocity);
printf("m_desiredPosition = %f\n", m_desiredPosition);
printf("m_maxAppliedImpulse = %f\n", m_maxAppliedImpulse);
printf("rhs = %f\n", rhs);
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,rhs);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{

21
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
View File

@ -24,9 +24,12 @@ struct btSolverInfo;
class btMultiBodyJointMotor : public btMultiBodyConstraint
{
protected:
btScalar m_desiredVelocity;
btScalar m_desiredVelocity;
btScalar m_desiredPosition;
btScalar m_kd;
btScalar m_kp;
public:
@ -42,10 +45,18 @@ public:
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void setVelocityTarget(btScalar velTarget)
virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
{
m_desiredVelocity = velTarget;
m_desiredVelocity = velTarget;
m_kd = kd;
}
virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
{
m_desiredPosition = posTarget;
m_kp = kp;
}
virtual void debugDraw(class btIDebugDraw* drawer)
{