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 |

_{1 }and θ

_{2 }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 |

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.

nice overview, but I think you switched the tibia and the femur. the coxa is a the shoulder, then the femur, last part is the tibia.

ReplyDeleteRegards

I did mix the tibia and femur. It has been fixed.

ReplyDelete