Diff: inverseKinematics.cpp

Revision:

0:537f81d7b756

Child:

1:81e4001f1082

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inverseKinematics.cpp	Thu Oct 26 13:29:25 2017 +0000
@@ -0,0 +1,57 @@
+#include "inverseKinematics.h"
+#include "mbed.h"
+#include "Matrix.h"
+#include "MatrixMath.h"
+
+// Member function definitions
+inverseKinematics::inverseKinematics(float _L1, float _L2, float _dt):L1(_L1), L2(_L2), dt(_dt){
+
+    }
+
+// computeAngles computes the 
+Matrix inverseKinematics :: computeAngles(float Vx, float Vy, float angle1, float angle2){
+    
+    Matrix T1(3,1);
+    T1 << 1 << 0 << 0;
+    
+    Matrix T2(3,1);
+    T2 << 1 << L1*cos(angle1) << L1*sin(angle1);
+    
+    Matrix J = T2;
+    J.AddCol(J,T1,1);
+//    J.print();
+
+    
+    Matrix H_f0(3,3);
+    H_f0 << 1  << 0  << -L1*sin(angle1)-L2*sin(angle1+angle2)
+         << 0  << 1  << L1*cos(angle1)+L2*cos(angle1+angle2)
+         << 0  << 0  << 1;
+
+    Matrix H_0f = MatrixMath::Inv( H_f0 );
+
+    // Computing adjoint matrix    
+    Matrix Ad_H_0f(3,3);
+    Ad_H_0f << 1 << 0 << 0
+            << -1*(L1*cos(angle1)+L2*cos(angle1+angle2)) << 1 << 0
+            << -L1*sin(angle1)-L2*sin(angle1+angle2) << 0 << 1;
+    
+    Matrix J2 = Ad_H_0f*J;
+    
+    
+    J2.DeleteRow(J2,1);
+    //J2.print();
+    
+    Matrix J2_T = MatrixMath::Transpose(J2);
+    Matrix J_fancy = MatrixMath::Inv(J2_T*J2) * J2_T;
+    //J_fancy.print();
+    
+    Matrix v_des(2,1);
+    v_des << Vx << Vy;
+    
+    Matrix q_dot = J_fancy*v_des;
+
+    //angle1 = angle1 + dt*q_dot(1,1);
+//    angle2 = angle2 + dt*q_dot(2,1);
+    
+    return q_dot;    
+    }
\ No newline at end of file