E.R.I.C. Step Algorithm
Following on from my work so far with E.R.I.C. I now am in a position to start the development of the code that will allow it to move.
The act of walking will really be an illusion created by controlling the positions of the feet at time t, providing that the foot positions can be expressed mathematically as a function of t. For movement in a straight line for just the two hind legs (the simplest walking algorithm), both feet will move in the same pattern but with a phase shift of 180° - meaning that when one leg is striding forwards the other will be moving backwards propelling E.R.I.C. along. Additionally for this type of movement the legs will remain on fixed parallel planes when viewed from above while moving in an elliptical motion when viewed from side-on. This motion is shown in the figure below.
So the main idea for this will be:
- Read the timer
- Use an algorithm to find the XYZ position of each foot for time t
- Use the IK_Engine to find out what the servo positions should be to achieve this position
- Move the servos to there new positions
- Repeat until the step is completed
Elliptical Maths Formula
The starting point is the formula for an ellipse. I have always found Wolfram Mathworld a good resource and it doesn't disappoint this time with its article on the ellipse. Looking at equations 13 and 14 on this page it shows that a point on the perimeter of a ellipse can be described in cartesian coordinates mathematically with the following equations:
Ellipse Equations
X = a cos t
Y = b sin t
Where a and b are the width and height of the ellipse. This means that I can send the walk function a distance to move (which would be 2*a) and let the maths take E.R.I.C. to the correct position.
Of course this would produce an ellipse around 0,0 which is the centre of the hip joint, clearly not quite what I'm after. To correct this I'll add an offset in the Z-axis to move the foot into the correct position.
Algorithm Design
So I'll write a little test program that will print the foot position over a period of time to TeraTerm and then plot the results to Excel hopefully resulting in a nice offset ellipse.
The code is something like this...
#include "mbed.h" #define OFFSET 150 // Offset for Z-axis #define RATE 2 // Time in seconds to complete one step #define b 20 // b constant for ellipse calculation #define PI 3.14159 Serial pc(USBTX,USBRX); // To communicate with TeraTerm void ellipse(int stride) { Timer t; // Used to control the ellipse float Y = 0; // Variable for foot extension float Z = 20; // Variable for foot height float a = stride / 2; // The a constant for ellipse calculation float angle = 360 / RATE; // The constant to make algorithm calculate for full 360° angle = angle * PI / 180; // Convert into radians float time = 0; // Variable to hold the timer value. t.start(); // Start the timer while (t < RATE) { time = t.read(); // Read the timer // Calculate the Y position Y = cos(time*angle); Y = a * Y; // Calculate the Z position and correct for Z-axis offset Z = sin(time*angle); Z = b * Z; Z = Z - OFFSET; // Print result to TeraTerm pc.printf("%.2f %.2f %.2f\n\r",time,Y,Z); // Wait a bit to limit the number of results! TESTING ONLY! wait(0.05); } } int main() { ellipse(200); // Call the ellipse function... }
When run this results in a plot like this...
This is not quite right because at t = 0 I want the leg to be at 0 on the Y-axis and to one extreme on the Z-axis. This can be easily corrected by swapping the cos and sin functions for the Y and Z axis.
The other Leg?
Well E.R.I.C. has two legs, so what about the other one? Well good point. To get the hind legs walking I will need the two legs to be anti-phase with each other, i.e. when one if moving forwards the other is moving backwards. To do this I will create an extra two variables for the other leg and add an offset of RATE/2 to the second leg. The plot now looks like this with the right leg in blue and the left leg in red.
N.B. the left leg has been moved up 10mm on the graph axis to make it visible!
The program used is published here
The Next Step...
Now that I have an algorithm that calculates the required position of both feet at any point throughout the step cycle and have control of how long I wish that step to be, it is time to start to add this code to the existing E.R.I.C. program and physically implement it.
The first thing to do here is to create a new 'pose' that moves E.R.I.C.'s feet into the starting position for the step, namely both feet at Y = 0 and Z = -130 for the right leg and Z = -170 for the left. This can easily be done by adding in another const array containing the appropriate values.
const int Step_Start[] = {0, 0, -130, 0, 0, -170};
So when integrated into E.R.I.C.'s main program it looks like this...
This program stands E.R.I.C. up and takes three steps each 150mm long. A video showing this is below.
further work
I could refine the algorithm to bit to break up the total movement into a series of steps, i.e. move 1000mm and function breaks it up into 5*200mm steps. I also need to find a method to give the foot some kind of feedback (a sensor) to tell it when it has hit solid ground. Also some error checking to prevent it trying to move into impossible positions e.g. if it tried to take a step 6 metres long.
Please log in to post comments.