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/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/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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index c7bbfdd5f..059b33dfe 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;
}
}
@@ -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."},