Add forward kinematics, too.

Fix the solver! (Reverse x,y in base due to angle semantics, fix cart2polar).