Diff: help_functions/kinematics.h

Branch:

bla

Revision:

13:397b7c22475c

Parent:

12:3c47c7b1d1d7

Child:

14:4744cc6c90f4

--- a/help_functions/kinematics.h	Tue Oct 23 10:47:19 2018 +0000
+++ b/help_functions/kinematics.h	Thu Oct 25 07:59:00 2018 +0000
@@ -5,30 +5,25 @@
 double x01 = 0.0;
 double y01 = 0.2;
 
-void forwardkinematics_function(double q1, double q2) {
+void forwardkinematics_function(double& q1, double& q2) {
     // input are joint angles, output are x and y position of end effector
     
     x = x01 + L1*cos(q1)-L2*cos(q2);
     y = y01 + L1 * sin(q1) - L2 * sin(q2);    
 }
 
-double inversekinematics_function() {
+double 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
-    // I assume here that reference is a vector (this should also be global I think)
-    // I also assume that the end effector position is global here and is denoted by x
-    // Assuming the ticker_rate is also set to be global
-    
-    double des_twist[2]; // the desired twist (DETERMINE IF WE WANT TO MAKE THIS STATIC)
+
     double q1_star_des; // desired joint velocity of q1_star
     double q2_star_des; // same as above but then for q2_star
-    
-    // We have to give a des_twist. If we do EMG we give a x and y velocity, so do we have to use the reference x and y position?
-    
+
     // The calculation below assumes that the end effector position is calculated before this function is executed
-    q1_star_des = 1/(L1*(-x*sin(q1)-(y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_twist[0]-x*des_twist[1]);
-    q2_star_des = 1/(L1*(-x*sin(q1)-(y+y01)*cos(q1)))*(-1*(-y+y01+L1*sin(q1))*des_twist[0]+1*(-y+y01))*des_twist[1]);
+    // 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);
     
-    qref1 = qref1+T*q1_star_des; // Yet to adapt all these equations
-    qref2 = qref2+T*(q2_star_des - q1_star_des);    
+    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