Finally got the model matching the forward kinematics, and giving
reasonable velocity outputs. In reference to Issue #14.