Biorobotics 7 / Mbed 2 deprecated Inverse_kinematic

Inverse kinematics

Dependencies:   Matrix mbed

main.cpp@10:c35d16ad1b6b, 2018-11-01 (annotated)

Committer:
MAHCSnijders
Date:
Thu Nov 01 10:49:37 2018 +0000
Revision:
10:c35d16ad1b6b
Parent:
9:2944c792daa2 
Bugs are not bugging us anymore, so r is happy :D

Who changed what in which revision?

UserRevisionLine numberNew 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 9:2944c792daa2 48 // Delta < 170 graden SAFETY LIMITATION
MAHCSnijders 10:c35d16ad1b6b 49 if (r > 0.2399) // If delta is larger than 175 degrees than r is this
MAHCSnijders 9:2944c792daa2 50 {
MAHCSnijders 9:2944c792daa2 51 r = 0.2399; // I CHANGED THIS
MAHCSnijders 9:2944c792daa2 52 safetyLED = 0;
MAHCSnijders 9:2944c792daa2 53 }
MAHCSnijders 9:2944c792daa2 54
MAHCSnijders 6:273cea24fab3 55 // Calculation of motor_angle2 I CHANGED THIS!!!
MAHCSnijders 6:273cea24fab3 56 float beta = acos(- (pow(L2,2) - pow(r,2) - pow(L1,2))/(2*L1*r)); // Angle between r and L1
MAHCSnijders 6:273cea24fab3 57 float zeta = acos(J2x_1/r); // Angle between r and x-axis of frame 1
MAHCSnijders 6:273cea24fab3 58 des_motor_angle2 = zeta - beta;
MAHCSnijders 0:4a9c733c3b53 59
MAHCSnijders 1:df3d7f71db4b 60
MAHCSnijders 1:df3d7f71db4b 61 // Determining angle delta for safety
MAHCSnijders 3:f0208237b6f7 62 float J1x_0 = L0 + L6 + L1*cos(des_motor_angle2); // x-coordinate of joint 1 in frame 0
MAHCSnijders 3:f0208237b6f7 63 float J1y_0 = L1*sin(des_motor_angle2); // y-coordinate of joint 1 in frame 0
MAHCSnijders 3:f0208237b6f7 64
MAHCSnijders 3:f0208237b6f7 65 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 66 float delta = acos(- (pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3)); // Angle between L2 and L3
MAHCSnijders 4:9f389b393af2 67
MAHCSnijders 4:9f389b393af2 68
MAHCSnijders 4:9f389b393af2 69 // Implementing stops for safety
MAHCSnijders 4:9f389b393af2 70 // 45 < Motor_angle1 < 70 graden
MAHCSnijders 5:aaf68c7482bc 71 if (des_motor_angle1 < 0.785398) // If motor_angle is smaller than 45 degrees
MAHCSnijders 4:9f389b393af2 72 {
MAHCSnijders 5:aaf68c7482bc 73 des_motor_angle1 = 0.785398;
MAHCSnijders 4:9f389b393af2 74 safetyLED = 0;
MAHCSnijders 5:aaf68c7482bc 75 }
MAHCSnijders 5:aaf68c7482bc 76 else if (des_motor_angle1 > 1.22173) // If motor_angle is larger than 70 degrees
MAHCSnijders 4:9f389b393af2 77 {
MAHCSnijders 5:aaf68c7482bc 78 des_motor_angle1 = 1.22173;
MAHCSnijders 4:9f389b393af2 79 safetyLED = 0;
MAHCSnijders 4:9f389b393af2 80 }
MAHCSnijders 5:aaf68c7482bc 81
MAHCSnijders 5:aaf68c7482bc 82 // -42 < Motor_angle2 < 85 graden
MAHCSnijders 5:aaf68c7482bc 83 if (des_motor_angle2 < -0.733038) // If motor_angle is smaller than -42 degrees
MAHCSnijders 5:aaf68c7482bc 84 {
MAHCSnijders 5:aaf68c7482bc 85 des_motor_angle2 = -0.733038;
MAHCSnijders 5:aaf68c7482bc 86 safetyLED = 0;
MAHCSnijders 5:aaf68c7482bc 87 }
MAHCSnijders 5:aaf68c7482bc 88 else if (des_motor_angle2 > 1.48353) // If motor_angle is larger than 85 degrees
MAHCSnijders 5:aaf68c7482bc 89 {
MAHCSnijders 5:aaf68c7482bc 90 des_motor_angle2 = 1.48353;
MAHCSnijders 5:aaf68c7482bc 91 safetyLED = 0;
MAHCSnijders 5:aaf68c7482bc 92 }
MAHCSnijders 0:4a9c733c3b53 93 }
MAHCSnijders 0:4a9c733c3b53 94
MAHCSnijders 0:4a9c733c3b53 95
MAHCSnijders 0:4a9c733c3b53 96 int main()
MAHCSnijders 0:4a9c733c3b53 97 {
MAHCSnijders 4:9f389b393af2 98 safetyLED = 1;
MAHCSnijders 4:9f389b393af2 99 while (true) {
MAHCSnijders 1:df3d7f71db4b 100 kinematics_ticker.attach(InverseKinematics,0.5);
MAHCSnijders 4:9f389b393af2 101 }
MAHCSnijders 0:4a9c733c3b53 102 }