Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/algorithm/kinematics.hxx	Lines:	86	90	95.6 %
Date:	2024-04-26 13:14:21	Branches:	71	269	26.4 %


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

#ifndef __pinocchio_kinematics_hxx__
#define __pinocchio_kinematics_hxx__

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

namespace pinocchio
{

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

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

    for(JointIndex i=1; i <(JointIndex) model.njoints; ++i)
    {
      const JointIndex & parent = model.parents[i];

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


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

    typedef boost::fusion::vector<const Model &,
                                  Data &,
                                  const ConfigVectorType &> 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)
    {
      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];
    }
  };

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

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

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

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  struct ForwardKinematicFirstStep
  : fusion::JointUnaryVisitorBase< ForwardKinematicFirstStep<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 ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;

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

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

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

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

  };


  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  inline void forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                const Eigen::MatrixBase<ConfigVectorType> & q,
                                const Eigen::MatrixBase<TangentVectorType> & v)
  {
    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");
    assert(model.check(data) && "data is not consistent with model.");

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

    data.v[0].setZero();

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

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
  struct ForwardKinematicSecondStep :
  fusion::JointUnaryVisitorBase< ForwardKinematicSecondStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> >
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typedef boost::fusion::vector<const Model &,
                                  Data &,
                                  const ConfigVectorType &,
                                  const TangentVectorType1 &,
                                  const TangentVectorType2 &
                                  > 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<TangentVectorType1> & v,
                     const Eigen::MatrixBase<TangentVectorType2> & a)
    {
      typedef typename Model::JointIndex JointIndex;

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

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

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

      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.a[i]  = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
      data.a[i] += data.liMi[i].actInv(data.a[parent]);
    }
  };

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
  inline void forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                const Eigen::MatrixBase<ConfigVectorType> & q,
                                const Eigen::MatrixBase<TangentVectorType1> & v,
                                const Eigen::MatrixBase<TangentVectorType2> & a)
  {
    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");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(a.size(), model.nv, "The acceleration vector is not of right size");
    assert(model.check(data) && "data is not consistent with model.");

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

    data.v[0].setZero();
    data.a[0].setZero();

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

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline MotionTpl<Scalar, Options>
  getVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
              const DataTpl<Scalar,Options,JointCollectionTpl> & data,
              const JointIndex jointId,
              const ReferenceFrame rf)
  {
    assert(model.check(data) && "data is not consistent with model.");
    PINOCCHIO_UNUSED_VARIABLE(model);
    switch(rf)
    {
      case LOCAL:
        return data.v[jointId];
      case WORLD:
        return data.oMi[jointId].act(data.v[jointId]);
      case LOCAL_WORLD_ALIGNED:
        return MotionTpl<Scalar, Options>(data.oMi[jointId].rotation() * data.v[jointId].linear(), data.oMi[jointId].rotation() * data.v[jointId].angular());
      default:
        throw std::invalid_argument("Bad reference frame.");
    }
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline MotionTpl<Scalar, Options>
  getAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                  const JointIndex jointId,
                  const ReferenceFrame rf)
  {
    assert(model.check(data) && "data is not consistent with model.");
    PINOCCHIO_UNUSED_VARIABLE(model);
    switch(rf)
    {
      case LOCAL:
        return data.a[jointId];
      case WORLD:
        return data.oMi[jointId].act(data.a[jointId]);
      case LOCAL_WORLD_ALIGNED:
        return MotionTpl<Scalar, Options>(data.oMi[jointId].rotation() * data.a[jointId].linear(), data.oMi[jointId].rotation() * data.a[jointId].angular());
      default:
        throw std::invalid_argument("Bad reference frame.");
    }
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline MotionTpl<Scalar, Options>
  getClassicalAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                           const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                           const JointIndex jointId,
                           const ReferenceFrame rf)
  {
    assert(model.check(data) && "data is not consistent with model.");

    typedef MotionTpl<Scalar, Options> Motion;
    Motion vel = getVelocity(model, data, jointId, rf);
    Motion acc = getAcceleration(model, data, jointId, rf);

    acc.linear() += vel.angular().cross(vel.linear());

    return acc;
  }
} // namespace pinocchio

