Fabrice Tshilumba / Mbed 2 deprecated frdm_inversekinematics

nnama inverse kinematics

Dependencies:   mbed MODSERIAL

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?

UserRevisionLine numberNew 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