Switch from MatrixXd to Data::Matrix6x for holding jacobians

src/algorithm/jacobian.hpp

@@ -53,7 +53,7 @@ namespace se3
   void getJacobian(const Model & model,
                    const Data & data,
                    Model::Index jointId,
-                   Eigen::MatrixXd & J);
+                   Data::Matrix6x & J);
   
   ///
   /// \brief Computes the jacobian of a specific joint frame expressed in the local frame of the joint. The result is stored in data.J.

src/algorithm/jacobian.hxx

@@ -77,7 +77,7 @@ namespace se3
   void getJacobian(const Model & model,
                    const Data & data,
                    Model::Index jointId,
-                   Eigen::MatrixXd & J)
+                   Data::Matrix6x & J)
   {
     assert( J.rows() == data.J.rows() );
     assert( J.cols() == data.J.cols() );

src/algorithm/operational-frames.hpp

@@ -109,7 +109,7 @@ inline void  framesForwardKinematic(const Model & model,
 
 template<bool localFrame>
 inline void getFrameJacobian(const Model & model, const Data& data,
-     Model::Index frame_id, Eigen::MatrixXd & J)
+     Model::Index frame_id, Data::Matrix6x & J)
 {
   assert( J.rows() == data.J.rows() );
   assert( J.cols() == data.J.cols() );

src/python/algorithms.hpp

@@ -92,14 +92,14 @@ namespace se3
            const VectorXd_fx & q )
       { return jacobianCenterOfMass(*model,*data,q); }
 
-      static Eigen::MatrixXd jacobian_proxy( const ModelHandler& model, 
+      static Data::Matrix6x jacobian_proxy( const ModelHandler& model, 
                DataHandler & data,
                const VectorXd_fx & q,
                Model::Index jointId,
                bool local,
 							 bool update_geometry )
       {
-  Eigen::MatrixXd J( 6,model->nv ); J.setZero();
+  Data::Matrix6x J( 6,model->nv ); J.setZero();
 	if (update_geometry)
   	computeJacobians( *model,*data,q );
   if(local) getJacobian<true> (*model, *data, jointId, J);
@@ -115,7 +115,7 @@ namespace se3
                                                         bool update_geometry
                                                         )
       {
-        Eigen::MatrixXd J( 6,model->nv ); J.setZero();
+        Data::Matrix6x J( 6,model->nv ); J.setZero();
 
         if (update_geometry)
           computeJacobians( *model,*data,q );

unittest/jacobian.cpp

@@ -44,7 +44,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
   computeJacobians(model,data,q);
 
   Model::Index idx = model.existBodyName("rarm2")?model.getBodyId("rarm2"):(Model::Index)(model.nbody-1); 
-  MatrixXd Jrh(6,model.nv); Jrh.fill(0);
+  Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
   getJacobian<false>(model,data,idx,Jrh);
 
    /* Test J*q == v */
@@ -56,9 +56,9 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
 
 
   /* Test local jacobian: rhJrh == rhXo oJrh */ 
-  MatrixXd rhJrh(6,model.nv); rhJrh.fill(0);
+  Data::Matrix6x rhJrh(6,model.nv); rhJrh.fill(0);
   getJacobian<true>(model,data,idx,rhJrh);
-  MatrixXd XJrh(6,model.nv); 
+  Data::Matrix6x XJrh(6,model.nv); 
   motionSet::se3Action( data.oMi[idx].inverse(), Jrh,XJrh );
   BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));
 
@@ -111,7 +111,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
   {
     computeJacobians(model,data,q);
     Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1); 
-    Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
+    Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
 
     timer.tic();
     SMOOTH(NBT)
@@ -126,7 +126,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
   {
     computeJacobians(model,data,q);
     Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1); 
-    Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
+    Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
 
     timer.tic();
     SMOOTH(NBT)
@@ -141,7 +141,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
   {
     computeJacobians(model,data,q);
     Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1); 
-    Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
+    Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
 
     timer.tic();
     SMOOTH(NBT)

unittest/operational-frames.cpp

@@ -79,8 +79,8 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
 
   /// In global frame
 
-  MatrixXd Joj(6,model.nv); Joj.fill(0);
-  MatrixXd Jof(6,model.nv); Jof.fill(0);
+  Data::Matrix6x Joj(6,model.nv); Joj.fill(0);
+  Data::Matrix6x Jof(6,model.nv); Jof.fill(0);
   Model::Index idx = model.getFrameId(frame_name);
 
   getFrameJacobian<false>(model,data,idx,Jof);
@@ -99,8 +99,8 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
 
   /// In local frame
 
-  MatrixXd Jjj(6,model.nv); Jjj.fill(0);
-  MatrixXd Jff(6,model.nv); Jff.fill(0);
+  Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
+  Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
   getFrameJacobian<true>(model,data,idx,Jff);
   getJacobian<true>(model, data, parent_idx, Jjj);