diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 63a66f110..5c0edd565 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2443,8 +2443,16 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemo return 0; } - - +B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int activationState) +{ + 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_activationState = activationState; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE; + return 0; +} B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b48258ce6..3e025393d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -142,6 +142,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemo B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor); B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius); 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 b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d10df0cdc..b612d9495 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6599,6 +6599,26 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc { btMultiBody* mb = body->m_multiBody; + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) + { + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateWakeUp) + { + mb->wakeUp(); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateSleep) + { + mb->goToSleep(); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateEnableSleeping) + { + mb->setCanSleep(true); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateDisableSleeping) + { + mb->setCanSleep(false); + } + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) { mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping); @@ -6724,6 +6744,27 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc { if (body && body->m_rigidBody) { + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) + { + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateEnableSleeping) + { + body->m_rigidBody->forceActivationState(ACTIVE_TAG); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateDisableSleeping) + { + body->m_rigidBody->forceActivationState(DISABLE_DEACTIVATION); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateWakeUp) + { + body->m_rigidBody->forceActivationState(ACTIVE_TAG); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateSleep) + { + body->m_rigidBody->forceActivationState(ISLAND_SLEEPING); + } + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) { btScalar angDamping = body->m_rigidBody->getAngularDamping(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 248f726d4..b095b186d 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -166,6 +166,7 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL = 1024, CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS = 2048, CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD = 4096, + CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE = 8192, }; struct ChangeDynamicsInfoArgs @@ -186,6 +187,7 @@ struct ChangeDynamicsInfoArgs int m_frictionAnchor; double m_ccdSweptSphereRadius; double m_contactProcessingThreshold; + int m_activationState; }; struct GetDynamicsInfoArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index f5682b1a2..691d739e6 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -314,6 +314,15 @@ struct b3BodyInfo char m_bodyName[1024]; // for btRigidBody, it does not have a base, but can still have a body name from urdf }; + +enum DynamicsActivationState +{ + eActivationStateEnableSleeping = 1, + eActivationStateDisableSleeping = 2, + eActivationStateWakeUp = 4, + eActivationStateSleep = 8, +}; + struct b3DynamicsInfo { double m_mass; diff --git a/examples/pybullet/examples/sleeping.py b/examples/pybullet/examples/sleeping.py index dbd0d0ec3..ab5c2ba57 100644 --- a/examples/pybullet/examples/sleeping.py +++ b/examples/pybullet/examples/sleeping.py @@ -9,12 +9,18 @@ p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.loadURDF("plane100.urdf",flags=flags, useMaximalCoordinates=useMaximalCoordinates) #p.loadURDF("cube_small.urdf", [0,0,0.5], flags=flags) +r2d2 = -1 for k in range (5): for i in range (5): r2d2=p.loadURDF("r2d2.urdf",[k*2,i*2,1], useMaximalCoordinates=useMaximalCoordinates, flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES+flags) + + #enable sleeping: you can pass the flag during URDF loading, or do it afterwards + #p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING) + + for j in range (p.getNumJoints(r2d2)): p.setJointMotorControl2(r2d2,j,p.VELOCITY_CONTROL,targetVelocity=0) - +print("r2d2=",r2d2) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) timestep = 1./240. p.setTimeStep(timestep) @@ -23,3 +29,6 @@ p.setGravity(0,0,-10) while p.isConnected(): p.stepSimulation() time.sleep(timestep) + #force the object to wake up + p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_WAKE_UP) + \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 09bd9cef8..b67025cdf 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1183,14 +1183,15 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO double ccdSweptSphereRadius=-1; int frictionAnchor = -1; double contactProcessingThreshold = -1; + int activationState = -1; PyObject* localInertiaDiagonalObj=0; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddii", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold,&activationState, &physicsClientId)) { return NULL; } @@ -1260,6 +1261,10 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO { b3ChangeDynamicsInfoSetCcdSweptSphereRadius(command,bodyUniqueId,linkIndex, ccdSweptSphereRadius); } + if (activationState >= 0) + { + b3ChangeDynamicsInfoSetActivationState(command, bodyUniqueId, activationState); + } if (contactProcessingThreshold >= 0) { b3ChangeDynamicsInfoSetContactProcessingThreshold(command, bodyUniqueId, linkIndex, contactProcessingThreshold); @@ -9617,7 +9622,11 @@ initpybullet(void) PyModule_AddIntConstant(m, "URDF_ENABLE_CACHED_GRAPHICS_SHAPES", URDF_ENABLE_CACHED_GRAPHICS_SHAPES); PyModule_AddIntConstant(m, "URDF_ENABLE_SLEEPING", URDF_ENABLE_SLEEPING); PyModule_AddIntConstant(m, "URDF_INITIALIZE_SAT_FEATURES", URDF_INITIALIZE_SAT_FEATURES); - + + PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_SLEEPING", eActivationStateEnableSleeping); + PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping); + PyModule_AddIntConstant(m, "ACTIVATION_STATE_WAKE_UP", eActivationStateWakeUp); + PyModule_AddIntConstant(m, "ACTIVATION_STATE_SLEEP", eActivationStateSleep); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 7b30dd12a..0493cdb02 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -1847,6 +1847,7 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link, void btMultiBody::wakeUp() { + m_sleepTimer = 0; m_awake = true; }