# Path Planning for Spherical Wrist (RRR)

I have found the inverse kinematics of a spherical wrist. The wrist is not attached to any other type of arm, it is the sole controller. Based on this IK solution, how can I generate a path plan for my robot?

As far as I understand, its motion is restricted to a spherical surface.

I have seen examples for path planning for linear motion using parametric equations, but not for spherical motion.

Note that this is the Forward Jacobian

The J-1 Solution from MATLAB is written below.

    J_inv =

[                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           (cos(theta1)*cos(theta2))/(sin(theta2)*cos(theta1)^2 + sin(theta2)*sin(theta1)^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           (cos(theta2)*sin(theta1))/(sin(theta2)*cos(theta1)^2 + sin(theta2)*sin(theta1)^2),                                                                                                                                                                                                                                                                                                         1]
[                                                                                                                                           (sin(conj(theta1))*(cos(conj(theta1))*cos(theta1) + cos(conj(theta1))*cos(conj(theta2))*cos(theta1)*cos(theta2) + sin(conj(theta1))*sin(conj(theta2))*sin(theta1)*sin(theta2)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2) - (cos(conj(theta1))*(sin(conj(theta1))*cos(theta1) + cos(conj(theta2))*sin(conj(theta1))*cos(theta1)*cos(theta2) - cos(conj(theta1))*sin(conj(theta2))*sin(theta1)*sin(theta2)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2),                                                                                                                                           (sin(conj(theta1))*(cos(conj(theta1))*sin(theta1) + cos(conj(theta1))*cos(conj(theta2))*cos(theta2)*sin(theta1) - sin(conj(theta1))*sin(conj(theta2))*cos(theta1)*sin(theta2)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2) - (cos(conj(theta1))*(sin(conj(theta1))*sin(theta1) + cos(conj(theta1))*sin(conj(theta2))*cos(theta1)*sin(theta2) + cos(conj(theta2))*sin(conj(theta1))*cos(theta2)*sin(theta1)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2),                                                                                                                                                                                                                                                                                                         0]
[ (cos(conj(theta1))*sin(conj(theta2))*(cos(conj(theta1))*cos(theta1) + cos(conj(theta1))*cos(conj(theta2))*cos(theta1)*cos(theta2) + sin(conj(theta1))*sin(conj(theta2))*sin(theta1)*sin(theta2)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2) - (cos(conj(theta2))*cos(theta1)*cos(theta2))/(sin(theta2)*cos(theta1)^2 + sin(theta2)*sin(theta1)^2) + (sin(conj(theta1))*sin(conj(theta2))*(sin(conj(theta1))*cos(theta1) + cos(conj(theta2))*sin(conj(theta1))*cos(theta1)*cos(theta2) - cos(conj(theta1))*sin(conj(theta2))*sin(theta1)*sin(theta2)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2), (cos(conj(theta1))*sin(conj(theta2))*(cos(conj(theta1))*sin(theta1) + cos(conj(theta1))*cos(conj(theta2))*cos(theta2)*sin(theta1) - sin(conj(theta1))*sin(conj(theta2))*cos(theta1)*sin(theta2)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2) - (cos(conj(theta2))*cos(theta2)*sin(theta1))/(sin(theta2)*cos(theta1)^2 + sin(theta2)*sin(theta1)^2) + (sin(conj(theta1))*sin(conj(theta2))*(sin(conj(theta1))*sin(theta1) + cos(conj(theta1))*sin(conj(theta2))*cos(theta1)*sin(theta2) + cos(conj(theta2))*sin(conj(theta1))*cos(theta2)*sin(theta1)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2), (cos(conj(theta1))^2*cos(conj(theta2))*sin(conj(theta2)))/(sin(conj(theta2))*cos(conj(theta1))^2 + sin(conj(theta2))*sin(conj(theta1))^2) - cos(conj(theta2)) + (cos(conj(theta2))*sin(conj(theta1))^2*sin(conj(theta2)))/(sin(conj(theta2))*cos(conj(theta1))^2 + sin(conj(theta2))*sin(conj(theta1))^2)]

>>

• Use simplify to present a condensed version of your Matlab expression – Akshay Kumar Feb 8 at 18:52
• Can you clarify whether you mean point to point movement (where a quaternion slerp is the common approach) or are you really asking about path planning and approaches to finding paths through obstacles on the manifold? – hauptmech Feb 9 at 21:59
• I am asking about point to point movement – Maurice Rahme Feb 10 at 22:38