Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/algorithm/frames.hxx	Lines:	135	142	95.1 %
Date:	2024-04-26 13:14:21	Branches:	109	267	40.8 %


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

#ifndef __pinocchio_algorithm_frames_hxx__
#define __pinocchio_algorithm_frames_hxx__

#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/check.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void updateFramePlacements(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::Frame Frame;
    typedef typename Model::FrameIndex FrameIndex;
    typedef typename Model::JointIndex JointIndex;

    // The following for loop starts by index 1 because the first frame is fixed
    // and corresponds to the universe.s
    for(FrameIndex i=1; i < (FrameIndex) model.nframes; ++i)
    {
      const Frame & frame = model.frames[i];
      const JointIndex & parent = frame.parent;
      data.oMf[i] = data.oMi[parent]*frame.placement;
    }
  }

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

    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    const typename Model::Frame & frame = model.frames[frame_id];
    const typename Model::JointIndex & parent = frame.parent;

    data.oMf[frame_id] = data.oMi[parent]*frame.placement;

    return data.oMf[frame_id];
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline void framesForwardKinematics(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.");

    forwardKinematics(model, data, q);
    updateFramePlacements(model, data);
  }

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

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

    const typename Model::Frame & frame = model.frames[frame_id];
    const typename Model::SE3 & oMi = data.oMi[frame.parent];
    const typename Model::Motion & v = data.v[frame.parent];
    switch(rf)
    {
      case LOCAL:
        return frame.placement.actInv(v);
      case WORLD:
        return oMi.act(v);
      case LOCAL_WORLD_ALIGNED:
        return Motion(oMi.rotation() * (v.linear() + v.angular().cross(frame.placement.translation())),
                      oMi.rotation() * v.angular());
      default:
        throw std::invalid_argument("Bad reference frame.");
    }
  }

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

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

    const typename Model::Frame & frame = model.frames[frame_id];
    const typename Model::SE3 & oMi = data.oMi[frame.parent];
    const typename Model::Motion & a = data.a[frame.parent];
    switch(rf)
    {
      case LOCAL:
        return frame.placement.actInv(a);
      case WORLD:
        return oMi.act(a);
      case LOCAL_WORLD_ALIGNED:
        return Motion(oMi.rotation() * (a.linear() + a.angular().cross(frame.placement.translation())),
                      oMi.rotation() * a.angular());
      default:
        throw std::invalid_argument("Bad reference frame.");
    }
  }

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

    typedef MotionTpl<Scalar, Options> Motion;
    Motion vel = getFrameVelocity(model, data, frame_id, rf);
    Motion acc = getFrameAcceleration(model, data, frame_id, rf);

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

    return acc;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
  inline void getFrameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                               DataTpl<Scalar,Options,JointCollectionTpl> & data,
                               const FrameIndex frame_id,
                               const ReferenceFrame rf,
                               const Eigen::MatrixBase<Matrix6xLike> & J)
  {

    PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), 6);
    PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv);
    assert(model.check(data) && "data is not consistent with model.");

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

    const Frame & frame = model.frames[frame_id];
    const JointIndex & joint_id = frame.parent;

    Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
    typename Data::SE3 & oMframe = data.oMf[frame_id];
    oMframe = data.oMi[joint_id] * frame.placement;

    details::translateJointJacobian(model,data,joint_id,rf,oMframe,data.J,J_);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
  inline void computeFrameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                   DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                   const Eigen::MatrixBase<ConfigVectorType> & q,
                                   const FrameIndex frameId,
                                   const ReferenceFrame reference_frame,
                                   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 DataTpl<Scalar,Options,JointCollectionTpl> Data;
    typedef typename Model::Frame Frame;
    typedef typename Model::JointIndex JointIndex;
    typedef typename Model::IndexVector IndexVector;

    const Frame & frame = model.frames[frameId];
    const JointIndex & joint_id = frame.parent;

    const IndexVector & joint_support = model.supports[joint_id];

    switch(reference_frame)
    {
      case WORLD:
      case LOCAL_WORLD_ALIGNED:
      {
        typedef JointJacobiansForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> Pass;
        for(size_t k = 1; k < joint_support.size(); k++)
        {
          JointIndex parent = joint_support[k];
          Pass::run(model.joints[parent],data.joints[parent],
                    typename Pass::ArgsType(model,data,q.derived(),
                                            PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J)));
        }

        if(reference_frame == LOCAL_WORLD_ALIGNED)
        {
          typename Data::SE3 & oMframe = data.oMf[frameId];
          oMframe = data.oMi[joint_id] * frame.placement;

          Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);

          const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
          for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
          {
            typedef typename Matrix6xLike::ColXpr ColXprOut;
            MotionRef<ColXprOut> J_col(J_.col(j));

            J_col.linear() -= oMframe.translation().cross(J_col.angular());
          }
        }
        break;
      }
      case LOCAL:
      {
        data.iMf[joint_id] = frame.placement;

        typedef JointJacobianForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> Pass;
        for(JointIndex i=joint_id; 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)));
        }
        break;
      }
      default:
      {
        assert(false && "must never happened");
      }
    }
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
  void getFrameJacobianTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                     DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                     const FrameIndex frame_id,
                                     const ReferenceFrame rf,
                                     const Eigen::MatrixBase<Matrix6xLike> & dJ)
  {
    assert(model.check(data) && "data is not consistent with model.");

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

    const Frame & frame = model.frames[frame_id];
    const JointIndex & joint_id = frame.parent;

    typename Data::SE3 & oMframe = data.oMf[frame_id];
    oMframe = data.oMi[joint_id] * frame.placement;

    details::translateJointJacobian(model,data,joint_id,rf,oMframe,
                                    data.dJ,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ));
  }


  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  InertiaTpl<Scalar, Options>
  computeSupportedInertiaByFrame(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                const FrameIndex frame_id,
                                bool with_subtree)
  {
    assert(model.check(data) && "data is not consistent with model.");

    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef InertiaTpl<Scalar, Options> Inertia;

    const Frame & frame = model.frames[frame_id];
    const JointIndex & joint_id = frame.parent;

    // Add all the inertia of child frames (i.e that are part of the same joint but comes after the given frame)
    std::vector<typename Model::JointIndex> child_frames = {frame_id};
    Inertia I = frame.placement.act(frame.inertia); // Express the inertia in the parent joint frame
    for(FrameIndex i=frame_id+1; i < (FrameIndex) model.nframes; ++i)
    {
      if(model.frames[i].parent != joint_id)
        continue;
      if(std::find(child_frames.begin(), child_frames.end(), model.frames[i].previousFrame) == child_frames.end())
        continue;
      child_frames.push_back(i);
      I += model.frames[i].placement.act(model.frames[i].inertia);
    }

    if(!with_subtree)
    {
      return frame.placement.actInv(I);
    }

    // Express the inertia in the origin frame for simplicity.
    I = data.oMi[joint_id].act(I);

    // Add inertia of child joints
    const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id];
    for(size_t k=1; k < subtree.size(); ++k) // Skip the first joint as it is the one before the frame
    {
        const typename Model::JointIndex j_id = subtree[k];
        I += data.oMi[j_id].act(model.inertias[j_id]);
    }

    const pinocchio::SE3 oMf = data.oMi[joint_id]*frame.placement;
    return oMf.actInv(I);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  ForceTpl<Scalar, Options>
  computeSupportedForceByFrame(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                               const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                               const FrameIndex frame_id)
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef InertiaTpl<Scalar, Options> Inertia;
    typedef MotionTpl<Scalar, Options> Motion;
    typedef ForceTpl<Scalar, Options> Force;

    const Frame & frame = model.frames[frame_id];
    const JointIndex & joint_id = frame.parent;

    // Compute 'in body' forces
    const Inertia fI = computeSupportedInertiaByFrame(model, data, frame_id, false);
    const pinocchio::SE3 oMf = data.oMi[joint_id]*frame.placement;
    const Motion v = getFrameVelocity(model, data, frame_id, LOCAL);
    const Motion a = getFrameAcceleration(model, data, frame_id, LOCAL);
    Force f = fI.vxiv(v) + fI * (a - oMf.actInv(model.gravity));

    // Add child joints forces
    f = frame.placement.act(f); // Express force in parent joint frame
    const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id];
    for(size_t k=1; k < subtree.size(); ++k) // Skip the first joint as it is the one before the frame
    {
        const typename Model::JointIndex j_id = subtree[k];
        if(model.parents[j_id] != joint_id) // Joint is not a direct child
        {
          continue;
        }
        f += data.liMi[j_id].act(data.f[j_id]);
    }

    // Transform back to local frame
    return frame.placement.actInv(f);
  }
} // namespace pinocchio

