pseudo-inverse
inverseKinematics.cpp@1:81e4001f1082, 2017-11-01 (annotated)
- Committer:
- tvlogman
- Date:
- Wed Nov 01 14:54:38 2017 +0000
- Revision:
- 1:81e4001f1082
- Parent:
- 0:537f81d7b756
nothing changed;
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:81e4001f1082 | 12 | Matrix inverseKinematics :: computeAngularVelocities(double Vx, double Vy, double angle1, double 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 | } |