Diff: help_functions/kinematics.h

Revision:

31:393a7ec1d396

Parent:

29:d1e8eb135e6c

--- a/help_functions/kinematics.h	Mon Oct 29 19:45:36 2018 +0000
+++ b/help_functions/kinematics.h	Mon Oct 29 20:07:54 2018 +0000
@@ -5,6 +5,7 @@
 double x01 = 0.0;       
 double y01 = 0.2;           //height base joint
 
+
 void forwardkinematics_function(double& q1, double& q2, double& x, double& y) {
     // input are joint angles, output are x and y position of end effector
 
@@ -24,8 +25,8 @@
 
     // 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);
+    q1_star_des = 1/(L_ua*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-x+L_ua*cos(q1))*des_vx-x*des_vy);
+    q2_star_des = 1/(L_ua*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-y+y01+L_ua*sin(q1))*des_vx+1*(-y+y01)*des_vy);
     
     qref1 = q1+T*q1_star_des; // Yet to adapt all these equations
     qref2 = q2+T*(q2_star_des - q1_star_des);