Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/algorithm/compute-all-terms.hxx	Lines:	79	79	100.0 %
Date:	2024-01-23 21:41:47	Branches:	150	332	45.2 %


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

#ifndef __pinocchio_compute_all_terms_hxx__
#define __pinocchio_compute_all_terms_hxx__

#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/energy.hpp"
#include "pinocchio/algorithm/check.hpp"

namespace pinocchio
{
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  struct CATForwardStep
  : public fusion::JointUnaryVisitorBase< CATForwardStep<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 SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;

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

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

      // CRBA
      data.liMi[i] = model.jointPlacements[i]*jdata.M();

      // Jacobian + NLE
      data.v[i] = jdata.v();

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

      data.ov[i] = data.oMi[i].act(data.v[i]);

      data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
      data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]);

      ColsBlock J_cols = jmodel.jointCols(data.J);
      J_cols = data.oMi[i].act(jdata.S());

      ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
      motionSet::motionAction(data.ov[i],J_cols,dJ_cols);

      data.a_gf[i] = data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
      if (parent > 0)
        data.a[i] += data.liMi[i].actInv(data.a[parent]);

      data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);

      data.h[i] = model.inertias[i]*data.v[i];
      data.f[i] = model.inertias[i]*data.a_gf[i] + data.v[i].cross(data.h[i]); // -f_ext

    }

  };

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

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

    template<typename JointModel>
    static void algo(const JointModelBase<JointModel> & jmodel,
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const Model & model,
                     Data & data)
    {
      typedef typename Model::JointIndex JointIndex;
      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;

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

      ColsBlock J_cols = data.J.template middleCols<JointModel::NV>(jmodel.idx_v());
      ColsBlock dJ_cols = data.dJ.template middleCols<JointModel::NV>(jmodel.idx_v());
      ColsBlock Ag_cols = data.Ag.template middleCols<JointModel::NV>(jmodel.idx_v());
      ColsBlock dAg_cols = data.dAg.template middleCols<JointModel::NV>(jmodel.idx_v());

      // Calc Ag = Y * S
      motionSet::inertiaAction(data.oYcrb[i],J_cols,Ag_cols);

      // Calc dAg = Ivx + vxI
      dAg_cols.noalias() = data.doYcrb[i] * J_cols;
      motionSet::inertiaAction<ADDTO>(data.oYcrb[i],dJ_cols,dAg_cols);

      /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
      data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
      = J_cols.transpose()*data.Ag.middleCols(jmodel.idx_v(),data.nvSubtree[i]);

      jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose()*data.f[i];

      data.oYcrb[parent] += data.oYcrb[i];
      data.doYcrb[parent] += data.doYcrb[i];
      data.h[parent] += data.liMi[i].act(data.h[i]);
      data.f[parent] += data.liMi[i].act(data.f[i]);

      // CoM
      data.mass[i] = data.oYcrb[i].mass();
      data.com[i] = data.oMi[i].actInv(data.oYcrb[i].lever());
      data.vcom[i] = data.h[i].linear() / data.mass[i];
    }
  };

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  inline void computeAllTerms(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 DataTpl<Scalar,Options,JointCollectionTpl> Data;

    data.v[0].setZero();
    data.a[0].setZero();
    data.h[0].setZero();
    data.a_gf[0] = -model.gravity;
    data.oYcrb[0].setZero();

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

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

    // CoM
    data.mass[0] = data.oYcrb[0].mass();
    data.com[0] = data.oYcrb[0].lever();
    data.vcom[0] = data.h[0].linear() / data.mass[0];

    // Centroidal
    typedef Eigen::Block<typename Data::Matrix6x,3,-1> Block3x;
    const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
    Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
    for(long i = 0; i<model.nv; ++i)
      Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);

    const Block3x dAg_lin = data.dAg.template middleRows<3>(Force::LINEAR);
    Block3x dAg_ang = data.dAg.template middleRows<3>(Force::ANGULAR);
    for(Eigen::DenseIndex i = 0; i<model.nv; ++i)
      dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]) + Ag_lin.col(i).cross(data.vcom[0]);

    data.hg = data.h[0];
    data.hg.angular() += data.hg.linear().cross(data.com[0]);

    data.dhg = data.f[0];
    data.dhg.angular() += data.dhg.linear().cross(data.com[0]);

    // JCoM
    data.Jcom = data.Ag.template middleRows<3>(Force::LINEAR)/data.mass[0];

    data.Ig.mass() = data.oYcrb[0].mass();
    data.Ig.lever().setZero();
    data.Ig.inertia() = data.oYcrb[0].inertia();

    // Gravity
    data.g.noalias() = -data.Ag.template middleRows<3>(Force::LINEAR).transpose() * model.gravity.linear();

    // Energy
    computeKineticEnergy(model, data);
    computePotentialEnergy(model, data);
  }

} // namespace pinocchio

