wrote a forward kinematics matrixy thing with baddu and a small test driver