AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

expose PyBullet API to wakeup/put objects to sleep, enable/disable deactivation

Browse Source 
fix wakeup -> reset deactivation clock
This commit is contained in:
Erwin Coumans 2018-06-15 21:26:26 -07:00
parent bb0d70d98b
commit fbbd675ed6
8 changed files with 86 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -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)

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -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);

41
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -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();

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -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

9
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -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;

11
examples/pybullet/examples/sleeping.py
View File

@ -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)

15
examples/pybullet/pybullet.c
View File

@ -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);

1
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@ -1847,6 +1847,7 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link,
void btMultiBody::wakeUp()
{
m_sleepTimer = 0;
m_awake = true;
}