Biorobotics 7
/
Inverse_kinematic
Inverse kinematics
main.cpp@8:01eb609fe0c1, 2018-11-01 (annotated)
- Committer:
- MAHCSnijders
- Date:
- Thu Nov 01 10:29:23 2018 +0000
- Revision:
- 8:01eb609fe0c1
- Parent:
- 7:f3c4ecbf864e
- Child:
- 9:2944c792daa2
Safety delta fixed by limiting r
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 | |
MAHCSnijders | 1:df3d7f71db4b | 4 | const float L0 = 0.15; // Length between two motors [meter] |
MAHCSnijders | 1:df3d7f71db4b | 5 | const float L1 = 0.10; // Length first beam from right motor2 [meter] |
MAHCSnijders | 1:df3d7f71db4b | 6 | const float L2 = 0.30; // Length second beam from right motor2 [meter] |
MAHCSnijders | 1:df3d7f71db4b | 7 | const float L3 = 0.15; // Length beam between L2 and L4 [meter] |
MAHCSnijders | 1:df3d7f71db4b | 8 | const float L4 = 0.30; // Length first beam from left motor1 [meter] |
MAHCSnijders | 1:df3d7f71db4b | 9 | const float L5 = 0.35; // Length from L3 to end-effector [meter] |
MAHCSnijders | 3:f0208237b6f7 | 10 | const float L6 = 1.00; // Length from frame 0 to motor 1 |
MAHCSnijders | 0:4a9c733c3b53 | 11 | const double PI = 3.14159265359; |
MAHCSnijders | 1:df3d7f71db4b | 12 | volatile float Pe_x; // x-coordinate of end-effector from frame 0 [meter] |
MAHCSnijders | 1:df3d7f71db4b | 13 | volatile float Pe_y; // y-coordinate of end-effector from frame 0 [meter] |
MAHCSnijders | 1:df3d7f71db4b | 14 | volatile static float des_motor_angle1; // Desired angle of motor 1 (left) based on kinematics [rad] |
MAHCSnijders | 1:df3d7f71db4b | 15 | volatile static float des_motor_angle2; // Desired angle of motor 2 (right) based on kinematics [rad] |
MAHCSnijders | 0:4a9c733c3b53 | 16 | |
MAHCSnijders | 4:9f389b393af2 | 17 | DigitalOut safetyLED(LED_BLUE); // Safety check LED |
MAHCSnijders | 4:9f389b393af2 | 18 | |
MAHCSnijders | 1:df3d7f71db4b | 19 | Ticker kinematics_ticker; // Ticker function for inverse kinematics |
MAHCSnijders | 0:4a9c733c3b53 | 20 | |
MAHCSnijders | 0:4a9c733c3b53 | 21 | void InverseKinematics() |
MAHCSnijders | 0:4a9c733c3b53 | 22 | { |
MAHCSnijders | 0:4a9c733c3b53 | 23 | // Calculation of the position of joint 3 in frame 0 |
MAHCSnijders | 3:f0208237b6f7 | 24 | float n = sqrt(pow((L6-Pe_x),2) + pow(Pe_y,2)); // Radius between motor 1 and endeffector [meter] |
MAHCSnijders | 3:f0208237b6f7 | 25 | float omega = acos(-(pow(n,2) - pow(L4,2) - pow(L5,2))/(2*L4*L5)); // Angle between L4 and L5 [rad] |
MAHCSnijders | 3:f0208237b6f7 | 26 | float q4 = PI - omega; // Angle of joint 3 between L3 and L4 |
MAHCSnijders | 3:f0208237b6f7 | 27 | float theta = acos( -(pow(L5,2) - pow(n,2) - pow(L4,2))/(2*n*L4) ); // Angle between n and L4 |
MAHCSnijders | 7:f3c4ecbf864e | 28 | float lambda_stuff = Pe_y/(L6-Pe_x); |
MAHCSnijders | 8:01eb609fe0c1 | 29 | float lambda = PI - atan(lambda_stuff); // Entire angle between L0 and n |
MAHCSnijders | 3:f0208237b6f7 | 30 | des_motor_angle1 = lambda - theta; |
MAHCSnijders | 3:f0208237b6f7 | 31 | float J3x_0 = L6 + L4*cos(des_motor_angle1); // x-coordinate of joint 3 in frame 0 |
MAHCSnijders | 3:f0208237b6f7 | 32 | float J3y_0 = L4*sin(des_motor_angle1); // y-coordinate of joint 3 in frame 0 |
MAHCSnijders | 0:4a9c733c3b53 | 33 | |
MAHCSnijders | 0:4a9c733c3b53 | 34 | // Calculation of the position of joint 2 in frame 0 |
MAHCSnijders | 7:f3c4ecbf864e | 35 | float S = J3y_0 - Pe_y; // I CHANGED THIS!!! |
MAHCSnijders | 7:f3c4ecbf864e | 36 | // Distance between height endeffector and joint 3 |
MAHCSnijders | 3:f0208237b6f7 | 37 | float kappa = asin(S/L5); // Angle of L5 |
MAHCSnijders | 3:f0208237b6f7 | 38 | float J2x_0 = (L3+L5)*cos(kappa) + Pe_x; // x-coordinate of joint 2 in frame 0 |
MAHCSnijders | 3:f0208237b6f7 | 39 | float J2y_0 = (L3+L5)*sin(kappa) + Pe_y; // y-coordinate of joint 2 in frame 0 |
MAHCSnijders | 0:4a9c733c3b53 | 40 | |
MAHCSnijders | 0:4a9c733c3b53 | 41 | // Calculation of the position of joint 1 in frame 0 |
MAHCSnijders | 3:f0208237b6f7 | 42 | float J2x_1 = J2x_0 - L0 - L6; // x-coordinate of joint 2 in frame 1 |
MAHCSnijders | 3:f0208237b6f7 | 43 | float J2y_1 = J2y_0; // y-coordinate of joint 2 in frame 1 |
MAHCSnijders | 3:f0208237b6f7 | 44 | float r = sqrt(pow(J2x_1,2) + pow(J2y_1,2)); // Radius between origin frame 1 and J2 |
MAHCSnijders | 3:f0208237b6f7 | 45 | float alfa = acos( -(pow(r,2) - pow(L1,2) - pow(L2,2))/(2*L1*L2) ); // Angle opposite of radius r |
MAHCSnijders | 3:f0208237b6f7 | 46 | float q2 = PI - alfa; // Angle between L1 and L2 |
MAHCSnijders | 0:4a9c733c3b53 | 47 | |
MAHCSnijders | 6:273cea24fab3 | 48 | // Calculation of motor_angle2 I CHANGED THIS!!! |
MAHCSnijders | 6:273cea24fab3 | 49 | float beta = acos(- (pow(L2,2) - pow(r,2) - pow(L1,2))/(2*L1*r)); // Angle between r and L1 |
MAHCSnijders | 6:273cea24fab3 | 50 | float zeta = acos(J2x_1/r); // Angle between r and x-axis of frame 1 |
MAHCSnijders | 6:273cea24fab3 | 51 | des_motor_angle2 = zeta - beta; |
MAHCSnijders | 0:4a9c733c3b53 | 52 | |
MAHCSnijders | 1:df3d7f71db4b | 53 | |
MAHCSnijders | 1:df3d7f71db4b | 54 | // Determining angle delta for safety |
MAHCSnijders | 3:f0208237b6f7 | 55 | float J1x_0 = L0 + L6 + L1*cos(des_motor_angle2); // x-coordinate of joint 1 in frame 0 |
MAHCSnijders | 3:f0208237b6f7 | 56 | float J1y_0 = L1*sin(des_motor_angle2); // y-coordinate of joint 1 in frame 0 |
MAHCSnijders | 3:f0208237b6f7 | 57 | |
MAHCSnijders | 3:f0208237b6f7 | 58 | float m = sqrt(pow((J1x_0 - J3x_0),2) + pow((J3y_0 - J1y_0),2)); // Radius between Joint 1 and Joint 3 |
MAHCSnijders | 3:f0208237b6f7 | 59 | float delta = acos(- (pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3)); // Angle between L2 and L3 |
MAHCSnijders | 4:9f389b393af2 | 60 | |
MAHCSnijders | 4:9f389b393af2 | 61 | |
MAHCSnijders | 4:9f389b393af2 | 62 | // Implementing stops for safety |
MAHCSnijders | 4:9f389b393af2 | 63 | // 45 < Motor_angle1 < 70 graden |
MAHCSnijders | 5:aaf68c7482bc | 64 | if (des_motor_angle1 < 0.785398) // If motor_angle is smaller than 45 degrees |
MAHCSnijders | 4:9f389b393af2 | 65 | { |
MAHCSnijders | 5:aaf68c7482bc | 66 | des_motor_angle1 = 0.785398; |
MAHCSnijders | 4:9f389b393af2 | 67 | safetyLED = 0; |
MAHCSnijders | 5:aaf68c7482bc | 68 | } |
MAHCSnijders | 5:aaf68c7482bc | 69 | else if (des_motor_angle1 > 1.22173) // If motor_angle is larger than 70 degrees |
MAHCSnijders | 4:9f389b393af2 | 70 | { |
MAHCSnijders | 5:aaf68c7482bc | 71 | des_motor_angle1 = 1.22173; |
MAHCSnijders | 4:9f389b393af2 | 72 | safetyLED = 0; |
MAHCSnijders | 4:9f389b393af2 | 73 | } |
MAHCSnijders | 5:aaf68c7482bc | 74 | |
MAHCSnijders | 5:aaf68c7482bc | 75 | // -42 < Motor_angle2 < 85 graden |
MAHCSnijders | 5:aaf68c7482bc | 76 | if (des_motor_angle2 < -0.733038) // If motor_angle is smaller than -42 degrees |
MAHCSnijders | 5:aaf68c7482bc | 77 | { |
MAHCSnijders | 5:aaf68c7482bc | 78 | des_motor_angle2 = -0.733038; |
MAHCSnijders | 5:aaf68c7482bc | 79 | safetyLED = 0; |
MAHCSnijders | 5:aaf68c7482bc | 80 | } |
MAHCSnijders | 5:aaf68c7482bc | 81 | else if (des_motor_angle2 > 1.48353) // If motor_angle is larger than 85 degrees |
MAHCSnijders | 5:aaf68c7482bc | 82 | { |
MAHCSnijders | 5:aaf68c7482bc | 83 | des_motor_angle2 = 1.48353; |
MAHCSnijders | 5:aaf68c7482bc | 84 | safetyLED = 0; |
MAHCSnijders | 5:aaf68c7482bc | 85 | } |
MAHCSnijders | 5:aaf68c7482bc | 86 | |
MAHCSnijders | 5:aaf68c7482bc | 87 | |
MAHCSnijders | 4:9f389b393af2 | 88 | // Delta < 170 graden |
MAHCSnijders | 5:aaf68c7482bc | 89 | if (delta > 2.96706) // If delta is larger than 180 degrees |
MAHCSnijders | 4:9f389b393af2 | 90 | { |
MAHCSnijders | 8:01eb609fe0c1 | 91 | r = 0.2399; // I CHANGED THIS |
MAHCSnijders | 4:9f389b393af2 | 92 | safetyLED = 0; |
MAHCSnijders | 4:9f389b393af2 | 93 | } |
MAHCSnijders | 0:4a9c733c3b53 | 94 | } |
MAHCSnijders | 0:4a9c733c3b53 | 95 | |
MAHCSnijders | 0:4a9c733c3b53 | 96 | |
MAHCSnijders | 0:4a9c733c3b53 | 97 | int main() |
MAHCSnijders | 0:4a9c733c3b53 | 98 | { |
MAHCSnijders | 4:9f389b393af2 | 99 | safetyLED = 1; |
MAHCSnijders | 4:9f389b393af2 | 100 | while (true) { |
MAHCSnijders | 1:df3d7f71db4b | 101 | kinematics_ticker.attach(InverseKinematics,0.5); |
MAHCSnijders | 4:9f389b393af2 | 102 | } |
MAHCSnijders | 0:4a9c733c3b53 | 103 | } |