AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #2919 from erwincoumans/master

Browse Source 
pybullet allow to enable/change joint limits:
This commit is contained in:
erwincoumans 2020-07-08 09:59:04 -07:00 committed by GitHub
commit 0dad934515
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
15 changed files with 177 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -3174,6 +3174,32 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetAnisotropicFriction(b3SharedMemoryComma
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimit(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLowerLimit, double jointUpperLimit)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_jointLowerLimit = jointLowerLimit;
command->m_changeDynamicsInfoArgs.m_jointUpperLimit = jointUpperLimit;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimitForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLimitForce)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_jointLimitForce = jointLimitForce;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double localInertiaDiagonal[])
{

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -152,8 +152,11 @@ extern "C"
B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double localInertiaDiagonal[]);
B3_SHARED_API int b3ChangeDynamicsInfoSetAnisotropicFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double anisotropicFriction[]);
B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimit(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLowerLimit, double jointUpperLimit);
B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimitForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLimitForce);
B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
B3_SHARED_API int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);

75
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -14,6 +14,9 @@
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "../Utils/b3BulletDefaultFileIO.h"
#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
@ -9308,6 +9311,78 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
{
if (linkIndex >= 0 && linkIndex < mb->getNumLinks())
{
if ((clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE) ||
(clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS))
{
btMultiBodyJointLimitConstraint* limC = 0;
int numConstraints = m_data->m_dynamicsWorld->getNumMultiBodyConstraints();
for (int c = 0; c < numConstraints; c++)
{
btMultiBodyConstraint* mbc = m_data->m_dynamicsWorld->getMultiBodyConstraint(c);
if (mbc->getConstraintType() == MULTIBODY_CONSTRAINT_LIMIT)
{
if (((mbc->getMultiBodyA() == mb) && (mbc->getLinkA() == linkIndex))
||
((mbc->getMultiBodyB() == mb) && ((mbc->getLinkB() == linkIndex)))
)
{
limC = (btMultiBodyJointLimitConstraint*)mbc;
}
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS)
{
//find a joint limit
btScalar prevUpper = mb->getLink(linkIndex).m_jointUpperLimit;
btScalar prevLower = mb->getLink(linkIndex).m_jointLowerLimit;
btScalar lower = clientCmd.m_changeDynamicsInfoArgs.m_jointLowerLimit;
btScalar upper = clientCmd.m_changeDynamicsInfoArgs.m_jointUpperLimit;
bool enableLimit = lower <= upper;
if (enableLimit)
{
if (limC == 0)
{
limC = new btMultiBodyJointLimitConstraint(mb, linkIndex, lower, upper);
m_data->m_dynamicsWorld->addMultiBodyConstraint(limC);
}
else
{
limC->setLowerBound(lower);
limC->setUpperBound(upper);
}
mb->getLink(linkIndex).m_jointLowerLimit = lower;
mb->getLink(linkIndex).m_jointUpperLimit = upper;
}
else
{
if (limC)
{
m_data->m_dynamicsWorld->removeMultiBodyConstraint(limC);
delete limC;
limC = 0;
}
mb->getLink(linkIndex).m_jointLowerLimit = 1;
mb->getLink(linkIndex).m_jointUpperLimit = -1;
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE)
{
btScalar fixedTimeSubStep = m_data->m_numSimulationSubSteps > 0 ? m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : m_data->m_physicsDeltaTime;
btScalar maxImpulse = clientCmd.m_changeDynamicsInfoArgs.m_jointLimitForce * fixedTimeSubStep;
if (limC)
{
//convert from force to impulse
limC->setMaxAppliedImpulse(maxImpulse);
}
}
}
if (mb->getLinkCollider(linkIndex))
{
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)

6
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -167,6 +167,8 @@ enum EnumChangeDynamicsInfoFlags
CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION = 32768,
CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY = 1<<16,
CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN = 1 << 17,
CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS = 1 << 18,
CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE = 1 << 19,
};
struct ChangeDynamicsInfoArgs
@ -192,6 +194,10 @@ struct ChangeDynamicsInfoArgs
double m_anisotropicFriction[3];
double m_maxJointVelocity;
double m_collisionMargin;
double m_jointLowerLimit;
double m_jointUpperLimit;
double m_jointLimitForce;
};
struct GetDynamicsInfoArgs

18
examples/pybullet/pybullet.c
View File

@ -1371,11 +1371,15 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
PyObject* anisotropicFrictionObj = 0;
double maxJointVelocity = -1;
double jointLowerLimit = 1;
double jointUpperLimit = -1;
double jointLimitForce = -1;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity", "collisionMargin", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOddi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &collisionMargin , &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity", "collisionMargin", "jointLowerLimit","jointUpperLimit", "jointLimitForce", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOdddddi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &collisionMargin , &jointLowerLimit , &jointUpperLimit , &jointLimitForce , &physicsClientId))
{
return NULL;
}
@ -1397,6 +1401,16 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm);
b3SharedMemoryStatusHandle statusHandle;
if (jointLimitForce >= 0)
{
b3ChangeDynamicsInfoSetJointLimitForce(command, bodyUniqueId, linkIndex, jointLimitForce);
}
if (jointLowerLimit <= jointUpperLimit)
{
b3ChangeDynamicsInfoSetJointLimit(command, bodyUniqueId, linkIndex, jointLowerLimit, jointUpperLimit);
}
if (mass >= 0)
{
b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, mass);

