Fabrice Tshilumba / Mbed 2 deprecated frdm_inversekinematics

nnama inverse kinematics

Dependencies:   mbed MODSERIAL

main.cpp@2:1d374991b3be, 2018-10-29 (annotated)

Committer:
KingMufasa
Date:
Mon Oct 29 13:44:06 2018 +0000
Revision:
2:1d374991b3be
Parent:
1:c0d1d60ff73e
Child:
3:1a7bba50fb97
1000 angle positions per second

Who changed what in which revision?

UserRevisionLine numberNew contents of line
KingMufasa 0:c866ac7b2501 1 #include "mbed.h"
KingMufasa 0:c866ac7b2501 2
KingMufasa 0:c866ac7b2501 3 template<std::size_t N, std::size_t M, std::size_t P>
KingMufasa 0:c866ac7b2501 4 int mult(int A[N][M], int B[M][P]) {
KingMufasa 0:c866ac7b2501 5
KingMufasa 0:c866ac7b2501 6 int C[N][P];
KingMufasa 0:c866ac7b2501 7
KingMufasa 0:c866ac7b2501 8 for (int n = 0; n < N; n++) {
KingMufasa 0:c866ac7b2501 9 for (int p = 0; p < P; p++) {
KingMufasa 0:c866ac7b2501 10 int num = 0;
KingMufasa 0:c866ac7b2501 11 for (int m = 0; m < M; m++) {
KingMufasa 0:c866ac7b2501 12 num += A[n][m] * B[m][p];
KingMufasa 0:c866ac7b2501 13 }
KingMufasa 0:c866ac7b2501 14 C[n][p] = num;
KingMufasa 0:c866ac7b2501 15 std::cout << num << " ";
KingMufasa 0:c866ac7b2501 16 }
KingMufasa 0:c866ac7b2501 17 std::cout << std::endl;
KingMufasa 0:c866ac7b2501 18 }
KingMufasa 0:c866ac7b2501 19
KingMufasa 0:c866ac7b2501 20 return 0;
KingMufasa 0:c866ac7b2501 21
KingMufasa 0:c866ac7b2501 22 }
KingMufasa 0:c866ac7b2501 23
KingMufasa 0:c866ac7b2501 24 AnalogIn pot1(A0); // X-coord knob
KingMufasa 0:c866ac7b2501 25 AnalogIn pot2(A3); // Y-coord knob
KingMufasa 0:c866ac7b2501 26
KingMufasa 0:c866ac7b2501 27 int main()
KingMufasa 0:c866ac7b2501 28 {
KingMufasa 0:c866ac7b2501 29 const double L1 = 0.328;
KingMufasa 0:c866ac7b2501 30 const double L2 = 0.218:
KingMufasa 0:c866ac7b2501 31
KingMufasa 0:c866ac7b2501 32 double q1 = 0;
KingMufasa 0:c866ac7b2501 33 double q2 =-q1 +0;
KingMufasa 0:c866ac7b2501 34
KingMufasa 0:c866ac7b2501 35 double T1[3][3] = {
KingMufasa 0:c866ac7b2501 36 {0, -1, 0},
KingMufasa 0:c866ac7b2501 37 {1, 0, 0,},
KingMufasa 0:c866ac7b2501 38 {0, 0, 0,}};
KingMufasa 0:c866ac7b2501 39 double T20[3][3] = {
KingMufasa 0:c866ac7b2501 40 {0, -1, 0},
KingMufasa 0:c866ac7b2501 41 {1, 0, -L1,},
KingMufasa 0:c866ac7b2501 42 {0, 0, 0,}};
KingMufasa 0:c866ac7b2501 43 double H200[3][3] = {
KingMufasa 0:c866ac7b2501 44 {1, 0, L1+L2},
KingMufasa 0:c866ac7b2501 45 {0, 1, 0,},
KingMufasa 0:c866ac7b2501 46 {0, 0, 1,}};
KingMufasa 0:c866ac7b2501 47 double Pe2 [3][1]={
KingMufasa 0:c866ac7b2501 48 {0},
KingMufasa 0:c866ac7b2501 49 {0},
KingMufasa 0:c866ac7b2501 50 {1}};
KingMufasa 0:c866ac7b2501 51 double q1 = q1*pi/180;
KingMufasa 0:c866ac7b2501 52 double q2 = q2*pi/180;
KingMufasa 0:c866ac7b2501 53
KingMufasa 0:c866ac7b2501 54 while (true) {
KingMufasa 0:c866ac7b2501 55 double Pe_set[1][3] = { // defining setpoint location of end effector
KingMufasa 1:c0d1d60ff73e 56 {pot1*0.546}, //setting xcoord to pot 1
KingMufasa 1:c0d1d60ff73e 57 {pot2*0.4}, // setting ycoord to pot 2
KingMufasa 0:c866ac7b2501 58 {1}};
KingMufasa 0:c866ac7b2501 59
KingMufasa 0:c866ac7b2501 60 //Calculating new H matrix
KingMufasa 0:c866ac7b2501 61 double T1e=[3][3] = {
KingMufasa 0:c866ac7b2501 62 {cos(q1), -sin(q1), 0},
KingMufasa 0:c866ac7b2501 63 {sin(q1), cos(q1), 0},
KingMufasa 0:c866ac7b2501 64 {0, 0, 1}};
KingMufasa 0:c866ac7b2501 65 double T20e[3][3] = {
KingMufasa 0:c866ac7b2501 66 {cos(q2), -sin(q2), L1-L1*cos(q2)},
KingMufasa 0:c866ac7b2501 67 {sin(q2), cos(q2), -L1*sin(q2)},
KingMufasa 0:c866ac7b2501 68 {0, 0, 1}};
KingMufasa 0:c866ac7b2501 69
KingMufasa 0:c866ac7b2501 70 double H20 = mult<3,3,3>(T1e,T20e); // matrix multiplication
KingMufasa 0:c866ac7b2501 71 double Pe0 = mult<3,3,1>(H20,Pe2); // matrix multiplication
KingMufasa 0:c866ac7b2501 72 double pe0x =Pe0[1]; // seperating coordinates of end effector location
KingMufasa 0:c866ac7b2501 73 double pe0y = Pe[2];
KingMufasa 0:c866ac7b2501 74
KingMufasa 0:c866ac7b2501 75 // Determing the jacobian
KingMufasa 0:c866ac7b2501 76
KingMufasa 0:c866ac7b2501 77 double T_1[3][1] = {
KingMufasa 0:c866ac7b2501 78 {1},
KingMufasa 0:c866ac7b2501 79 {T1[1][3]},
KingMufasa 0:c866ac7b2501 80 {T1[2][3]};
KingMufasa 0:c866ac7b2501 81
KingMufasa 0:c866ac7b2501 82 double T_2[3][1] = {
KingMufasa 0:c866ac7b2501 83 {1},
KingMufasa 0:c866ac7b2501 84 {L1*sin(q1)},
KingMufasa 0:c866ac7b2501 85 {-L1*cos(q1)}};
KingMufasa 0:c866ac7b2501 86
KingMufasa 0:c866ac7b2501 87 double J[3][2] = {
KingMufasa 0:c866ac7b2501 88 {T_1[1][1], T_2[1][1]},
KingMufasa 0:c866ac7b2501 89 {T_1[2][1], T_2[2][1]},
KingMufasa 0:c866ac7b2501 90 {T_1[3][1], T_2[3][1]}};
KingMufasa 0:c866ac7b2501 91
KingMufasa 0:c866ac7b2501 92 //Determing 'Pulling" force to setpoint
KingMufasa 0:c866ac7b2501 93
KingMufasa 0:c866ac7b2501 94 double k= 1; //virtual stifness of the force
KingMufasa 0:c866ac7b2501 95
KingMufasa 0:c866ac7b2501 96 double Fs[3][1] = { //force vector from end effector to setpoint
KingMufasa 0:c866ac7b2501 97 {k*Pe_set[1][1] - k*Pe0[1][1]},
KingMufasa 0:c866ac7b2501 98 {k*Pe_set[2][1] - k*Pe0[2][1]},
KingMufasa 0:c866ac7b2501 99 {k*Pe_set[3][1] - k*Pe0[3][1]}};
KingMufasa 0:c866ac7b2501 100
KingMufasa 0:c866ac7b2501 101 double Fx = Fs[1][1];
KingMufasa 0:c866ac7b2501 102 double Fy = Fs[2][1];
KingMufasa 0:c866ac7b2501 103 double W0t[3][1] = {
KingMufasa 0:c866ac7b2501 104 {Pe0x*Fy - Pe0y*Fx},
KingMufasa 0:c866ac7b2501 105 {Fx},
KingMufasa 0:c866ac7b2501 106 {Fy}};
KingMufasa 0:c866ac7b2501 107
KingMufasa 0:c866ac7b2501 108
KingMufasa 0:c866ac7b2501 109 double Jt[2][3] = { // transposing jacobian matrix
KingMufasa 0:c866ac7b2501 110 {J[1][1], J[1][2], J[1][3]},
KingMufasa 0:c866ac7b2501 111 {J[2][1], J[2][2], J[2][3]}};
KingMufasa 0:c866ac7b2501 112
KingMufasa 0:c866ac7b2501 113 double tau_st = mult<2,3,1>(Jt,W0t);
KingMufasa 0:c866ac7b2501 114
KingMufasa 0:c866ac7b2501 115 //Calculating joint behaviour
KingMufasa 0:c866ac7b2501 116
KingMufasa 0:c866ac7b2501 117 double b =1; //joint friction coefficent
KingMufasa 2:1d374991b3be 118 double Ts = 1/1000; //Time step to reach the new angle
KingMufasa 0:c866ac7b2501 119 double w_s = tau_st/b; // angular velocity
KingMufasa 0:c866ac7b2501 120 double q1 = q1 +w_s*Ts; // calculating new angle of q1 in time step Ts
KingMufasa 0:c866ac7b2501 121 double q2 = q2 + w_s*Ts; // calculating new angle of q2 in time step Ts
KingMufasa 0:c866ac7b2501 122
KingMufasa 0:c866ac7b2501 123 }
KingMufasa 0:c866ac7b2501 124
KingMufasa 0:c866ac7b2501 125 }