From 84a63cf221437d356057e670fe46dcb11971e2ec Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Mon, 19 Apr 2021 20:37:26 -0700 Subject: [PATCH 1/4] allow to use the useMultiBody argument for loadMJCF command note that for dynamic and static objects (without joints) it is best to set useMultiBody=False --- examples/SharedMemory/PhysicsClientC_API.cpp | 11 +++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 ++ examples/pybullet/pybullet.c | 12 +++++++++--- 3 files changed, 22 insertions(+), 3 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index c70889872..bf2b71c3e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -260,6 +260,17 @@ B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle command } } +B3_SHARED_API void b3LoadMJCFCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_MJCF); + if (command->m_type == CMD_LOAD_MJCF) + { + command->m_updateFlags |= URDF_ARGS_USE_MULTIBODY; + command->m_mjcfArguments.m_useMultiBody = useMultiBody; + } +} + B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSoftBodyCommandInit(b3PhysicsClientHandle physClient, const char* fileName) { PhysicsClient* cl = (PhysicsClient*)physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 101868028..49b76eca0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -415,6 +415,8 @@ extern "C" B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName); B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* fileName); B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); + B3_SHARED_API void b3LoadMJCFCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); + ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d4d45db6e..8dfb0f06c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1336,9 +1336,10 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key PyObject* pylist = 0; int physicsClientId = 0; int flags = -1; + int useMultiBody = -1; - static char* kwlist[] = {"mjcfFileName", "flags", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|ii", kwlist, &mjcfFileName, &flags, &physicsClientId)) + static char* kwlist[] = {"mjcfFileName", "flags", "useMultiBody", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|iii", kwlist, &mjcfFileName, &flags, &useMultiBody, &physicsClientId)) { return NULL; } @@ -1354,6 +1355,11 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key { b3LoadMJCFCommandSetFlags(command, flags); } + if (useMultiBody>=0) + { + b3LoadMJCFCommandSetUseMultiBody(command, useMultiBody); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType != CMD_MJCF_LOADING_COMPLETED) @@ -1361,7 +1367,7 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key PyErr_SetString(SpamError, "Couldn't load .mjcf file."); return NULL; } - + numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); if (numBodies > MAX_SDF_BODIES) From 78f5b141f92750b8ff0921052d7e9e649e2a457c Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 20 Apr 2021 10:24:09 -0700 Subject: [PATCH 2/4] fix for 'useMaximalCoordinates' rigid body in PyBullet: activate a rigid body after picking --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 88b50fcca..ed7a38d21 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -14602,7 +14602,12 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons { m_data->m_pickedBody = body; m_data->m_savedActivationState = body->getActivationState(); + if (m_data->m_savedActivationState==ISLAND_SLEEPING) + { + m_data->m_savedActivationState = ACTIVE_TAG; + } m_data->m_pickedBody->setActivationState(DISABLE_DEACTIVATION); + m_data->m_pickedBody->setDeactivationTime(0); //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); From fd154cee8f352b9e23c73b2c87dac4bdf27ad016 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 20 Apr 2021 11:14:28 -0700 Subject: [PATCH 3/4] set default sleeping threshold for useMaximalCoordinate btRigidBody to be similar to btMultiBody (0.05) --- examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp index e41ef7881..815e8b9bf 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp @@ -33,6 +33,10 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc { btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal); rbci.m_startWorldTransform = initialWorldTrans; + btScalar sleep_threshold = btScalar(0.22360679775);//sqrt(0.05) to be similar to btMultiBody (SLEEP_THRESHOLD) + rbci.m_angularSleepingThreshold = sleep_threshold; + rbci.m_linearSleepingThreshold = sleep_threshold; + btRigidBody* body = new btRigidBody(rbci); if (m_rigidBody == 0) { From e58e5bdf9534b084c802235f40d88d7148f781a2 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 20 Apr 2021 11:29:27 -0700 Subject: [PATCH 4/4] PyBullet: expose pybullet.changeDynamics(sleepThreshold), default 0.05, for both useMaximalCoordinates=False and True When the dot(vel,vel) < sleepThreshold for longer than 2 seconds (this timeout is hard coded) objects that have the ACTIVATION_STATE_ENABLE_SLEEPING enabled with be deactivated Example: p.changeDynamics(ob, -1, activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING, sleepThreshold=0.05) --- examples/SharedMemory/PhysicsClientC_API.cpp | 12 +++++ examples/SharedMemory/PhysicsClientC_API.h | 3 +- .../PhysicsServerCommandProcessor.cpp | 13 +++++ examples/SharedMemory/SharedMemoryCommands.h | 4 ++ .../gym/pybullet_examples/sleeptest.py | 53 +++++++++++++++++++ examples/pybullet/pybullet.c | 11 +++- .../Featherstone/btMultiBody.cpp | 11 ++-- src/BulletDynamics/Featherstone/btMultiBody.h | 13 +++++ 8 files changed, 113 insertions(+), 7 deletions(-) create mode 100644 examples/pybullet/gym/pybullet_examples/sleeptest.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index bf2b71c3e..705c8e35f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3305,6 +3305,18 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimitForce(b3SharedMemoryCommandHa return 0; } +B3_SHARED_API int b3ChangeDynamicsInfoSetSleepThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double sleepThreshold) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_sleepThreshold = sleepThreshold; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD; + return 0; +} + + + B3_SHARED_API int b3ChangeDynamicsInfoSetDynamicType(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, int dynamicType) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 49b76eca0..66ece152b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -156,7 +156,8 @@ extern "C" 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 b3ChangeDynamicsInfoSetDynamicType(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, int dynamicType); - + + B3_SHARED_API int b3ChangeDynamicsInfoSetSleepThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double sleepThreshold); 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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index ed7a38d21..5edbe46f6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -9751,6 +9751,11 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD) + { + mb->setSleepThreshold(clientCmd.m_changeDynamicsInfoArgs.m_sleepThreshold); + } + if (linkIndex == -1) { if (mb->getBaseCollider()) @@ -10035,6 +10040,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc btRigidBody* rb = 0; if (body && body->m_rigidBody) { + if (linkIndex == -1) { rb = body->m_rigidBody; @@ -10170,6 +10176,13 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc m_data->m_dynamicsWorld->addCollisionObject(rb, collisionFilterGroup, collisionFilterMask); } } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD) + { + btScalar threshold2 = btSqrt(clientCmd.m_changeDynamicsInfoArgs.m_sleepThreshold); + rb->setSleepingThresholds(threshold2,threshold2); + } + } } #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index e463b2dc5..fc2a7f12f 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -174,6 +174,7 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS = 1 << 18, CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE = 1 << 19, CHANGE_DYNAMICS_INFO_SET_DYNAMIC_TYPE = 1 << 20, + CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD = 1 << 21, }; struct ChangeDynamicsInfoArgs @@ -205,6 +206,9 @@ struct ChangeDynamicsInfoArgs double m_jointLimitForce; int m_dynamicType; + + double m_sleepThreshold; + }; struct GetDynamicsInfoArgs diff --git a/examples/pybullet/gym/pybullet_examples/sleeptest.py b/examples/pybullet/gym/pybullet_examples/sleeptest.py new file mode 100644 index 000000000..564d0f74b --- /dev/null +++ b/examples/pybullet/gym/pybullet_examples/sleeptest.py @@ -0,0 +1,53 @@ +import pybullet as p +import pybullet_data as pd +import time +p.connect(p.GUI) +p.setAdditionalSearchPath(pd.getDataPath()) +objects=[] +useMaximalCoordinates=False + +if 0: + collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_SPHERE, radius = 0.1) + batchPositions = [] + index = 1 + for x in range(2): + for y in range(1): + for z in range(1): + batchPositions.append(arr[index]) + index=index+1 + bodyUids = p.createMultiBody(baseMass=10, + baseInertialFramePosition=[0, 0, 0], + baseCollisionShapeIndex=collisionShapeId, + baseVisualShapeIndex=-1, + basePosition=[0, 0, 2], + batchPositions=batchPositions, + useMaximalCoordinates=useMaximalCoordinates) + for b in bodyUids: + objects.append(b) + +else: + for i in range(2): + for j in range(1): + for k in range(1): + ob = p.loadURDF("sphere_1cm.urdf", [0.210050 * i, 0.210050 * j, 1 + 0.210050 * k],globalScaling=20, + useMaximalCoordinates=useMaximalCoordinates) + objects.append(ob) + p.changeDynamics(ob, -1, activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING, linearDamping=0, angularDamping=0, sleepThreshold=0.05) + if (i==0): + p.resetBaseVelocity(ob, [0,0,0], [0,0,0.22]) + + +timeid = p.addUserDebugText("t=", [0,0,2]) +lvelid = p.addUserDebugText("lvel", [0,0,1.8]) +avelid = p.addUserDebugText("avel", [0,0,1.6]) +t=0 +dt=1./240. +while p.isConnected(): + p.stepSimulation() + t+=dt + txtid = p.addUserDebugText("t="+str(t), [0,0,2],replaceItemUniqueId=timeid) + lin, ang = p.getBaseVelocity(ob) + txtid = p.addUserDebugText("lvel="+"{:.4f}".format(lin[0])+","+"{:.4f}".format(lin[1])+","+"{:.4f}".format(lin[2]), [0,0,1.8],replaceItemUniqueId=lvelid) + txtid = p.addUserDebugText("avel="+"{:.4f}".format(ang[0])+","+"{:.4f}".format(ang[1])+","+"{:.4f}".format(ang[2]), [0,0,1.6],replaceItemUniqueId=avelid) + time.sleep(dt) + \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 8dfb0f06c..13425473c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1418,11 +1418,14 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO double jointUpperLimit = -1; double jointLimitForce = -1; + double sleepThreshold = -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", "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)) + 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", "sleepThreshold", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOddddddi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &collisionMargin , &jointLowerLimit , &jointUpperLimit , &jointLimitForce , &sleepThreshold, &physicsClientId)) { return NULL; } @@ -1448,6 +1451,10 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO { b3ChangeDynamicsInfoSetJointLimitForce(command, bodyUniqueId, linkIndex, jointLimitForce); } + if (sleepThreshold >=0) + { + b3ChangeDynamicsInfoSetSleepThreshold(command, bodyUniqueId, sleepThreshold); + } if (jointLowerLimit <= jointUpperLimit) { diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index dfc4f92a3..d7588aedc 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -33,8 +33,8 @@ namespace { -const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) -const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds +const btScalar INITIAL_SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) +const btScalar INITIAL_SLEEP_TIMEOUT = btScalar(2); // in seconds } // namespace void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame @@ -110,6 +110,9 @@ btMultiBody::btMultiBody(int n_links, m_canSleep(canSleep), m_canWakeup(true), m_sleepTimer(0), + m_sleepEpsilon(INITIAL_SLEEP_EPSILON), + m_sleepTimeout(INITIAL_SLEEP_TIMEOUT), + m_userObjectPointer(0), m_userIndex2(-1), m_userIndex(-1), @@ -2104,10 +2107,10 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) motion += m_realBuf[i] * m_realBuf[i]; } - if (motion < SLEEP_EPSILON) + if (motion < m_sleepEpsilon) { m_sleepTimer += timestep; - if (m_sleepTimer > SLEEP_TIMEOUT) + if (m_sleepTimer > m_sleepTimeout) { goToSleep(); } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 9cf1ceb5a..b48b3d1a0 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -726,6 +726,17 @@ public: bool isLinkAndAllAncestorsKinematic(const int i) const; + void setSleepThreshold(btScalar sleepThreshold) + { + m_sleepEpsilon = sleepThreshold; + } + + void setSleepTimeout(btScalar sleepTimeout) + { + this->m_sleepTimeout = sleepTimeout; + } + + private: btMultiBody(const btMultiBody &); // not implemented void operator=(const btMultiBody &); // not implemented @@ -801,6 +812,8 @@ private: bool m_canSleep; bool m_canWakeup; btScalar m_sleepTimer; + btScalar m_sleepEpsilon; + btScalar m_sleepTimeout; void *m_userObjectPointer; int m_userIndex2;