#endif // ifndef __pinocchio_algorithm_frames_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2020 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_algorithm_frames_hxx__
6			#define __pinocchio_algorithm_frames_hxx__
7
8			#include "pinocchio/algorithm/kinematics.hpp"
9			#include "pinocchio/algorithm/jacobian.hpp"
10			#include "pinocchio/algorithm/check.hpp"
11
12			namespace pinocchio
13			{
14
15			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
16		8995	inline void updateFramePlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
17			DataTpl<Scalar,Options,JointCollectionTpl> & data)
18			{
19	✓✗	8995	assert(model.check(data) && "data is not consistent with model.");
20
21			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
22			typedef typename Model::Frame Frame;
23			typedef typename Model::FrameIndex FrameIndex;
24			typedef typename Model::JointIndex JointIndex;
25
26			// The following for loop starts by index 1 because the first frame is fixed
27			// and corresponds to the universe.s
28	✓✓	503702	for(FrameIndex i=1; i < (FrameIndex) model.nframes; ++i)
29			{
30		494707	const Frame & frame = model.frames[i];
31		494707	const JointIndex & parent = frame.parent;
32		494707	data.oMf[i] = data.oMi[parent]*frame.placement;
33			}
34		8995	}
35
36			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
37			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::SE3 &
38		4	updateFramePlacement(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
39			DataTpl<Scalar,Options,JointCollectionTpl> & data,
40			const FrameIndex frame_id)
41			{
42	✓✗	4	assert(model.check(data) && "data is not consistent with model.");
43
44			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
45		4	const typename Model::Frame & frame = model.frames[frame_id];
46		4	const typename Model::JointIndex & parent = frame.parent;
47
48		4	data.oMf[frame_id] = data.oMi[parent]*frame.placement;
49
50		4	return data.oMf[frame_id];
51			}
52
53			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
54		9	inline void framesForwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
55			DataTpl<Scalar,Options,JointCollectionTpl> & data,
56			const Eigen::MatrixBase<ConfigVectorType> & q)
57			{
58	✓✗	9	assert(model.check(data) && "data is not consistent with model.");
59
60		9	forwardKinematics(model, data, q);
61		9	updateFramePlacements(model, data);
62		9	}
63
64			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
65			inline MotionTpl<Scalar, Options>
66		216	getFrameVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
67			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
68			const FrameIndex frame_id,
69			const ReferenceFrame rf)
70			{
71	✓✗	216	assert(model.check(data) && "data is not consistent with model.");
72
73			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
74			typedef typename Model::Motion Motion;
75
76		216	const typename Model::Frame & frame = model.frames[frame_id];
77		216	const typename Model::SE3 & oMi = data.oMi[frame.parent];
78		216	const typename Model::Motion & v = data.v[frame.parent];
79	✓✓✓✗	216	switch(rf)
80			{
81		78	case LOCAL:
82		78	return frame.placement.actInv(v);
83		69	case WORLD:
84		69	return oMi.act(v);
85		69	case LOCAL_WORLD_ALIGNED:
86	✓✗✓✗	138	return Motion(oMi.rotation() * (v.linear() + v.angular().cross(frame.placement.translation())),
87	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	207	oMi.rotation() * v.angular());
88			default:
89			throw std::invalid_argument("Bad reference frame.");
90			}
91			}
92
93			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
94			inline MotionTpl<Scalar, Options>
95		315	getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
96			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
97			const FrameIndex frame_id,
98			const ReferenceFrame rf)
99			{
100	✓✗	315	assert(model.check(data) && "data is not consistent with model.");
101
102			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
103			typedef typename Model::Motion Motion;
104
105		315	const typename Model::Frame & frame = model.frames[frame_id];
106		315	const typename Model::SE3 & oMi = data.oMi[frame.parent];
107		315	const typename Model::Motion & a = data.a[frame.parent];
108	✓✓✓✗	315	switch(rf)
109			{
110		111	case LOCAL:
111		111	return frame.placement.actInv(a);
112		102	case WORLD:
113		102	return oMi.act(a);
114		102	case LOCAL_WORLD_ALIGNED:
115	✓✗✓✗	204	return Motion(oMi.rotation() * (a.linear() + a.angular().cross(frame.placement.translation())),
116	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	306	oMi.rotation() * a.angular());
117			default:
118			throw std::invalid_argument("Bad reference frame.");
119			}
120			}
121
122			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
123			inline MotionTpl<Scalar, Options>
124		9	getFrameClassicalAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
125			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
126			const FrameIndex frame_id,
127			const ReferenceFrame rf)
128			{
129	✓✗✓✗	9	assert(model.check(data) && "data is not consistent with model.");
130
131			typedef MotionTpl<Scalar, Options> Motion;
132	✓✗	9	Motion vel = getFrameVelocity(model, data, frame_id, rf);
133	✓✗	9	Motion acc = getFrameAcceleration(model, data, frame_id, rf);
134
135	✓✗✓✗ ✓✗✓✗ ✓✗	9	acc.linear() += vel.angular().cross(vel.linear());
136
137		18	return acc;
138			}
139
140			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
141		20	inline void getFrameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
142			DataTpl<Scalar,Options,JointCollectionTpl> & data,
143			const FrameIndex frame_id,
144			const ReferenceFrame rf,
145			const Eigen::MatrixBase<Matrix6xLike> & J)
146			{
147
148	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	20	PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), 6);
149	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	20	PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv);
150	✓✗	20	assert(model.check(data) && "data is not consistent with model.");
151
152			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
153			typedef typename Model::Frame Frame;
154			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
155			typedef typename Model::JointIndex JointIndex;
156
157		20	const Frame & frame = model.frames[frame_id];
158		20	const JointIndex & joint_id = frame.parent;
159
160		20	Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
161		20	typename Data::SE3 & oMframe = data.oMf[frame_id];
162		20	oMframe = data.oMi[joint_id] * frame.placement;
163
164		20	details::translateJointJacobian(model,data,joint_id,rf,oMframe,data.J,J_);
165		20	}
166
167			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
168		8	inline void computeFrameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
169			DataTpl<Scalar,Options,JointCollectionTpl> & data,
170			const Eigen::MatrixBase<ConfigVectorType> & q,
171			const FrameIndex frameId,
172			const ReferenceFrame reference_frame,
173			const Eigen::MatrixBase<Matrix6xLike> & J)
174			{
175	✓✗	8	assert(model.check(data) && "data is not consistent with model.");
176	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	8	PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
177
178			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
179			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
180			typedef typename Model::Frame Frame;
181			typedef typename Model::JointIndex JointIndex;
182			typedef typename Model::IndexVector IndexVector;
183
184		8	const Frame & frame = model.frames[frameId];
185		8	const JointIndex & joint_id = frame.parent;
186
187		8	const IndexVector & joint_support = model.supports[joint_id];
188
189	✓✓✗	8	switch(reference_frame)
190			{
191		2	case WORLD:
192			case LOCAL_WORLD_ALIGNED:
193			{
194			typedef JointJacobiansForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> Pass;
195	✓✓	12	for(size_t k = 1; k < joint_support.size(); k++)
196			{
197		10	JointIndex parent = joint_support[k];
198	✓✗	10	Pass::run(model.joints[parent],data.joints[parent],
199			typename Pass::ArgsType(model,data,q.derived(),
200		10	PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J)));
201			}
202
203	✓✓	2	if(reference_frame == LOCAL_WORLD_ALIGNED)
204			{
205		1	typename Data::SE3 & oMframe = data.oMf[frameId];
206		1	oMframe = data.oMi[joint_id] * frame.placement;
207
208		1	Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
209
210		1	const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
211	✓✓	11	for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
212			{
213			typedef typename Matrix6xLike::ColXpr ColXprOut;
214	✓✗✓✗	10	MotionRef<ColXprOut> J_col(J_.col(j));
215
216	✓✗✓✗ ✓✗✓✗ ✓✗	10	J_col.linear() -= oMframe.translation().cross(J_col.angular());
217			}
218			}
219		2	break;
220			}
221		6	case LOCAL:
222			{
223		6	data.iMf[joint_id] = frame.placement;
224
225			typedef JointJacobianForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> Pass;
226	✓✓	32	for(JointIndex i=joint_id; i>0; i=model.parents[i])
227			{
228	✓✗	26	Pass::run(model.joints[i],data.joints[i],
229			typename Pass::ArgsType(model,data,q.derived(),
230		26	PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J)));
231			}
232		6	break;
233			}
234			default:
235			{
236			assert(false && "must never happened");
237			}
238			}
239		8	}
240
241			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
242		6	void getFrameJacobianTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
243			DataTpl<Scalar,Options,JointCollectionTpl> & data,
244			const FrameIndex frame_id,
245			const ReferenceFrame rf,
246			const Eigen::MatrixBase<Matrix6xLike> & dJ)
247			{
248	✓✗	6	assert(model.check(data) && "data is not consistent with model.");
249
250			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
251			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
252			typedef typename Model::Frame Frame;
253
254		6	const Frame & frame = model.frames[frame_id];
255		6	const JointIndex & joint_id = frame.parent;
256
257		6	typename Data::SE3 & oMframe = data.oMf[frame_id];
258		6	oMframe = data.oMi[joint_id] * frame.placement;
259
260		6	details::translateJointJacobian(model,data,joint_id,rf,oMframe,
261		6	data.dJ,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ));
262		6	}
263
264
265			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
266			InertiaTpl<Scalar, Options>
267		3	computeSupportedInertiaByFrame(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
268			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
269			const FrameIndex frame_id,
270			bool with_subtree)
271			{
272	✓✗✓✗	3	assert(model.check(data) && "data is not consistent with model.");
273
274			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
275			typedef InertiaTpl<Scalar, Options> Inertia;
276
277		3	const Frame & frame = model.frames[frame_id];
278		3	const JointIndex & joint_id = frame.parent;
279
280			// Add all the inertia of child frames (i.e that are part of the same joint but comes after the given frame)
281	✓✗	6	std::vector<typename Model::JointIndex> child_frames = {frame_id};
282	✓✗	3	Inertia I = frame.placement.act(frame.inertia); // Express the inertia in the parent joint frame
283	✓✓	96	for(FrameIndex i=frame_id+1; i < (FrameIndex) model.nframes; ++i)
284			{
285	✓✓	93	if(model.frames[i].parent != joint_id)
286		90	continue;
287	✓✗✗✓	3	if(std::find(child_frames.begin(), child_frames.end(), model.frames[i].previousFrame) == child_frames.end())
288			continue;
289	✓✗	3	child_frames.push_back(i);
290	✓✗✓✗	3	I += model.frames[i].placement.act(model.frames[i].inertia);
291			}
292
293	✓✓	3	if(!with_subtree)
294			{
295	✓✗	2	return frame.placement.actInv(I);
296			}
297
298			// Express the inertia in the origin frame for simplicity.
299	✓✗✓✗	1	I = data.oMi[joint_id].act(I);
300
301			// Add inertia of child joints
302		1	const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id];
303	✓✓	15	for(size_t k=1; k < subtree.size(); ++k) // Skip the first joint as it is the one before the frame
304			{
305		14	const typename Model::JointIndex j_id = subtree[k];
306	✓✗✓✗	14	I += data.oMi[j_id].act(model.inertias[j_id]);
307			}
308
309	✓✗	1	const pinocchio::SE3 oMf = data.oMi[joint_id]*frame.placement;
310	✓✗	1	return oMf.actInv(I);
311			}
312
313			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
314			ForceTpl<Scalar, Options>
315		1	computeSupportedForceByFrame(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
316			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
317			const FrameIndex frame_id)
318			{
319			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
320			typedef InertiaTpl<Scalar, Options> Inertia;
321			typedef MotionTpl<Scalar, Options> Motion;
322			typedef ForceTpl<Scalar, Options> Force;
323
324		1	const Frame & frame = model.frames[frame_id];
325		1	const JointIndex & joint_id = frame.parent;
326
327			// Compute 'in body' forces
328	✓✗	1	const Inertia fI = computeSupportedInertiaByFrame(model, data, frame_id, false);
329	✓✗	1	const pinocchio::SE3 oMf = data.oMi[joint_id]*frame.placement;
330	✓✗	1	const Motion v = getFrameVelocity(model, data, frame_id, LOCAL);
331	✓✗	1	const Motion a = getFrameAcceleration(model, data, frame_id, LOCAL);
332	✓✗✓✗ ✓✗✓✗ ✓✗	1	Force f = fI.vxiv(v) + fI * (a - oMf.actInv(model.gravity));
333
334			// Add child joints forces
335	✓✗✓✗	1	f = frame.placement.act(f); // Express force in parent joint frame
336		1	const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id];
337	✓✓	15	for(size_t k=1; k < subtree.size(); ++k) // Skip the first joint as it is the one before the frame
338			{
339		14	const typename Model::JointIndex j_id = subtree[k];
340	✓✓	14	if(model.parents[j_id] != joint_id) // Joint is not a direct child
341			{
342		11	continue;
343			}
344	✓✗✓✗	3	f += data.liMi[j_id].act(data.f[j_id]);
345			}
346
347			// Transform back to local frame
348	✓✗	2	return frame.placement.actInv(f);
349			}
350			} // namespace pinocchio
351
352			#endif // ifndef __pinocchio_algorithm_frames_hxx__