I am trying to implement the Jacobian IK solution in my program.
I have a single kinematic chain with N rotational or transnational joints. The implementation is independent of configuration (i.e I want to be able to solve this for any input system)
The paper I am basing this on is this one http://www.cs.cmu.edu/~15464-s13/lectures/lecture6/iksurvey.pdf
I don't have code written for it yet, just the pseudo implementation steps. I want to verify these steps are correct before I proceed
I understand its possibly faster to convert each matrix to a quaternion first, then do the multiplication. This will be in the next revision if this works
// 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