[C++] Fix bug in JointModelFreeFlyer and JointModelSpherical

* forwardKinematics takes a vector of size NQ as input.

src/multibody/joint/joint-free-flyer.hpp

@@ -208,11 +208,11 @@ namespace se3
     template<typename V>
     inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<V> & q_joint) const
     {
+      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,V);
       using std::sqrt;
       typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
-      typename Eigen::MatrixBase<V>::template ConstFixedSegmentReturnType<NQ>::Type & q = q_joint.template segment<NQ> (idx_q ());
 
-      ConstQuaternionMap_t quat(q.template tail<4>().data());
+      ConstQuaternionMap_t quat(q_joint.template tail<4>().data());
       assert(std::fabs(quat.coeffs().norm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon()));
       
       M.rotation(quat.matrix());

src/multibody/joint/joint-spherical.hpp

@@ -271,10 +271,10 @@ namespace se3
     template<typename V>
     inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<V> & q_joint) const
     {
+      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,V);
       using std::sqrt;
-      typename Eigen::MatrixBase<V>::template ConstFixedSegmentReturnType<NQ>::Type & q = q_joint.template segment<NQ> (idx_q ());
 
-      ConstQuaternionMap_t quat(q.data());
+      ConstQuaternionMap_t quat(q_joint.derived().data());
       assert(std::fabs(quat.coeffs().norm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon()));
       
       M.rotation(quat.matrix());