Bresenham.cpp (7437Bytes)
GetSingleKinematicAction.cpp (2280Bytes)
+++ UPDATE (12-march-2014): Rename title as "Biped Howto".
Hi All, I would like to share with you the way I move feet of my robot POPSTAR. Placing a robot's foot in a target coordinate implies that legs joints must rotate to a determined angle. These angles depends on legs lenghts and the position of the end-effector (at the foot). The way to determine these angles is known as Inverse Kinematics.
In the case of the POPSTAR_1 robot, the way I've calculated the inverse kinematics model of the legs was as shown below.
In a first step, I proceed to get leg lengths: thigh, shin and ankle, and I stablished the origin of coordinates (0,0,0) at the hip level.
The end-effector was placed in the foot, and I had to calculate the angles of the 3 joints of the leg (AngThigh, AngShin and AngAnkle), applying the inverse kinematics model.
In the case of POPSTAR_1 robot, legs move only along X axis and Z axis. This simplifies the trigonomotric equations of the model. Below in file <GetSingleKinematicAction.cpp> is the C++ routine that calculates the angles to place the end-effector in a target coordinate.
Once, I had the inverse kinematics model ready for movements, it was time to define the way in which the leg would move through a predefined path to reach such target position. In this case, I decided to use the Bresenham's line algorithm. This algorithm determines the path points in a raster to form a straight line between two points. And as it is commonly used in plotters or graphic rastering due to its speed and simplicity, I supposed it'd be a very good solution for realtime locomotion path planning.
The routine I adapted to generate Bresenham’s line is attached in file <Bresenham.cpp>.
In that routine, the value of variable <stepDistance> will give the amount of milimeters, that the end-effector will move, per update cycle (in my case 50ms) . For example, a stepdistance=2 will move the foot ahead 2mm each 50ms,what is translated in a movement of 40mm each second. So, modifying this variable I could make the robot to move faster or slower (Bresenham’s algorithm would generate more or less intermediate path points to reach the
target destination, according with that variable).
Once, defined both inverse kinematics model and Bresenham’s line algorithm, was time to test it. In a first attempt, to test a right foot step, I defined the path as a 2 straight line paths. Right foot should move from point P0 to point P1 and from point P1 to point P2.
And goal reached!!! Now I could move the robot’s legs as I desired.
The whole process to carry out a complete movement then is formed by these operations:
- Establish a target destination for each foot and a speed of movement (setup stepDistance variable).
- Executes the Bresenham’s line algorithm to obtain an array of points xyz[] for each foot.
- Executes the Inverse Kinematics model for each point in the array xyz, and store those values in an array of actions[] for each foot.
- Each 50ms sends a new actions[] value to the servo driver to move all joints a step.
- Repeat point 4 until target position is reached.
And that’s all!! See you, guys.