## Saturday, 29 December 2012

### Hexapod Inverse Kinematics - Part 1

Inverse kinematics refers to the set of kinematic equations that defines the angle of joints given the position end-effector. Fortunately the inverse kinematic problem can be analytically solved for the simple case of a 3 degree of freedom leg. This post will discuss how to find the angle of the joints for the leg of a Phoenix hexapod leg. The motion of the leg was constrained to the x and y dimensions to simplify the problem. However it should not be overly difficult to extend to the z axis as well.

A diagram of a Phoenix leg is shown below

 Figure 1: Phoenix Leg

The figure depicts the leg configuration when the tibia and femur servos in the neutral position. Notice that there is an angle of approximately 112 degrees between the end effector (tip of the tibia) and the femur.

If the motion of the the leg is constrained to the x and y axis, then it can be drawn as shown below
 Figure 2: hexapod leg model
where angles θ1 and θare the angles of the femur and tibia. The equations to derive the angles are as follows
The equations presented defined the angle to be positive moves counter-clockwise from the positive x axis. This is the standard convention when specifying an angle. Also note that the equations presented were for the the right leg. Rotational directions must be corrected for the left legs.

It is important to also consider the configuration the hexapod leg when determining the kinematic equation. For example
 Figure 3: rotation of femur
the tibia will rotate in the counter clockwise (red arrow) direction if the tibia servo is assumed to rotate in the clockwise direction (purple arrow). The femur however, will rotate in the same direction as the femur servo. The behaviour of the femur and tibia is a direct consequence of the connection of the femur and tibia to their respective servos.

Presented below is a task (CoOS) that will determine joint angles for the specified x and y coordinates. The task was setup such that the leg would move from a starting position to an end position and back to the start (see video below). The code is for the left leg.

```void task_IK (void* pdata)
{
float theta_femur = 0.0;
float theta_tibia = 0.0;
float phi_1 = 0.0;
float phi_2 = 0.0;
float gamma = 0.0;
float L3 = 0.0;
int count = 0;
int test = 0;
float X = 116.0; //X coordinate of leg
float Y = -100.0; //Y coordinate of leg
int direction = 1;

for(;;)
{
L3 = sqrtf(X*X + Y*Y);
gamma = atanf(Y/X);
phi_1 = acosf((L_femur*L_femur + L3*L3 - L_tibia*L_tibia)/(2*L_femur*L3));
theta_femur = (phi_1 + gamma)*180.0/PI;

phi_2 = acosf((L_femur*L_femur + L_tibia*L_tibia - L3*L3)/(2*L_femur*L_tibia));
theta_tibia = phi_2*180.0/PI - 112.0;

//neutral position of servo corresponds to a pulse wide of 4500 units.
//the servo netural position corresonds to an angle of 0 degrees
pwm_femur = (int)(4500.0 + theta_femur*30.0); //be careful about left and right leg and rotational direction
pwm_tibia = (int)(4500.0 - theta_tibia*30.0); //careful about rotational direction
TIM3->CCR3 = pwm_femur; //change duty cycle of Femur servo
TIM3->CCR4 = pwm_tibia; //change duty cycle of Tibia servo
if(direction == 1) //move leg up
{
X = X - 1.0;
Y = Y + 1.0;
count = count + 1;
}
else //move leg down
{
X = X + 1.0;
Y = Y - 1.0;
count = count - 1;
}
if(count >= 60) direction = 0; //count down
if(count <= 0) direction = 1; //count up
CoTickDelay(10); //insert 10 ms delay
}
}
```

The motion of the leg is shown in the video below.

Note: you will have to modify the equations presented to your particular setup.
Part 2 will discuss how to move in the z axis as well as X and Y axis.