IK

kousheekc · Mar 11, 2022 · c045e3c · c045e3c
1 parent 12c8cfc
commit c045e3c
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 63 deletions.
diff --git a/examples/forward_kinematics/forward_kinematics.ino b/examples/forward_kinematics/forward_kinematics.ino
@@ -1,11 +1,13 @@
 #include "Kinematics.h"
 #include "MatrixUtils.h"
 
+#define N 3
+
 void setup() {
     // put your setup code here, to run once:
     Serial.begin(9600);
 
-    Kinematics kin(3);
+    Kinematics kin(N);
     MatrixUtils mat_utils;
 
     kin.add_joint_axis(0, 0,  1,  4, 0,    0);
@@ -17,11 +19,18 @@ void setup() {
                                        0, 0, -1, 2,
                                        0, 0,  0, 1);
 
-    float joint_angles[3] = {PI/2.0, 3, PI};
+    float joint_angles[N] = {PI/2.0, 3, PI};
     float transform[4][4];
 
     kin.forward(joint_angles, (float*)transform);
     mat_utils.print_matrix((float*)transform, 4, 4, "Transform");
+
+    // Output
+    // Transform
+    // 0.00    1.00    0.00    -5.00
+    // 1.00    -0.00   0.00    4.00
+    // 0.00    0.00    -1.00   1.69
+    // 0.00    0.00    0.00    1.00
 }
 
 void loop() {

diff --git a/examples/inverse_kinematics/inverse_kinematics.ino b/examples/inverse_kinematics/inverse_kinematics.ino
@@ -1,79 +1,42 @@
 #include "Kinematics.h"
 #include "MatrixUtils.h"
 
+#define N 3
+
 void setup() {
-  // put your setup code here, to run once:
+    // put your setup code here, to run once:
     Serial.begin(9600);
 
-    Kinematics kin(6);
+    Kinematics kin(N);
     MatrixUtils mat_utils;
 
-//    kin.add_joint_axis(0, 0,  1,  4, 0,    0);
-//    kin.add_joint_axis(0, 0,  0,  0, 1,    0);
-//    kin.add_joint_axis(0, 0, -1, -6, 0, -0.1);
-//
-//    kin.add_initial_end_effector_pose(-1, 0,  0, 0,
-//                                    0, 1, 0, 6,
-//                                    0, 0, -1, 2,
-//                                    0, 0, 0, 1);
-//
-//    float T[4][4] = {
-//        {0, 1,  0,     -5},
-//        {1, 0,  0,      4},
-//        {0, 0, -1, 1.6858},
-//        {0, 0,  0,      1}
-//    };
-//
-//    float jac[6][3];
-//    float jac_t[6][3];
-//    float AA_t[6][6];
-//    float A_tA[3][3];
-//    float pinv[3][6];
-//
-//    float thetalist0[3] = {1.0, 2.5, 3};
-//    float thetalist[3];
-//
-//    kin.inverse((float*)T, (float*)jac, (float*)pinv, (float*)jac_t, (float*)AA_t, (float*)A_tA, thetalist0, 0.01, 0.001, 20, thetalist);
-//    mat_utils.print_matrix(thetalist, 1, 3, "Joint angles");
+    kin.add_joint_axis(0, 0,  1,  4, 0,    0);
+    kin.add_joint_axis(0, 0,  0,  0, 1,    0);
+    kin.add_joint_axis(0, 0, -1, -6, 0, -0.1);
 
-    kin.add_joint_axis(0, 0, 1, 0, 0, 0);
-    kin.add_joint_axis(0, 1, 0, -290, 0, 0);
-    kin.add_joint_axis(0, 1, 0, -560, 0, 0);
-    kin.add_joint_axis(1, 0, 0, 0, 630, 0);
-    kin.add_joint_axis(0, 1, 0, -302, 0, 630);
-    kin.add_joint_axis(1, 0, 0, 0, 630, 0);
-
-    kin.add_initial_end_effector_pose(0,  0,  1, 374,
-                                     0, -1,  0, 0,
-                                     1,  0,  0, 630,
-                                     0,  0,  0, 1);
+    kin.add_initial_end_effector_pose(-1, 0,  0, 0,
+                                       0, 1,  0, 6,
+                                       0, 0, -1, 2,
+                                       0, 0,  0, 1);
 
-    float T[4][4] = {
-        {0,  0,  1, 374,},
-        {0, -1,  0, 0},
-        {1,  0,  0, 550},
-        {0,  0,  0, 1}
+    float desired_transform[4][4] = {
+        {0, 1,  0,     -5},
+        {1, 0,  0,      4},
+        {0, 0, -1, 1.6858},
+        {0, 0,  0,      1}
     };
 
-    float jac[6][6];
-    float jac_t[6][6];
+    float jac[6][N];
+    float jac_t[6][N];
     float AA_t[6][6];
-    float A_tA[6][6];
-    float pinv[6][6];
-    float transform[4][4];
-
-//    float thetalist0[6] = {0, -1.62, 16.53, 0, 0, 0};
-    float thetalist0[6] = {0, 0, 0, 0, PI/2, 0};
-    float thetalist[6];
+    float A_tA[N][N];
+    float pinv[N][6];
 
-//    for (int i = 0; i < 6; i++) {
-//      thetalist0[i] = thetalist0[i] * PI / 180.0;
-//    }
+    float joint_angles_0[N] = {1.0, 2.5, 3};
+    float joint_angles[N];
 
-//    kin.inverse((float*)T, (float*)jac, (float*)pinv, (float*)jac_t, (float*)AA_t, (float*)A_tA, thetalist0, 0.01, 0.001, 20, thetalist);
-//    mat_utils.print_matrix(thetalist, 1, 6, "Joint angles");
-    kin.forward(thetalist0, (float*)transform);
-    mat_utils.print_matrix((float*)transform, 4, 4, "T");
+    kin.inverse((float*)desired_transform, (float*)jac, (float*)pinv, (float*)jac_t, (float*)AA_t, (float*)A_tA, joint_angles_0, 0.01, 0.001, 20, joint_angles);
+    mat_utils.print_matrix(joint_angles, 1, N, "Joint angles");
 }
 
 void loop() {