Jurriën Bos
Inverse kinematica
Revision 0:a204dfa75158, committed 2018-10-26
- Comitter:
- JurrienBos
- Date:
- Fri Oct 26 15:19:17 2018 +0000
- Commit message:
- Inverse kinematica
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r a204dfa75158 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Oct 26 15:19:17 2018 +0000
@@ -0,0 +1,47 @@
+#include "mbed.h"
+
+volatile float q_1;
+volatile float q_2;
+volatile float r_1;
+volatile float r_2;
+volatile const float r_3 = 0.035;
+
+int main()
+{
+ q_1= 1.05;
+ q_2= 2.5;
+ r_1= -0.2;
+ r_2= -0.2;
+ float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2);
+ float z = 2.0*(r_2*cos(q_1)*cos(q_2))-r_3;
+ 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;
+ float x = (-2.0)*r_2*sin(q_1)*cos(q_2);
+ float D =1.0/(u*z-x*y); // Determinant
+ printf("Determinant is %f\n", D);
+
+ float a = D*z; // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix
+ float b = -D*x; // Inverse jacobian
+ float c = -D*y; // Inverse jacobian
+ float d = D*u; // Inverse jacobian
+
+ float vx = 0.01; // uit emg data
+ float vy = 0.0; // uit emg data
+ float w_1 = vx*a+vy*b;
+ float w_2 = vx*c+vy*d;
+ printf("%f\n", w_1);
+ printf("%f\n", w_2);
+
+ /* printf("a is%f\n", a);
+ printf("%f\n", b);
+ printf("%f\n", c);
+ printf("%f\n", d);
+*/
+ /* float Adj_jac [2][2];
+ Adj_jac[0][0] = 2.0;
+ Adj_jac[0][1] = 3.0;
+ Adj_jac[1][0] = 4.0;
+ Adj_jac[1][1] = 5.0;
+ printarray("%f", Adj_jac);
+*/
+
+}
diff -r 000000000000 -r a204dfa75158 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Oct 26 15:19:17 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187 \ No newline at end of file