Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/multibody/joint/joint-composite.hxx	Lines:	44	44	100.0 %
Date:	2024-01-23 21:41:47	Branches:	39	72	54.2 %


//
// Copyright (c) 2016-2019 CNRS INRIA
//

#ifndef __pinocchio_joint_composite_hxx__
#define __pinocchio_joint_composite_hxx__

#include "pinocchio/multibody/visitor.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename ConfigVectorType>
  struct JointCompositeCalcZeroOrderStep
  : fusion::JointUnaryVisitorBase< JointCompositeCalcZeroOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> >
  {
    typedef JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModelComposite;
    typedef JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> JointDataComposite;

    typedef boost::fusion::vector<const JointModelComposite &,
                                  JointDataComposite &,
                                  const ConfigVectorType &
                                  > ArgsType;

    template<typename JointModel>
    static void algo(const pinocchio::JointModelBase<JointModel> & jmodel,
                     pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const JointModelComposite & model,
                     JointDataComposite & data,
                     const Eigen::MatrixBase<ConfigVectorType> & q)
    {
      const JointIndex & i  = jmodel.id();
      const JointIndex succ = i+1; // successor

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

      data.pjMi[i] = model.jointPlacements[i] * jdata.M ();

      if ( succ == model.joints.size() )
      {
        data.iMlast[i] = data.pjMi[i];
        data.S.matrix().rightCols(model.m_nvs[i]) = jdata.S().matrix();
      }
      else
      {
        const int idx_v = model.m_idx_v[i] - model.m_idx_v[0];

        data.iMlast[i] = data.pjMi[i] * data.iMlast[succ];
        data.S.matrix().middleCols(idx_v,model.m_nvs[i]) = data.iMlast[succ].actInv(jdata.S());
      }

    }

  };

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  template<typename ConfigVectorType>
  inline void JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>::
  calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const
  {
    assert(joints.size() > 0);
    assert(data.joints.size() == joints.size());

    typedef JointCompositeCalcZeroOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> Algo;

    for (int i=(int)(joints.size()-1); i >= 0; --i)
    {
      Algo::run(joints[(size_t)i],
                data.joints[(size_t)i],
                typename Algo::ArgsType(*this,data,qs.derived()));
    }
    data.M = data.iMlast.front();
  }

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  struct JointCompositeCalcFirstOrderStep
  : public fusion::JointUnaryVisitorBase< JointCompositeCalcFirstOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> >
  {
    typedef JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModelComposite;
    typedef JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> JointDataComposite;

    typedef boost::fusion::vector<const JointModelComposite &,
                                  JointDataComposite &,
                                  const ConfigVectorType &,
                                  const TangentVectorType &
                                  > ArgsType;

    template<typename JointModel>
    static void algo(const pinocchio::JointModelBase<JointModel> & jmodel,
                     pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const JointModelComposite & model,
                     JointDataComposite & data,
                     const Eigen::MatrixBase<ConfigVectorType> & q,
                     const Eigen::MatrixBase<TangentVectorType> & v)
    {
      const JointIndex & i  = jmodel.id();
      const JointIndex succ = i+1; // successor

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

      data.pjMi[i] = model.jointPlacements[i] * jdata.M();

      if (succ == model.joints.size())
      {
        data.iMlast[i] = data.pjMi[i];
        data.S.matrix().rightCols(model.m_nvs[i]) = jdata.S().matrix();
        data.v = jdata.v();
        data.c = jdata.c();
      }
      else
      {
        const int idx_v = model.m_idx_v[i] - model.m_idx_v[0];

        data.iMlast[i] = data.pjMi[i] * data.iMlast[succ];
        data.S.matrix().middleCols(idx_v,model.m_nvs[i]) = data.iMlast[succ].actInv(jdata.S());

        typename JointModelComposite::Motion v_tmp = data.iMlast[succ].actInv(jdata.v());

        data.v += v_tmp;

        data.c -= data.v.cross(v_tmp);
        data.c += data.iMlast[succ].actInv(jdata.c());
      }

    }

  };

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  template<typename ConfigVectorType, typename TangentVectorType>
  inline void JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>
  ::calc(JointDataDerived & jdata,
         const Eigen::MatrixBase<ConfigVectorType> & qs,
         const Eigen::MatrixBase<TangentVectorType> & vs) const
  {
    assert(joints.size() > 0);
    assert(jdata.joints.size() == joints.size());

    typedef JointCompositeCalcFirstOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Algo;

    for (int i=(int)(joints.size()-1); i >= 0; --i)
    {
      Algo::run(joints[(size_t)i],
                jdata.joints[(size_t)i],
                typename Algo::ArgsType(*this,jdata,qs.derived(),vs.derived()));
    }

    jdata.M = jdata.iMlast.front();
  }

} // namespace pinocchio

