| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2019 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_algorithm_centroidal_hxx__ |
||
6 |
#define __pinocchio_algorithm_centroidal_hxx__ |
||
7 |
|||
8 |
#include "pinocchio/multibody/visitor.hpp" |
||
9 |
#include "pinocchio/spatial/act-on-set.hpp" |
||
10 |
#include "pinocchio/algorithm/kinematics.hpp" |
||
11 |
#include "pinocchio/algorithm/check.hpp" |
||
12 |
|||
13 |
/// @cond DEV |
||
14 |
|||
15 |
namespace pinocchio |
||
16 |
{ |
||
17 |
|||
18 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
19 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Force & |
||
20 |
4 |
computeCentroidalMomentum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
21 |
DataTpl<Scalar,Options,JointCollectionTpl> & data) |
||
22 |
{ |
||
23 |
✓✗ | 4 |
assert(model.check(data) && "data is not consistent with model."); |
24 |
|||
25 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
26 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
27 |
typedef typename Model::JointIndex JointIndex; |
||
28 |
|||
29 |
✓✓ | 116 |
for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) |
30 |
{ |
||
31 |
112 |
const typename Data::Inertia & Y = model.inertias[i]; |
|
32 |
|||
33 |
112 |
data.mass[i] = Y.mass(); |
|
34 |
✓✗✓✗ |
112 |
data.com[i] = Y.mass() * Y.lever(); |
35 |
|||
36 |
✓✗ | 112 |
data.h[i] = Y * data.v[i]; |
37 |
} |
||
38 |
|||
39 |
4 |
data.mass[0] = 0.; data.com[0].setZero(); data.h[0].setZero(); |
|
40 |
✓✓ | 116 |
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) |
41 |
{ |
||
42 |
112 |
const JointIndex & parent = model.parents[i]; |
|
43 |
112 |
const typename Data::SE3 & liMi = data.liMi[i]; |
|
44 |
|||
45 |
112 |
data.mass[parent] += data.mass[i]; |
|
46 |
✓✗✓✗ ✓✗✓✗ |
112 |
data.com[parent] += liMi.rotation()*data.com[i] + data.mass[i]*liMi.translation(); |
47 |
✓✗ | 112 |
data.h[parent] += data.liMi[i].act(data.h[i]); |
48 |
} |
||
49 |
|||
50 |
4 |
data.com[0] /= data.mass[0]; |
|
51 |
|||
52 |
4 |
data.hg = data.h[0]; |
|
53 |
✓✗✓✗ ✓✗ |
4 |
data.hg.angular() += data.hg.linear().cross(data.com[0]); |
54 |
|||
55 |
✓✗✓✗ ✓✗ |
4 |
data.vcom[0].noalias() = data.hg.linear()/data.mass[0]; |
56 |
|||
57 |
4 |
return data.hg; |
|
58 |
} |
||
59 |
|||
60 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
61 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Force & |
||
62 |
109 |
computeCentroidalMomentumTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
63 |
DataTpl<Scalar,Options,JointCollectionTpl> & data) |
||
64 |
{ |
||
65 |
✓✗ | 109 |
assert(model.check(data) && "data is not consistent with model."); |
66 |
|||
67 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
68 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
69 |
typedef typename Model::JointIndex JointIndex; |
||
70 |
|||
71 |
✓✓ | 3161 |
for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) |
72 |
{ |
||
73 |
3052 |
const typename Data::Inertia & Y = model.inertias[i]; |
|
74 |
|||
75 |
3052 |
data.mass[i] = Y.mass(); |
|
76 |
✓✗✓✗ |
3052 |
data.com[i] = Y.mass() * Y.lever(); |
77 |
|||
78 |
✓✗ | 3052 |
data.h[i] = Y * data.v[i]; |
79 |
✓✗✓✗ ✓✗ |
3052 |
data.f[i] = Y * data.a[i] + data.v[i].cross(data.h[i]); |
80 |
} |
||
81 |
|||
82 |
109 |
data.mass[0] = 0.; data.com[0].setZero(); |
|
83 |
109 |
data.h[0].setZero(); |
|
84 |
109 |
data.f[0].setZero(); |
|
85 |
|||
86 |
✓✓ | 3161 |
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) |
87 |
{ |
||
88 |
3052 |
const JointIndex & parent = model.parents[i]; |
|
89 |
3052 |
const typename Data::SE3 & liMi = data.liMi[i]; |
|
90 |
|||
91 |
3052 |
data.mass[parent] += data.mass[i]; |
|
92 |
✓✗✓✗ ✓✗✓✗ |
3052 |
data.com[parent] += liMi.rotation()*data.com[i] + data.mass[i]*liMi.translation(); |
93 |
✓✗ | 3052 |
data.h[parent] += data.liMi[i].act(data.h[i]); |
94 |
✓✗ | 3052 |
data.f[parent] += data.liMi[i].act(data.f[i]); |
95 |
} |
||
96 |
|||
97 |
109 |
data.com[0] /= data.mass[0]; |
|
98 |
|||
99 |
109 |
data.hg = data.h[0]; |
|
100 |
✓✗✓✗ ✓✗ |
109 |
data.hg.angular() += data.hg.linear().cross(data.com[0]); |
101 |
|||
102 |
109 |
data.dhg = data.f[0]; |
|
103 |
✓✗✓✗ ✓✗ |
109 |
data.dhg.angular() += data.dhg.linear().cross(data.com[0]); |
104 |
|||
105 |
✓✗✓✗ ✓✗ |
109 |
data.vcom[0].noalias() = data.hg.linear()/data.mass[0]; |
106 |
|||
107 |
109 |
return data.dhg; |
|
108 |
} |
||
109 |
|||
110 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
111 |
struct CcrbaBackwardStep |
||
112 |
: public fusion::JointUnaryVisitorBase< CcrbaBackwardStep<Scalar,Options,JointCollectionTpl> > |
||
113 |
{ |
||
114 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
115 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
116 |
|||
117 |
typedef boost::fusion::vector<const Model &, |
||
118 |
Data & |
||
119 |
> ArgsType; |
||
120 |
|||
121 |
template<typename JointModel> |
||
122 |
2512 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
123 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
124 |
const Model & model, |
||
125 |
Data & data) |
||
126 |
{ |
||
127 |
typedef typename Model::JointIndex JointIndex; |
||
128 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
129 |
|||
130 |
✓✗ | 2512 |
const JointIndex & i = jmodel.id(); |
131 |
2512 |
const JointIndex & parent = model.parents[i]; |
|
132 |
|||
133 |
✓✗ | 2512 |
ColsBlock J_cols = jmodel.jointCols(data.J); |
134 |
✓✗✓✗ ✓✗ |
2512 |
J_cols = data.oMi[i].act(jdata.S()); |
135 |
|||
136 |
✓✗ | 2512 |
ColsBlock Ag_cols = jmodel.jointCols(data.Ag); |
137 |
✓✗ | 2512 |
motionSet::inertiaAction(data.oYcrb[i],J_cols,Ag_cols); |
138 |
✓✗ | 2512 |
data.oYcrb[parent] += data.oYcrb[i]; |
139 |
} |
||
140 |
|||
141 |
}; // struct CcrbaBackwardStep |
||
142 |
|||
143 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
||
144 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x & |
||
145 |
44 |
ccrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
146 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
147 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
148 |
const Eigen::MatrixBase<TangentVectorType> & v) |
||
149 |
{ |
||
150 |
✓✗✓✗ |
44 |
assert(model.check(data) && "data is not consistent with model."); |
151 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
44 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
152 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
44 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
153 |
|||
154 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
155 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
156 |
typedef typename Model::JointIndex JointIndex; |
||
157 |
|||
158 |
✓✗ | 44 |
forwardKinematics(model, data, q); |
159 |
✓✗ | 44 |
data.oYcrb[0].setZero(); |
160 |
✓✓ | 1273 |
for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) |
161 |
✓✗✓✗ |
1229 |
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
162 |
|||
163 |
typedef CcrbaBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
164 |
✓✓ | 1273 |
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) |
165 |
{ |
||
166 |
✓✗✓✗ |
1229 |
Pass2::run(model.joints[i],data.joints[i], |
167 |
typename Pass2::ArgsType(model,data)); |
||
168 |
} |
||
169 |
|||
170 |
// Express the centroidal map around the center of mass |
||
171 |
✓✗ | 44 |
data.com[0] = data.oYcrb[0].lever(); |
172 |
|||
173 |
typedef Eigen::Block<typename Data::Matrix6x,3,-1> Block3x; |
||
174 |
✓✗ | 44 |
const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR); |
175 |
✓✗ | 44 |
Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR); |
176 |
✓✓ | 1575 |
for (long i = 0; i<model.nv; ++i) |
177 |
✓✗✓✗ ✓✗✓✗ |
1531 |
Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]); |
178 |
|||
179 |
✓✗✓✗ ✓✗✓✗ |
44 |
data.hg.toVector().noalias() = data.Ag*v; |
180 |
|||
181 |
44 |
data.Ig.mass() = data.oYcrb[0].mass(); |
|
182 |
✓✗ | 44 |
data.Ig.lever().setZero(); |
183 |
✓✗ | 44 |
data.Ig.inertia() = data.oYcrb[0].inertia(); |
184 |
|||
185 |
44 |
return data.Ag; |
|
186 |
} |
||
187 |
|||
188 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
||
189 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x & |
||
190 |
1 |
computeCentroidalMap(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
191 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
192 |
const Eigen::MatrixBase<ConfigVectorType> & q) |
||
193 |
{ |
||
194 |
✓✗✓✗ |
1 |
assert(model.check(data) && "data is not consistent with model."); |
195 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
196 |
|||
197 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
198 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
199 |
typedef typename Model::JointIndex JointIndex; |
||
200 |
|||
201 |
✓✗ | 1 |
forwardKinematics(model, data, q); |
202 |
✓✗ | 1 |
data.oYcrb[0].setZero(); |
203 |
✓✓ | 28 |
for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) |
204 |
✓✗✓✗ |
27 |
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
205 |
|||
206 |
typedef CcrbaBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
207 |
✓✓ | 28 |
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) |
208 |
{ |
||
209 |
✓✗✓✗ |
27 |
Pass2::run(model.joints[i],data.joints[i], |
210 |
typename Pass2::ArgsType(model,data)); |
||
211 |
} |
||
212 |
|||
213 |
// Express the centroidal map around the center of mass |
||
214 |
✓✗ | 1 |
data.com[0] = data.oYcrb[0].lever(); |
215 |
|||
216 |
typedef Eigen::Block<typename Data::Matrix6x,3,-1> Block3x; |
||
217 |
✓✗ | 1 |
const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR); |
218 |
✓✗ | 1 |
Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR); |
219 |
✓✓ | 33 |
for (long i = 0; i<model.nv; ++i) |
220 |
✓✗✓✗ ✓✗✓✗ |
32 |
Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]); |
221 |
|||
222 |
1 |
return data.Ag; |
|
223 |
} |
||
224 |
|||
225 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
226 |
struct DCcrbaBackwardStep |
||
227 |
: public fusion::JointUnaryVisitorBase< DCcrbaBackwardStep<Scalar,Options,JointCollectionTpl> > |
||
228 |
{ |
||
229 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
230 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
231 |
|||
232 |
typedef boost::fusion::vector<const Model &, |
||
233 |
Data & |
||
234 |
> ArgsType; |
||
235 |
|||
236 |
template<typename JointModel> |
||
237 |
714 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
238 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
239 |
const Model & model, |
||
240 |
Data & data) |
||
241 |
{ |
||
242 |
typedef typename Model::JointIndex JointIndex; |
||
243 |
typedef typename Data::Inertia Inertia; |
||
244 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
245 |
|||
246 |
✓✗ | 714 |
const JointIndex & i = jmodel.id(); |
247 |
714 |
const JointIndex & parent = model.parents[i]; |
|
248 |
714 |
const Inertia & Y = data.oYcrb[i]; |
|
249 |
714 |
const typename Inertia::Matrix6 & doYcrb = data.doYcrb[i]; |
|
250 |
|||
251 |
✓✗ | 714 |
ColsBlock J_cols = jmodel.jointCols(data.J); |
252 |
✓✗✓✗ ✓✗ |
714 |
J_cols = data.oMi[i].act(jdata.S()); |
253 |
|||
254 |
✓✗ | 714 |
ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
255 |
✓✗ | 714 |
motionSet::motionAction(data.ov[i],J_cols,dJ_cols); |
256 |
|||
257 |
✓✗ | 714 |
data.oYcrb[parent] += Y; |
258 |
✓✓ | 714 |
if(parent > 0) |
259 |
✓✗ | 688 |
data.doYcrb[parent] += doYcrb; |
260 |
|||
261 |
// Calc Ag |
||
262 |
✓✗ | 714 |
ColsBlock Ag_cols = jmodel.jointCols(data.Ag); |
263 |
✓✗ | 714 |
motionSet::inertiaAction(Y,J_cols,Ag_cols); |
264 |
|||
265 |
// Calc dAg = Ivx + vxI |
||
266 |
✓✗ | 714 |
ColsBlock dAg_cols = jmodel.jointCols(data.dAg); |
267 |
✓✗✓✗ ✓✗ |
714 |
dAg_cols.noalias() = doYcrb * J_cols; |
268 |
✓✗ | 714 |
motionSet::inertiaAction<ADDTO>(Y,dJ_cols,dAg_cols); |
269 |
} |
||
270 |
|||
271 |
}; // struct DCcrbaBackwardStep |
||
272 |
|||
273 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
||
274 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x & |
||
275 |
19 |
dccrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
276 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
277 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
278 |
const Eigen::MatrixBase<TangentVectorType> & v) |
||
279 |
{ |
||
280 |
✓✗✓✗ |
19 |
assert(model.check(data) && "data is not consistent with model."); |
281 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
19 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
282 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
19 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
283 |
|||
284 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
285 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
286 |
typedef typename Model::JointIndex JointIndex; |
||
287 |
|||
288 |
✓✗ | 19 |
forwardKinematics(model,data,q,v); |
289 |
✓✗ | 19 |
data.oYcrb[0].setZero(); |
290 |
✓✓ | 544 |
for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) |
291 |
{ |
||
292 |
✓✗✓✗ |
525 |
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
293 |
✓✗ | 525 |
data.ov[i] = data.oMi[i].act(data.v[i]); // v_i expressed in the world frame |
294 |
✓✗ | 525 |
data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]); |
295 |
} |
||
296 |
|||
297 |
typedef DCcrbaBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
298 |
✓✓ | 544 |
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) |
299 |
{ |
||
300 |
✓✗✓✗ |
525 |
Pass2::run(model.joints[i],data.joints[i], |
301 |
typename Pass2::ArgsType(model,data)); |
||
302 |
} |
||
303 |
|||
304 |
// Express the centroidal map around the center of mass |
||
305 |
✓✗ | 19 |
data.com[0] = data.oYcrb[0].lever(); |
306 |
|||
307 |
typedef Eigen::Block<typename Data::Matrix6x,3,-1> Block3x; |
||
308 |
|||
309 |
✓✗ | 19 |
const Block3x Ag_lin = data.Ag.template middleRows<3> (Force::LINEAR); |
310 |
✓✗ | 19 |
Block3x Ag_ang = data.Ag.template middleRows<3> (Force::ANGULAR); |
311 |
✓✓ | 663 |
for(Eigen::DenseIndex i = 0; i<model.nv; ++i) |
312 |
✓✗✓✗ ✓✗✓✗ |
644 |
Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]); |
313 |
|||
314 |
✓✗✓✗ ✓✗✓✗ |
19 |
data.hg.toVector().noalias() = data.Ag*v; |
315 |
✓✗✓✗ ✓✗✓✗ |
19 |
data.vcom[0].noalias() = data.hg.linear()/data.oYcrb[0].mass(); |
316 |
|||
317 |
✓✗ | 19 |
const Block3x dAg_lin = data.dAg.template middleRows<3>(Force::LINEAR); |
318 |
✓✗ | 19 |
Block3x dAg_ang = data.dAg.template middleRows<3>(Force::ANGULAR); |
319 |
✓✓ | 663 |
for(Eigen::DenseIndex i = 0; i<model.nv; ++i) |
320 |
✓✗✓✗ ✓✗✓✗ |
644 |
dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]); |
321 |
|||
322 |
19 |
data.Ig.mass() = data.oYcrb[0].mass(); |
|
323 |
✓✗ | 19 |
data.Ig.lever().setZero(); |
324 |
✓✗ | 19 |
data.Ig.inertia() = data.oYcrb[0].inertia(); |
325 |
|||
326 |
19 |
return data.dAg; |
|
327 |
} |
||
328 |
|||
329 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
||
330 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x & |
||
331 |
1 |
computeCentroidalMapTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
332 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
333 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
334 |
const Eigen::MatrixBase<TangentVectorType> & v) |
||
335 |
{ |
||
336 |
✓✗✓✗ |
1 |
assert(model.check(data) && "data is not consistent with model."); |
337 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
338 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
339 |
|||
340 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
341 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
342 |
typedef typename Model::JointIndex JointIndex; |
||
343 |
|||
344 |
✓✗ | 1 |
forwardKinematics(model,data,q,v); |
345 |
✓✗ | 1 |
data.oYcrb[0].setZero(); |
346 |
✓✓ | 28 |
for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) |
347 |
{ |
||
348 |
✓✗✓✗ |
27 |
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
349 |
✓✗ | 27 |
data.ov[i] = data.oMi[i].act(data.v[i]); // v_i expressed in the world frame |
350 |
✓✗ | 27 |
data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]); |
351 |
} |
||
352 |
|||
353 |
typedef DCcrbaBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
354 |
✓✓ | 28 |
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) |
355 |
{ |
||
356 |
✓✗✓✗ |
27 |
Pass2::run(model.joints[i],data.joints[i], |
357 |
typename Pass2::ArgsType(model,data)); |
||
358 |
} |
||
359 |
|||
360 |
// Express the centroidal map around the center of mass |
||
361 |
✓✗ | 1 |
data.com[0] = data.oYcrb[0].lever(); |
362 |
|||
363 |
typedef Eigen::Block<typename Data::Matrix6x,3,-1> Block3x; |
||
364 |
|||
365 |
✓✗ | 1 |
const Block3x Ag_lin = data.Ag.template middleRows<3> (Force::LINEAR); |
366 |
✓✗ | 1 |
Block3x Ag_ang = data.Ag.template middleRows<3> (Force::ANGULAR); |
367 |
✓✓ | 33 |
for(Eigen::DenseIndex i = 0; i<model.nv; ++i) |
368 |
✓✗✓✗ ✓✗✓✗ |
32 |
Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]); |
369 |
|||
370 |
// Express the centroidal time derivative map around the center of mass |
||
371 |
✓✗ | 1 |
const Block3x dAg_lin = data.dAg.template middleRows<3>(Force::LINEAR); |
372 |
✓✗ | 1 |
Block3x dAg_ang = data.dAg.template middleRows<3>(Force::ANGULAR); |
373 |
✓✓ | 33 |
for(Eigen::DenseIndex i = 0; i<model.nv; ++i) |
374 |
✓✗✓✗ ✓✗✓✗ |
32 |
dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]); |
375 |
|||
376 |
1 |
return data.dAg; |
|
377 |
} |
||
378 |
|||
379 |
} // namespace pinocchio |
||
380 |
|||
381 |
/// @endcond |
||
382 |
|||
383 |
#endif // ifndef __pinocchio_algorithm_centroidal_hxx__ |
| Generated by: GCOVR (Version 4.2) |