I am trying to make an analytical inverse kinematics solver for the Fanuc CRX-10iA robot. I already made such a solver for "normal" 6-axis robots with spherical wrists (spherical = axis 4-5-6 intersect in the same point), and for UR-style robots.
For robots with spherical wrist, I use the flange->wrist offset to determine the location of the center of the wrist. The A1, A2, and A3 angles can then be found using a little geometry. A4, A5 and A6 angles are found using ZYZ Euler angles.
The UR robot does not have a spherical wrist, but I know that A4 is parallel with A2 and that both A4 and A6 are perpendicular to A5. So I can find the A5-A6 intersection, calculate an initial angle to that one (first guess for A1), compensate for the A4 offset to get the correct A1 angle, then subtract the A4 offset to find the intersection between A4-A5. This point is then used for the formulas to derive the A1-A3 angles, and the A4-A5-A6 angles are derived using ZYZ Euler angles like I did for the robots with spherical wrists.
But now I want to make an analytical solver for the Fanuc CRX-10iA robot. It is in many ways like a robot with a spherical wrist, only the wrist axes do not all intersect in the same point. See attached picture. Do any of you have a good idea for how to solve this using geometry? I know that I can find a solution using the jacobian method, but I want to use the analytical solver to find a good starting guess for the jacobian solver.
Thanks in advance!