Expose anisotropic friction, add snake demo. Simple snake slither locomotion from > 15 years ago, thanks to Michael Ewert @ Havok!

Visit http://www.snakerobots.com to see one of these in the wild

6 files changed, 169 insertions, 3 deletions

diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index cc2187ed5..12ef3975a 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -2780,6 +2780,21 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle comman
 	return 0;
 }
 
+B3_SHARED_API int b3ChangeDynamicsInfoSetAnisotropicFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double anisotropicFriction[])
+{
+	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 = linkIndex;
+	command->m_changeDynamicsInfoArgs.m_anisotropicFriction[0] = anisotropicFriction[0];
+	command->m_changeDynamicsInfoArgs.m_anisotropicFriction[1] = anisotropicFriction[1];
+	command->m_changeDynamicsInfoArgs.m_anisotropicFriction[2] = anisotropicFriction[2];
+
+	command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION;
+	return 0;
+}
+
+
 B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double localInertiaDiagonal[])
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 01529410b..3499a8c88 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -148,7 +148,9 @@ extern "C"
 
 	B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
 	B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double localInertiaDiagonal[]);
+	B3_SHARED_API int b3ChangeDynamicsInfoSetAnisotropicFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double anisotropicFriction[]);
 
+	
 	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);
 	B3_SHARED_API int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 0c21c325e..f0f8dc6d2 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -7664,6 +7664,9 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 	btVector3 newLocalInertiaDiagonal(clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[0],
 									  clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[1],
 									  clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[2]);
+	btVector3 anisotropicFriction(clientCmd.m_changeDynamicsInfoArgs.m_anisotropicFriction[0],
+		clientCmd.m_changeDynamicsInfoArgs.m_anisotropicFriction[1],
+		clientCmd.m_changeDynamicsInfoArgs.m_anisotropicFriction[2]);
 
 	btAssert(bodyUniqueId >= 0);
 
@@ -7796,6 +7799,12 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 			{
 				mb->setBaseInertia(newLocalInertiaDiagonal);
 			}
+			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION)
+			{
+				mb->getBaseCollider()->setAnisotropicFriction(anisotropicFriction);
+			}
+
+
 		}
 		else
 		{
@@ -7858,6 +7867,10 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 				{
 					mb->getLink(linkIndex).m_inertiaLocal = newLocalInertiaDiagonal;
 				}
+				if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION)
+				{
+					mb->getLinkCollider(linkIndex)->setAnisotropicFriction(anisotropicFriction);
+				}
 			}
 		}
 	}
@@ -7946,7 +7959,11 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 					body->m_rigidBody->setMassProps(mass, newLocalInertiaDiagonal);
 				}
 			}
-
+			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION)
+			{
+				body->m_rigidBody->setAnisotropicFriction(anisotropicFriction);
+			}
+			
 			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD)
 			{
 				body->m_rigidBody->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index 8b4dfcb4c..7c25bb3f1 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -164,6 +164,7 @@ enum EnumChangeDynamicsInfoFlags
 	CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD = 4096,
 	CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE = 8192,
 	CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING = 16384,
+	CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION = 32768,
 };
 
 struct ChangeDynamicsInfoArgs
@@ -186,6 +187,7 @@ struct ChangeDynamicsInfoArgs
 	double m_contactProcessingThreshold;
 	int m_activationState;
 	double m_jointDamping;
+	double m_anisotropicFriction[3];
 };
 
 struct GetDynamicsInfoArgs
