Fabrice Tshilumba
/
frdm_inversekinematics
nnama inverse kinematics
main.cpp@3:1a7bba50fb97, 2018-10-31 (annotated)
- Committer:
- KingMufasa
- Date:
- Wed Oct 31 10:45:29 2018 +0000
- Revision:
- 3:1a7bba50fb97
- Parent:
- 2:1d374991b3be
inverse kinematics
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
KingMufasa | 0:c866ac7b2501 | 1 | #include "mbed.h" |
KingMufasa | 3:1a7bba50fb97 | 2 | #include "MODSERIAL.h" |
KingMufasa | 3:1a7bba50fb97 | 3 | #include <iostream> |
KingMufasa | 3:1a7bba50fb97 | 4 | #include <string> |
KingMufasa | 3:1a7bba50fb97 | 5 | |
KingMufasa | 3:1a7bba50fb97 | 6 | |
KingMufasa | 3:1a7bba50fb97 | 7 | |
KingMufasa | 0:c866ac7b2501 | 8 | |
KingMufasa | 0:c866ac7b2501 | 9 | |
KingMufasa | 3:1a7bba50fb97 | 10 | //AnalogIn pot1(A0); // X-coord knob |
KingMufasa | 3:1a7bba50fb97 | 11 | //AnalogIn pot2(A3); // Y-coord knob |
KingMufasa | 3:1a7bba50fb97 | 12 | MODSERIAL pc(USBTX, USBRX); |
KingMufasa | 3:1a7bba50fb97 | 13 | Ticker flipper; |
KingMufasa | 3:1a7bba50fb97 | 14 | |
KingMufasa | 3:1a7bba50fb97 | 15 | const double L1 = 0.328; |
KingMufasa | 3:1a7bba50fb97 | 16 | const double L2 = 0.218; |
KingMufasa | 0:c866ac7b2501 | 17 | |
KingMufasa | 3:1a7bba50fb97 | 18 | const double PI = 3.14159265359; |
KingMufasa | 3:1a7bba50fb97 | 19 | const double Ts = 1; |
KingMufasa | 0:c866ac7b2501 | 20 | |
KingMufasa | 3:1a7bba50fb97 | 21 | double qRef1 = 0*PI/180; |
KingMufasa | 3:1a7bba50fb97 | 22 | double qRef2 =(-qRef1 +0)*PI/180; |
KingMufasa | 3:1a7bba50fb97 | 23 | |
KingMufasa | 3:1a7bba50fb97 | 24 | double tau_st1 =0; |
KingMufasa | 3:1a7bba50fb97 | 25 | double tau_st2 =0; |
KingMufasa | 3:1a7bba50fb97 | 26 | |
KingMufasa | 3:1a7bba50fb97 | 27 | double pe0x =0; |
KingMufasa | 3:1a7bba50fb97 | 28 | double pe0y =0; |
KingMufasa | 3:1a7bba50fb97 | 29 | |
KingMufasa | 3:1a7bba50fb97 | 30 | double Fx = 0; |
KingMufasa | 3:1a7bba50fb97 | 31 | double Fy = 0; |
KingMufasa | 0:c866ac7b2501 | 32 | |
KingMufasa | 3:1a7bba50fb97 | 33 | |
KingMufasa | 0:c866ac7b2501 | 34 | |
KingMufasa | 3:1a7bba50fb97 | 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 | 3:1a7bba50fb97 | 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 | 3:1a7bba50fb97 | 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 | 3:1a7bba50fb97 | 47 | double Pe2 [3][1]{ |
KingMufasa | 0:c866ac7b2501 | 48 | {0}, |
KingMufasa | 0:c866ac7b2501 | 49 | {0}, |
KingMufasa | 0:c866ac7b2501 | 50 | {1}}; |
KingMufasa | 3:1a7bba50fb97 | 51 | |
KingMufasa | 0:c866ac7b2501 | 52 | |
KingMufasa | 3:1a7bba50fb97 | 53 | |
KingMufasa | 3:1a7bba50fb97 | 54 | double C[5][5]; |
KingMufasa | 3:1a7bba50fb97 | 55 | |
KingMufasa | 3:1a7bba50fb97 | 56 | template<std::size_t N, std::size_t M, std::size_t P> |
KingMufasa | 3:1a7bba50fb97 | 57 | void mult(double A[N][M], double B[M][P]) { |
KingMufasa | 3:1a7bba50fb97 | 58 | |
KingMufasa | 3:1a7bba50fb97 | 59 | for( int n =0; n < 5; n++){ |
KingMufasa | 3:1a7bba50fb97 | 60 | for(int p =0; p < 5; p++){ |
KingMufasa | 3:1a7bba50fb97 | 61 | C[n][p] =0; |
KingMufasa | 3:1a7bba50fb97 | 62 | } |
KingMufasa | 3:1a7bba50fb97 | 63 | } |
KingMufasa | 3:1a7bba50fb97 | 64 | for (int n = 0; n < N; n++) { |
KingMufasa | 3:1a7bba50fb97 | 65 | for (int p = 0; p < P; p++) { |
KingMufasa | 3:1a7bba50fb97 | 66 | double num = 0; |
KingMufasa | 3:1a7bba50fb97 | 67 | for (int m = 0; m < M; m++) { |
KingMufasa | 3:1a7bba50fb97 | 68 | num += A[n][m] * B[m][p]; |
KingMufasa | 3:1a7bba50fb97 | 69 | |
KingMufasa | 3:1a7bba50fb97 | 70 | } |
KingMufasa | 3:1a7bba50fb97 | 71 | |
KingMufasa | 3:1a7bba50fb97 | 72 | C[n][p] = num; |
KingMufasa | 3:1a7bba50fb97 | 73 | |
KingMufasa | 3:1a7bba50fb97 | 74 | //std::cout << num << " "; |
KingMufasa | 3:1a7bba50fb97 | 75 | } |
KingMufasa | 3:1a7bba50fb97 | 76 | //std::cout << std::endl; |
KingMufasa | 3:1a7bba50fb97 | 77 | } |
KingMufasa | 3:1a7bba50fb97 | 78 | |
KingMufasa | 3:1a7bba50fb97 | 79 | } |
KingMufasa | 3:1a7bba50fb97 | 80 | |
KingMufasa | 3:1a7bba50fb97 | 81 | |
KingMufasa | 3:1a7bba50fb97 | 82 | |
KingMufasa | 3:1a7bba50fb97 | 83 | void flip () { |
KingMufasa | 3:1a7bba50fb97 | 84 | double potx = 0.218;//pot1.read()*0.546; |
KingMufasa | 3:1a7bba50fb97 | 85 | double poty = 0.328;//pot2.read()*0.4; |
KingMufasa | 3:1a7bba50fb97 | 86 | |
KingMufasa | 3:1a7bba50fb97 | 87 | double Pe_set[3][1]{ // defining setpoint location of end effector |
KingMufasa | 3:1a7bba50fb97 | 88 | {potx}, //setting xcoord to pot 1 |
KingMufasa | 3:1a7bba50fb97 | 89 | {poty}, // setting ycoord to pot 2 |
KingMufasa | 0:c866ac7b2501 | 90 | {1}}; |
KingMufasa | 0:c866ac7b2501 | 91 | |
KingMufasa | 0:c866ac7b2501 | 92 | //Calculating new H matrix |
KingMufasa | 3:1a7bba50fb97 | 93 | double T1e[3][3]{ |
KingMufasa | 3:1a7bba50fb97 | 94 | {cos(qRef1), -sin(qRef1), 0}, |
KingMufasa | 3:1a7bba50fb97 | 95 | {sin(qRef1), cos(qRef1), 0}, |
KingMufasa | 0:c866ac7b2501 | 96 | {0, 0, 1}}; |
KingMufasa | 3:1a7bba50fb97 | 97 | double T20e[3][3]{ |
KingMufasa | 3:1a7bba50fb97 | 98 | {cos(qRef2), -sin(qRef2), L1-L1*cos(qRef2)}, |
KingMufasa | 3:1a7bba50fb97 | 99 | {sin(qRef2), cos(qRef2), -L1*sin(qRef2)}, |
KingMufasa | 0:c866ac7b2501 | 100 | {0, 0, 1}}; |
KingMufasa | 0:c866ac7b2501 | 101 | |
KingMufasa | 3:1a7bba50fb97 | 102 | |
KingMufasa | 3:1a7bba50fb97 | 103 | mult<3,3,3>(T1e,T20e); // matrix multiplication |
KingMufasa | 3:1a7bba50fb97 | 104 | double H201[3][3]{ |
KingMufasa | 3:1a7bba50fb97 | 105 | {C[0][0],C[0][1],C[0][2]}, |
KingMufasa | 3:1a7bba50fb97 | 106 | {C[1][0],C[1][1],C[1][2]}, |
KingMufasa | 3:1a7bba50fb97 | 107 | {C[2][0],C[2][1],C[2][2]}}; |
KingMufasa | 3:1a7bba50fb97 | 108 | |
KingMufasa | 3:1a7bba50fb97 | 109 | mult<3,3,3>(H201,H200); // matrix multiplication |
KingMufasa | 3:1a7bba50fb97 | 110 | double H20 [3][3]{ |
KingMufasa | 3:1a7bba50fb97 | 111 | {C[0][0],C[0][1],C[0][2]}, |
KingMufasa | 3:1a7bba50fb97 | 112 | {C[1][0],C[1][1],C[1][2]}, |
KingMufasa | 3:1a7bba50fb97 | 113 | {C[2][0],C[2][1],C[2][2]}}; |
KingMufasa | 3:1a7bba50fb97 | 114 | |
KingMufasa | 3:1a7bba50fb97 | 115 | mult<3,3,1>(H20,Pe2); // matrix multiplication |
KingMufasa | 3:1a7bba50fb97 | 116 | double Pe0[3][1] { |
KingMufasa | 3:1a7bba50fb97 | 117 | {C[0][0]}, |
KingMufasa | 3:1a7bba50fb97 | 118 | {C[1][0]}, |
KingMufasa | 3:1a7bba50fb97 | 119 | {C[2][0]}}; |
KingMufasa | 3:1a7bba50fb97 | 120 | |
KingMufasa | 3:1a7bba50fb97 | 121 | double pe0x = Pe0[0][0]; // seperating coordinates of end effector location |
KingMufasa | 3:1a7bba50fb97 | 122 | double pe0y = Pe0[1][0]; |
KingMufasa | 0:c866ac7b2501 | 123 | |
KingMufasa | 0:c866ac7b2501 | 124 | // Determing the jacobian |
KingMufasa | 0:c866ac7b2501 | 125 | |
KingMufasa | 3:1a7bba50fb97 | 126 | double T_1[3][1]{ |
KingMufasa | 0:c866ac7b2501 | 127 | {1}, |
KingMufasa | 3:1a7bba50fb97 | 128 | {T1[0][2]}, |
KingMufasa | 3:1a7bba50fb97 | 129 | {T1[1][2]}}; |
KingMufasa | 0:c866ac7b2501 | 130 | |
KingMufasa | 3:1a7bba50fb97 | 131 | double T_2[3][1]{ |
KingMufasa | 0:c866ac7b2501 | 132 | {1}, |
KingMufasa | 3:1a7bba50fb97 | 133 | {L1*sin(qRef1)}, |
KingMufasa | 3:1a7bba50fb97 | 134 | {-L1*cos(qRef1)}}; |
KingMufasa | 0:c866ac7b2501 | 135 | |
KingMufasa | 3:1a7bba50fb97 | 136 | double J[3][2]{ |
KingMufasa | 3:1a7bba50fb97 | 137 | {T_1[0][0], T_2[0][0]}, |
KingMufasa | 3:1a7bba50fb97 | 138 | {T_1[1][0], T_2[1][0]}, |
KingMufasa | 3:1a7bba50fb97 | 139 | {T_1[2][0], T_2[2][0]}}; |
KingMufasa | 0:c866ac7b2501 | 140 | |
KingMufasa | 0:c866ac7b2501 | 141 | //Determing 'Pulling" force to setpoint |
KingMufasa | 0:c866ac7b2501 | 142 | |
KingMufasa | 0:c866ac7b2501 | 143 | double k= 1; //virtual stifness of the force |
KingMufasa | 3:1a7bba50fb97 | 144 | double Fs[3][1]{ //force vector from end effector to setpoint |
KingMufasa | 3:1a7bba50fb97 | 145 | {k*Pe_set[0][0] - k*Pe0[0][0]}, |
KingMufasa | 3:1a7bba50fb97 | 146 | {k*Pe_set[1][0] - k*Pe0[1][0]}, |
KingMufasa | 3:1a7bba50fb97 | 147 | {k*Pe_set[2][0] - k*Pe0[2][0]}}; |
KingMufasa | 3:1a7bba50fb97 | 148 | double Fx = k*potx - k*pe0x; |
KingMufasa | 3:1a7bba50fb97 | 149 | double Fy = k*poty - k*pe0y; |
KingMufasa | 3:1a7bba50fb97 | 150 | double W0t[3][1]{ |
KingMufasa | 3:1a7bba50fb97 | 151 | {pe0x*Fy - pe0y*Fx}, |
KingMufasa | 0:c866ac7b2501 | 152 | {Fx}, |
KingMufasa | 0:c866ac7b2501 | 153 | {Fy}}; |
KingMufasa | 3:1a7bba50fb97 | 154 | |
KingMufasa | 3:1a7bba50fb97 | 155 | double Jt[2][3]{ // transposing jacobian matrix |
KingMufasa | 3:1a7bba50fb97 | 156 | {J[0][0], J[1][0], J[2][0]}, |
KingMufasa | 3:1a7bba50fb97 | 157 | {T_2[0][0], T_2[1][0], T_2[2][0]}}; |
KingMufasa | 0:c866ac7b2501 | 158 | |
KingMufasa | 3:1a7bba50fb97 | 159 | mult<2,3,1>(Jt,W0t); |
KingMufasa | 3:1a7bba50fb97 | 160 | tau_st1 = C[0][0]; |
KingMufasa | 3:1a7bba50fb97 | 161 | tau_st2 = C[1][0]; |
KingMufasa | 0:c866ac7b2501 | 162 | |
KingMufasa | 0:c866ac7b2501 | 163 | //Calculating joint behaviour |
KingMufasa | 0:c866ac7b2501 | 164 | |
KingMufasa | 3:1a7bba50fb97 | 165 | double b =1; |
KingMufasa | 3:1a7bba50fb97 | 166 | //joint friction coefficent |
KingMufasa | 3:1a7bba50fb97 | 167 | //double Ts = 1/1000; //Time step to reach the new angle |
KingMufasa | 3:1a7bba50fb97 | 168 | double w_s1 = tau_st1/b; // angular velocity |
KingMufasa | 3:1a7bba50fb97 | 169 | double w_s2 = tau_st2/b; // angular velocity |
KingMufasa | 3:1a7bba50fb97 | 170 | //checking angle boundaries |
KingMufasa | 3:1a7bba50fb97 | 171 | qRef1 = qRef1 +w_s1*Ts; // calculating new angle of qRef1 in time step Ts |
KingMufasa | 3:1a7bba50fb97 | 172 | if (qRef1 > 2*PI/3) { |
KingMufasa | 3:1a7bba50fb97 | 173 | qRef1 = 2*PI/3; |
KingMufasa | 3:1a7bba50fb97 | 174 | } |
KingMufasa | 3:1a7bba50fb97 | 175 | else if (qRef1 < PI/6) { |
KingMufasa | 3:1a7bba50fb97 | 176 | qRef1= PI/6; |
KingMufasa | 3:1a7bba50fb97 | 177 | } |
KingMufasa | 3:1a7bba50fb97 | 178 | |
KingMufasa | 3:1a7bba50fb97 | 179 | qRef2 = qRef2 + w_s2*Ts; // calculating new angle of qRef2 in time step Ts |
KingMufasa | 3:1a7bba50fb97 | 180 | if (qRef2 > -PI/4) { |
KingMufasa | 3:1a7bba50fb97 | 181 | qRef2 = -PI/4; |
KingMufasa | 3:1a7bba50fb97 | 182 | } |
KingMufasa | 3:1a7bba50fb97 | 183 | else if (qRef2 < -PI/2) { |
KingMufasa | 3:1a7bba50fb97 | 184 | qRef2= -PI/2; |
KingMufasa | 3:1a7bba50fb97 | 185 | } |
KingMufasa | 3:1a7bba50fb97 | 186 | |
KingMufasa | 3:1a7bba50fb97 | 187 | } |
KingMufasa | 3:1a7bba50fb97 | 188 | |
KingMufasa | 3:1a7bba50fb97 | 189 | int main() |
KingMufasa | 3:1a7bba50fb97 | 190 | { |
KingMufasa | 3:1a7bba50fb97 | 191 | pc.printf("start main function"); |
KingMufasa | 3:1a7bba50fb97 | 192 | pc.baud(115200); |
KingMufasa | 3:1a7bba50fb97 | 193 | flipper.attach(&flip, Ts); |
KingMufasa | 3:1a7bba50fb97 | 194 | while (true){ |
KingMufasa | 3:1a7bba50fb97 | 195 | pc.printf("qRef1 %f\n qRef2 %f\n, X %f\n Y %f \n",qRef1*180/PI,qRef2*180/PI,tau_st1,tau_st2); |
KingMufasa | 3:1a7bba50fb97 | 196 | wait (2.0); |
KingMufasa | 3:1a7bba50fb97 | 197 | } |
KingMufasa | 3:1a7bba50fb97 | 198 | |
KingMufasa | 0:c866ac7b2501 | 199 | } |
KingMufasa | 3:1a7bba50fb97 | 200 | |
KingMufasa | 3:1a7bba50fb97 | 201 | |
KingMufasa | 0:c866ac7b2501 | 202 | |
KingMufasa | 3:1a7bba50fb97 | 203 |