Forward and Inverse kinematics are now being correctly calculated.

Until now I could tell the motors to move to a given angle. The closed feedback loop and a PID controller would drive the motors to their destination. There were two things missing to make this great.

Forward kinematics says "take the angles of the joints and the measurements of the parts and find the XYZ coordinates of the tool tip." I got that in a few hours by hacking the graphics code to give me visual representations of my math as I stumbled through the problem.

Inverse kinematics says "I want the tool tip at XYZ. What angles do I need at each joint?" Now this is especially tricky, because there are lots of XYZs that are simply not valid - further than the arm can reach is a good example. Inside itself might be another. Again, using the graphics system to help me see what I was doing wrong let me work the problem out one step at a time.

Now I will be able to say "drive the finger tip to XYZ, then drive in a straight line to a different XYZ." From there I can drive in arcs and implement the rest of the normal GCODE you may have seen in 3D printers and other CNC machines.