diff --git a/examples/pybullet/examples/snake.py b/examples/pybullet/examples/snake.py
new file mode 100644
index 000000000..52d876aeb
--- /dev/null
+++ b/examples/pybullet/examples/snake.py
@@ -0,0 +1,123 @@
+import pybullet as p
+import time
+import math
+
+
+# This simple snake logic is from some 15 year old Havok C++ demo
+# Thanks to Michael Ewert!
+  
+  
+p.connect(p.GUI)
+plane = p.createCollisionShape(p.GEOM_PLANE)
+
+p.createMultiBody(0,plane)
+
+useMaximalCoordinates = False
+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]])
+
+mass = 1
+visualShapeId = -1
+  
+link_Masses=[]
+linkCollisionShapeIndices=[]
+linkVisualShapeIndices=[]
+linkPositions=[]
+linkOrientations=[]
+linkInertialFramePositions=[]
+linkInertialFrameOrientations=[]
+indices=[]
+jointTypes=[]
+axis=[]
+
+for i in range (36):
+  link_Masses.append(1)
+  linkCollisionShapeIndices.append(colBoxId)
+  linkVisualShapeIndices.append(-1)
+  linkPositions.append([0,sphereRadius*2.0+0.01,0])
+  linkOrientations.append([0,0,0,1])
+  linkInertialFramePositions.append([0,0,0])
+  linkInertialFrameOrientations.append([0,0,0,1])
+  indices.append(i)
+  jointTypes.append(p.JOINT_REVOLUTE)
+  axis.append([0,0,1])
+
+basePosition = [0,0,1]
+baseOrientation = [0,0,0,1]
+sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,basePosition,baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis, useMaximalCoordinates=useMaximalCoordinates)
+
+p.setGravity(0,0,-10)
+p.setRealTimeSimulation(0)
+
+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])
+  
+dt = 1./240.
+SNAKE_NORMAL_PERIOD=0.1#1.5
+m_wavePeriod = SNAKE_NORMAL_PERIOD
+
+m_waveLength=4
+m_wavePeriod=1.5
+m_waveAmplitude=0.4
+m_waveFront = 0.0
+#our steering value
+m_steering = 0.0
+m_segmentLength = sphereRadius*2.0
+forward = 0
+
+while (1):
+  keys = p.getKeyboardEvents()
+  for k,v in keys.items():
+    
+    if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_TRIGGERED)):
+      m_steering = -.2
+    if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_RELEASED)):
+      m_steering = 0
+    if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_TRIGGERED)):
+      m_steering = .2
+    if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_RELEASED)):
+      m_steering = 0
+    
+  amp = 0.2
+  offset = 0.6
+  numMuscles = p.getNumJoints(sphereUid)
+  scaleStart = 1.0
+
+  #start of the snake with smaller waves.  
+  #I think starting the wave at the tail would work better ( while it still goes from head to tail )
+  if( m_waveFront < m_segmentLength*4.0 ):
+    scaleStart = m_waveFront/(m_segmentLength*4.0)
+
+  segment = numMuscles-1
+
+  #we simply move a sin wave down the body of the snake.
+  #this snake may be going backwards, but who can tell ;)
+  for joint in range (p.getNumJoints(sphereUid)):
+    segment = joint#numMuscles-1-joint
+    #map segment to phase
+    phase = (m_waveFront - (segment+1)*m_segmentLength)/ m_waveLength
+    phase -= math.floor(phase)
+    phase *= math.pi * 2.0
+
+    #map phase to curvature
+    targetPos = math.sin( phase ) * scaleStart * m_waveAmplitude
+
+    #// steer snake by squashing +ve or -ve side of sin curve
+    if( m_steering > 0 and targetPos < 0 ):
+      targetPos *= 1.0/( 1.0 + m_steering )
+      
+    if( m_steering < 0 and targetPos > 0 ):
+     targetPos *= 1.0/( 1.0 - m_steering )
+    
+    #set our motor
+    p.setJointMotorControl2(sphereUid,joint,p.POSITION_CONTROL,targetPosition=targetPos+m_steering,force=30)
+
+  #wave keeps track of where the wave is in time
+  m_waveFront += dt/m_wavePeriod * m_waveLength;
+  p.stepSimulation()
+
+  
+  time.sleep(dt)
+  
\ No newline at end of file
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 7f800d40c..755fcea13 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -1242,12 +1242,13 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 	int activationState = -1;
 	double jointDamping = -1;
 	PyObject* localInertiaDiagonalObj = 0;
+	PyObject* anisotropicFrictionObj = 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", "activationState", "jointDamping", "physicsClientId", NULL};
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &physicsClientId))
+	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "physicsClientId", NULL};
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -1273,6 +1274,12 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 		{
 			b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, mass);
 		}
+		if (anisotropicFrictionObj)
+		{
+			double anisotropicFriction[3];
+			pybullet_internalSetVectord(anisotropicFrictionObj, anisotropicFriction);
+			b3ChangeDynamicsInfoSetAnisotropicFriction(command, bodyUniqueId, linkIndex, anisotropicFriction);
+		}
 		if (localInertiaDiagonalObj)
 		{
 			double localInertiaDiagonal[3];