Merge pull request #2276 from erwincoumans/master

add toes to Mini Cheetah

8 files changed, 292 insertions, 69 deletions

diff --git a/examples/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp
index f4bd6c187..9fea9ce35 100644
--- a/examples/InverseKinematics/InverseKinematicsExample.cpp
+++ b/examples/InverseKinematics/InverseKinematicsExample.cpp
@@ -72,18 +72,22 @@ void Reset(Tree& tree, Jacobian* m_ikJacobian)
 
 // Update target positions
 
-void UpdateTargets(double T2, Tree& treeY)
+void UpdateTargets(double T, Tree& treeY)
 {
-	double T = T2 / 5.;
-	targetaa[0].Set(0.6 * b3Sin(0), 0.6 * b3Cos(0), 0.5 + 0.4 * b3Sin(3 * T));
+	targetaa[0].Set(2.0f + 1.5*sin(3 * T) * 2, -0.5 + 1.0f + 0.2*sin(7 * T) * 2, 0.3f + 0.7*sin(5 * T) * 2);
+	targetaa[1].Set(0.5f + 0.4*sin(4 * T) * 2, -0.5 + 0.9f + 0.3*sin(4 * T) * 2, -0.2f + 1.0*sin(3 * T) * 2);
+	targetaa[2].Set(-0.5f + 0.8*sin(6 * T) * 2, -0.5 + 1.1f + 0.2*sin(7 * T) * 2, 0.3f + 0.5*sin(8 * T) * 2);
+	targetaa[3].Set(-1.6f + 0.8*sin(4 * T) * 2, -0.5 + 0.8f + 0.3*sin(4 * T) * 2, -0.2f + 0.3*sin(3 * T) * 2);
+
 }
 
-// Does a single update (on one kind of tree)
+// Does a single update (on one kind of m_ikTree)
 void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod)
 {
+	B3_PROFILE("IK_DoUpdateStep");
 	if (SleepCounter == 0)
 	{
-		T += Tstep;
+		T += Tstep*0.1;
 		UpdateTargets(T, treeY);
 	}
 
@@ -142,7 +146,7 @@ class InverseKinematicsExample : public CommonExampleInterface
 	Jacobian* m_ikJacobian;
 
 	b3AlignedObjectArray<int> m_movingInstances;
-	int m_targetInstance;
+	b3AlignedObjectArray<int> m_targetInstances;
 	enum
 	{
 		numCubesX = 20,
@@ -152,10 +156,9 @@ class InverseKinematicsExample : public CommonExampleInterface
 public:
 	InverseKinematicsExample(CommonGraphicsApp* app, int option)
 		: m_app(app),
-		  m_ikMethod(option),
-		  m_targetInstance(-1)
+		  m_ikMethod(option)		  
 	{
-		m_app->setUpAxis(2);
+		m_app->setUpAxis(1);
 
 		{
 			b3Vector3 extents = b3MakeVector3(100, 100, 100);
@@ -166,7 +169,7 @@ public:
 
 			b3Vector4 color0 = b3MakeVector4(0.4, 0.4, 0.4, 1);
 			b3Vector4 color1 = b3MakeVector4(0.6, 0.6, 0.6, 1);
-			m_app->registerGrid(xres, yres, color0, color1);
+			//m_app->registerGrid(xres, yres, color0, color1);
 		}
 
 		///create some graphics proxy for the tracking target
@@ -177,8 +180,11 @@ public:
 			pos[app->getUpAxis()] = 1;
 			b3Quaternion orn(0, 0, 0, 1);
 			b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1);
-			b3Vector3 scaling = b3MakeVector3(.02, .02, .02);
-			m_targetInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling);
+			b3Vector3 scaling = b3MakeVector3(.1, .1, .1);
+			m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling));
+			m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling));
+			m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling));
+			m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling));
 		}
 		m_app->m_renderer->writeTransforms();
 	}
@@ -215,17 +221,17 @@ public:
 		act.setRotation(rot);
 		act.setOrigin(b3MakeVector3(node->r.x, node->r.y, node->r.z));
 	}
