Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/algorithm/jacobian.hxx	Lines:	116	118	98.3 %
Date:	2024-01-23 21:41:47	Branches:	103	451	22.8 %


//
// Copyright (c) 2015-2020 CNRS INRIA
//

#ifndef __pinocchio_jacobian_hxx__
#define __pinocchio_jacobian_hxx__

#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/algorithm/check.hpp"

/// @cond DEV

namespace pinocchio
{

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
  struct JointJacobiansForwardStep
  : public fusion::JointUnaryVisitorBase< JointJacobiansForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> >
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typedef boost::fusion::vector<const Model &,
                                  Data &,
                                  const ConfigVectorType &,
                                  Matrix6xLike &> ArgsType;

    template<typename JointModel>
    static void algo(const JointModelBase<JointModel> & jmodel,
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const Model & model,
                     Data & data,
                     const Eigen::MatrixBase<ConfigVectorType> & q,
                     const Eigen::MatrixBase<Matrix6xLike> & J)
    {
      typedef typename Model::JointIndex JointIndex;

      const JointIndex & i = jmodel.id();
      const JointIndex & parent = model.parents[i];

      jmodel.calc(jdata.derived(),q.derived());

      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];

      Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
      jmodel.jointCols(J_) = data.oMi[i].act(jdata.S());
    }

  };

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
  computeJointJacobians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<ConfigVectorType> & q)
  {
    assert(model.check(data) && "data is not consistent with model.");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");

    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
    typedef typename Model::JointIndex JointIndex;
    typedef typename Data::Matrix6x Matrix6x;

    typedef JointJacobiansForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6x> Pass;
    typedef typename Pass::ArgsType ArgsType;
    for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
    {
      Pass::run(model.joints[i],data.joints[i],
                ArgsType(model,data,q.derived(),
                         PINOCCHIO_EIGEN_CONST_CAST(Matrix6x,data.J)));
    }

    return data.J;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  struct JointJacobiansForwardStep2
  : public fusion::JointUnaryVisitorBase< JointJacobiansForwardStep2<Scalar,Options,JointCollectionTpl> >
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typedef boost::fusion::vector<Data &> ArgsType;

    template<typename JointModel>
    static void algo(const JointModelBase<JointModel> & jmodel,
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     Data & data)
    {
      typedef typename Model::JointIndex JointIndex;

      const JointIndex & i = jmodel.id();
      jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
    }

  };

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
  computeJointJacobians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        DataTpl<Scalar,Options,JointCollectionTpl> & data)
  {
    assert(model.check(data) && "data is not consistent with model.");

    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;

    typedef JointJacobiansForwardStep2<Scalar,Options,JointCollectionTpl> Pass;
    for(JointIndex i=1; i< (JointIndex)model.njoints; ++i)
    {
      Pass::run(model.joints[i],data.joints[i],
                typename Pass::ArgsType(data));
    }

    return data.J;
  }

  namespace details
  {
    template<typename Scalar, int Options, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
    void translateJointJacobian(const SE3Tpl<Scalar,Options> & placement,
                                const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
                                const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
    {
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6);
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), Jout.cols());
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);

      Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut,Jout);

      typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
      typedef const MotionRef<ConstColXprIn> MotionIn;

      typedef typename Matrix6xLikeOut::ColXpr ColXprOut;
      typedef MotionRef<ColXprOut> MotionOut;

      for(Eigen::DenseIndex j=0; j < Jin.cols(); ++j)
      {
        MotionIn v_in(Jin.col(j));
        MotionOut v_out(Jout_.col(j));

        v_out = v_in;
        v_out.linear() -= placement.translation().cross(v_in.angular());
      }
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
    void translateJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex joint_id,
                                const ReferenceFrame rf,
                                const SE3Tpl<Scalar,Options> & placement,
                                const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
                                const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
    {
      assert(model.check(data) && "data is not consistent with model.");

      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6);
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), model.nv);

      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv);

      Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut,Jout);

      typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
      typedef const MotionRef<ConstColXprIn> MotionIn;

      typedef typename Matrix6xLikeOut::ColXpr ColXprOut;
      typedef MotionRef<ColXprOut> MotionOut;

      const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
      switch(rf)
      {
        case WORLD:
        {
          for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
          {
            MotionIn v_in(Jin.col(j));
            MotionOut v_out(Jout_.col(j));

            v_out = v_in;
          }
          break;
        }
        case LOCAL_WORLD_ALIGNED:
        {
          for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
          {
            MotionIn v_in(Jin.col(j));
            MotionOut v_out(Jout_.col(j));

            v_out = v_in;
            v_out.linear() -= placement.translation().cross(v_in.angular());
          }
          break;
        }
        case LOCAL:
        {
          for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
          {
            MotionIn v_in(Jin.col(j));
            MotionOut v_out(Jout_.col(j));

            v_out = placement.actInv(v_in);
          }
          break;
        }
        default:
          PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "must never happened");
          break;
      }
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
    void translateJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex joint_id,
                                const ReferenceFrame rf,
                                const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
                                const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
    {
      typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
      const typename Data::SE3 & oMjoint = data.oMi[joint_id];

      translateJointJacobian(model,data,joint_id,rf,oMjoint,Jin,Jout);
    }
  } // namespace details

  /* 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<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
  inline void getJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                               const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                               const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex jointId,
                               const ReferenceFrame rf,
                               const Eigen::MatrixBase<Matrix6xLike> & J)
  {
    assert(model.check(data) && "data is not consistent with model.");

    details::translateJointJacobian(model,data,jointId,rf,
                                    data.J,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J));
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
  struct JointJacobianForwardStep
  : public fusion::JointUnaryVisitorBase< JointJacobianForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> >
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typedef boost::fusion::vector<const Model &,
                                  Data &,
                                  const ConfigVectorType &,
                                  Matrix6xLike &
                                  > ArgsType;

    template<typename JointModel>
    static void algo(const JointModelBase<JointModel> & jmodel,
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const Model & model,
                     Data & data,
                     const Eigen::MatrixBase<ConfigVectorType> & q,
                     const Eigen::MatrixBase<Matrix6xLike> & J)
    {
      typedef typename Model::JointIndex JointIndex;
      const JointIndex & i = jmodel.id();
      const JointIndex & parent = model.parents[i];

      jmodel.calc(jdata.derived(),q.derived());

      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      data.iMf[parent] = data.liMi[i]*data.iMf[i];

      Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
      jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S());
    }

  };

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
  inline void computeJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                   DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                   const Eigen::MatrixBase<ConfigVectorType> & q,
                                   const JointIndex jointId,
                                   const Eigen::MatrixBase<Matrix6xLike> & J)
  {
    assert(model.check(data) && "data is not consistent with model.");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");

    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;

    data.iMf[jointId].setIdentity();
    typedef JointJacobianForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> Pass;
    for(JointIndex i=jointId; i>0; i=model.parents[i])
    {
      Pass::run(model.joints[i],data.joints[i],
                typename Pass::ArgsType(model,data,q.derived(),PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J)));
    }
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  struct JointJacobiansTimeVariationForwardStep
  : public fusion::JointUnaryVisitorBase< JointJacobiansTimeVariationForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> >
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typedef boost::fusion::vector<const Model &,
                                  Data &,
                                  const ConfigVectorType &,
                                  const TangentVectorType &> ArgsType;

    template<typename JointModel>
    static void algo(const JointModelBase<JointModel> & jmodel,
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const Model & model,
                     Data & data,
                     const Eigen::MatrixBase<ConfigVectorType> & q,
                     const Eigen::MatrixBase<TangentVectorType> & v)
    {
      typedef typename Model::JointIndex JointIndex;
      typedef typename Data::SE3 SE3;
      typedef typename Data::Motion Motion;

      const JointIndex & i = (JointIndex) jmodel.id();
      const JointIndex & parent = model.parents[i];

      SE3 & oMi = data.oMi[i];
      Motion & vJ = data.v[i];

      jmodel.calc(jdata.derived(),q.derived(),v.derived());

      vJ = jdata.v();

      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      if(parent>0)
      {
        oMi = data.oMi[parent]*data.liMi[i];
        vJ += data.liMi[i].actInv(data.v[parent]);
      }
      else
      {
        oMi = data.liMi[i];
      }

      jmodel.jointCols(data.J) = oMi.act(jdata.S());

      // Spatial velocity of joint i expressed in the global frame o
      data.ov[i] = oMi.act(vJ);

      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
      ColsBlock dJcols = jmodel.jointCols(data.dJ);
      ColsBlock Jcols = jmodel.jointCols(data.J);

      motionSet::motionAction(data.ov[i],Jcols,dJcols);
    }

  };

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
  computeJointJacobiansTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                     DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                     const Eigen::MatrixBase<ConfigVectorType> & q,
                                     const Eigen::MatrixBase<TangentVectorType> & v)
  {
    assert(model.check(data) && "data is not consistent with model.");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");

    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;

    typedef JointJacobiansTimeVariationForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Pass;
    for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
    {
      Pass::run(model.joints[i],data.joints[i],
                typename Pass::ArgsType(model,data,q.derived(),v.derived()));
    }

    return data.dJ;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
  inline void getJointJacobianTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                            const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                            const JointIndex jointId,
                                            const ReferenceFrame rf,
                                            const Eigen::MatrixBase<Matrix6xLike> & dJ)
  {
    details::translateJointJacobian(model,data,jointId,rf,
                                    data.dJ,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ));
  }

} // namespace pinocchio

/// @endcond

#endif // ifndef __pinocchio_jacobian_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2020 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_jacobian_hxx__
6			#define __pinocchio_jacobian_hxx__
7
8			#include "pinocchio/multibody/visitor.hpp"
9			#include "pinocchio/algorithm/check.hpp"
10
11			/// @cond DEV
12
13			namespace pinocchio
14			{
15
16			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
17			struct JointJacobiansForwardStep
18			: public fusion::JointUnaryVisitorBase< JointJacobiansForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> >
19			{
20			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
21			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
22
23			typedef boost::fusion::vector<const Model &,
24			Data &,
25			const ConfigVectorType &,
26			Matrix6xLike &> ArgsType;
27
28			template<typename JointModel>
29		14358	static void algo(const JointModelBase<JointModel> & jmodel,
30			JointDataBase<typename JointModel::JointDataDerived> & jdata,
31			const Model & model,
32			Data & data,
33			const Eigen::MatrixBase<ConfigVectorType> & q,
34			const Eigen::MatrixBase<Matrix6xLike> & J)
35			{
36			typedef typename Model::JointIndex JointIndex;
37
38		14358	const JointIndex & i = jmodel.id();
39		14358	const JointIndex & parent = model.parents[i];
40
41		14358	jmodel.calc(jdata.derived(),q.derived());
42
43	✓✗	14358	data.liMi[i] = model.jointPlacements[i]*jdata.M();
44	✓✓	14358	if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
45		562	else data.oMi[i] = data.liMi[i];
46
47		14358	Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
48	✓✗✓✗	14358	jmodel.jointCols(J_) = data.oMi[i].act(jdata.S());
49			}
50
51			};
52
53			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
54			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
55		285	computeJointJacobians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
56			DataTpl<Scalar,Options,JointCollectionTpl> & data,
57			const Eigen::MatrixBase<ConfigVectorType> & q)
58			{
59	✓✗	285	assert(model.check(data) && "data is not consistent with model.");
60	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	285	PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
61
62			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
63			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
64			typedef typename Model::JointIndex JointIndex;
65			typedef typename Data::Matrix6x Matrix6x;
66
67			typedef JointJacobiansForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6x> Pass;
68			typedef typename Pass::ArgsType ArgsType;
69	✓✓	7460	for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
70			{
71	✓✗	7175	Pass::run(model.joints[i],data.joints[i],
72			ArgsType(model,data,q.derived(),
73		7175	PINOCCHIO_EIGEN_CONST_CAST(Matrix6x,data.J)));
74			}
75
76		285	return data.J;
77			}
78
79			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
80			struct JointJacobiansForwardStep2
81			: public fusion::JointUnaryVisitorBase< JointJacobiansForwardStep2<Scalar,Options,JointCollectionTpl> >
82			{
83			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
84			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
85
86			typedef boost::fusion::vector<Data &> ArgsType;
87
88			template<typename JointModel>
89		54	static void algo(const JointModelBase<JointModel> & jmodel,
90			JointDataBase<typename JointModel::JointDataDerived> & jdata,
91			Data & data)
92			{
93			typedef typename Model::JointIndex JointIndex;
94
95		54	const JointIndex & i = jmodel.id();
96	✓✗✓✗	54	jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
97			}
98
99			};
100
101			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
102			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
103		1	computeJointJacobians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
104			DataTpl<Scalar,Options,JointCollectionTpl> & data)
105			{
106	✓✗	1	assert(model.check(data) && "data is not consistent with model.");
107
108			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
109			typedef typename Model::JointIndex JointIndex;
110
111			typedef JointJacobiansForwardStep2<Scalar,Options,JointCollectionTpl> Pass;
112	✓✓	28	for(JointIndex i=1; i< (JointIndex)model.njoints; ++i)
113			{
114	✓✗	27	Pass::run(model.joints[i],data.joints[i],
115			typename Pass::ArgsType(data));
116			}
117
118		1	return data.J;
119			}
120
121			namespace details
122			{
123			template<typename Scalar, int Options, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
124		140	void translateJointJacobian(const SE3Tpl<Scalar,Options> & placement,
125			const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
126			const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
127			{
128	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	140	PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6);
129	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗	140	PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), Jout.cols());
130	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	140	PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);
131
132		140	Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut,Jout);
133
134			typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
135			typedef const MotionRef<ConstColXprIn> MotionIn;
136
137			typedef typename Matrix6xLikeOut::ColXpr ColXprOut;
138			typedef MotionRef<ColXprOut> MotionOut;
139
140	✓✓	420	for(Eigen::DenseIndex j=0; j < Jin.cols(); ++j)
141			{
142	✓✗✓✗	280	MotionIn v_in(Jin.col(j));
143	✓✗✓✗	280	MotionOut v_out(Jout_.col(j));
144
145	✓✗	280	v_out = v_in;
146	✓✗✓✗ ✓✗✓✗ ✓✗	280	v_out.linear() -= placement.translation().cross(v_in.angular());
147			}
148			}
149
150			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
151		363	void translateJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
152			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
153			const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex joint_id,
154			const ReferenceFrame rf,
155			const SE3Tpl<Scalar,Options> & placement,
156			const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
157			const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
158			{
159	✓✗	363	assert(model.check(data) && "data is not consistent with model.");
160
161	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	363	PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6);
162	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	363	PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), model.nv);
163
164	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	363	PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);
165	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	363	PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv);
166
167		363	Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut,Jout);
168
169			typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
170			typedef const MotionRef<ConstColXprIn> MotionIn;
171
172			typedef typename Matrix6xLikeOut::ColXpr ColXprOut;
173			typedef MotionRef<ColXprOut> MotionOut;
174
175		363	const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
176	✓✓✓✗	363	switch(rf)
177			{
178		53	case WORLD:
179			{
180	✓✓	617	for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
181			{
182	✓✗✓✗	564	MotionIn v_in(Jin.col(j));
183	✓✗✓✗	564	MotionOut v_out(Jout_.col(j));
184
185	✓✗	564	v_out = v_in;
186			}
187		53	break;
188			}
189		46	case LOCAL_WORLD_ALIGNED:
190			{
191	✓✓	514	for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
192			{
193	✓✗✓✗	468	MotionIn v_in(Jin.col(j));
194	✓✗✓✗	468	MotionOut v_out(Jout_.col(j));
195
196	✓✗	468	v_out = v_in;
197	✓✗✓✗ ✓✗✓✗ ✓✗	468	v_out.linear() -= placement.translation().cross(v_in.angular());
198			}
199		46	break;
200			}
201		264	case LOCAL:
202			{
203	✓✓	3121	for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
204			{
205	✓✗✓✗	2857	MotionIn v_in(Jin.col(j));
206	✓✗✓✗	2857	MotionOut v_out(Jout_.col(j));
207
208	✓✗✓✗	2857	v_out = placement.actInv(v_in);
209			}
210		264	break;
211			}
212			default:
213			PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "must never happened");
214			break;
215			}
216		363	}
217
218			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
219		337	void translateJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
220			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
221			const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex joint_id,
222			const ReferenceFrame rf,
223			const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
224			const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
225			{
226			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
227		337	const typename Data::SE3 & oMjoint = data.oMi[joint_id];
228
229		337	translateJointJacobian(model,data,joint_id,rf,oMjoint,Jin,Jout);
230		337	}
231			} // namespace details
232
233			/* Return the jacobian of the output frame attached to joint <jointId> in the
234			world frame or in the local frame depending on the template argument. The
235			function computeJacobians should have been called first. */
236			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
237		331	inline void getJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
238			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
239			const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex jointId,
240			const ReferenceFrame rf,
241			const Eigen::MatrixBase<Matrix6xLike> & J)
242			{
243	✓✗	331	assert(model.check(data) && "data is not consistent with model.");
244
245		331	details::translateJointJacobian(model,data,jointId,rf,
246		331	data.J,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J));
247		331	}
248
249			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
250			struct JointJacobianForwardStep
251			: public fusion::JointUnaryVisitorBase< JointJacobianForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> >
252			{
253			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
254			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
255
256			typedef boost::fusion::vector<const Model &,
257			Data &,
258			const ConfigVectorType &,
259			Matrix6xLike &
260			> ArgsType;
261
262			template<typename JointModel>
263		2264	static void algo(const JointModelBase<JointModel> & jmodel,
264			JointDataBase<typename JointModel::JointDataDerived> & jdata,
265			const Model & model,
266			Data & data,
267			const Eigen::MatrixBase<ConfigVectorType> & q,
268			const Eigen::MatrixBase<Matrix6xLike> & J)
269			{
270			typedef typename Model::JointIndex JointIndex;
271		2264	const JointIndex & i = jmodel.id();
272		2264	const JointIndex & parent = model.parents[i];
273
274		2264	jmodel.calc(jdata.derived(),q.derived());
275
276	✓✗	2264	data.liMi[i] = model.jointPlacements[i]*jdata.M();
277		2264	data.iMf[parent] = data.liMi[i]*data.iMf[i];
278
279		2264	Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
280	✓✗✓✗	2264	jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S());
281			}
282
283			};
284
285			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
286		183	inline void computeJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
287			DataTpl<Scalar,Options,JointCollectionTpl> & data,
288			const Eigen::MatrixBase<ConfigVectorType> & q,
289			const JointIndex jointId,
290			const Eigen::MatrixBase<Matrix6xLike> & J)
291			{
292	✓✗	183	assert(model.check(data) && "data is not consistent with model.");
293	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	183	PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
294
295			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
296			typedef typename Model::JointIndex JointIndex;
297
298		183	data.iMf[jointId].setIdentity();
299			typedef JointJacobianForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> Pass;
300	✓✓	1289	for(JointIndex i=jointId; i>0; i=model.parents[i])
301			{
302	✓✗	1106	Pass::run(model.joints[i],data.joints[i],
303		1106	typename Pass::ArgsType(model,data,q.derived(),PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J)));
304			}
305		183	}
306
307			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
308			struct JointJacobiansTimeVariationForwardStep
309			: public fusion::JointUnaryVisitorBase< JointJacobiansTimeVariationForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> >
310			{
311			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
312			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
313
314			typedef boost::fusion::vector<const Model &,
315			Data &,
316			const ConfigVectorType &,
317			const TangentVectorType &> ArgsType;
318
319			template<typename JointModel>
320		702	static void algo(const JointModelBase<JointModel> & jmodel,
321			JointDataBase<typename JointModel::JointDataDerived> & jdata,
322			const Model & model,
323			Data & data,
324			const Eigen::MatrixBase<ConfigVectorType> & q,
325			const Eigen::MatrixBase<TangentVectorType> & v)
326			{
327			typedef typename Model::JointIndex JointIndex;
328			typedef typename Data::SE3 SE3;
329			typedef typename Data::Motion Motion;
330
331	✓✗	702	const JointIndex & i = (JointIndex) jmodel.id();
332		702	const JointIndex & parent = model.parents[i];
333
334		702	SE3 & oMi = data.oMi[i];
335		702	Motion & vJ = data.v[i];
336
337	✓✗	702	jmodel.calc(jdata.derived(),q.derived(),v.derived());
338
339	✓✗✓✗	702	vJ = jdata.v();
340
341	✓✗✓✓ ✗✓✗	702	data.liMi[i] = model.jointPlacements[i]*jdata.M();
342	✓✓	702	if(parent>0)
343			{
344	✓✗	676	oMi = data.oMi[parent]*data.liMi[i];
345	✓✗✓✗	676	vJ += data.liMi[i].actInv(data.v[parent]);
346			}
347			else
348			{
349	✓✗	26	oMi = data.liMi[i];
350			}
351
352	✓✗✓✗ ✓✗✓✗	702	jmodel.jointCols(data.J) = oMi.act(jdata.S());
353
354			// Spatial velocity of joint i expressed in the global frame o
355	✓✗	702	data.ov[i] = oMi.act(vJ);
356
357			typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
358	✓✗	702	ColsBlock dJcols = jmodel.jointCols(data.dJ);
359	✓✗	702	ColsBlock Jcols = jmodel.jointCols(data.J);
360
361	✓✗	702	motionSet::motionAction(data.ov[i],Jcols,dJcols);
362			}
363
364			};
365
366			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
367			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
368		13	computeJointJacobiansTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
369			DataTpl<Scalar,Options,JointCollectionTpl> & data,
370			const Eigen::MatrixBase<ConfigVectorType> & q,
371			const Eigen::MatrixBase<TangentVectorType> & v)
372			{
373	✓✗	13	assert(model.check(data) && "data is not consistent with model.");
374	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	13	PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
375	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	13	PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
376
377			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
378			typedef typename Model::JointIndex JointIndex;
379
380			typedef JointJacobiansTimeVariationForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Pass;
381	✓✓	364	for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
382			{
383	✓✗	351	Pass::run(model.joints[i],data.joints[i],
384			typename Pass::ArgsType(model,data,q.derived(),v.derived()));
385			}
386
387		13	return data.dJ;
388			}
389
390			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
391		6	inline void getJointJacobianTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
392			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
393			const JointIndex jointId,
394			const ReferenceFrame rf,
395			const Eigen::MatrixBase<Matrix6xLike> & dJ)
396			{
397		6	details::translateJointJacobian(model,data,jointId,rf,
398		6	data.dJ,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ));
399		6	}
400
401			} // namespace pinocchio
402
403			/// @endcond
404
405			#endif // ifndef __pinocchio_jacobian_hxx__