Jurriën Bos
/
Inverse
Inverse kinematica
main.cpp@0:a204dfa75158, 2018-10-26 (annotated)
- Committer:
- JurrienBos
- Date:
- Fri Oct 26 15:19:17 2018 +0000
- Revision:
- 0:a204dfa75158
Inverse kinematica
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JurrienBos | 0:a204dfa75158 | 1 | #include "mbed.h" |
JurrienBos | 0:a204dfa75158 | 2 | |
JurrienBos | 0:a204dfa75158 | 3 | volatile float q_1; |
JurrienBos | 0:a204dfa75158 | 4 | volatile float q_2; |
JurrienBos | 0:a204dfa75158 | 5 | volatile float r_1; |
JurrienBos | 0:a204dfa75158 | 6 | volatile float r_2; |
JurrienBos | 0:a204dfa75158 | 7 | volatile const float r_3 = 0.035; |
JurrienBos | 0:a204dfa75158 | 8 | |
JurrienBos | 0:a204dfa75158 | 9 | int main() |
JurrienBos | 0:a204dfa75158 | 10 | { |
JurrienBos | 0:a204dfa75158 | 11 | q_1= 1.05; |
JurrienBos | 0:a204dfa75158 | 12 | q_2= 2.5; |
JurrienBos | 0:a204dfa75158 | 13 | r_1= -0.2; |
JurrienBos | 0:a204dfa75158 | 14 | r_2= -0.2; |
JurrienBos | 0:a204dfa75158 | 15 | float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2); |
JurrienBos | 0:a204dfa75158 | 16 | float z = 2.0*(r_2*cos(q_1)*cos(q_2))-r_3; |
JurrienBos | 0:a204dfa75158 | 17 | float y = r_2*cos(q_1)*cos(q_2)-r_2*sin(q_1)*sin(q_2)+2.0*(r_1*cos(q_1))-r_3; |
JurrienBos | 0:a204dfa75158 | 18 | float x = (-2.0)*r_2*sin(q_1)*cos(q_2); |
JurrienBos | 0:a204dfa75158 | 19 | float D =1.0/(u*z-x*y); // Determinant |
JurrienBos | 0:a204dfa75158 | 20 | printf("Determinant is %f\n", D); |
JurrienBos | 0:a204dfa75158 | 21 | |
JurrienBos | 0:a204dfa75158 | 22 | float a = D*z; // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix |
JurrienBos | 0:a204dfa75158 | 23 | float b = -D*x; // Inverse jacobian |
JurrienBos | 0:a204dfa75158 | 24 | float c = -D*y; // Inverse jacobian |
JurrienBos | 0:a204dfa75158 | 25 | float d = D*u; // Inverse jacobian |
JurrienBos | 0:a204dfa75158 | 26 | |
JurrienBos | 0:a204dfa75158 | 27 | float vx = 0.01; // uit emg data |
JurrienBos | 0:a204dfa75158 | 28 | float vy = 0.0; // uit emg data |
JurrienBos | 0:a204dfa75158 | 29 | float w_1 = vx*a+vy*b; |
JurrienBos | 0:a204dfa75158 | 30 | float w_2 = vx*c+vy*d; |
JurrienBos | 0:a204dfa75158 | 31 | printf("%f\n", w_1); |
JurrienBos | 0:a204dfa75158 | 32 | printf("%f\n", w_2); |
JurrienBos | 0:a204dfa75158 | 33 | |
JurrienBos | 0:a204dfa75158 | 34 | /* printf("a is%f\n", a); |
JurrienBos | 0:a204dfa75158 | 35 | printf("%f\n", b); |
JurrienBos | 0:a204dfa75158 | 36 | printf("%f\n", c); |
JurrienBos | 0:a204dfa75158 | 37 | printf("%f\n", d); |
JurrienBos | 0:a204dfa75158 | 38 | */ |
JurrienBos | 0:a204dfa75158 | 39 | /* float Adj_jac [2][2]; |
JurrienBos | 0:a204dfa75158 | 40 | Adj_jac[0][0] = 2.0; |
JurrienBos | 0:a204dfa75158 | 41 | Adj_jac[0][1] = 3.0; |
JurrienBos | 0:a204dfa75158 | 42 | Adj_jac[1][0] = 4.0; |
JurrienBos | 0:a204dfa75158 | 43 | Adj_jac[1][1] = 5.0; |
JurrienBos | 0:a204dfa75158 | 44 | printarray("%f", Adj_jac); |
JurrienBos | 0:a204dfa75158 | 45 | */ |
JurrienBos | 0:a204dfa75158 | 46 | |
JurrienBos | 0:a204dfa75158 | 47 | } |