Working on RigidBodyTransformTest. Need to remove dependence on EJML.
Adding more unit tests.
Have RigidBodyTransform tests passing.