#endif // ifndef __pinocchio_joint_composite_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2019 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_joint_composite_hxx__
6			#define __pinocchio_joint_composite_hxx__
7
8			#include "pinocchio/multibody/visitor.hpp"
9
10			namespace pinocchio
11			{
12
13			template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename ConfigVectorType>
14			struct JointCompositeCalcZeroOrderStep
15			: fusion::JointUnaryVisitorBase< JointCompositeCalcZeroOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> >
16			{
17			typedef JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModelComposite;
18			typedef JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> JointDataComposite;
19
20			typedef boost::fusion::vector<const JointModelComposite &,
21			JointDataComposite &,
22			const ConfigVectorType &
23			> ArgsType;
24
25			template<typename JointModel>
26		70	static void algo(const pinocchio::JointModelBase<JointModel> & jmodel,
27			pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
28			const JointModelComposite & model,
29			JointDataComposite & data,
30			const Eigen::MatrixBase<ConfigVectorType> & q)
31			{
32		70	const JointIndex & i = jmodel.id();
33		70	const JointIndex succ = i+1; // successor
34
35		70	jmodel.calc(jdata.derived(), q.derived());
36
37	✓✗	70	data.pjMi[i] = model.jointPlacements[i] * jdata.M ();
38
39	✓✓	70	if ( succ == model.joints.size() )
40			{
41		40	data.iMlast[i] = data.pjMi[i];
42	✓✗✗✓ ✗✓✗	40	data.S.matrix().rightCols(model.m_nvs[i]) = jdata.S().matrix();
43			}
44			else
45			{
46		30	const int idx_v = model.m_idx_v[i] - model.m_idx_v[0];
47
48		30	data.iMlast[i] = data.pjMi[i] * data.iMlast[succ];
49	✓✗✓✗ ✓✗	30	data.S.matrix().middleCols(idx_v,model.m_nvs[i]) = data.iMlast[succ].actInv(jdata.S());
50			}
51
52			}
53
54			};
55
56			template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
57			template<typename ConfigVectorType>
58		20	inline void JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>::
59			calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const
60			{
61	✗✓	20	assert(joints.size() > 0);
62	✗✓	20	assert(data.joints.size() == joints.size());
63
64			typedef JointCompositeCalcZeroOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> Algo;
65
66	✓✓	55	for (int i=(int)(joints.size()-1); i >= 0; --i)
67			{
68	✓✗	70	Algo::run(joints[(size_t)i],
69		35	data.joints[(size_t)i],
70			typename Algo::ArgsType(*this,data,qs.derived()));
71			}
72		20	data.M = data.iMlast.front();
73		20	}
74
75			template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
76			struct JointCompositeCalcFirstOrderStep
77			: public fusion::JointUnaryVisitorBase< JointCompositeCalcFirstOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> >
78			{
79			typedef JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModelComposite;
80			typedef JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> JointDataComposite;
81
82			typedef boost::fusion::vector<const JointModelComposite &,
83			JointDataComposite &,
84			const ConfigVectorType &,
85			const TangentVectorType &
86			> ArgsType;
87
88			template<typename JointModel>
89		148	static void algo(const pinocchio::JointModelBase<JointModel> & jmodel,
90			pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
91			const JointModelComposite & model,
92			JointDataComposite & data,
93			const Eigen::MatrixBase<ConfigVectorType> & q,
94			const Eigen::MatrixBase<TangentVectorType> & v)
95			{
96		148	const JointIndex & i = jmodel.id();
97		148	const JointIndex succ = i+1; // successor
98
99		148	jmodel.calc(jdata.derived(), q.derived(), v.derived());
100
101	✓✗	148	data.pjMi[i] = model.jointPlacements[i] * jdata.M();
102
103	✓✓	148	if (succ == model.joints.size())
104			{
105		84	data.iMlast[i] = data.pjMi[i];
106	✓✗✗✓ ✗✓✗	84	data.S.matrix().rightCols(model.m_nvs[i]) = jdata.S().matrix();
107		84	data.v = jdata.v();
108		84	data.c = jdata.c();
109			}
110			else
111			{
112		64	const int idx_v = model.m_idx_v[i] - model.m_idx_v[0];
113
114	✓✗	64	data.iMlast[i] = data.pjMi[i] * data.iMlast[succ];
115	✓✗✓✗ ✓✗✓✗ ✓✗	64	data.S.matrix().middleCols(idx_v,model.m_nvs[i]) = data.iMlast[succ].actInv(jdata.S());
116
117	✓✗✓✗	64	typename JointModelComposite::Motion v_tmp = data.iMlast[succ].actInv(jdata.v());
118
119	✓✗	64	data.v += v_tmp;
120
121	✓✗✓✗	64	data.c -= data.v.cross(v_tmp);
122	✓✗✓✗ ✓✗	64	data.c += data.iMlast[succ].actInv(jdata.c());
123			}
124
125			}
126
127			};
128
129			template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
130			template<typename ConfigVectorType, typename TangentVectorType>
131		42	inline void JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>
132			::calc(JointDataDerived & jdata,
133			const Eigen::MatrixBase<ConfigVectorType> & qs,
134			const Eigen::MatrixBase<TangentVectorType> & vs) const
135			{
136	✗✓	42	assert(joints.size() > 0);
137	✗✓	42	assert(jdata.joints.size() == joints.size());
138
139			typedef JointCompositeCalcFirstOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Algo;
140
141	✓✓	116	for (int i=(int)(joints.size()-1); i >= 0; --i)
142			{
143	✓✗	148	Algo::run(joints[(size_t)i],
144		74	jdata.joints[(size_t)i],
145			typename Algo::ArgsType(*this,jdata,qs.derived(),vs.derived()));
146			}
147
148		42	jdata.M = jdata.iMlast.front();
149		42	}
150
151			} // namespace pinocchio
152
153			#endif // ifndef __pinocchio_joint_composite_hxx__