Table of Contents
Lynx-6: Inverse Kinematics
Using the link lengths of your manipulator as defined in its forward kinematics, work out the inverse kinematics for the Lynx-6 end-effector. That is, given a desired end-effector configuration $g_e^* \in SE(3)$, figure out a joint configuration $\vec \alpha$ that places the manipulator end-effector there if it is possible to do so. If it is not, then indicate as much.
Dealing with Kinematic Insufficiency
The Lynx-6 manipulator is not fully dextrous, so many end-effector configurations are simply not possible. The first logical test is whether the wrist is achievable or not. The next sensible test deals with the orientation at the wrist: the Lynx-6 cannot arbitrarily reorient. Still work out the solution as though the wrist does have full $SO(3)$ control. When you compute the rotation matrix of achievable orientations, you will get a special form.
Explain what that special form is and describe what test should be performed to verify that the desired orientation is realizable. Incorporate this test into the inverse kinematics so that a failure flag is returned if it is not possible.
Checking Validity
Validity of the results can be done by applying the forward kinematics routine to the found joint angles. It should return the configuration requested.
A more fun option would be to use the lynx6 display code to see if it really works out. To visualize the target end-effector configuration, plot it using the $SE(3)$ class, then see if the end-effector really ends up there.
Debugging
To debug your program for the inverse kinematics, it helps a lot to first compute the forward kinematics for easy angles. Things like the zero configuration (your output should be the zero-vector for $\alpha$). Then set $\alpha_1$ to something easy, like 45 or 30 degrees. After that move on to $\alpha_3$ or some other joint that does sensible things, and use easy angles (like $\pm$90 degrees).
Position Inverse Kinematics
Not Required
An extra bonus would be to check if the position can be achieved even if the orientation cannot. This requires identifying yet a different test that would indicate the hand can be set to point at the desired position even if the orientation cannot be achieved. This one is a bit trickier.