3
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@ -2,11 +2,12 @@
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral)
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type)
: m_bodyA(bodyA),
m_bodyB(bodyB),
m_linkA(linkA),
m_linkB(linkB),
m_type(type),
m_numRows(numRows),
m_jacSizeA(0),
m_jacSizeBoth(0),

23
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@ -20,6 +20,21 @@ subject to the following restrictions:
#include "LinearMath/btAlignedObjectArray.h"
#include "btMultiBody.h"
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
enum btTypedMultiBodyConstraintType
{
MULTIBODY_CONSTRAINT_LIMIT=3,
MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR,
MULTIBODY_CONSTRAINT_GEAR,
MULTIBODY_CONSTRAINT_POINT_TO_POINT,
MULTIBODY_CONSTRAINT_SLIDER,
MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR,
MULTIBODY_CONSTRAINT_FIXED,
MAX_MULTIBODY_CONSTRAINT_TYPE,
};
class btMultiBody;
struct btSolverInfo;
@ -46,6 +61,8 @@ protected:
int m_linkA;
int m_linkB;
int m_type; //btTypedMultiBodyConstraintType
int m_numRows;
int m_jacSizeA;
int m_jacSizeBoth;
@ -82,12 +99,16 @@ protected:
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral);
btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type);
virtual ~btMultiBodyConstraint();
void updateJacobianSizes();
void allocateJacobiansMultiDof();
int getConstraintType() const
{
return m_type;
}
//many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later.
virtual void setFrameInB(const btMatrix3x3& frameInB) {}
virtual void setPivotInB(const btVector3& pivotInB) {}

4
src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
View File

@ -24,7 +24,7 @@ subject to the following restrictions:
#define BTMBFIXEDCONSTRAINT_DIM 6
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
: btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false),
: btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
@ -36,7 +36,7 @@ btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int li
}
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false),
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),

2
src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
View File

@ -21,7 +21,7 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false),
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false, MULTIBODY_CONSTRAINT_GEAR),
m_gearRatio(1),
m_gearAuxLink(-1),
m_erp(0),

2
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
View File

@ -22,7 +22,7 @@ subject to the following restrictions:
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
//:btMultiBodyConstraint(body,0,link,-1,2,true),
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true),
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT),
m_lowerBound(lower),
m_upperBound(upper)
{

16
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
View File

@ -42,6 +42,22 @@ public:
{
//todo(erwincoumans)
}
btScalar getLowerBound() const
{
return m_lowerBound;
}
btScalar getUpperBound() const
{
return m_upperBound;
}
void setLowerBound(btScalar lower)
{
m_lowerBound = lower;
}
void setUpperBound(btScalar upper)
{
m_upperBound = upper;
}
};
#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H

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

@ -21,7 +21,7 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),
@ -51,7 +51,7 @@ 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),
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),

4
src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
View File

@ -27,7 +27,7 @@ subject to the following restrictions:
#endif
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
: btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false),
: btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
@ -37,7 +37,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi
}
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false),
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),

4
src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
View File

@ -25,7 +25,7 @@ subject to the following restrictions:
#define EPSILON 0.000001
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
: btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false),
: btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
@ -38,7 +38,7 @@ btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int
}
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false),
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),

2
src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp
View File

@ -23,7 +23,7 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse)
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true),
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR),
m_desiredVelocity(0, 0, 0),
m_desiredPosition(0,0,0,1),
m_kd(1.),