<commit_before>#ifndef __se3_crba2_hpp__
#define __se3_crba_hpp__

#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include <iostream>
  
namespace se3
{
  inline const Eigen::MatrixXd&
  crba(const Model & model, Data& data,
       const Eigen::VectorXd & q);

} // namespace se3 

/* --- Details -------------------------------------------------------------------- */
namespace se3 
{
  struct CrbaForwardStep : public fusion::JointVisitor<CrbaForwardStep>
  {
    typedef boost::fusion::vector< const se3::Model&,
				   se3::Data&,
				   const Eigen::VectorXd &
				   > ArgsType;

    JOINT_VISITOR_INIT(CrbaForwardStep);

    template<typename JointModel>
    static void algo(const se3::JointModelBase<JointModel> & jmodel,
		     se3::JointDataBase<typename JointModel::JointData> & jdata,
		     const se3::Model& model,
		     se3::Data& data,
		     const Eigen::VectorXd & q)
    {
      using namespace Eigen;
      using namespace se3;

      const typename JointModel::Index & i = jmodel.id();
      jmodel.calc(jdata.derived(),q);
      
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      data.Ycrb[i] = model.inertias[i];
    }

  };

  namespace internal 
  {

    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;
      // }
      
    };

    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);
    }

    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 MatRet::ColXpr jFc = jF.col(col);
	  se3Action(m,iF.col(col),jFc);
	}
    }
    
  } // namespace internal

  struct CrbaBackwardStep : public fusion::JointVisitor<CrbaBackwardStep>
  {
    typedef boost::fusion::vector<const Model&,
				  Data&>  ArgsType;
    
    JOINT_VISITOR_INIT(CrbaBackwardStep);

    template<typename JointModel>
    static void algo(const JointModelBase<JointModel> & jmodel,
		     JointDataBase<typename JointModel::JointData> & jdata,
		     const Model& model,
		     Data& data)
    {
      /*
       * F[1:6,i] = Y*S
       * M[i,SUBTREE] = S'*F[1:6,SUBTREE]
       * if li>0 
       *   Yli += liXi Yi
       *   F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
       */
      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();

      /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
      data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]) 
	= jdata.S().transpose()*data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);

      const Model::Index & parent   = model.parents[i];
      if(parent>0) 
	{
	  /*   Yli += liXi Yi */
 	  data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);

	  /*   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::se3Action(data.liMi[i],
			      data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]),
			      jF);
	}
      <commit_msg>Modified CRBA to link properly with the new act-on-set. Timings untouched (4.5us).<commit_after>#ifndef __se3_crba_hpp__
#define __se3_crba_hpp__

#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/spatial/act-on-set.hpp"

#include <iostream>
  
namespace se3
{
  inline const Eigen::MatrixXd&
  crba(const Model & model, Data& data,
       const Eigen::VectorXd & q);

} // namespace se3 

/* --- Details -------------------------------------------------------------------- */
namespace se3 
{
  struct CrbaForwardStep : public fusion::JointVisitor<CrbaForwardStep>
  {
    typedef boost::fusion::vector< const se3::Model&,
				   se3::Data&,
				   const Eigen::VectorXd &
				   > ArgsType;

    JOINT_VISITOR_INIT(CrbaForwardStep);

    template<typename JointModel>
    static void algo(const se3::JointModelBase<JointModel> & jmodel,
		     se3::JointDataBase<typename JointModel::JointData> & jdata,
		     const se3::Model& model,
		     se3::Data& data,
		     const Eigen::VectorXd & q)
    {
      using namespace Eigen;
      using namespace se3;

      const typename JointModel::Index & i = jmodel.id();
      jmodel.calc(jdata.derived(),q);
      
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      data.Ycrb[i] = model.inertias[i];
    }

  };

  struct CrbaBackwardStep : public fusion::JointVisitor<CrbaBackwardStep>
  {
    typedef boost::fusion::vector<const Model&,
				  Data&>  ArgsType;
    
    JOINT_VISITOR_INIT(CrbaBackwardStep);

    template<typename JointModel>
    static void algo(const JointModelBase<JointModel> & jmodel,
		     JointDataBase<typename JointModel::JointData> & jdata,
		     const Model& model,
		     Data& data)
    {
      /*
       * F[1:6,i] = Y*S
       * M[i,SUBTREE] = S'*F[1:6,SUBTREE]
       * if li>0 
       *   Yli += liXi Yi
       *   F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
       */
      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();

      /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
      data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]) 
	= jdata.S().transpose()*data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);

      const Model::Index & parent   = model.parents[i];
      if(parent>0) 
	{
	  /*   Yli += liXi Yi */
 	  data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);

	  /*   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]);
 	  forceSet::se3Action(data.liMi[i],
			      data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]),
			      jF);
	}
      
{'fraction_non_alphanumeric': 0.16761880031160736, 'fraction_numerical': 0.014152168267982343, 'mean_word_length': 2.7833988212180745, 'pattern_counts': {'":': 0, '<': 59, '<?xml version=': 0, '>': 62, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7211831', 'n_tokens_mistral': 3027, 'n_tokens_neox': 2797, 'n_words': 483}