I am trying to compute the joint velocities a 6-DOF manipulator(a typical industrial robot).
I have an initial position [xi, yi, zi, T1i, T2i, T3i] and goal position [xf, yf, zf, T1f, T2f, T3f] in the Cartesian task space and a given cartesian velocity in mm/s. T1, T2 and T3 are ZYZ Euler orientation angles.
My approach to computing the joint velocities is to use the inverse Jacobian. I multiply the 6x1 velocities vector, [Vx, Vy, Vz, w1, w2, w3], with the 6x6 inverse Jacobian matrix. Vx, Vy, Vz are in mm/s and w1, w2, w3 in rad/s. The result is the 6x1 joint velocities vector.
Vx, Vy, Vz are easy to compute. At every scan, I compute:
Px=(Goal_positionX - Current_positionX)
Py=(Goal_positionY - Current_positionY)
Pz=(Goal_positionZ - Current_positionY)
Then I compute the magnitude:
MAG_P=SQRT(Px^2 + Py^2 + Pz^2)
The velocity is set by the user in mm/s, e.g. Vo=5mm/s. It is interpolated by a t factor [0-->1](a function of time), which I compute using the first derivative of a 5th order polynomial, to provide smooth acceleration, deceleration and zero final velocity.
Then:
Vx= (Vo*Px/MAG_P)*t
Vy= (Vo*Py/MAG_P)*t
Vz= (Vo*Pz/MAG_P)*t
I use the velocities above to multiply with the 6x6 inverse jacobian the result are the joint velocities, with no orientation change of the robot end-effector. Everything works fine. The robot reaches its goal position as expected following an expected linear path.
Now I going to the main topic of my question. I want to interpolate the orientation velocities as well [w1, w2, w3].
I have done a lot of research and reading and still could not figure out. I learned that I can't just subtract the orientation angles e.g. [T1f - T1i] and divide by total time to get velocities.
I found that a common approach is to use SLERP interpolation, with quaternions and a t [0-->1] interpolation factor as input parameters. I convert my [T1i, T2i, T3i] and [T1f, T2f, T3f] to quaternions, qI and qF, and use the 5th order t interpolation factor computed above. The result is a quaternion representing the orientation at a given time, not velocities.
Every scan my system computes new t value factor(that progresses over time) which I use with SLERP to give me the next computed orientation:
qN=SLERP(qI,qF,t), which is the next interpolated orientation.
I compute the current end-effector orientation in quaternion: qC, and subtract qN - qC to give me the displacement to use as orientation velocities.
qV = qN - qC (doing proper quaternion subtraction = (qc)-1*qN)
Then I convert qV to ZYZ angles in radians:
[w1,w2,w3] = TO_ZYZ_FROM_QUATERNION(qV).
And use the above orientation velocities together with the cartesian velocities to multiple with the (J)-1.
Unfortunately, this approach does not work. The robot does not interpolate correctly to the final goal position.
I have read a lot of literature and posts approaching this same topic:
Controlling the pose of a manipulator using inverse kinematics
Jacobian-based trajectory following
Inverse kinematic orientation problem
Inverse Kinematics with Orientations
I would appreciate it if someone could point me in the right direction. I would like to stick with this approach using the inverse Jacobian and not use IK.
Note: My system is a PLC(industrial programmable controller)and there are no built-in complex math libraries. So approaches using simpler math functions would be better. All of the main math functions had to be implemented by me, e.g matrix dot product, matrix inverse, transpose, quaternion conversion, SLERP algorithm, ZYZ conversion, Jacobian inverse, etc. I have tested my functions comparing the results with online tools and I'm confident my implementations are working correctly.
Below I illustrate my problem:
