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

change snake.py to use useMaximalCoordinate = True by default

3 files changed, 57 insertions, 30 deletions

diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
index 905a6cc00..e41ef7881 100644
--- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
+++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
@@ -33,9 +33,13 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc
 {
 	btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
 	rbci.m_startWorldTransform = initialWorldTrans;
-	m_rigidBody = new btRigidBody(rbci);
-
-	return m_rigidBody;
+	btRigidBody* body = new btRigidBody(rbci);
+	if (m_rigidBody == 0)
+	{
+		//only store the root of the multi body
+		m_rigidBody = body;
+	}
+	return body;
 }
 
 class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index f0f8dc6d2..9de30169f 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -7876,104 +7876,124 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 	}
 	else
 	{
+
+		btRigidBody* rb = 0;
 		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_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping)
 				{
-					body->m_rigidBody->forceActivationState(ACTIVE_TAG);
+					rb->forceActivationState(ACTIVE_TAG);
 				}
 				if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping)
 				{
-					body->m_rigidBody->forceActivationState(DISABLE_DEACTIVATION);
+					rb->forceActivationState(DISABLE_DEACTIVATION);
 				}
 				if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp)
 				{
-					body->m_rigidBody->forceActivationState(ACTIVE_TAG);
+					rb->forceActivationState(ACTIVE_TAG);
 				}
 				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)
 			{
-				btScalar angDamping = body->m_rigidBody->getAngularDamping();
-				body->m_rigidBody->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping, angDamping);
+				btScalar angDamping = rb->getAngularDamping();
+				rb->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping, angDamping);
 			}
 			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
 			{
-				btScalar linDamping = body->m_rigidBody->getLinearDamping();
-				body->m_rigidBody->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
+				btScalar linDamping = rb->getLinearDamping();
+				rb->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
 			}
 
 			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)
 			{
-				body->m_rigidBody->setRestitution(restitution);
+				rb->setRestitution(restitution);
 			}
 			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)
 			{
-				body->m_rigidBody->setSpinningFriction(spinningFriction);
+				rb->setSpinningFriction(spinningFriction);
 			}
 			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_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
 				{
-					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)
 			{
 				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)
 			{
-				btScalar orgMass = body->m_rigidBody->getInvMass();
+				btScalar orgMass = rb->getInvMass();
 				if (orgMass > 0)
 				{
-					body->m_rigidBody->setMassProps(mass, newLocalInertiaDiagonal);
+					rb->setMassProps(mass, newLocalInertiaDiagonal);
 				}
 			}
 			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)
 			{
-				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)
 			{
-				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
-				body->m_rigidBody->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.);
+				rb->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.);
 			}
 		}
 	}
diff --git a/examples/pybullet/examples/snake.py b/examples/pybullet/examples/snake.py
index 52d876aeb..af2c6d512 100644
--- a/examples/pybullet/examples/snake.py
+++ b/examples/pybullet/examples/snake.py
@@ -12,9 +12,10 @@ plane = p.createCollisionShape(p.GEOM_PLANE)
 
 p.createMultiBody(0,plane)
 
-useMaximalCoordinates = False
+useMaximalCoordinates = True
 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
 visualShapeId = -1
@@ -49,10 +50,12 @@ sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,basePosition,baseOrien
 p.setGravity(0,0,-10)
 p.setRealTimeSimulation(0)
 
+anistropicFriction = [1,0.01,0.01]
+p.changeDynamics(sphereUid,-1,lateralFriction=2,anisotropicFriction=anistropicFriction)
 p.getNumJoints(sphereUid)
 for i in range (p.getNumJoints(sphereUid)):
   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.
 SNAKE_NORMAL_PERIOD=0.1#1.5