Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

allow to PyBullet.changeDynamics for all links in maximal coordinate rigid bodies

Browse Source 
change snake.py to use useMaximalCoordinate = True by default
This commit is contained in:
erwincoumans
2019-03-08 09:20:32 -08:00
committed by fuchuyuan
parent 53979b1b7b
commit ce531e6015
3 changed files with 57 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@@ -33,9 +33,13 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc
{ {
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal); btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
rbci.m_startWorldTransform = initialWorldTrans; rbci.m_startWorldTransform = initialWorldTrans;
m_rigidBody = new btRigidBody(rbci); btRigidBody* body = new btRigidBody(rbci);
if (m_rigidBody == 0)
return m_rigidBody; {
//only store the root of the multi body
m_rigidBody = body;
}
return body;
} }
class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody) class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)

68
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -7876,104 +7876,124 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
} }
else else
{ {
btRigidBody* rb = 0;
if (body && body->m_rigidBody) if (body && body->m_rigidBody)
{
if (linkIndex == -1)
{
rb = body->m_rigidBody;
}
else
{
if (linkIndex >= 0 && linkIndex < body->m_rigidBodyJoints.size())
{
btRigidBody* parentRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyA();
btRigidBody* childRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyB();
rb = childRb;
}
}
}
if (rb)
{ {
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE)
{ {
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping) if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping)
{ {
body->m_rigidBody->forceActivationState(ACTIVE_TAG); rb->forceActivationState(ACTIVE_TAG);
} }
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping) if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping)
{ {
body->m_rigidBody->forceActivationState(DISABLE_DEACTIVATION); rb->forceActivationState(DISABLE_DEACTIVATION);
} }
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp)
{ {
body->m_rigidBody->forceActivationState(ACTIVE_TAG); rb->forceActivationState(ACTIVE_TAG);
} }
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep)
{ {
body->m_rigidBody->forceActivationState(ISLAND_SLEEPING); rb->forceActivationState(ISLAND_SLEEPING);
} }
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
{ {
btScalar angDamping = body->m_rigidBody->getAngularDamping(); btScalar angDamping = rb->getAngularDamping();
body->m_rigidBody->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping, angDamping); rb->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping, angDamping);
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
{ {
btScalar linDamping = body->m_rigidBody->getLinearDamping(); btScalar linDamping = rb->getLinearDamping();
body->m_rigidBody->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); rb->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
{ {
body->m_rigidBody->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); rb->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
{ {
body->m_rigidBody->setRestitution(restitution); rb->setRestitution(restitution);
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
{ {
body->m_rigidBody->setFriction(lateralFriction); rb->setFriction(lateralFriction);
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
{ {
body->m_rigidBody->setSpinningFriction(spinningFriction); rb->setSpinningFriction(spinningFriction);
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
{ {
body->m_rigidBody->setRollingFriction(rollingFriction); rb->setRollingFriction(rollingFriction);
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR)
{ {
if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor)
{ {
body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); rb->setCollisionFlags(rb->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
} }
else else
{ {
body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); rb->setCollisionFlags(rb->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
} }
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
{ {
btVector3 localInertia; btVector3 localInertia;
if (body->m_rigidBody->getCollisionShape()) if (rb->getCollisionShape())
{ {
body->m_rigidBody->getCollisionShape()->calculateLocalInertia(mass, localInertia); rb->getCollisionShape()->calculateLocalInertia(mass, localInertia);
} }
body->m_rigidBody->setMassProps(mass, localInertia); rb->setMassProps(mass, localInertia);
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
{ {
btScalar orgMass = body->m_rigidBody->getInvMass(); btScalar orgMass = rb->getInvMass();
if (orgMass > 0) if (orgMass > 0)
{ {
body->m_rigidBody->setMassProps(mass, newLocalInertiaDiagonal); rb->setMassProps(mass, newLocalInertiaDiagonal);
} }
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION)
{ {
body->m_rigidBody->setAnisotropicFriction(anisotropicFriction); rb->setAnisotropicFriction(anisotropicFriction);
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD)
{ {
body->m_rigidBody->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold); rb->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS)
{ {
body->m_rigidBody->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius); rb->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius);
//for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled //for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled
body->m_rigidBody->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.); rb->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.);
} }
} }
} }

9
examples/pybullet/examples/snake.py
View File

@@ -12,9 +12,10 @@ plane = p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0,plane) p.createMultiBody(0,plane)
useMaximalCoordinates = False useMaximalCoordinates = True
sphereRadius = 0.25 sphereRadius = 0.25
colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]]) #colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]])
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
mass = 1 mass = 1
visualShapeId = -1 visualShapeId = -1
@@ -49,10 +50,12 @@ sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,basePosition,baseOrien
p.setGravity(0,0,-10) p.setGravity(0,0,-10)
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
anistropicFriction = [1,0.01,0.01]
p.changeDynamics(sphereUid,-1,lateralFriction=2,anisotropicFriction=anistropicFriction)
p.getNumJoints(sphereUid) p.getNumJoints(sphereUid)
for i in range (p.getNumJoints(sphereUid)): for i in range (p.getNumJoints(sphereUid)):
p.getJointInfo(sphereUid,i) p.getJointInfo(sphereUid,i)
p.changeDynamics(sphereUid,i,lateralFriction=2,anisotropicFriction=[1,0.01,0.01])#0,0,0])#1,0.01,0.01]) p.changeDynamics(sphereUid,i,lateralFriction=2,anisotropicFriction=anistropicFriction)
dt = 1./240. dt = 1./240.
SNAKE_NORMAL_PERIOD=0.1#1.5 SNAKE_NORMAL_PERIOD=0.1#1.5