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