# IK Jacobian is always a singular matrix

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]

• Are you sure that the order in the matrix multiplications are correct? For example in step 7, you have written the following to find the required joint displacements: (Jᵀ·J)¯¹·Jᵀ·E = ΔΘ This is what the paper you use suggests: Jᵀ·(Jᵀ·J)¯¹.E = ΔΘ These the operations will yields different results, since matrix multiplication does not have commutative property. I did not inspect the whole algorithm you have written but this is what caught my attention. Sep 14 '21 at 16:07
• Thanks, I didnt link to any paper though. I think my math is correct, I always multiply matrices fromt he left, nonetheless its the (Jᵀ·J) matrix that is singular, so I have no way to inverse it in either equation
– Mich
Sep 14 '21 at 21:13