Change the prototype of CRBA::groupAct.

src/algorithm/crba.hpp

@@ -35,7 +35,7 @@ namespace se3
       using namespace Eigen;
       using namespace se3;
 
-      const int & i = jmodel.id();
+      const typename JointModel::Index & i = jmodel.id();
       jmodel.calc(jdata.derived(),q);
       
       data.liMi[i] = model.jointPlacements[i]*jdata.M();
@@ -46,58 +46,70 @@ namespace se3
 
   namespace internal 
   {
-    /* Compute jF = jXi * iF, where jXi is the dual action matrix associated with m,
-     * and iF, jF are vectors. */
-    template<typename D,typename Dret>
-    static void actVec( const SE3 & m, 
-			   const Eigen::MatrixBase<D> & iF,
-			   Eigen::MatrixBase<Dret> & jF )
-    { 
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(D);
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Dret);
-      Eigen::VectorBlock<const D,3> linear = iF.template head<3>();
-      Eigen::VectorBlock<const D,3> angular = iF.template tail<3>();
+
+    template<typename Mat,typename MatRet, int NCOLS>
+    struct ForceSetSe3Action
+    {
+      /* Compute jF = jXi * iF, where jXi is the dual action matrix associated
+       * with m, and iF, jF are matrices whose columns are forces. The resolution
+       * is done by block operation. It is less efficient than the colwise
+       * operation and should not be used. */ 
+      static void run( const SE3 & m, 
+		       const Eigen::MatrixBase<Mat> & iF,
+		       Eigen::MatrixBase<MatRet> & jF );
+      // {
+      //   typename Mat::template ConstNRowsBlockXpr<3>::Type linear  = iF.template topRows<3>();
+      //   typename MatRet::template ConstNRowsBlockXpr<3>::Type angular = iF.template bottomRows<3>();
+      
+      //   jF.template topRows   <3>().noalias() = m.rotation()*linear;
+      //   jF.template bottomRows<3>().noalias()
+      // 	= skew(m.translation())*jF.template topRows<3>() +
+      //      m.rotation()*angular;
+      // }
       
-      jF.template head <3>() = m.rotation()*linear;
-      jF.template tail <3>() = (m.translation().cross(jF.template head<3>())
-				+ m.rotation()*angular);
+    };
+
+    template<typename Mat,typename MatRet>
+    struct ForceSetSe3Action<Mat,MatRet,1>
+    {
+      /* Compute jF = jXi * iF, where jXi is the dual action matrix associated with m,
+       * and iF, jF are vectors. */
+      static void run( const SE3 & m, 
+		       const Eigen::MatrixBase<Mat> & iF,
+		       Eigen::MatrixBase<MatRet> & jF )
+      { 
+	EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
+	EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet);
+	Eigen::VectorBlock<const Mat,3> linear = iF.template head<3>();
+	Eigen::VectorBlock<const Mat,3> angular = iF.template tail<3>();
+	
+	jF.template head <3>() = m.rotation()*linear;
+	jF.template tail <3>() = (m.translation().cross(jF.template head<3>())
+				  + m.rotation()*angular);
+      }
+    };
+
+    template<typename Mat,typename MatRet>
+    static void se3Action( const SE3 & m, 
+			   const Eigen::MatrixBase<Mat> & iF,
+			   Eigen::MatrixBase<MatRet> & jF )
+    {
+      ForceSetSe3Action<Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iF,jF);
     }
 
-    /* Compute jF = jXi * iF, where jXi is the dual action matrix associated
-     * with m, and iF, jF are matrices whose columns are forces. The resolution
-     * is done column by column. */
-    template<typename D,typename Dret>
-    static void
-    act_alt( const SE3 & m, 
-	     const Eigen::MatrixBase<D> & iF,
-	     Eigen::MatrixBase<Dret> & jF )
+    template<typename Mat,typename MatRet,int NCOLS>
+    void ForceSetSe3Action<Mat,MatRet,NCOLS>::
+    run( const SE3 & m, 
+	 const Eigen::MatrixBase<Mat> & iF,
+	 Eigen::MatrixBase<MatRet> & jF )
     {
       for(int col=0;col<jF.cols();++col) 
 	{
-	  typename Dret::ColXpr jFc = jF.col(col);
-	  actVec(m,iF.col(col),jFc);
+	  typename MatRet::ColXpr jFc = jF.col(col);
+	  se3Action(m,iF.col(col),jFc);
 	}
     }
     
-    /* Compute jF = jXi * iF, where jXi is the dual action matrix associated
-     * with m, and iF, jF are matrices whose columns are forces. The resolution
-     * is done by block operation. It is less efficient than the colwise
-     * operation and should not be used. */
-    template<typename D,typename Dret>
-    static void
-    act( const SE3 & m, 
-	 const Eigen::MatrixBase<D> & iF,
-	 Eigen::MatrixBase<Dret> & jF )
-    {
-      typename D::template ConstNRowsBlockXpr<3>::Type linear  = iF.template topRows<3>();
-      typename D::template ConstNRowsBlockXpr<3>::Type angular = iF.template bottomRows<3>();
-      
-      jF.template topRows   <3>().noalias() = m.rotation()*linear;
-      jF.template bottomRows<3>().noalias()
-	= skew(m.translation())*jF.template topRows<3>() +
-         m.rotation()*angular;
-    }
-    
   } // namespace internal
 
   struct CrbaBackwardStep : public fusion::JointVisitor<CrbaBackwardStep>
@@ -120,7 +132,7 @@ namespace se3
        *   Yli += liXi Yi
        *   F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
        */
-      const int & i = jmodel.id();
+      const Model::Index & i = jmodel.id();
 
       /* F[1:6,i] = Y*S */
       data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
@@ -138,9 +150,9 @@ namespace se3
 	  /*   F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
 	  Eigen::Block<typename Data::Matrix6x> jF
 	    = data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
- 	  internal::act_alt(data.liMi[i],
-			data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]),
-			jF);
+ 	  internal::se3Action(data.liMi[i],
+			      data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]),
+			      jF);
 	}
       
       // std::cout << "iYi = " << (Inertia::Matrix6)data.Ycrb[i] << std::endl;

unittest/crba.cpp

@@ -24,12 +24,9 @@ int main(int argc, const char ** argv)
   _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
 #endif
 
-
   using namespace Eigen;
   using namespace se3;
 
-  SE3::Matrix3 I3 = SE3::Matrix3::Identity();
-
   se3::Model model;
 
   std::string filename = "/home/nmansard/src/metapod/data/simple_arm.urdf";