-	void MyDrawTree(Node* node, const b3Transform& tr)
+	void MyDrawTree(Node* node, const b3Transform& tr, const b3Transform& parentTr)
 	{
-		b3Vector3 lineColor = b3MakeVector3(0, 0, 0);
+		
 		int lineWidth = 2;
 		if (node != 0)
 		{
 			//	glPushMatrix();
 			b3Vector3 pos = b3MakeVector3(tr.getOrigin().x, tr.getOrigin().y, tr.getOrigin().z);
-			b3Vector3 color = b3MakeVector3(0, 1, 0);
+			b3Vector3 color1 = b3MakeVector3(0, 1, 0);
 			int pointSize = 10;
-			m_app->m_renderer->drawPoint(pos, color, pointSize);
+			m_app->m_renderer->drawPoint(pos, color1, pointSize);
 
 			m_app->m_renderer->drawLine(pos, pos + 0.05 * tr.getBasis().getColumn(0), b3MakeVector3(1, 0, 0), lineWidth);
 			m_app->m_renderer->drawLine(pos, pos + 0.05 * tr.getBasis().getColumn(1), b3MakeVector3(0, 1, 0), lineWidth);
@@ -236,25 +242,29 @@ public:
 
 			m_app->m_renderer->drawLine(pos, pos + 0.1 * axisWorld, b3MakeVector3(.2, 0.2, 0.7), 5);
 
+			//	glPopMatrix();
+			if (node->right)
+			{
+				b3Transform act;
+				getLocalTransform(node->right, act);
+				b3Transform trr = tr * act;
+				b3Transform ptrr = parentTr * act;
+				b3Vector3 lineColor = b3MakeVector3(0, 1, 0);
+				m_app->m_renderer->drawLine(tr.getOrigin(), ptrr.getOrigin(), lineColor, lineWidth);
+				MyDrawTree(node->right, ptrr, parentTr);  // Draw right siblings recursively
+			}
+
 			//node->DrawNode(node == root);	// Recursively draw node and update ModelView matrix
 			if (node->left)
 			{
 				b3Transform act;
 				getLocalTransform(node->left, act);
-
+				b3Vector3 lineColor = b3MakeVector3(1, 0, 0);
 				b3Transform trl = tr * act;
 				m_app->m_renderer->drawLine(tr.getOrigin(), trl.getOrigin(), lineColor, lineWidth);
-				MyDrawTree(node->left, trl);  // Draw tree of children recursively
-			}
-			//	glPopMatrix();
-			if (node->right)
-			{
-				b3Transform act;
-				getLocalTransform(node->right, act);
-				b3Transform trr = tr * act;
-				m_app->m_renderer->drawLine(tr.getOrigin(), trr.getOrigin(), lineColor, lineWidth);
-				MyDrawTree(node->right, trr);  // Draw right siblings recursively
+				MyDrawTree(node->left, trl, tr);  // Draw m_ikTree of children recursively
 			}
+			
 		}
 	}
 	virtual void stepSimulation(float deltaTime)
@@ -265,12 +275,15 @@ public:
 	{
 		b3Transform act;
 		getLocalTransform(m_ikTree.GetRoot(), act);
-		MyDrawTree(m_ikTree.GetRoot(), act);
+		MyDrawTree(m_ikTree.GetRoot(), act, b3Transform::getIdentity());
 
-		b3Vector3 pos = b3MakeVector3(targetaa[0].x, targetaa[0].y, targetaa[0].z);
-		b3Quaternion orn(0, 0, 0, 1);
+		for (int i = 0; i < m_targetInstances.size(); i++)
+		{
+			b3Vector3 pos = b3MakeVector3(targetaa[i].x, targetaa[i].y, targetaa[i].z);
+			b3Quaternion orn(0, 0, 0, 1);
 
-		m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstance);
+			m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstances[i]);
+		}
 		m_app->m_renderer->writeTransforms();
 		m_app->m_renderer->renderScene();
 	}
