When I created the eight degree of freedom (8 DOF) biped robot in my last blog post, I wrote a C# application to calculate servo positions, which in turn generated a smooth, life-like, walking gait. In this post I will walk through the application logic in more detail. The application generates a motion plan, runs the inverse kinematics calculations and allows me to visualise the results as rendered a stick man. The complete source code is below.

According to Wikipedia, “Inverse kinematics refers to the use of the kinematics equations of a robot to determine the joint parameters that provide a desired position of the end-effector. Specification of the movement of a robot so that its end-effector achieves a desired task is known as motion planning. Inverse kinematics transforms the motion plan into joint actuator trajectories for the robot.”

Starting with the motion plan and using the two rules established in my previous post (#1 static hip height and #2 clipped sinusoidal foot motion), I knew roughly what I wanted each joint to do. Next I had to model that mathematically. Hip height was easy, as it’s constant. Left and right feet follow the pattern illustrated below.

Biped Robot Hip & Foot Height Against Time

I created a system of triangles to represent each of the robot joints and could now begin to calculate the relative angles between them for each time interval. To preserve symmetry, I decided that the feet would always remain parallel with the body (and floor) and that the horizontal plane would always bisect the knee angle. These principles helped determine many of the joint angles using some simple trigonometry.

Biped Robot Limb Angles

All that remained was to solve any outstanding angles using the law of cosines. The law of cosines can be used in a number of ways – such as calculating the third side of a triangle when two sides and their enclosed angle are known, or to determine the angles of a triangle if all three sides are known. Once the angles are known, these can be translated into servo positions, with the appropriate amount of offset and direction applied.

This could now be implemented in code – the motion plan (determining foot geometry), the inverse kinematics (determining servo angle) calculation and the joint visualisation. I won’t walk through every step as the source code is pretty easy to decipher.

Note about the code: I separate the code into a few classes to keep key objects and values partitioned (legs, hip, etc). In this post I’ve compressed everything into a single file, which will execute – just paste the entire block into a new Windows Form project. However, if you want to modify the code, for maintainability, it would be best to break out again into discrete class files.

I hope you find this useful.

 

Code Snippet
  1. using System;
  2. using System.Collections.Generic;
  3. using System.Drawing;
  4. using System.IO;
  5. using System.Threading;
  6. using System.Windows.Forms;
  7. namespace Biped
  8. {
  9.     public class Canvass : Form
  10.     {
  11.         private List<int[]> _patterns = new List<int[]>();
  12.         private Hip _hip = new Hip();
  13.         private int _p = 0;
  14.         [STAThread]
  15.         static void Main()
  16.         {
  17.             Application.Run(new Canvass());
  18.         }
  19.         public Canvass()
  20.         {
  21.             this.Paint += Render;
  22.             this.Height = 300;
  23.             this.Width = 250;
  24.             CalcFeetCoordinates();
  25.         }
  26.         private void CalcFeetCoordinates()
  27.         {
  28.             // Move left leg forward.
  29.             for (int i = 0; i < 20; i++)
  30.             {
  31.                 double x = (i – 10) * (Leg.StrideLength / 20.0);
  32.                 double y = Leg.HipHeight;
  33.                 AddStridePosition(x, y, -45);
  34.             }
  35.             // Move left leg backward.
  36.             for (int i = 0; i < 20; i++)
  37.             {
  38.                 double x = (10 – i) * (Leg.StrideLength / 20.0);
  39.                 double y = FootHeight(x);
  40.                 AddStridePosition(x, y, 45);
  41.             }
  42.             // Build right leg from phase shift clone of left.
  43.             for (int i = 0; i < 40; i++)
  44.                 for (int j = 0; j < 4; j++)
  45.                     _patterns[i][j + 4] = -_patterns[(i + 20) % 40][j];
  46.             // Roll ankles on transition.
  47.             RollAnkle(19, 20, 6);
  48.             RollAnkle(45, 0, 6);
  49.             // Write servo positions to file.
  50.             DumpToFile();
  51.         }
  52.         private double FootHeight(double x)
  53.         {
  54.             return Leg.HipHeight – Leg.FootLift * Math.Cos(Math.Abs(x * Math.PI / Leg.StrideLength));
  55.         }
  56.         private void AddStridePosition(double x, double y, int tilt)
  57.         {
  58.             // Cosine rule: cos A = (b^2 + c^2 – a^2) / 2bc
  59.             int[] pos = new int[8];
  60.             double hypSqrd = Math.Pow(x, 2) + Math.Pow(y, 2);
  61.             double hyp = Math.Sqrt(hypSqrd);
  62.             pos[0] = 0 – RadToStep(Math.Acos(hyp / (2 * Leg.Bone)) – Math.Atan2(x, y));
  63.             pos[1] = RadToStep(Math.Acos((2 * Leg.BoneSqrd – hypSqrd) / (2 * Leg.BoneSqrd))) – 512;
  64.             pos[2] = pos[0] – pos[1];
  65.             pos[3] = tilt;
  66.             _patterns.Add(pos);
  67.         }
  68.         private void RollAnkle(int r1, int r2, int steps)
  69.         {
  70.             int[] row1 = _patterns[r1];
  71.             int[] row2 = _patterns[r2];
  72.             for (int i = 0; i < steps; i++)
  73.             {
  74.                 int[] pos = new int[8];
  75.                 for (int j = 0; j < 8; j++)
  76.                     pos[j] = row1[j] – ((row1[j] – row2[j]) * (i + 1)) / (steps + 1);
  77.                 _patterns.Insert(r1 + 1 + i, pos);
  78.             }
  79.         }
  80.         private void Render(object sender, PaintEventArgs e)
  81.         {
  82.             _hip.Render(_patterns[_p++], e.Graphics);
  83.             if (_p == _patterns.Count) _p = 0;
  84.             this.Invalidate();
  85.             Thread.Sleep(100);
  86.         }
  87.         private int RadToStep(double rads)
  88.         {
  89.             return (int)(rads * 512 / Math.PI);
  90.         }
  91.         private void DumpToFile()
  92.         {
  93.             using (TextWriter tw = new StreamWriter(“biped.csv”, false))
  94.             {
  95.                 foreach (int[] pos in _patterns)
  96.                     tw.WriteLine(“{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}”,
  97.                         pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6], pos[7]);
  98.                 tw.Close();
  99.             }
  100.         }
  101.     }
  102.     public class Hip
  103.     {
  104.         private Leg _leftLeg = new Leg();
  105.         private Leg _rightLeg = new Leg();
  106.         public void Render(int[] steps, Graphics graph)
  107.         {
  108.             _leftLeg.SetServos(steps[0], steps[1], steps[2], -1);
  109.             _leftLeg.Render(graph, Pens.Black);
  110.             _rightLeg.SetServos(steps[4], steps[5], steps[6], 1);
  111.             _rightLeg.Render(graph, Pens.Blue);
  112.         }
  113.     }
  114.     public class Leg
  115.     {
  116.         public static int Bone = 100;
  117.         public static int BoneSqrd = Bone * Bone;
  118.         public static int HipHeight = 180;
  119.         public static int StrideLength = 60;
  120.         public static int FootLift = 20;
  121.         private static int _foot = Bone / 5;
  122.         private double[] _joints = new double[3];
  123.         public void SetServos(int hip, int knee, int ankle, int direction)
  124.         {
  125.             _joints[0] = StepToRad(hip * direction);
  126.             _joints[1] = StepToRad(-knee * direction);
  127.             _joints[2] = StepToRad(-ankle * direction + 256);
  128.         }
  129.         public void Render(Graphics g, Pen pen)
  130.         {
  131.             Point[] points = new Point[4];
  132.             points[0] = new Point(100, 40);
  133.             points[1] = new Point();
  134.             points[1].X = points[0].X + (int)(Math.Sin(_joints[0]) * Bone);
  135.             points[1].Y = points[0].Y + (int)(Math.Cos(_joints[0]) * Bone);
  136.             points[2] = new Point();
  137.             points[2].X = points[1].X + (int)(Math.Sin(_joints[0] + _joints[1]) * Bone);
  138.             points[2].Y = points[1].Y + (int)(Math.Cos(_joints[0] + _joints[1]) * Bone);
  139.             points[3] = new Point();
  140.             points[3].X = points[2].X + (int)(Math.Sin(_joints[0] + _joints[1] + _joints[2]) * _foot);
  141.             points[3].Y = points[2].Y + (int)(Math.Cos(_joints[0] + _joints[1] + _joints[2]) * _foot);
  142.             for (int i = 0; i < 3; i++)
  143.                 g.DrawLine(pen, points[i], points[i + 1]);
  144.         }
  145.         private double StepToRad(int steps)
  146.         {
  147.             return Math.PI * steps / 512.0;
  148.         }
  149.     }
  150. }