Decide to write RelativeConstraint using quaternion. Using homogeneous transformation requires writing several utility functions. This should not be the bottleneck of speed