@@ -309,40 +322,117 @@ public:
 
 void InverseKinematicsExample::BuildKukaIIWAShape()
 {
-	//const VectorR3& unitx = VectorR3::UnitX;
+	m_ikNodes.resize(29);
+	const VectorR3& unitx = VectorR3::UnitX;
 	const VectorR3& unity = VectorR3::UnitY;
 	const VectorR3& unitz = VectorR3::UnitZ;
 	const VectorR3 unit1(sqrt(14.0) / 8.0, 1.0 / 8.0, 7.0 / 8.0);
 	const VectorR3& zero = VectorR3::Zero;
-
-	float minTheta = -4 * PI;
-	float maxTheta = 4 * PI;
-
-	m_ikNodes.resize(8);  //7DOF+additional endeffector
-
-	m_ikNodes[0] = new Node(VectorR3(0.100000, 0.000000, 0.087500), unitz, 0.08, JOINT, -1e30, 1e30, RADIAN(0.));
+	VectorR3 p0(0.0f, -1.5f, 0.0f);
+	VectorR3 p1(0.0f, -1.0f, 0.0f);
+	VectorR3 p2(0.0f, -0.5f, 0.0f);
+	VectorR3 p3(0.5f*Root2Inv, -0.5 + 0.5*Root2Inv, 0.0f);
+	VectorR3 p4(0.5f*Root2Inv + 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*0.5, 0.0f);
+	VectorR3 p5(0.5f*Root2Inv + 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*0.5, 0.0f);
+	VectorR3 p6(0.5f*Root2Inv + 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*0.5, 0.0f);
+	VectorR3 p7(0.5f*Root2Inv + 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*HalfRoot3, 0.0f);
+	VectorR3 p8(0.5f*Root2Inv + 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*HalfRoot3, 0.0f);
+	VectorR3 p9(0.5f*Root2Inv + 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*HalfRoot3, 0.0f);
+	VectorR3 p10(-0.5f*Root2Inv, -0.5 + 0.5*Root2Inv, 0.0f);
+	VectorR3 p11(-0.5f*Root2Inv - 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*HalfRoot3, 0.0f);
+	VectorR3 p12(-0.5f*Root2Inv - 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*HalfRoot3, 0.0f);
+	VectorR3 p13(-0.5f*Root2Inv - 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*HalfRoot3, 0.0f);
+	VectorR3 p14(-0.5f*Root2Inv - 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*0.5, 0.0f);
+	VectorR3 p15(-0.5f*Root2Inv - 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*0.5, 0.0f);
+	VectorR3 p16(-0.5f*Root2Inv - 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*0.5, 0.0f);
+
+	m_ikNodes[0] = new Node(p0, unit1, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
 	m_ikTree.InsertRoot(m_ikNodes[0]);
 
-	m_ikNodes[1] = new Node(VectorR3(0.100000, -0.000000, 0.290000), unity, 0.08, JOINT, -0.5, 0.4, RADIAN(0.));
+	m_ikNodes[1] = new Node(p1, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
 	m_ikTree.InsertLeftChild(m_ikNodes[0], m_ikNodes[1]);
 
-	m_ikNodes[2] = new Node(VectorR3(0.100000, -0.000000, 0.494500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
+	m_ikNodes[2] = new Node(p1, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
 	m_ikTree.InsertLeftChild(m_ikNodes[1], m_ikNodes[2]);
 
-	m_ikNodes[3] = new Node(VectorR3(0.100000, 0.000000, 0.710000), -unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
+	m_ikNodes[3] = new Node(p2, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
 	m_ikTree.InsertLeftChild(m_ikNodes[2], m_ikNodes[3]);
 
-	m_ikNodes[4] = new Node(VectorR3(0.100000, 0.000000, 0.894500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
-	m_ikTree.InsertLeftChild(m_ikNodes[3], m_ikNodes[4]);
+	m_ikNodes[4] = new Node(p2, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertRightSibling(m_ikNodes[3], m_ikNodes[4]);
+
+	m_ikNodes[5] = new Node(p3, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[3], m_ikNodes[5]);
+
+	m_ikNodes[6] = new Node(p3, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertRightSibling(m_ikNodes[5], m_ikNodes[6]);
+
+	m_ikNodes[7] = new Node(p3, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[5], m_ikNodes[7]);
+
+	m_ikNodes[8] = new Node(p4, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[7], m_ikNodes[8]);
+
+	m_ikNodes[9] = new Node(p5, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[8], m_ikNodes[9]);
+
+	m_ikNodes[10] = new Node(p5, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[9], m_ikNodes[10]);
+
+	m_ikNodes[11] = new Node(p6, zero, 0.08, EFFECTOR);
+	m_ikTree.InsertLeftChild(m_ikNodes[10], m_ikNodes[11]);
+
+	m_ikNodes[12] = new Node(p3, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[6], m_ikNodes[12]);
+
+	m_ikNodes[13] = new Node(p7, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[12], m_ikNodes[13]);
+
+	m_ikNodes[14] = new Node(p8, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[13], m_ikNodes[14]);
+
+	m_ikNodes[15] = new Node(p8, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[14], m_ikNodes[15]);
+
+	m_ikNodes[16] = new Node(p9, zero, 0.08, EFFECTOR);
+	m_ikTree.InsertLeftChild(m_ikNodes[15], m_ikNodes[16]);
+
+	m_ikNodes[17] = new Node(p10, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[4], m_ikNodes[17]);
+
+	m_ikNodes[18] = new Node(p10, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[17], m_ikNodes[18]);
+
+	m_ikNodes[19] = new Node(p10, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertRightSibling(m_ikNodes[17], m_ikNodes[19]);
+
+	m_ikNodes[20] = new Node(p11, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[18], m_ikNodes[20]);
+
+	m_ikNodes[21] = new Node(p12, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[20], m_ikNodes[21]);
+
+	m_ikNodes[22] = new Node(p12, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[21], m_ikNodes[22]);
+
+	m_ikNodes[23] = new Node(p13, zero, 0.08, EFFECTOR);
+	m_ikTree.InsertLeftChild(m_ikNodes[22], m_ikNodes[23]);
+
+	m_ikNodes[24] = new Node(p10, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[19], m_ikNodes[24]);
+
+	m_ikNodes[25] = new Node(p14, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[24], m_ikNodes[25]);
+
+	m_ikNodes[26] = new Node(p15, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[25], m_ikNodes[26]);
 
-	m_ikNodes[5] = new Node(VectorR3(0.100000, 0.000000, 1.110000), unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
-	m_ikTree.InsertLeftChild(m_ikNodes[4], m_ikNodes[5]);
+	m_ikNodes[27] = new Node(p15, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
+	m_ikTree.InsertLeftChild(m_ikNodes[26], m_ikNodes[27]);
 
-	m_ikNodes[6] = new Node(VectorR3(0.100000, 0.000000, 1.191000), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
-	m_ikTree.InsertLeftChild(m_ikNodes[5], m_ikNodes[6]);
+	m_ikNodes[28] = new Node(p16, zero, 0.08, EFFECTOR);
+	m_ikTree.InsertLeftChild(m_ikNodes[27], m_ikNodes[28]);
 
-	m_ikNodes[7] = new Node(VectorR3(0.100000, 0.000000, 1.20000), zero, 0.08, EFFECTOR);
-	m_ikTree.InsertLeftChild(m_ikNodes[6], m_ikNodes[7]);
 }
 
 class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options)
diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
index bc9a772cb..3dfff6f7b 100644
--- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
+++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
@@ -472,8 +472,8 @@ void Jacobian::CalcDeltaThetasSDLS()
 	// Calculate response vector dTheta that is the SDLS solution.
 	//	Delta target values are the dS values
 	int nRows = J.GetNumRows();
-	// TODO: Modify it to work with multiple end effectors.
-	int numEndEffectors = 1;
+	
+	int numEndEffectors = m_tree->GetNumEffector();
 	int nCols = J.GetNumColumns();
 	dTheta.SetZero();
 
diff --git a/examples/pybullet/gym/pybullet_data/mini_cheetah/mini_cheetah.urdf b/examples/pybullet/gym/pybullet_data/mini_cheetah/mini_cheetah.urdf
index 64c8391f3..57da7c0a4 100644
--- a/examples/pybullet/gym/pybullet_data/mini_cheetah/mini_cheetah.urdf
+++ b/examples/pybullet/gym/pybullet_data/mini_cheetah/mini_cheetah.urdf
@@ -100,7 +100,40 @@
             <origin rpy="0 3.141592 0" xyz="0 0 0"/>
         </collision>
     </link>
+    <link name="toe_fr">
+    <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.3"/>
+      <lateral_friction value="3.0"/>
+    </contact>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.015"/>
+      </geometry>
+      <material name="darkgray"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.015"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.15"/>
+      <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+    </inertial>
+  </link>
+  <joint name="toe_fr_joint" type="fixed">
+    <parent link="shank_fr"/>
+    <child link="toe_fr"/>
+    <origin xyz="0 0 -0.18"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
 
+ 
 <!--!!!!!!!!!!!! Front Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
     <joint name="torso_to_abduct_fl_j" type="continuous">
         <axis xyz="1 0 0"/>
@@ -181,6 +214,39 @@
             <origin rpy="0 3.141592 0" xyz="0 0 0"/>
         </collision>
     </link>
+    <link name="toe_fl">
+      <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.3"/>
+      <lateral_friction value="3.0"/>
+    </contact>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.015"/>
+      </geometry>
+      <material name="darkgray"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.015"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.15"/>
+      <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+    </inertial>
+  </link>
+  <joint name="toe_fl_joint" type="fixed">
+    <parent link="shank_fl"/>
+    <child link="toe_fl"/>
+    <origin xyz="0 0 -0.18"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+
 
 <!--!!!!!!!!!!!! Hind Right Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
     <joint name="torso_to_abduct_hr_j" type="continuous">
@@ -262,6 +328,39 @@
             <origin rpy="0 3.141592 0" xyz="0 0 0"/>
         </collision>
     </link>
+     <link name="toe_hr">
+      <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.3"/>
+      <lateral_friction value="3.0"/>
+    </contact>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.015"/>
+      </geometry>
+      <material name="darkgray"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.015"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.15"/>
+      <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+    </inertial>
+  </link>
+  <joint name="toe_hr_joint" type="fixed">
+    <parent link="shank_hr"/>
+    <child link="toe_hr"/>
+    <origin xyz="0 0 -0.18"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+
 
 <!--!!!!!!!!!!!! Hind Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
     <joint name="torso_to_abduct_hl_j" type="continuous">
@@ -343,5 +442,39 @@
             <origin rpy="0 3.141592 0" xyz="0 0 0"/>
         </collision>
     </link>
+    <link name="toe_hl">
+      <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.3"/>
+      <lateral_friction value="3.0"/>
+    </contact>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.015"/>
+      </geometry>
+      <material name="darkgray"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.015"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.15"/>
+      <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+    </inertial>
+  </link>
+  <joint name="toe_hl_joint" type="fixed">
+    <parent link="shank_hl"/>
+    <child link="toe_hl"/>
+    <origin xyz="0 0 -0.18"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+
+
 
 </robot>
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
index 8f6d05a71..93626f18f 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
@@ -311,9 +311,9 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 		case RO_XYZ:
 		{
 			//Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles)
-			//The two planes are non-homologous, so this is a Tait–Bryan angle formalism and not a proper Euler
+			//The two planes are non-homologous, so this is a Tait Bryan angle formalism and not a proper Euler
 			//Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation)
-			//that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait–Bryan angles)
+			//that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait Bryan angles)
 			// x' = Nperp = N.cross(axis2)
 			// y' = N = axis2.cross(axis0)
 			// z' = z
@@ -866,7 +866,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		// vel + f / m * (rotational ? -1 : 1)
 		// so in theory this should be set here for m_constraintError
 		// (with m_constraintError we set a desired velocity for the affected body(es))
-		// however in practice any value is fine as long as it is greater then the "proper" velocity,
+		// however in practice any value is fine as long as it is greater than the "proper" velocity,
 		// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
 		// so it is much simpler (and more robust) just to simply use inf (with the proper sign)
 		// (Even with our best intent the "new" velocity is only an estimation. If we underestimate
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
index bc3ee6d21..00e24364e 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
@@ -294,7 +294,7 @@ protected:
 	bool m_hasStaticBody;
 	int m_flags;
 
-	btGeneric6DofSpring2Constraint& operator=(btGeneric6DofSpring2Constraint&)
+	btGeneric6DofSpring2Constraint& operator=(const btGeneric6DofSpring2Constraint&)
 	{
 		btAssert(0);
 		return *this;
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index d4f8a42c5..3e210d752 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -352,9 +352,9 @@ void btMultiBody::finalizeMultiDof()
 	updateLinksDofOffsets();
 }
 
-int btMultiBody::getParent(int i) const
+int btMultiBody::getParent(int link_num) const
 {
-	return m_links[i].m_parent;
+	return m_links[link_num].m_parent;
 }
 
 btScalar btMultiBody::getLinkMass(int i) const
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index 79fae9efd..c0b0d003b 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -65,7 +65,7 @@ public:
 	virtual ~btMultiBody();
 
 	//note: fixed link collision with parent is always disabled
-	void setupFixed(int linkIndex,
+	void setupFixed(int i, //linkIndex
 					btScalar mass,
 					const btVector3 &inertia,
 					int parent,
@@ -83,7 +83,7 @@ public:
 						const btVector3 &thisPivotToThisComOffset,
 						bool disableParentCollision);
 
-	void setupRevolute(int linkIndex,  // 0 to num_links-1
+	void setupRevolute(int i,  // 0 to num_links-1
 					   btScalar mass,
 					   const btVector3 &inertia,
 					   int parentIndex,
@@ -93,7 +93,7 @@ public:
 					   const btVector3 &thisPivotToThisComOffset,    // vector from joint axis to my COM, in MY frame
 					   bool disableParentCollision = false);
 
-	void setupSpherical(int linkIndex,  // 0 to num_links-1
+	void setupSpherical(int i,  // linkIndex, 0 to num_links-1
 						btScalar mass,
 						const btVector3 &inertia,
 						int parent,
@@ -277,15 +277,15 @@ public:
 	//
 	// transform vectors in local frame of link i to world frame (or vice versa)
 	//
-	btVector3 localPosToWorld(int i, const btVector3 &vec) const;
-	btVector3 localDirToWorld(int i, const btVector3 &vec) const;
-	btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
-	btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
+	btVector3 localPosToWorld(int i, const btVector3 &local_pos) const;
+	btVector3 localDirToWorld(int i, const btVector3 &local_dir) const;
+	btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const;
+	btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const;
 
 	//
 	// transform a frame in local coordinate to a frame in world coordinate
 	//
-	btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const;
+	btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const;
 
 	//
 	// calculate kinetic energy and angular momentum
@@ -576,11 +576,11 @@ public:
 	{
 		return m_internalNeedsJointFeedback;
 	}
-	void forwardKinematics(btAlignedObjectArray<btQuaternion> & scratch_q, btAlignedObjectArray<btVector3> & scratch_m);
+	void forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local, btAlignedObjectArray<btVector3> & local_origin);
 
 	void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;
 
-	void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & scratch_q, btAlignedObjectArray<btVector3> & scratch_m);
+	void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
 
 	virtual int calculateSerializeBufferSize() const;
 
diff --git a/src/LinearMath/btMatrixX.h b/src/LinearMath/btMatrixX.h
index 0fa8cd0d3..388c57c2d 100644
--- a/src/LinearMath/btMatrixX.h
+++ b/src/LinearMath/btMatrixX.h
@@ -284,7 +284,7 @@ struct btMatrixX
 		}
 	}
 
-	void printMatrix(const char* msg)
+	void printMatrix(const char* msg) const
 	{
 		printf("%s ---------------------\n", msg);
 		for (int i = 0; i < rows(); i++)