Biorobotics 7
Inverse kinematics
Diff: main.cpp
- Revision:
- 3:f0208237b6f7
- Parent:
- 2:8632e61cafc8
- Child:
- 4:9f389b393af2
--- a/main.cpp Tue Oct 30 13:36:23 2018 +0000
+++ b/main.cpp Tue Oct 30 19:51:44 2018 +0000
@@ -7,6 +7,7 @@
const float L3 = 0.15; // Length beam between L2 and L4 [meter]
const float L4 = 0.30; // Length first beam from left motor1 [meter]
const float L5 = 0.35; // Length from L3 to end-effector [meter]
+const float L6 = 1.00; // Length from frame 0 to motor 1
const double PI = 3.14159265359;
volatile float Pe_x; // x-coordinate of end-effector from frame 0 [meter]
volatile float Pe_y; // y-coordinate of end-effector from frame 0 [meter]
@@ -18,41 +19,41 @@
void InverseKinematics()
{
// Calculation of the position of joint 3 in frame 0
- float n = sqrt(pow(Pe_x,2) + pow(Pe_y,2)); // Radius between origin frame 0 and endeffector [meter]
- float omega = acos(-(pow(n,2) - pow(L4,2) - pow(L5,2))/(2*L4*L5)); // Angle between L4 and L5 [rad]
- float q4 = PI - omega; // Angle of joint 3 between L3 and L4
- float theta = atan(L5*sin(q4)/(L4 + L5*cos(q4))); // Angle between n and L4
- float lambda = PI - atan(abs(Pe_y/Pe_x)); // Entire angle between x-axis frame 0 and n
- float des_motor_angle1 = lambda - theta;
- float J3x_0 = L4*cos(des_motor_angle1); // x-coordinate of joint 3 in frame 0
- float J3y_0 = L4*sin(des_motor_angle1); // y-coordinate of joint 3 in frame 0
+ float n = sqrt(pow((L6-Pe_x),2) + pow(Pe_y,2)); // Radius between motor 1 and endeffector [meter]
+ float omega = acos(-(pow(n,2) - pow(L4,2) - pow(L5,2))/(2*L4*L5)); // Angle between L4 and L5 [rad]
+ float q4 = PI - omega; // Angle of joint 3 between L3 and L4
+ float theta = acos( -(pow(L5,2) - pow(n,2) - pow(L4,2))/(2*n*L4) ); // Angle between n and L4
+ float lambda = PI - atan(Pe_y/(L6-Pe_x)); // Entire angle between L0 and n
+ des_motor_angle1 = lambda - theta;
+ float J3x_0 = L6 + L4*cos(des_motor_angle1); // x-coordinate of joint 3 in frame 0
+ float J3y_0 = L4*sin(des_motor_angle1); // y-coordinate of joint 3 in frame 0
// Calculation of the position of joint 2 in frame 0
- float S = abs(J3y_0 - Pe_y); // Distance between height endeffector and joint 3
- float kappa = asin(S/L5); // Angle of L5
- float J2x_0 = (L3+L5)*cos(kappa) + Pe_x; // x-coordinate of joint 2 in frame 0
- float J2y_0 = (L3+L5)*sin(kappa) + Pe_y; // y-coordinate of joint 2 in frame 0
+ float S = abs(J3y_0 - Pe_y); // Distance between height endeffector and joint 3
+ float kappa = asin(S/L5); // Angle of L5
+ float J2x_0 = (L3+L5)*cos(kappa) + Pe_x; // x-coordinate of joint 2 in frame 0
+ float J2y_0 = (L3+L5)*sin(kappa) + Pe_y; // y-coordinate of joint 2 in frame 0
// Calculation of the position of joint 1 in frame 0
- float J2x_1 = J2x_0 - L0; // x-coordinate of joint 2 in frame 1
- float J2y_1 = J2y_0; // y-coordinate of joint 2 in frame 1
- float r = sqrt(pow(J2x_1,2) + pow(J2y_1,2)); // Radius between origin frame 1 and J2
- float alfa = acos( -(pow(r,2) - pow(L1,2) - pow(L2,2))/(2*L1*L2) ); // Angle opposite of radius r
- float q2 = PI - alfa; // Angle between L1 and L2
+ float J2x_1 = J2x_0 - L0 - L6; // x-coordinate of joint 2 in frame 1
+ float J2y_1 = J2y_0; // y-coordinate of joint 2 in frame 1
+ float r = sqrt(pow(J2x_1,2) + pow(J2y_1,2)); // Radius between origin frame 1 and J2
+ float alfa = acos( -(pow(r,2) - pow(L1,2) - pow(L2,2))/(2*L1*L2) ); // Angle opposite of radius r
+ float q2 = PI - alfa; // Angle between L1 and L2
// Calculation of motor_angle2
- float beta = atan(L2*sin(q2)/(L1+L2*cos(q2))); // Angle between r and L1
- float gamma = PI - atan(abs(J2y_1/J2x_1)); // Angle between r and x-axis
+ float beta = atan(L2*sin(q2)/(L1+L2*cos(q2))); // Angle between r and L1
+ float gamma = PI - atan(abs(J2y_1/J2x_1)); // Angle between r and x-axis
// check if gamma works!
des_motor_angle2 = gamma - beta;
- float J1x_0 = L0 + L1*cos(des_motor_angle2); // x-coordinate of joint 1 in frame 0
- float J1y_0 = L1*sin(des_motor_angle2); // y-coordinate of joint 1 in frame 0
// Determining angle delta for safety
- float m = sqrt(pow((abs(J3x_0)+J1x_0),2) + pow((J3y_0 - J1y_0),2)); // Radius between Joint 1 and Joint 3
- float delta = acos(- (pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3)); // Angle between L2 and L3
-
+ float J1x_0 = L0 + L6 + L1*cos(des_motor_angle2); // x-coordinate of joint 1 in frame 0
+ float J1y_0 = L1*sin(des_motor_angle2); // y-coordinate of joint 1 in frame 0
+
+ float m = sqrt(pow((J1x_0 - J3x_0),2) + pow((J3y_0 - J1y_0),2)); // Radius between Joint 1 and Joint 3
+ float delta = acos(- (pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3)); // Angle between L2 and L3
}