all: use RowMatrixXs instead of RowMatrixXd

bindings/python/algorithm/expose-aba.cpp

@@ -23,7 +23,7 @@ namespace se3
   namespace python
   {
     
-    const Data::RowMatrixXd &
+    const Data::RowMatrixXs &
     computeMinverse_proxy(const Model & model, Data & data, const Eigen::VectorXd & q)
     {
       computeMinverse(model,data,q);

bindings/python/multibody/data.hpp

@@ -76,7 +76,7 @@ namespace se3
         .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,ddq,"Joint accelerations (output of ABA)")
         .ADD_DATA_PROPERTY(container::aligned_vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body")
         .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,M,"The joint space inertia matrix")
-        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Data::RowMatrixXd,Minv,"The inverse of the joint space inertia matrix")
+        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Data::RowMatrixXs,Minv,"The inverse of the joint space inertia matrix")
         .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,C,"The Coriolis C(q,v) matrix such that the Coriolis effects are given by c(q,v) = C(q,v)v")
         .ADD_DATA_PROPERTY(container::aligned_vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
         .ADD_DATA_PROPERTY(std::vector<int>,lastChild,"Index of the last child (for CRBA)")
@@ -134,7 +134,7 @@ namespace se3
         bp::class_< std::vector<int> >("StdVec_int")
         .def(bp::vector_indexing_suite< std::vector<int> >());
         
-        eigenpy::enableEigenPySpecific<Data::RowMatrixXd>();
+        eigenpy::enableEigenPySpecific<Data::RowMatrixXs>();
       }
 
     };

src/algorithm/aba.hpp

@@ -88,7 +88,7 @@ namespace se3
   /// \return The inverse of the joint space inertia matrix stored in data.ddq.
   ///
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
-  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::RowMatrixXd &
+  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::RowMatrixXs &
   computeMinverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                   DataTpl<Scalar,Options,JointCollectionTpl> & data,
                   const Eigen::MatrixBase<ConfigVectorType> & q);

src/algorithm/aba.hxx

@@ -377,7 +377,7 @@ namespace se3
       const JointIndex & parent  = model.parents[i];
       
       typename Inertia::Matrix6 & Ia = data.Yaba[i];
-      typename Data::RowMatrixXd & Minv = data.Minv;
+      typename Data::RowMatrixXs & Minv = data.Minv;
       typename Data::Matrix6x & Fcrb = data.Fcrb[0];
       typename Data::Matrix6x & FcrbTmp = data.Fcrb.back();
 
@@ -437,7 +437,7 @@ namespace se3
       
       const JointIndex & i = jmodel.id();
       const JointIndex & parent = model.parents[i];
-      typename Data::RowMatrixXd & Minv = data.Minv;
+      typename Data::RowMatrixXs & Minv = data.Minv;
       typename Data::Matrix6x & FcrbTmp = data.Fcrb.back();
       
       typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
@@ -461,7 +461,7 @@ namespace se3
   };
 
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
-  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::RowMatrixXd &
+  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::RowMatrixXs &
   computeMinverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                   DataTpl<Scalar,Options,JointCollectionTpl> & data,
                   const Eigen::MatrixBase<ConfigVectorType> & q)

src/multibody/data.hpp

@@ -82,7 +82,7 @@ namespace se3
     typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
     
     typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
-    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXd;
+    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXs;
     
     /// \brief Vector of se3::JointData associated to the se3::JointModel stored in model, 
     /// encapsulated in JointDataAccessor.
@@ -150,7 +150,7 @@ namespace se3
     MatrixXs M;
     
     /// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
-    RowMatrixXd Minv;
+    RowMatrixXs Minv;
     
     /// \brief The Coriolis matrix (a square matrix of dim model.nv).
     MatrixXs C;

unittest/aba-derivatives.cpp

@@ -54,7 +54,7 @@ BOOST_AUTO_TEST_CASE(test_aba_derivatives)
   
   MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
   MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
-  Data::RowMatrixXd aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
+  Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
   
   computeABADerivatives(model, data, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
   computeRNEADerivatives(model,data_ref,q,v,a);
@@ -152,7 +152,7 @@ BOOST_AUTO_TEST_CASE(test_aba_minimal_argument)
   
   MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
   MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
-  Data::RowMatrixXd aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
+  Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
   
   computeABADerivatives(model, data_ref, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
   
@@ -194,7 +194,7 @@ BOOST_AUTO_TEST_CASE(test_aba_derivatives_fext)
   
   MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
   MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
-  Data::RowMatrixXd aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
+  Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
   
   computeABADerivatives(model, data, q, v, tau, fext,
                         aba_partial_dq, aba_partial_dv, aba_partial_dtau);