I am trying to solve an inverse kinematics equation for 6DOF system, 3 rotation and 3 prismatic joints. I am trying to solve using perturbation method, by adjusting each joint and calculating the final Euler angels and transformation.
The math is as follows:
// 1. Save min and max (position or rotation) variable Θ for each of N joints, start with each joint in the middle position, choose Δ to be some small value
//Compute Jacobian
// 2. Find transformation matrices for current value Θ for each joint (Call them #M matrix)
// 3. Find transformation matrices for Θ+Δ and Θ-Δ of each joint independently from the current position (Call them #M+ and #M- matrices)
// 4. Multiply (1M+)·(2M)·(3M)·(4M)... and (1M-)·(2M)·(3M)·(4M)... to get F1M+ and F1M- matrices
// 5. Convert F1M+ and F1M- to quaternion and translation vectors, combine into 1 vector [qw, qx, qy, qz, x, y, z]
// 6. Subtract (F1M+)-(F1M-) vectors
// 7. These 7 variables become first column of the Jacobian matrix
// 8. Repeat steps 4-7 for 2M+-, 3M+-, etc to complete jacobian matrix with N columns and 7 rows
//Compute Error
// 9. Compute (1M)·(2M)·(3M)·(4M)... convert to quaternion plus translation vector denote as 7 axis vector S
// 10. Take end effector matrix, convert to quaterion plus translation vector denote as 7 axis vector T (only needs to be done once)
// 11. Find E = T - S
//Update Θ
// 12. Solve equation E = J · ΔΘ for ΔΘ using Jᵀ·E = Jᵀ·J·ΔΘ then (Jᵀ·J)¯¹·Jᵀ·E = ΔΘ
// 13. Add ΔΘ to current Θ
// 14. Repeat Steps 2-13 Until E is sufficiently small or T¯¹·S is close enough to identity matrix
In the code below, I am using euler angles instead of quaternion, and I am also not looking at end effector rotation to simplify
The problem is when I calculate Jᵀ·J (Matrix j_T_j below) I always get a matrix with a zero determinant
Final S
1.000000 0.000000 -0.000000 0.000000
0.000000 1.000000 0.000000 127.000000
-0.000000 0.000000 1.000000 -0.000000
0.000000 0.000000 0.000000 1.000000
Joint ptb# 0
1.000000 0.000000 -0.000000 -0.100000
0.000000 1.000000 0.000000 127.000000
-0.000000 0.000000 1.000000 0.000000
0.000000 0.000000 0.000000 1.000000
Joint ptb# 1
1.000000 0.000000 -0.000000 -0.000000
0.000000 1.000000 0.000000 127.000000
-0.000000 0.000000 1.000000 -0.100000
0.000000 0.000000 0.000000 1.000000
Joint ptb# 2
1.000000 0.000000 -0.000000 -0.000000
0.000000 1.000000 0.000000 127.100006
-0.000000 0.000000 1.000000 0.000000
0.000000 0.000000 0.000000 1.000000
Joint ptb# 3
1.000000 -0.000000 -0.000000 -0.000000
0.000000 0.995004 0.099833 126.746216
-0.000000 -0.099833 0.995004 -5.071538
0.000000 0.000000 0.000000 1.000000
Joint ptb# 4
0.995004 0.000000 -0.099833 0.000000
0.000000 1.000000 0.000000 127.000000
0.099833 -0.000000 0.995004 -0.000000
0.000000 0.000000 0.000000 1.000000
Joint ptb# 5
0.995004 -0.099833 -0.000000 0.000000
0.099833 0.995004 0.000000 127.000000
-0.000000 0.000000 1.000000 -0.000000
0.000000 0.000000 0.000000 1.000000
v_T =
[0;
0;
-100]
v_S =
[2.273736754432321e-13;
127;
-1.355252715606881e-20]
v_E =
[-2.273736754432321e-13;
-127;
-100]
v_jacobian =
[-1, -1.192092895507813e-07, -5.960464477539063e-08, -3.022872147515284e-06, 7.563471514076884e-08, 0;
0, 0, 1.00006102025418, -2.537841759058211, 0, 0;
5.960464477539063e-08, -1, 3.552713678800501e-15, -50.71537895924779, -1.511437494843092e-06, 0]
j_T =
[-1, 0, 5.960464477539063e-08;
-1.192092895507813e-07, 0, -1;
-5.960464477539063e-08, 1.00006102025418, 3.552713678800501e-15;
-3.022872147515284e-06, -2.537841759058211, -50.71537895924779;
7.563471514076884e-08, 0, -1.511437494843092e-06;
0, 0, 0]
j_T_j =
[1.000000000000004, 5.960464477539063e-08, 5.960464477539084e-08, 0, -7.563480522946383e-08, 0;
5.960464477539063e-08, 1.000000000000014, 3.552713678800501e-15, 50.71537895924815, 1.511437485826732e-06, 0;
5.960464477539084e-08, 3.552713678800501e-15, 1.000122044231834, -2.537996618807416, -4.508185698358049e-15, 0;
0, 50.71537895924815, -2.537996618807416, 2578.490303774142, 7.665312509554948e-05, 0;
-7.563480522946383e-08, 1.511437485826732e-06, -4.508185698358049e-15, 7.665312509554948e-05, 2.290163910951988e-12, 0;
0, 0, 0, 0, 0, 0]
j_T_j determinant =
0
j_T_j_inv =
[0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0]
j_T_j_inv_j_T =
[0, 0, 0;
0, 0, 0;
0, 0, 0;
0, 0, 0;
0, 0, 0;
0, 0, 0]
d_theta =
[0;
0;
0;
0;
0;
0]
