Merge pull request #2626 from erwincoumans/master

unsupported: expose collisionMargin to changeDynamics/getDynamicsInfo
This commit is contained in:
erwincoumans
2020-02-20 10:55:42 -08:00
committed by GitHub
11 changed files with 375 additions and 4 deletions
+32
View File
@@ -0,0 +1,32 @@
<?xml version="1.0" ?>
<robot name="cube">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="cube.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="cube.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
</robot>
@@ -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)
@@ -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);
@@ -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;
@@ -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
@@ -388,6 +388,7 @@ struct b3DynamicsInfo
double m_ccdSweptSphereRadius;
double m_contactProcessingThreshold;
int m_frictionAnchor;
double m_collisionMargin;
};
// copied from btMultiBodyLink.h
@@ -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())
@@ -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);
@@ -0,0 +1,148 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="pendulum">
<!--
Author: Erwin Coumans <erwincoumans@google.com>
Contributers:
-->
<link name="world"/>
<material name="Blue">
<color rgba="0.28 0.52 0.92 1.0"/>
</material>
<link name="link0">
<visual>
<geometry>
<sphere radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<material name="Blue"/>
</visual>
<collision>
<geometry>
<sphere radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.5 0.0"/>
<mass value="1."/>
<inertia ixx="0.00494875" ixy="0" ixz="0" iyy="0.00494174" iyz="0" izz="0.002219"/>
</inertial>
</link>
<joint name="joint0" type="continuous">
<parent link="world"/>
<child link="link0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<link name="link1">
<visual>
<geometry>
<sphere radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<material name="Red"/>
</visual>
<collision>
<geometry>
<sphere radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<mass value="2.16"/>
<inertia ixx="0.00539427" ixy="0" ixz="0" iyy="0.0048979" iyz="0" izz="0.00311573"/>
</inertial>
</link>
<joint name="joint1" type="continuous">
<parent link="link0"/>
<child link="link1"/>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<axis xyz="1 0 0"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<link name="link2">
<visual>
<geometry>
<sphere radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<material name="Red"/>
</visual>
<collision>
<geometry>
<sphere radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<mass value="2.16"/>
<inertia ixx="0.00539427" ixy="0" ixz="0" iyy="0.0048979" iyz="0" izz="0.00311573"/>
</inertial>
</link>
<joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link2"/>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<axis xyz="1 0 0"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<link name="link3">
<visual>
<geometry>
<sphere radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<material name="Red"/>
</visual>
<collision>
<geometry>
<sphere radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<mass value="2.16"/>
<inertia ixx="0.00539427" ixy="0" ixz="0" iyy="0.0048979" iyz="0" izz="0.00311573"/>
</inertial>
</link>
<joint name="joint3" type="continuous">
<parent link="link2"/>
<child link="link3"/>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<axis xyz="1 0 0"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<link name="link4">
<visual>
<geometry>
<sphere radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<material name="Red"/>
</visual>
<collision>
<geometry>
<sphere radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<mass value="2.16"/>
<inertia ixx="0.00539427" ixy="0" ixz="0" iyy="0.0048979" iyz="0" izz="0.00311573"/>
</inertial>
</link>
<joint name="joint4" type="continuous">
<parent link="link3"/>
<child link="link4"/>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<axis xyz="1 0 0"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
</robot>
@@ -0,0 +1,63 @@
<?xml version="0.0" ?>
<robot name="urdf_robot">
<link name="baseLink">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="10.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="cube.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
<specular rgb="11 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.4 0.4 0.4"/>
<geometry>
<sphere radius="0.1"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-0.4 0.4 0.4"/>
<geometry>
<sphere radius="0.1"/>
</geometry>
</collision><collision>
<origin rpy="0 0 0" xyz="0.4 -0.4 0.4"/>
<geometry>
<sphere radius="0.1"/>
</geometry>
</collision><collision>
<origin rpy="0 0 0" xyz="-0.4 -0.4 0.4"/>
<geometry>
<sphere radius="0.1"/>
</geometry>
</collision><collision>
<origin rpy="0 0 0" xyz="0.4 0.4 -0.4"/>
<geometry>
<sphere radius="0.1"/>
</geometry>
</collision><collision>
<origin rpy="0 0 0" xyz="-0.4 0.4 -0.4"/>
<geometry>
<sphere radius="0.1"/>
</geometry>
</collision><collision>
<origin rpy="0 0 0" xyz="0.4 -0.4 -0.4"/>
<geometry>
<sphere radius="0.1"/>
</geometry>
</collision><collision>
<origin rpy="0 0 0" xyz="-0.4 -0.4 -0.4"/>
<geometry>
<sphere radius="0.1"/>
</geometry>
</collision>
</link>
</robot>
+45 -4
View File
@@ -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;
}
}
@@ -3938,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;
@@ -11787,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);
@@ -11947,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."},