<commit_before>#ifndef __se3_jacobian_hpp__
#define __se3_jacobian_hpp__

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

} // namespace se3 

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

    JOINT_VISITOR_INIT(JacobiansForwardStep);

    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 Model::Index & i = jmodel.id();
      const Model::Index & parent = model.parents[i];

      jmodel.calc(jdata.derived(),q);
      
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
      else         data.oMi[i] = data.liMi[i];

      data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.oMi[i].act(jdata.S());
    }

  };


  inline const Eigen::MatrixXd&
  computeJacobians(const Model & model, Data& data,
		   const Eigen::VectorXd & q)
  {
    for( int i=1;i<model.nbody;++i )
      {
	JacobiansForwardStep::run(model.joints[i],data.joints[i],
				  JacobiansForwardStep::ArgsType(model,data,q));
      }

    return data.J;
  }

  template<bool localFrame>
  void getJacobian(const Model & model, const Data& data,
		   Model::Index jointId, Eigen::MatrixXd & J)
  {
    assert( J.rows() == data.J.rows() );
    assert( J.cols() == data.J.cols() );

    const SE3 & oMjoint = data.oMi[jointId];
    int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
    for(int j=colRef;j>=0;j=data.parents_fromRow[j])
      {
	if(! localFrame )   J.col(j) = data.J.col(j);
	else                J.col(j) = oMjoint.actInv(Motion(data.J.col(j))).toVector();
      }
  }


  struct JacobianForwardStep : public fusion::JointVisitor<JacobianForwardStep>
  {
    typedef boost::fusion::vector< const se3::Model&,
				   se3::Data&,
				   const Eigen::VectorXd &
				   > ArgsType;

    JOINT_VISITOR_INIT(JacobianForwardStep);

    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 Model::Index & i = jmodel.id();
      const Model::Index & parent = model.parents[i];

      jmodel.calc(jdata.derived(),q);
      
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      data.iMf[parent] = data.liMi[i]*data.iMf[i];

      data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.iMf[i].inverse().act(jdata.S());
    }

  };

  /* Compute the jacobian in the local frame. */
  const Eigen::MatrixXd&
  jacobian(const Model & model, Data& data,
	   const Eigen::VectorXd & q,
	   const Model::Index & idx )
  {
    data.iMf[idx] = SE3::Identity();
    for( int i=idx;i>0;i=model.parents[i] )<commit_msg>Added documentation to the jacobian algo.<commit_after>#ifndef __se3_jacobian_hpp__
#define __se3_jacobian_hpp__

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

} // namespace se3 

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

    JOINT_VISITOR_INIT(JacobiansForwardStep);

    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 Model::Index & i = jmodel.id();
      const Model::Index & parent = model.parents[i];

      jmodel.calc(jdata.derived(),q);
      
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
      else         data.oMi[i] = data.liMi[i];

      data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.oMi[i].act(jdata.S());
    }

  };


  inline const Eigen::MatrixXd&
  computeJacobians(const Model & model, Data& data,
		   const Eigen::VectorXd & q)
  {
    for( int i=1;i<model.nbody;++i )
      {
	JacobiansForwardStep::run(model.joints[i],data.joints[i],
				  JacobiansForwardStep::ArgsType(model,data,q));
      }

    return data.J;
  }

  /* Return the jacobian of the output frame attached to joint <jointId> in the
     world frame or in the local frame depending on the template argument. The
     function computeJacobians should have been called first. */
  template<bool localFrame>
  void getJacobian(const Model & model, const Data& data,
		   Model::Index jointId, Eigen::MatrixXd & J)
  {
    assert( J.rows() == data.J.rows() );
    assert( J.cols() == data.J.cols() );

    const SE3 & oMjoint = data.oMi[jointId];
    int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
    for(int j=colRef;j>=0;j=data.parents_fromRow[j])
      {
	if(! localFrame )   J.col(j) = data.J.col(j);
	else                J.col(j) = oMjoint.actInv(Motion(data.J.col(j))).toVector();
      }
  }


  struct JacobianForwardStep : public fusion::JointVisitor<JacobianForwardStep>
  {
    typedef boost::fusion::vector< const se3::Model&,
				   se3::Data&,
				   const Eigen::VectorXd &
				   > ArgsType;

    JOINT_VISITOR_INIT(JacobianForwardStep);

    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 Model::Index & i = jmodel.id();
      const Model::Index & parent = model.parents[i];

      jmodel.calc(jdata.derived(),q);
      
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      data.iMf[parent] = data.liMi[i]*data.iMf[i];

      data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.iMf[i].inverse().act(jdata.S());
    }

  };

  /* Compute the jacobian of the output frame of the joint <idx> in the local frame. */
  const Eigen::MatrixXd&
  jacobian(const Model & model, Data& data,
	   const Eigen::VectorXd & q,
	   const Model::Index & idx )
  {
    data.iMf[idx] = SE3::Identity();
    for( int i=idx;i>0;i=model.parents[i] )
{'fraction_non_alphanumeric': 0.16372105767913414, 'fraction_numerical': 0.008220304151253596, 'mean_word_length': 2.9037433155080214, 'pattern_counts': {'":': 0, '<': 31, '<?xml version=': 0, '>': 35, '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': '2614421', 'n_tokens_mistral': 2800, 'n_tokens_neox': 2538, 'n_words': 474}