Biorobotics 7
/
Inverse_kinematic
Inverse kinematics
main.cpp@0:4a9c733c3b53, 2018-10-22 (annotated)
- Committer:
- MAHCSnijders
- Date:
- Mon Oct 22 14:52:06 2018 +0000
- Revision:
- 0:4a9c733c3b53
- Child:
- 1:df3d7f71db4b
Inverse kinematics version 1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MAHCSnijders | 0:4a9c733c3b53 | 1 | #include "mbed.h" |
MAHCSnijders | 0:4a9c733c3b53 | 2 | #include "math.h" |
MAHCSnijders | 0:4a9c733c3b53 | 3 | #include "Matrix.h" |
MAHCSnijders | 0:4a9c733c3b53 | 4 | |
MAHCSnijders | 0:4a9c733c3b53 | 5 | const float L0; // Length between two motors [meter] |
MAHCSnijders | 0:4a9c733c3b53 | 6 | const float L1; // Length first beam from right motor2 [meter] |
MAHCSnijders | 0:4a9c733c3b53 | 7 | const float L2; // Length second beam from right motor2 [meter] |
MAHCSnijders | 0:4a9c733c3b53 | 8 | const float L3; // Length beam between L2 and L4 [meter] |
MAHCSnijders | 0:4a9c733c3b53 | 9 | const float L4; // Length first beam from left motor1 [meter] |
MAHCSnijders | 0:4a9c733c3b53 | 10 | const float L5; // Length from L3 to end-effector [meter] |
MAHCSnijders | 0:4a9c733c3b53 | 11 | const double PI = 3.14159265359; |
MAHCSnijders | 0:4a9c733c3b53 | 12 | volatile float Pe_x; // x-coordinate of end-effector from frame 0 [meter] |
MAHCSnijders | 0:4a9c733c3b53 | 13 | volatile float Pe_y; // y-coordinate of end-effector from frame 0 [meter] |
MAHCSnijders | 0:4a9c733c3b53 | 14 | volatile float motor_angle1; // Desired angle of motor 1 (left) [rad] |
MAHCSnijders | 0:4a9c733c3b53 | 15 | volatile float motor_angle2; // Desired angle of motor 2 (right) [rad] |
MAHCSnijders | 0:4a9c733c3b53 | 16 | |
MAHCSnijders | 0:4a9c733c3b53 | 17 | ticker IK // Ticker function for inverse kinematics |
MAHCSnijders | 0:4a9c733c3b53 | 18 | |
MAHCSnijders | 0:4a9c733c3b53 | 19 | void InverseKinematics() |
MAHCSnijders | 0:4a9c733c3b53 | 20 | { |
MAHCSnijders | 0:4a9c733c3b53 | 21 | // Calculation of the position of joint 3 in frame 0 |
MAHCSnijders | 0:4a9c733c3b53 | 22 | float n = sqrt(pow(Pe_x,2) + pow(Pe_y,2); // Radius between origin frame 0 and endeffector [meter] |
MAHCSnijders | 0:4a9c733c3b53 | 23 | float omega = acos(-(pow(n,2) - pow(L4,2) - pow(L5,2))/(2*L4*L5)); // Angle between L4 and L5 [rad] |
MAHCSnijders | 0:4a9c733c3b53 | 24 | float q4 = PI - omega; // Angle of joint 3 between L3 and L4 |
MAHCSnijders | 0:4a9c733c3b53 | 25 | float theta = atan(L5*sin(q4)/(L4 + L5*cos(q4)); // Angle between n and L4 |
MAHCSnijders | 0:4a9c733c3b53 | 26 | float lambda = PI - atan(abs(Pe_y/Pe_x)); // Entire angle between x-axis frame 0 and n |
MAHCSnijders | 0:4a9c733c3b53 | 27 | float motor_angle1 = lambda - theta; |
MAHCSnijders | 0:4a9c733c3b53 | 28 | float J3x_0 = L4*cos(q3); // x-coordinate of joint 3 in frame 0 |
MAHCSnijders | 0:4a9c733c3b53 | 29 | float J3y_0 = L4*sin(q3); // y-coordinate of joint 3 in frame 0 |
MAHCSnijders | 0:4a9c733c3b53 | 30 | |
MAHCSnijders | 0:4a9c733c3b53 | 31 | // Calculation of the position of joint 2 in frame 0 |
MAHCSnijders | 0:4a9c733c3b53 | 32 | float S = abs(J3y_0 - Pe_y); // Distance between height endeffector and joint 3 |
MAHCSnijders | 0:4a9c733c3b53 | 33 | float kappa = asin(S/L5); // Angle of L5 |
MAHCSnijders | 0:4a9c733c3b53 | 34 | float J2x_0 = (L3+L5)*cos(kappa) + Pe_x; // x-coordinate of joint 2 in frame 0 |
MAHCSnijders | 0:4a9c733c3b53 | 35 | float J2y_0 = (L3+L5)*sin(kappa) + Pe_y; // y-coordinate of joint 2 in frame 0 |
MAHCSnijders | 0:4a9c733c3b53 | 36 | |
MAHCSnijders | 0:4a9c733c3b53 | 37 | // Calculation of the position of joint 1 in frame 0 |
MAHCSnijders | 0:4a9c733c3b53 | 38 | float J2x_1 = J2x_0 - L0; // x-coordinate of joint 2 in frame 1 |
MAHCSnijders | 0:4a9c733c3b53 | 39 | float J2y_1 = J2y_0; // y-coordinate of joint 2 in frame 1 |
MAHCSnijders | 0:4a9c733c3b53 | 40 | float r = sqrt(pow(J2x_1,2) + pow(J2y_1,2); // Radius between origin frame 1 and J2 |
MAHCSnijders | 0:4a9c733c3b53 | 41 | float alfa = acos(-(pow(r,2) - pow(L1,2) - pow(L2,2))/(2*L1*L2 // Angle opposite of radius r |
MAHCSnijders | 0:4a9c733c3b53 | 42 | float q2 = PI - alfa; // Angle between L1 and L2 |
MAHCSnijders | 0:4a9c733c3b53 | 43 | |
MAHCSnijders | 0:4a9c733c3b53 | 44 | // Calculation of motor_angle2 |
MAHCSnijders | 0:4a9c733c3b53 | 45 | float beta = atan(L2*sin(q2)/(L1+L2*cos(q2))); // Angle between r and L1 |
MAHCSnijders | 0:4a9c733c3b53 | 46 | float gamma = PI - atan(abs(J2y_1/J2x_1); // Angle between r and x-axis |
MAHCSnijders | 0:4a9c733c3b53 | 47 | // check if gamma works! |
MAHCSnijders | 0:4a9c733c3b53 | 48 | m_anle2 = gamma - beta; |
MAHCSnijders | 0:4a9c733c3b53 | 49 | float J1x_0 = L0 + L1*cos(q1); // x-coordinate of joint 1 in frame 0 |
MAHCSnijders | 0:4a9c733c3b53 | 50 | float J1y_0 = L1*sin(q1); // y-coordinate of joint 1 in frame 0 |
MAHCSnijders | 0:4a9c733c3b53 | 51 | |
MAHCSnijders | 0:4a9c733c3b53 | 52 | |
MAHCSnijders | 0:4a9c733c3b53 | 53 | |
MAHCSnijders | 0:4a9c733c3b53 | 54 | |
MAHCSnijders | 0:4a9c733c3b53 | 55 | return motor_angle1 |
MAHCSnijders | 0:4a9c733c3b53 | 56 | return motor_angle2 |
MAHCSnijders | 0:4a9c733c3b53 | 57 | } |
MAHCSnijders | 0:4a9c733c3b53 | 58 | |
MAHCSnijders | 0:4a9c733c3b53 | 59 | |
MAHCSnijders | 0:4a9c733c3b53 | 60 | int main() |
MAHCSnijders | 0:4a9c733c3b53 | 61 | { |
MAHCSnijders | 0:4a9c733c3b53 | 62 | IK.attach(InverseKinematics) |
MAHCSnijders | 0:4a9c733c3b53 | 63 | } |