0
$\begingroup$

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]
Improve this question
$\endgroup$
2
  • $\begingroup$ 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. $\endgroup$
    – kucar
    Sep 14 '21 at 16:07
  • $\begingroup$ 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 $\endgroup$
    – Mich
    Sep 14 '21 at 21:13

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.