From e7ad8a9d6704819f95e1a47e9bd391eb41a81cf0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 11 Feb 2020 15:10:22 -0800 Subject: [PATCH 1/4] add urdf file for testing --- .../gym/pybullet_data/sphere8cube.urdf | 63 +++++++++++++++++++ 1 file changed, 63 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_data/sphere8cube.urdf diff --git a/examples/pybullet/gym/pybullet_data/sphere8cube.urdf b/examples/pybullet/gym/pybullet_data/sphere8cube.urdf new file mode 100644 index 000000000..54f114d9d --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/sphere8cube.urdf @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From b3ff3ebcb16eb42977cf48f2502d5f7f3eb8364a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 12 Feb 2020 13:05:51 -0800 Subject: [PATCH 2/4] add pendulum of 5 links --- .../pybullet/gym/pybullet_data/pendulum5.urdf | 148 ++++++++++++++++++ 1 file changed, 148 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_data/pendulum5.urdf diff --git a/examples/pybullet/gym/pybullet_data/pendulum5.urdf b/examples/pybullet/gym/pybullet_data/pendulum5.urdf new file mode 100644 index 000000000..e38ae5901 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/pendulum5.urdf @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 6afa0a463d31862f096aa14539ee6e28ddc6eb3c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 14 Feb 2020 17:36:40 -0800 Subject: [PATCH 3/4] unsupported: expose collisionMargin to changeDynamics/getDynamicsInfo. add cube_convex.urdf for testing this collisionMargin. Test script: import pybullet as p import time p.connect(p.GUI) plane = p.loadURDF("plane_implicit.urdf") cube = p.loadURDF("cube_convex.urdf",[0,0,1]) p.setGravity(0,0,-10) while (1): p.stepSimulation() pts = p.getContactPoints() p.changeDynamics(plane,-1,collisionMargin=0.3) p.changeDynamics(cube,-1,collisionMargin=0.3) print("===================") print("cube pos=", p.getBasePositionAndOrientation(cube)[0]) print("margin=", p.getDynamicsInfo(plane,-1)[11]) #time.sleep(1./10.) --- data/cube_convex.urdf | 32 +++++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.cpp | 13 ++++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 ++ .../PhysicsServerCommandProcessor.cpp | 26 +++++++++++++++ examples/SharedMemory/SharedMemoryCommands.h | 2 ++ examples/SharedMemory/SharedMemoryPublic.h | 1 + examples/pybullet/pybullet.c | 13 ++++++-- 7 files changed, 86 insertions(+), 3 deletions(-) create mode 100644 data/cube_convex.urdf diff --git a/data/cube_convex.urdf b/data/cube_convex.urdf new file mode 100644 index 000000000..be0f961de --- /dev/null +++ b/data/cube_convex.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 687ee105a..967feaa77 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3256,6 +3256,19 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetMaxJointVelocity(b3SharedMemoryCommandH return 0; } +B3_SHARED_API int b3ChangeDynamicsInfoSetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double collisionMargin) +{ + 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 = -1; + command->m_changeDynamicsInfoArgs.m_collisionMargin = collisionMargin; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN; + return 0; +} + + + B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int activationState) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 529edd2e8..f1d911266 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -168,6 +168,8 @@ extern "C" B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold); B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int activationState); B3_SHARED_API int b3ChangeDynamicsInfoSetMaxJointVelocity(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double maxJointVelocity); + B3_SHARED_API int b3ChangeDynamicsInfoSetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double collisionMargin); + B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info); B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand2(b3SharedMemoryCommandHandle commandHandle, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f706b70f1..8d0a52db0 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8927,6 +8927,18 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc { mb->setMaxCoordinateVelocity(clientCmd.m_changeDynamicsInfoArgs.m_maxJointVelocity); } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN) + { + mb->getBaseCollider()->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin); + if (mb->getBaseCollider()->getCollisionShape()->isCompound()) + { + btCompoundShape* compound = (btCompoundShape*)mb->getBaseCollider()->getCollisionShape(); + for (int s = 0; s < compound->getNumChildShapes(); s++) + { + compound->getChildShape(s)->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin); + } + } + } } else { @@ -8968,6 +8980,10 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc { mb->getLinkCollider(linkIndex)->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN) + { + mb->getLinkCollider(linkIndex)->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin); + } } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING) @@ -9115,6 +9131,10 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc //for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled rb->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.); } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN) + { + rb->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin); + } } } @@ -9165,6 +9185,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = mb->getBaseCollider()->getContactProcessingThreshold(); serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = mb->getBaseCollider()->getCcdSweptSphereRadius(); serverCmd.m_dynamicsInfo.m_frictionAnchor = mb->getBaseCollider()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR; + serverCmd.m_dynamicsInfo.m_collisionMargin = mb->getBaseCollider()->getCollisionShape()->getMargin(); } else { @@ -9172,6 +9193,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = 0; serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = 0; serverCmd.m_dynamicsInfo.m_frictionAnchor = 0; + serverCmd.m_dynamicsInfo.m_collisionMargin = 0; } serverCmd.m_dynamicsInfo.m_localInertialDiagonal[0] = mb->getBaseInertia()[0]; serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getBaseInertia()[1]; @@ -9214,6 +9236,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = mb->getLinkCollider(linkIndex)->getContactProcessingThreshold(); serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = mb->getLinkCollider(linkIndex)->getCcdSweptSphereRadius(); serverCmd.m_dynamicsInfo.m_frictionAnchor = mb->getLinkCollider(linkIndex)->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR; + serverCmd.m_dynamicsInfo.m_collisionMargin = mb->getLinkCollider(linkIndex)->getCollisionShape()->getMargin(); } else { @@ -9221,6 +9244,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = 0; serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = 0; serverCmd.m_dynamicsInfo.m_frictionAnchor = 0; + serverCmd.m_dynamicsInfo.m_collisionMargin = 0; } serverCmd.m_dynamicsInfo.m_localInertialDiagonal[0] = mb->getLinkInertia(linkIndex)[0]; @@ -9277,12 +9301,14 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S serverCmd.m_dynamicsInfo.m_angularDamping = rb->getAngularDamping(); serverCmd.m_dynamicsInfo.m_linearDamping = rb->getLinearDamping(); serverCmd.m_dynamicsInfo.m_mass = rb->getMass(); + serverCmd.m_dynamicsInfo.m_collisionMargin = rb->getCollisionShape() ? rb->getCollisionShape()->getMargin() : 0; } #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD else if (body && body->m_softBody){ SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; serverCmd.m_dynamicsInfo.m_bodyType = BT_SOFT_BODY; + serverCmd.m_dynamicsInfo.m_collisionMargin = 0; } #endif return hasStatus; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index d17415839..a39ebfa08 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -166,6 +166,7 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING = 16384, CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION = 32768, CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY = 1<<16, + CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN = 1 << 17, }; struct ChangeDynamicsInfoArgs @@ -190,6 +191,7 @@ struct ChangeDynamicsInfoArgs double m_jointDamping; double m_anisotropicFriction[3]; double m_maxJointVelocity; + double m_collisionMargin; }; struct GetDynamicsInfoArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 3adb7e267..eac155862 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -388,6 +388,7 @@ struct b3DynamicsInfo double m_ccdSweptSphereRadius; double m_contactProcessingThreshold; int m_frictionAnchor; + double m_collisionMargin; }; // copied from btMultiBodyLink.h diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index c7bbfdd5f..e6a9c076c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1324,6 +1324,7 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO double contactStiffness = -1; double contactDamping = -1; double ccdSweptSphereRadius = -1; + double collisionMargin = -1; int frictionAnchor = -1; double contactProcessingThreshold = -1; int activationState = -1; @@ -1335,8 +1336,8 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO 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", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOdi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &physicsClientId)) + 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)) { return NULL; } @@ -1428,6 +1429,11 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO { b3ChangeDynamicsInfoSetMaxJointVelocity(command, bodyUniqueId, maxJointVelocity); } + + if (collisionMargin >= 0) + { + b3ChangeDynamicsInfoSetCollisionMargin(command, bodyUniqueId, collisionMargin); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } @@ -1483,7 +1489,7 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje if (b3GetDynamicsInfo(status_handle, &info)) { - int numFields = 11; + int numFields = 12; PyObject* pyDynamicsInfo = PyTuple_New(numFields); PyTuple_SetItem(pyDynamicsInfo, 0, PyFloat_FromDouble(info.m_mass)); PyTuple_SetItem(pyDynamicsInfo, 1, PyFloat_FromDouble(info.m_lateralFrictionCoeff)); @@ -1516,6 +1522,7 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje PyTuple_SetItem(pyDynamicsInfo, 8, PyFloat_FromDouble(info.m_contactDamping)); PyTuple_SetItem(pyDynamicsInfo, 9, PyFloat_FromDouble(info.m_contactStiffness)); PyTuple_SetItem(pyDynamicsInfo, 10, PyInt_FromLong(info.m_bodyType)); + PyTuple_SetItem(pyDynamicsInfo, 11, PyFloat_FromDouble(info.m_collisionMargin)); return pyDynamicsInfo; } } From 0617f4f3cd6ce93d8d9cbc198b0f4320ad7c5534 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 14 Feb 2020 19:58:32 -0800 Subject: [PATCH 4/4] expose computeDofCount to pybullet expose computeDofCount and calculateMassMatrix to BulletRobotics C++ API (all untested) --- .../b3RobotSimulatorClientAPI_NoDirect.cpp | 39 +++++++++++++++++++ .../b3RobotSimulatorClientAPI_NoDirect.h | 4 ++ examples/pybullet/pybullet.c | 36 ++++++++++++++++- 3 files changed, 78 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index cfcd7b00a..3ef53adbd 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -988,6 +988,45 @@ bool b3RobotSimulatorClientAPI_NoDirect::calculateInverseKinematics(const struct return result; } +int b3RobotSimulatorClientAPI_NoDirect::computeDofCount(int bodyUniqueId) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return 0; + } + return b3ComputeDofCount(m_data->m_physicsClientHandle, bodyUniqueId); +} + +int b3RobotSimulatorClientAPI_NoDirect::calculateMassMatrix(int bodyUniqueId, const double* jointPositions, int numJointPositions, double* massMatrix, int flags) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return 0; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = + b3CalculateMassMatrixCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, jointPositions, numJointPositions); + b3CalculateMassMatrixSetFlags(commandHandle, flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CALCULATED_MASS_MATRIX_COMPLETED) + { + int dofCount; + b3GetStatusMassMatrix(m_data->m_physicsClientHandle, statusHandle, &dofCount, NULL); + if (dofCount) + { + b3GetStatusMassMatrix(m_data->m_physicsClientHandle, statusHandle, NULL, massMatrix); + return dofCount; + } + } + + return 0; +} + bool b3RobotSimulatorClientAPI_NoDirect::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian) { if (!isConnected()) diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index 26237117a..1c92746b4 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -783,8 +783,12 @@ public: void setNumSolverIterations(int numIterations); void setContactBreakingThreshold(double threshold); + int computeDofCount(int bodyUniqueId) const; + bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs &args, struct b3RobotSimulatorInverseKinematicsResults &results); + int calculateMassMatrix(int bodyUniqueId, const double* jointPositions, int numJointPositions, double* massMatrix, int flags); + bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double *localPosition, const double *jointPositions, const double *jointVelocities, const double *jointAccelerations, double *linearJacobian, double *angularJacobian); void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable); diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e6a9c076c..059b33dfe 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3945,6 +3945,37 @@ static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args, PyObject* } } + +static PyObject* pybullet_computeDofCount(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + int bodyUniqueId = -1; + b3PhysicsClientHandle sm = 0; + + static char* kwlist[] = { "bodyUniqueId", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + int dofCount = b3ComputeDofCount(sm, bodyUniqueId); + +#if PY_MAJOR_VERSION >= 3 + return PyLong_FromLong(dofCount); +#else + return PyInt_FromLong(dofCount); +#endif + } +} + + static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObject* keywds) { int physicsClientId = 0; @@ -11794,7 +11825,7 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py else { PyErr_SetString(SpamError, - "Internal error in calculateJacobian"); + "Internal error in calculateMassMatrix"); } } free(jointPositions); @@ -11954,6 +11985,9 @@ static PyMethodDef SpamMethods[] = { {"getBodyInfo", (PyCFunction)pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS, "Get the body info, given a body unique id."}, + { "computeDofCount", (PyCFunction)pybullet_computeDofCount, METH_VARARGS | METH_KEYWORDS, + "computeDofCount returns the number of degrees of freedom, including 7 degrees of freedom for the base in case of floating base" }, + {"syncBodyInfo", (PyCFunction)pybullet_syncBodyInfo, METH_VARARGS | METH_KEYWORDS, "syncBodyInfo(physicsClientId=0)\n" "Update body and constraint/joint information, in case other clients made changes."},