Tristan Vlogman / inverseKinematics

pseudo-inverse

inverseKinematics.cpp@0:537f81d7b756, 2017-10-26 (annotated)

Committer:
tvlogman
Date:
Thu Oct 26 13:29:25 2017 +0000
Revision:
0:537f81d7b756
Child:
1:81e4001f1082
Working library implementing inverse kinematics (using Penrose-Moore Inverse)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tvlogman 0:537f81d7b756 1 #include "inverseKinematics.h"
tvlogman 0:537f81d7b756 2 #include "mbed.h"
tvlogman 0:537f81d7b756 3 #include "Matrix.h"
tvlogman 0:537f81d7b756 4 #include "MatrixMath.h"
tvlogman 0:537f81d7b756 5
tvlogman 0:537f81d7b756 6 // Member function definitions
tvlogman 0:537f81d7b756 7 inverseKinematics::inverseKinematics(float _L1, float _L2, float _dt):L1(_L1), L2(_L2), dt(_dt){
tvlogman 0:537f81d7b756 8
tvlogman 0:537f81d7b756 9 }
tvlogman 0:537f81d7b756 10
tvlogman 0:537f81d7b756 11 // computeAngles computes the
tvlogman 0:537f81d7b756 12 Matrix inverseKinematics :: computeAngles(float Vx, float Vy, float angle1, float angle2){
tvlogman 0:537f81d7b756 13
tvlogman 0:537f81d7b756 14 Matrix T1(3,1);
tvlogman 0:537f81d7b756 15 T1 << 1 << 0 << 0;
tvlogman 0:537f81d7b756 16
tvlogman 0:537f81d7b756 17 Matrix T2(3,1);
tvlogman 0:537f81d7b756 18 T2 << 1 << L1*cos(angle1) << L1*sin(angle1);
tvlogman 0:537f81d7b756 19
tvlogman 0:537f81d7b756 20 Matrix J = T2;
tvlogman 0:537f81d7b756 21 J.AddCol(J,T1,1);
tvlogman 0:537f81d7b756 22 // J.print();
tvlogman 0:537f81d7b756 23
tvlogman 0:537f81d7b756 24
tvlogman 0:537f81d7b756 25 Matrix H_f0(3,3);
tvlogman 0:537f81d7b756 26 H_f0 << 1 << 0 << -L1*sin(angle1)-L2*sin(angle1+angle2)
tvlogman 0:537f81d7b756 27 << 0 << 1 << L1*cos(angle1)+L2*cos(angle1+angle2)
tvlogman 0:537f81d7b756 28 << 0 << 0 << 1;
tvlogman 0:537f81d7b756 29
tvlogman 0:537f81d7b756 30 Matrix H_0f = MatrixMath::Inv( H_f0 );
tvlogman 0:537f81d7b756 31
tvlogman 0:537f81d7b756 32 // Computing adjoint matrix
tvlogman 0:537f81d7b756 33 Matrix Ad_H_0f(3,3);
tvlogman 0:537f81d7b756 34 Ad_H_0f << 1 << 0 << 0
tvlogman 0:537f81d7b756 35 << -1*(L1*cos(angle1)+L2*cos(angle1+angle2)) << 1 << 0
tvlogman 0:537f81d7b756 36 << -L1*sin(angle1)-L2*sin(angle1+angle2) << 0 << 1;
tvlogman 0:537f81d7b756 37
tvlogman 0:537f81d7b756 38 Matrix J2 = Ad_H_0f*J;
tvlogman 0:537f81d7b756 39
tvlogman 0:537f81d7b756 40
tvlogman 0:537f81d7b756 41 J2.DeleteRow(J2,1);
tvlogman 0:537f81d7b756 42 //J2.print();
tvlogman 0:537f81d7b756 43
tvlogman 0:537f81d7b756 44 Matrix J2_T = MatrixMath::Transpose(J2);
tvlogman 0:537f81d7b756 45 Matrix J_fancy = MatrixMath::Inv(J2_T*J2) * J2_T;
tvlogman 0:537f81d7b756 46 //J_fancy.print();
tvlogman 0:537f81d7b756 47
tvlogman 0:537f81d7b756 48 Matrix v_des(2,1);
tvlogman 0:537f81d7b756 49 v_des << Vx << Vy;
tvlogman 0:537f81d7b756 50
tvlogman 0:537f81d7b756 51 Matrix q_dot = J_fancy*v_des;
tvlogman 0:537f81d7b756 52
tvlogman 0:537f81d7b756 53 //angle1 = angle1 + dt*q_dot(1,1);
tvlogman 0:537f81d7b756 54 // angle2 = angle2 + dt*q_dot(2,1);
tvlogman 0:537f81d7b756 55
tvlogman 0:537f81d7b756 56 return q_dot;
tvlogman 0:537f81d7b756 57 }