| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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__ |
| Generated by: GCOVR (Version 4.2) |