/// \endinternal

#endif // ifndef __pinocchio_compute_all_terms_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2021 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_compute_all_terms_hxx__
6			#define __pinocchio_compute_all_terms_hxx__
7
8			#include "pinocchio/multibody/visitor.hpp"
9			#include "pinocchio/spatial/act-on-set.hpp"
10			#include "pinocchio/algorithm/center-of-mass.hpp"
11			#include "pinocchio/algorithm/energy.hpp"
12			#include "pinocchio/algorithm/check.hpp"
13
14			namespace pinocchio
15			{
16			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
17			struct CATForwardStep
18			: public fusion::JointUnaryVisitorBase< CATForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> >
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			const TangentVectorType &
27			> ArgsType;
28
29			template<typename JointModel>
30		11024	static void algo(const JointModelBase<JointModel> & jmodel,
31			JointDataBase<typename JointModel::JointDataDerived> & jdata,
32			const Model & model,
33			Data & data,
34			const Eigen::MatrixBase<ConfigVectorType> & q,
35			const Eigen::MatrixBase<TangentVectorType> & v)
36			{
37			typedef typename Model::JointIndex JointIndex;
38			typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
39
40	✓✗	11024	const JointIndex i = jmodel.id();
41		11024	const JointIndex parent = model.parents[i];
42
43	✓✗	11024	jmodel.calc(jdata.derived(),q.derived(),v.derived());
44
45			// CRBA
46	✓✗✓✓ ✗✓✗	11024	data.liMi[i] = model.jointPlacements[i]*jdata.M();
47
48			// Jacobian + NLE
49	✓✗✓✗	11024	data.v[i] = jdata.v();
50
51	✓✓	11024	if(parent>0)
52			{
53	✓✗	10584	data.oMi[i] = data.oMi[parent]*data.liMi[i];
54	✓✗✓✗	10584	data.v[i] += data.liMi[i].actInv(data.v[parent]);
55			}
56			else
57	✓✗	440	data.oMi[i] = data.liMi[i];
58
59	✓✗	11024	data.ov[i] = data.oMi[i].act(data.v[i]);
60
61	✓✗✓✗	11024	data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
62	✓✗	11024	data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]);
63
64	✓✗	11024	ColsBlock J_cols = jmodel.jointCols(data.J);
65	✓✗✓✗ ✓✗	11024	J_cols = data.oMi[i].act(jdata.S());
66
67	✓✗	11024	ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
68	✓✗	11024	motionSet::motionAction(data.ov[i],J_cols,dJ_cols);
69
70	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	11024	data.a_gf[i] = data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
71	✓✓	11024	if (parent > 0)
72	✓✗✓✗	10584	data.a[i] += data.liMi[i].actInv(data.a[parent]);
73
74	✓✗✓✗	11024	data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
75
76	✓✗✓✗	11024	data.h[i] = model.inertias[i]*data.v[i];
77	✓✗✓✗ ✓✗✓✗	11024	data.f[i] = model.inertias[i]*data.a_gf[i] + data.v[i].cross(data.h[i]); // -f_ext
78
79			}
80
81			};
82
83			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
84			struct CATBackwardStep
85			: public fusion::JointUnaryVisitorBase <CATBackwardStep<Scalar,Options,JointCollectionTpl> >
86			{
87			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
88			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
89
90			typedef boost::fusion::vector<const Model &,
91			Data &
92			> ArgsType;
93
94			template<typename JointModel>
95		11024	static void algo(const JointModelBase<JointModel> & jmodel,
96			JointDataBase<typename JointModel::JointDataDerived> & jdata,
97			const Model & model,
98			Data & data)
99			{
100			typedef typename Model::JointIndex JointIndex;
101			typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
102
103	✓✗	11024	const JointIndex i = jmodel.id();
104		11024	const JointIndex parent = model.parents[i];
105
106	✓✗✓✗	11024	ColsBlock J_cols = data.J.template middleCols<JointModel::NV>(jmodel.idx_v());
107	✓✗✓✗	11024	ColsBlock dJ_cols = data.dJ.template middleCols<JointModel::NV>(jmodel.idx_v());
108	✓✗✓✗	11024	ColsBlock Ag_cols = data.Ag.template middleCols<JointModel::NV>(jmodel.idx_v());
109	✓✗✓✗	11024	ColsBlock dAg_cols = data.dAg.template middleCols<JointModel::NV>(jmodel.idx_v());
110
111			// Calc Ag = Y * S
112	✓✗	11024	motionSet::inertiaAction(data.oYcrb[i],J_cols,Ag_cols);
113
114			// Calc dAg = Ivx + vxI
115	✓✗✓✗ ✓✗	11024	dAg_cols.noalias() = data.doYcrb[i] * J_cols;
116	✓✗	11024	motionSet::inertiaAction<ADDTO>(data.oYcrb[i],dJ_cols,dAg_cols);
117
118			/* M[i,SUBTREE] = S'F[1:6,SUBTREE] /
119	✓✗✓✗ ✓✗✓✗	11024	data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
120	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	22048	= J_cols.transpose()*data.Ag.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
121
122	✓✗✓✓ ✗✓✓✗ ✓✓✗✓ ✗	11024	jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose()*data.f[i];
123
124	✓✗	11024	data.oYcrb[parent] += data.oYcrb[i];
125	✓✗	11024	data.doYcrb[parent] += data.doYcrb[i];
126	✓✗✓✗	11024	data.h[parent] += data.liMi[i].act(data.h[i]);
127	✓✗✓✗	11024	data.f[parent] += data.liMi[i].act(data.f[i]);
128
129			// CoM
130		11024	data.mass[i] = data.oYcrb[i].mass();
131	✓✗	11024	data.com[i] = data.oMi[i].actInv(data.oYcrb[i].lever());
132	✓✗✓✗ ✓✗	11024	data.vcom[i] = data.h[i].linear() / data.mass[i];
133			}
134			};
135
136			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
137		234	inline void computeAllTerms(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
138			DataTpl<Scalar,Options,JointCollectionTpl> & data,
139			const Eigen::MatrixBase<ConfigVectorType> & q,
140			const Eigen::MatrixBase<TangentVectorType> & v)
141			{
142	✓✗✓✗	234	assert(model.check(data) && "data is not consistent with model.");
143	✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗	234	PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
144	✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗	234	PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
145
146			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
147
148	✓✗	234	data.v[0].setZero();
149	✓✗	234	data.a[0].setZero();
150	✓✗	234	data.h[0].setZero();
151	✓✗	234	data.a_gf[0] = -model.gravity;
152	✓✗	234	data.oYcrb[0].setZero();
153
154			typedef CATForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Pass1;
155	✓✓	5760	for(JointIndex i=1;i<(JointIndex) model.njoints;++i)
156			{
157	✓✗✓✗	5526	Pass1::run(model.joints[i],data.joints[i],
158			typename Pass1::ArgsType(model,data,q.derived(),v.derived()));
159			}
160
161			typedef CATBackwardStep<Scalar,Options,JointCollectionTpl> Pass2;
162	✓✓	5760	for(JointIndex i=(JointIndex)(model.njoints-1);i>0;--i)
163			{
164	✓✗✓✗	5526	Pass2::run(model.joints[i],data.joints[i],
165			typename Pass2::ArgsType(model,data));
166			}
167
168			// CoM
169		234	data.mass[0] = data.oYcrb[0].mass();
170	✓✗	234	data.com[0] = data.oYcrb[0].lever();
171	✓✗✓✗ ✓✗	234	data.vcom[0] = data.h[0].linear() / data.mass[0];
172
173			// Centroidal
174			typedef Eigen::Block<typename Data::Matrix6x,3,-1> Block3x;
175	✓✗	234	const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
176	✓✗	234	Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
177	✓✓	6836	for(long i = 0; i<model.nv; ++i)
178	✓✗✓✗ ✓✗✓✗	6602	Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
179
180	✓✗	234	const Block3x dAg_lin = data.dAg.template middleRows<3>(Force::LINEAR);
181	✓✗	234	Block3x dAg_ang = data.dAg.template middleRows<3>(Force::ANGULAR);
182	✓✓	6836	for(Eigen::DenseIndex i = 0; i<model.nv; ++i)
183	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	6602	dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]) + Ag_lin.col(i).cross(data.vcom[0]);
184
185	✓✗	234	data.hg = data.h[0];
186	✓✗✓✗ ✓✗✓✗	234	data.hg.angular() += data.hg.linear().cross(data.com[0]);
187
188	✓✗	234	data.dhg = data.f[0];
189	✓✗✓✗ ✓✗✓✗	234	data.dhg.angular() += data.dhg.linear().cross(data.com[0]);
190
191			// JCoM
192	✓✗✓✗ ✓✗	234	data.Jcom = data.Ag.template middleRows<3>(Force::LINEAR)/data.mass[0];
193
194		234	data.Ig.mass() = data.oYcrb[0].mass();
195	✓✗	234	data.Ig.lever().setZero();
196	✓✗	234	data.Ig.inertia() = data.oYcrb[0].inertia();
197
198			// Gravity
199	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	234	data.g.noalias() = -data.Ag.template middleRows<3>(Force::LINEAR).transpose() * model.gravity.linear();
200
201			// Energy
202	✓✗	234	computeKineticEnergy(model, data);
203	✓✗	234	computePotentialEnergy(model, data);
204		234	}
205
206			} // namespace pinocchio
207
208			/// \endinternal
209
210			#endif // ifndef __pinocchio_compute_all_terms_hxx__