Diff: help_functions/kinematics.h

Branch:

bla

Revision:

16:0280a604cf7e

Parent:

14:4744cc6c90f4

Child:

17:1f93c83e211f

--- a/help_functions/kinematics.h	Thu Oct 25 08:51:32 2018 +0000
+++ b/help_functions/kinematics.h	Thu Oct 25 17:50:26 2018 +0000
@@ -12,9 +12,9 @@
     y = y01 + L1 * sin(q1) - L2 * sin(q2);    
 }
 
-double inversekinematics_function(double& x, double& y, const double& T, double& qref1, double& qref2, double& q1, double& q2, double& des_vx, double& des_vy) {
+void inversekinematics_function(double& x, double& y, const double& T, double& qref1, double& qref2, double& q1, double& q2, double& des_vx, double& des_vy) {
     // pseudo inverse jacobian to get joint speeds
-    // input are desired vx and vy of end effector, output joint angle speeds
+    // input are desired vx and vy of end effector, output are reference angles
 
     double q1_star_des; // desired joint velocity of q1_star
     double q2_star_des; // same as above but then for q2_star
@@ -22,7 +22,7 @@
     // 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);
+    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);
     
     qref1 = q1+T*q1_star_des; // Yet to adapt all these equations
     qref2 = q2+T*(q2_star_des - q1_star_des);