#endif // ifndef __pinocchio_kinematics_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2019 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_kinematics_hxx__
6			#define __pinocchio_kinematics_hxx__
7
8			#include "pinocchio/multibody/visitor.hpp"
9			#include "pinocchio/algorithm/check.hpp"
10
11			namespace pinocchio
12			{
13
14			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
15		1	inline void updateGlobalPlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
16			DataTpl<Scalar,Options,JointCollectionTpl> & data)
17			{
18	✓✗	1	assert(model.check(data) && "data is not consistent with model.");
19
20			typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
21
22	✓✓	28	for(JointIndex i=1; i <(JointIndex) model.njoints; ++i)
23			{
24		27	const JointIndex & parent = model.parents[i];
25
26	✓✓	27	if (parent>0)
27		26	data.oMi[i] = data.oMi[parent] * data.liMi[i];
28			else
29		1	data.oMi[i] = data.liMi[i];
30			}
31		1	}
32
33
34			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
35			struct ForwardKinematicZeroStep
36			: fusion::JointUnaryVisitorBase< ForwardKinematicZeroStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> >
37			{
38			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
39			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
40
41			typedef boost::fusion::vector<const Model &,
42			Data &,
43			const ConfigVectorType &> ArgsType;
44
45			template<typename JointModel>
46		776854	static void algo(const JointModelBase<JointModel> & jmodel,
47			JointDataBase<typename JointModel::JointDataDerived> & jdata,
48			const Model & model,
49			Data & data,
50			const Eigen::MatrixBase<ConfigVectorType> & q)
51			{
52			typedef typename Model::JointIndex JointIndex;
53
54		776854	const JointIndex & i = jmodel.id();
55		776854	const JointIndex & parent = model.parents[i];
56
57		776854	jmodel.calc(jdata.derived(),q.derived());
58
59	✓✗	776854	data.liMi[i] = model.jointPlacements[i] * jdata.M();
60
61	✓✓	776854	if (parent>0)
62		747770	data.oMi[i] = data.oMi[parent] * data.liMi[i];
63			else
64		29084	data.oMi[i] = data.liMi[i];
65			}
66			};
67
68			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
69		14534	inline void forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
70			DataTpl<Scalar,Options,JointCollectionTpl> & data,
71			const Eigen::MatrixBase<ConfigVectorType> & q)
72			{
73	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	14534	PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
74	✓✗	14534	assert(model.check(data) && "data is not consistent with model.");
75
76			typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
77
78			typedef ForwardKinematicZeroStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> Algo;
79	✓✓	403015	for(JointIndex i=1; i < (JointIndex)model.njoints; ++i)
80			{
81	✓✗	388481	Algo::run(model.joints[i], data.joints[i],
82			typename Algo::ArgsType(model,data,q.derived()));
83			}
84		14534	}
85
86			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
87			struct ForwardKinematicFirstStep
88			: fusion::JointUnaryVisitorBase< ForwardKinematicFirstStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> >
89			{
90			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
91			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
92
93			typedef boost::fusion::vector<const Model &,
94			Data &,
95			const ConfigVectorType &,
96			const TangentVectorType &
97			> ArgsType;
98
99			template<typename JointModel>
100		11238	static void algo(const JointModelBase<JointModel> & jmodel,
101			JointDataBase<typename JointModel::JointDataDerived> & jdata,
102			const Model & model,
103			Data & data,
104			const Eigen::MatrixBase<ConfigVectorType> & q,
105			const Eigen::MatrixBase<TangentVectorType> & v)
106			{
107			typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
108
109		11238	const JointIndex & i = jmodel.id();
110		11238	const JointIndex & parent = model.parents[i];
111
112		11238	jmodel.calc(jdata.derived(),q.derived(),v.derived());
113
114		11238	data.v[i] = jdata.v();
115	✓✗	11238	data.liMi[i] = model.jointPlacements[i]*jdata.M();
116
117	✓✓	11238	if(parent>0)
118			{
119		10794	data.oMi[i] = data.oMi[parent]*data.liMi[i];
120	✓✗	10794	data.v[i] += data.liMi[i].actInv(data.v[parent]);
121			}
122			else
123		444	data.oMi[i] = data.liMi[i];
124			}
125
126			};
127
128
129			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
130		250	inline void forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
131			DataTpl<Scalar,Options,JointCollectionTpl> & data,
132			const Eigen::MatrixBase<ConfigVectorType> & q,
133			const Eigen::MatrixBase<TangentVectorType> & v)
134			{
135	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	250	PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
136	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	250	PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
137	✓✗	250	assert(model.check(data) && "data is not consistent with model.");
138
139			typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
140
141		250	data.v[0].setZero();
142
143			typedef ForwardKinematicFirstStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Algo;
144	✓✓	6273	for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
145			{
146	✓✗	6023	Algo::run(model.joints[i],data.joints[i],
147			typename Algo::ArgsType(model,data,q.derived(),v.derived()));
148			}
149		250	}
150
151			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
152			struct ForwardKinematicSecondStep :
153			fusion::JointUnaryVisitorBase< ForwardKinematicSecondStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> >
154			{
155			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
156			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
157
158			typedef boost::fusion::vector<const Model &,
159			Data &,
160			const ConfigVectorType &,
161			const TangentVectorType1 &,
162			const TangentVectorType2 &
163			> ArgsType;
164
165			template<typename JointModel>
166		23092	static void algo(const JointModelBase<JointModel> & jmodel,
167			JointDataBase<typename JointModel::JointDataDerived> & jdata,
168			const Model & model,
169			Data & data,
170			const Eigen::MatrixBase<ConfigVectorType> & q,
171			const Eigen::MatrixBase<TangentVectorType1> & v,
172			const Eigen::MatrixBase<TangentVectorType2> & a)
173			{
174			typedef typename Model::JointIndex JointIndex;
175
176		23092	const JointIndex & i = jmodel.id();
177		23092	const JointIndex & parent = model.parents[i];
178
179		23092	jmodel.calc(jdata.derived(),q.derived(),v.derived());
180
181		23092	data.v[i] = jdata.v();
182	✓✗	23092	data.liMi[i] = model.jointPlacements[i] * jdata.M();
183
184	✓✓	23092	if(parent>0)
185			{
186		22238	data.oMi[i] = data.oMi[parent] * data.liMi[i];
187	✓✗	22238	data.v[i] += data.liMi[i].actInv(data.v[parent]);
188			}
189			else
190		854	data.oMi[i] = data.liMi[i];
191
192	✓✗✓✗ ✓✗✓✗ ✓✓✗✓ ✗	23092	data.a[i] = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
193	✓✗	23092	data.a[i] += data.liMi[i].actInv(data.a[parent]);
194			}
195			};
196
197			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
198		530	inline void forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
199			DataTpl<Scalar,Options,JointCollectionTpl> & data,
200			const Eigen::MatrixBase<ConfigVectorType> & q,
201			const Eigen::MatrixBase<TangentVectorType1> & v,
202			const Eigen::MatrixBase<TangentVectorType2> & a)
203			{
204	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	530	PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
205	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	530	PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
206	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	530	PINOCCHIO_CHECK_ARGUMENT_SIZE(a.size(), model.nv, "The acceleration vector is not of right size");
207	✓✗	530	assert(model.check(data) && "data is not consistent with model.");
208
209			typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
210
211		530	data.v[0].setZero();
212		530	data.a[0].setZero();
213
214			typedef ForwardKinematicSecondStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> Algo;
215	✓✓	14857	for(JointIndex i=1; i < (JointIndex)model.njoints; ++i)
216			{
217	✓✗	14327	Algo::run(model.joints[i],data.joints[i],
218			typename Algo::ArgsType(model,data,q.derived(),v.derived(),a.derived()));
219			}
220		530	}
221
222			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
223			inline MotionTpl<Scalar, Options>
224		224	getVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
225			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
226			const JointIndex jointId,
227			const ReferenceFrame rf)
228			{
229	✓✗	224	assert(model.check(data) && "data is not consistent with model.");
230			PINOCCHIO_UNUSED_VARIABLE(model);
231	✓✓✓✗	224	switch(rf)
232			{
233		112	case LOCAL:
234		112	return data.v[jointId];
235		56	case WORLD:
236		56	return data.oMi[jointId].act(data.v[jointId]);
237		56	case LOCAL_WORLD_ALIGNED:
238	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	56	return MotionTpl<Scalar, Options>(data.oMi[jointId].rotation() * data.v[jointId].linear(), data.oMi[jointId].rotation() * data.v[jointId].angular());
239			default:
240			throw std::invalid_argument("Bad reference frame.");
241			}
242			}
243
244			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
245			inline MotionTpl<Scalar, Options>
246		224	getAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
247			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
248			const JointIndex jointId,
249			const ReferenceFrame rf)
250			{
251	✓✗	224	assert(model.check(data) && "data is not consistent with model.");
252			PINOCCHIO_UNUSED_VARIABLE(model);
253	✓✓✓✗	224	switch(rf)
254			{
255		112	case LOCAL:
256		112	return data.a[jointId];
257		56	case WORLD:
258		56	return data.oMi[jointId].act(data.a[jointId]);
259		56	case LOCAL_WORLD_ALIGNED:
260	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	56	return MotionTpl<Scalar, Options>(data.oMi[jointId].rotation() * data.a[jointId].linear(), data.oMi[jointId].rotation() * data.a[jointId].angular());
261			default:
262			throw std::invalid_argument("Bad reference frame.");
263			}
264			}
265
266			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
267			inline MotionTpl<Scalar, Options>
268		112	getClassicalAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
269			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
270			const JointIndex jointId,
271			const ReferenceFrame rf)
272			{
273	✓✗✓✗	112	assert(model.check(data) && "data is not consistent with model.");
274
275			typedef MotionTpl<Scalar, Options> Motion;
276	✓✗	112	Motion vel = getVelocity(model, data, jointId, rf);
277	✓✗	112	Motion acc = getAcceleration(model, data, jointId, rf);
278
279	✓✗✓✗ ✓✗✓✗ ✓✗	112	acc.linear() += vel.angular().cross(vel.linear());
280
281		224	return acc;
282			}
283			} // namespace pinocchio
284
285			#endif // ifndef __pinocchio_kinematics_hxx__