pseudo-inverse
Diff: inverseKinematics.cpp
- Revision:
- 0:537f81d7b756
- Child:
- 1:81e4001f1082
diff -r 000000000000 -r 537f81d7b756 inverseKinematics.cpp --- /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