From c17c39c2c9e3660a324259c7db8e2e80636bf0d0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 24 Jun 2016 11:06:56 -0700 Subject: [PATCH] move PD control from PhysicsServerCommandProcessor into btMultiBodyJointMotor improvements/changes in pybullet API --- build_and_run_cmake.sh | 2 +- .../PhysicsServerCommandProcessor.cpp | 24 +-- examples/pybullet/pybullet.c | 151 +++++++++++------- .../Featherstone/btMultiBodyJointMotor.cpp | 29 +++- .../Featherstone/btMultiBodyJointMotor.h | 21 ++- 5 files changed, 146 insertions(+), 81 deletions(-) diff --git a/build_and_run_cmake.sh b/build_and_run_cmake.sh index 1405060f4..28354357b 100755 --- a/build_and_run_cmake.sh +++ b/build_and_run_cmake.sh @@ -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 diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 3cdaac902..99b883454 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b978feb47..78e770dbe 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 50268611b..3d0f45b77 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -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;rowgetJointPosMultiDof(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; { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index d322f87e7..2a55be035 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -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) {