Diff: help_functions/kinematics.h

Revision:

22:31065a83d9e8

Parent:

20:31876566d70f

Parent:

17:1f93c83e211f

Child:

23:7d83af123c43

--- a/help_functions/kinematics.h	Fri Oct 26 12:59:52 2018 +0000
+++ b/help_functions/kinematics.h	Mon Oct 29 12:21:26 2018 +0000
@@ -5,17 +5,28 @@
 double x01 = 0.0;       
 double y01 = 0.2;           //height base joint
 
-void forwardkinematics_function(double q1, double q2) {
+void forwardkinematics_function(double& q1, double& q2, double& x, double& y) {
     // input are joint angles, output are x and y position of end effector
-    
+
     double robot_end_x = x01 + L_ua * cos(q1) - L_la * cos(q2);
     double robot_end_y = y01 + L_ua * sin(q1) - L_la * sin(q2);   
      
+
 }
 
-double inversekinematics_function(double q1, double q2, double reference) {
-    // pseudo inverse jacobian to get joint speeds
-    // input are desired vx and vy of end effector, output joint angle speeds
+void inversekinematics_function(double &x, double &y, const double &T, double &qref1, double &qref2, double &q1, double &q2, double &des_vx, double &des_vy) {
+    // input is desired x and y velocity, output reference angles
+    // reference angle spees are calculated using the inverse of jacobian
+    // from the reference angle speeds the reference angles are computed
+
+    double q1_star_des; // desired joint velocity of q1_star
+    double q2_star_des; // same as above but then for q2_star
+
+    // The calculation below assumes that the end effector position is calculated before this function is executed
+    // In our case the determinant will not equal zero, hence no problems with singularies I think.
+    q1_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_vx-x*des_vy);
+    q2_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-y+y01+L1*sin(q1))*des_vx+1*(-y+y01)*des_vy);
     
-    return 5.5;    
+    qref1 = q1+T*q1_star_des; // Yet to adapt all these equations
+    qref2 = q2+T*(q2_star_des - q1_star_des);    
 }
\ No newline at end of file