Biorobotics 7 / Mbed 2 deprecated Inverse_kinematic

Inverse kinematics

Dependencies:   Matrix mbed

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?

UserRevisionLine numberNew 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 }