Forward and inverse kinematics from "Redundancy Parameterization of the ABB YuMi Robot Arm."
- Forward kinematics, including calculation of the SEW angle (i.e., arm angle)
- Inverse kinematics using 2D search
- Calculation of the Jacobian, including the 6x7 Jacobian and the 1x7 SEW Jacobian, which combine to form the 7x7 augmented Jacobian