Use SpaceVecAlg template version.
Template Joint::Pose and QuatToE method.
Fix some conversion issue.