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."},