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) { diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 91dd9328b..ca60e9e13 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; @@ -3296,6 +3307,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 779509069..cd71fabc6 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); @@ -415,6 +416,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/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 50e88f083..6e2ee11b9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -9761,6 +9761,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()) @@ -10045,6 +10050,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc btRigidBody* rb = 0; if (body && body->m_rigidBody) { + if (linkIndex == -1) { rb = body->m_rigidBody; @@ -10180,6 +10186,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 @@ -14612,7 +14625,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); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 655d13c05..803346926 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 effdfb07c..f0637919d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1354,9 +1354,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; } @@ -1372,6 +1373,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) @@ -1379,7 +1385,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) @@ -1430,11 +1436,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; } @@ -1460,6 +1469,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;