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

2 files changed, 132 insertions, 2 deletions

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