| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2018-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_algorithm_aba_derivatives_hxx__ |
||
6 |
#define __pinocchio_algorithm_aba_derivatives_hxx__ |
||
7 |
|||
8 |
#include "pinocchio/multibody/visitor.hpp" |
||
9 |
#include "pinocchio/algorithm/check.hpp" |
||
10 |
#include "pinocchio/algorithm/aba.hpp" |
||
11 |
|||
12 |
namespace pinocchio |
||
13 |
{ |
||
14 |
|||
15 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
||
16 |
struct ComputeABADerivativesForwardStep1 |
||
17 |
: public fusion::JointUnaryVisitorBase< ComputeABADerivativesForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> > |
||
18 |
{ |
||
19 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
20 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
21 |
|||
22 |
typedef boost::fusion::vector<const Model &, |
||
23 |
Data &, |
||
24 |
const ConfigVectorType &, |
||
25 |
const TangentVectorType & |
||
26 |
> ArgsType; |
||
27 |
|||
28 |
template<typename JointModel> |
||
29 |
1578 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
30 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
31 |
const Model & model, |
||
32 |
Data & data, |
||
33 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
34 |
const Eigen::MatrixBase<TangentVectorType> & v) |
||
35 |
{ |
||
36 |
typedef typename Model::JointIndex JointIndex; |
||
37 |
|||
38 |
✓✗ | 1578 |
const JointIndex & i = jmodel.id(); |
39 |
1578 |
const JointIndex & parent = model.parents[i]; |
|
40 |
1578 |
typename Data::Motion & ov = data.ov[i]; |
|
41 |
|||
42 |
✓✗ | 1578 |
jmodel.calc(jdata.derived(),q.derived(),v.derived()); |
43 |
|||
44 |
✓✗✓✓ ✗✓✗ |
1578 |
data.liMi[i] = model.jointPlacements[i]*jdata.M(); |
45 |
✓✗✓✗ |
1578 |
data.v[i] = jdata.v(); |
46 |
|||
47 |
✓✓ | 1578 |
if(parent > 0) |
48 |
{ |
||
49 |
✓✗ | 1518 |
data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
50 |
✓✗✓✗ |
1518 |
data.v[i] += data.liMi[i].actInv(data.v[parent]); |
51 |
} |
||
52 |
else |
||
53 |
✓✗ | 60 |
data.oMi[i] = data.liMi[i]; |
54 |
|||
55 |
✓✗ | 1578 |
ov = data.oMi[i].act(data.v[i]); |
56 |
✓✗✓✗ ✓✗✗✗ ✓✗ |
1578 |
data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); |
57 |
✓✗ | 1578 |
data.Yaba[i] = model.inertias[i].matrix(); |
58 |
✓✗✓✗ ✓✗ |
1578 |
data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]); |
59 |
|||
60 |
✓✗✓✗ |
1578 |
data.oh[i] = data.oYcrb[i] * ov; |
61 |
✓✗✓✗ |
1578 |
data.of[i] = ov.cross(data.oh[i]); |
62 |
✓✗✓✗ |
1578 |
data.f[i] = data.oMi[i].actInv(data.of[i]); |
63 |
|||
64 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
65 |
✓✗ | 1578 |
ColsBlock J_cols = jmodel.jointCols(data.J); |
66 |
✓✗✓✗ ✓✗ |
1578 |
J_cols = data.oMi[i].act(jdata.S()); |
67 |
} |
||
68 |
|||
69 |
}; |
||
70 |
|||
71 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MatrixType> |
||
72 |
struct ComputeABADerivativesBackwardStep1 |
||
73 |
: public fusion::JointUnaryVisitorBase< ComputeABADerivativesBackwardStep1<Scalar,Options,JointCollectionTpl,MatrixType> > |
||
74 |
{ |
||
75 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
76 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
77 |
|||
78 |
typedef boost::fusion::vector<const Model &, |
||
79 |
Data &, |
||
80 |
MatrixType &> ArgsType; |
||
81 |
|||
82 |
template<typename JointModel> |
||
83 |
1578 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
84 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
85 |
const Model & model, |
||
86 |
Data & data, |
||
87 |
const Eigen::MatrixBase<MatrixType> & Minv) |
||
88 |
{ |
||
89 |
typedef typename Model::JointIndex JointIndex; |
||
90 |
|||
91 |
✓✗ | 1578 |
const JointIndex & i = jmodel.id(); |
92 |
1578 |
const JointIndex & parent = model.parents[i]; |
|
93 |
|||
94 |
1578 |
typename Data::Inertia::Matrix6 & Ia = data.Yaba[i]; |
|
95 |
✓✗ | 1578 |
jmodel.calc_aba(jdata.derived(), Ia, parent > 0); |
96 |
|||
97 |
1578 |
typename Data::Matrix6x & Fcrb = data.Fcrb[0]; |
|
98 |
1578 |
typename Data::Matrix6x & FcrbTmp = data.Fcrb.back(); |
|
99 |
|||
100 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
101 |
|||
102 |
✓✗ | 1578 |
ColsBlock U_cols = jmodel.jointCols(data.IS); |
103 |
✓✗✓✗ |
1578 |
forceSet::se3Action(data.oMi[i],jdata.U(),U_cols); // expressed in the world frame |
104 |
|||
105 |
1578 |
MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType,Minv); |
|
106 |
|||
107 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1578 |
Minv_.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),jmodel.nv()) = jdata.Dinv(); |
108 |
✓✗ | 1578 |
const int nv_children = data.nvSubtree[i] - jmodel.nv(); |
109 |
✓✓ | 1578 |
if(nv_children > 0) |
110 |
{ |
||
111 |
✓✗ | 1344 |
ColsBlock J_cols = jmodel.jointCols(data.J); |
112 |
✓✗ | 1344 |
ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); |
113 |
✓✗✓✗ ✓✗✓✗ |
1344 |
SDinv_cols.noalias() = J_cols * jdata.Dinv(); |
114 |
|||
115 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1344 |
Minv_.block(jmodel.idx_v(),jmodel.idx_v()+jmodel.nv(),jmodel.nv(),nv_children).noalias() |
116 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2688 |
= -SDinv_cols.transpose() * Fcrb.middleCols(jmodel.idx_v()+jmodel.nv(),nv_children); |
117 |
|||
118 |
✓✓ | 1344 |
if(parent > 0) |
119 |
{ |
||
120 |
1284 |
FcrbTmp.leftCols(data.nvSubtree[i]).noalias() |
|
121 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2568 |
= U_cols * Minv_.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]); |
122 |
✓✗✓✗ ✓✗✓✗ |
1284 |
Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]) += FcrbTmp.leftCols(data.nvSubtree[i]); |
123 |
} |
||
124 |
} |
||
125 |
else // This a leaf of the kinematic tree |
||
126 |
{ |
||
127 |
234 |
Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]).noalias() |
|
128 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
468 |
= U_cols * Minv_.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]); |
129 |
} |
||
130 |
|||
131 |
✓✗✓✓ ✗✓✓✗ ✓✓✗✓ ✗ |
1578 |
jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose()*data.f[i]; |
132 |
|||
133 |
✓✓ | 1578 |
if (parent > 0) |
134 |
{ |
||
135 |
1518 |
typename Data::Force & pa = data.f[i]; |
|
136 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1518 |
pa.toVector() += Ia * data.a_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); |
137 |
✓✗✓✗ |
1518 |
data.Yaba[parent] += internal::SE3actOn<Scalar>::run(data.liMi[i], Ia); |
138 |
✓✗✓✗ |
1518 |
data.f[parent] += data.liMi[i].act(pa); |
139 |
} |
||
140 |
|||
141 |
} |
||
142 |
|||
143 |
}; |
||
144 |
|||
145 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MatrixType> |
||
146 |
struct ComputeABADerivativesForwardStep2 |
||
147 |
: public fusion::JointUnaryVisitorBase< ComputeABADerivativesForwardStep2<Scalar,Options,JointCollectionTpl,MatrixType> > |
||
148 |
{ |
||
149 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
150 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
151 |
|||
152 |
typedef boost::fusion::vector<const Model &, |
||
153 |
Data &, |
||
154 |
MatrixType &> ArgsType; |
||
155 |
|||
156 |
template<typename JointModel> |
||
157 |
1578 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
158 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
159 |
const Model & model, |
||
160 |
Data & data, |
||
161 |
MatrixType & Minv) |
||
162 |
{ |
||
163 |
typedef typename Model::JointIndex JointIndex; |
||
164 |
|||
165 |
✓✗ | 1578 |
const JointIndex & i = jmodel.id(); |
166 |
1578 |
const JointIndex & parent = model.parents[i]; |
|
167 |
|||
168 |
1578 |
typename Data::Motion & ov = data.ov[i]; |
|
169 |
1578 |
typename Data::Motion & oa = data.oa[i]; |
|
170 |
1578 |
typename Data::Motion & oa_gf = data.oa_gf[i]; |
|
171 |
1578 |
typename Data::Force & of = data.of[i]; |
|
172 |
|||
173 |
✓✗✓✗ |
1578 |
data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); |
174 |
✓✗✓✗ ✓✗✓✗ |
1578 |
jmodel.jointVelocitySelector(data.ddq).noalias() = |
175 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1578 |
jdata.Dinv() * jmodel.jointVelocitySelector(data.u) - jdata.UDinv().transpose() * data.a_gf[i].toVector(); |
176 |
|||
177 |
✓✗✓✗ ✓✗✓✗ |
1578 |
data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); |
178 |
✓✗ | 1578 |
oa_gf = data.oMi[i].act(data.a_gf[i]); |
179 |
✓✗ | 1578 |
oa = oa_gf + model.gravity; |
180 |
✓✗✓✗ ✓✗✓✗ |
1578 |
of = data.oYcrb[i] * oa_gf + ov.cross(data.oh[i]); |
181 |
|||
182 |
1578 |
typename Data::Matrix6x & FcrbTmp = data.Fcrb.back(); |
|
183 |
|||
184 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
185 |
✓✗ | 1578 |
ColsBlock UDinv_cols = jmodel.jointCols(data.UDinv); |
186 |
✓✗✓✗ |
1578 |
forceSet::se3Action(data.oMi[i],jdata.UDinv(),UDinv_cols); // expressed in the world frame |
187 |
|||
188 |
1578 |
MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType,Minv); |
|
189 |
|||
190 |
✓✓ | 1578 |
if(parent > 0) |
191 |
{ |
||
192 |
✓✗✓✗ ✓✗✓✗ |
1518 |
FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v()).noalias() |
193 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
3036 |
= UDinv_cols.transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); |
194 |
✓✗✓✗ ✓✗✓✗ |
1518 |
Minv_.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()) |
195 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
3036 |
-= FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v()); |
196 |
} |
||
197 |
|||
198 |
✓✗ | 1578 |
ColsBlock J_cols = jmodel.jointCols(data.J); |
199 |
✓✗ | 1578 |
data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()).noalias() |
200 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
3156 |
= J_cols * Minv_.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()); |
201 |
✓✓ | 1578 |
if(parent > 0) |
202 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1518 |
data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) += data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); |
203 |
|||
204 |
✓✗ | 1578 |
ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
205 |
✓✗ | 1578 |
ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
206 |
✓✗ | 1578 |
ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
207 |
✓✗ | 1578 |
ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); |
208 |
|||
209 |
✓✗ | 1578 |
motionSet::motionAction(ov,J_cols,dJ_cols); |
210 |
✓✗ | 1578 |
motionSet::motionAction(data.oa_gf[parent],J_cols,dAdq_cols); |
211 |
✓✗ | 1578 |
dAdv_cols = dJ_cols; |
212 |
✓✓ | 1578 |
if(parent > 0) |
213 |
{ |
||
214 |
✓✗ | 1518 |
motionSet::motionAction(data.ov[parent],J_cols,dVdq_cols); |
215 |
✓✗ | 1518 |
motionSet::motionAction<ADDTO>(data.ov[parent],dVdq_cols,dAdq_cols); |
216 |
✓✗ | 1518 |
dAdv_cols += dVdq_cols; |
217 |
} |
||
218 |
else |
||
219 |
✓✗ | 60 |
dVdq_cols.setZero(); |
220 |
|||
221 |
// computes variation of inertias |
||
222 |
✓✗ | 1578 |
data.doYcrb[i] = data.oYcrb[i].variation(ov); |
223 |
✓✗ | 1578 |
addForceCrossMatrix(data.oh[i],data.doYcrb[i]); |
224 |
} |
||
225 |
|||
226 |
template<typename ForceDerived, typename M6> |
||
227 |
1545 |
static void addForceCrossMatrix(const ForceDense<ForceDerived> & f, |
|
228 |
const Eigen::MatrixBase<M6> & mout) |
||
229 |
{ |
||
230 |
1545 |
M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,mout); |
|
231 |
✓✗✓✗ ✓✗ |
1545 |
addSkew(-f.linear(),mout_.template block<3,3>(ForceDerived::LINEAR,ForceDerived::ANGULAR)); |
232 |
✓✗✓✗ ✓✗ |
1545 |
addSkew(-f.linear(),mout_.template block<3,3>(ForceDerived::ANGULAR,ForceDerived::LINEAR)); |
233 |
✓✗✓✗ ✓✗ |
1545 |
addSkew(-f.angular(),mout_.template block<3,3>(ForceDerived::ANGULAR,ForceDerived::ANGULAR)); |
234 |
1545 |
} |
|
235 |
|||
236 |
}; |
||
237 |
|||
238 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
239 |
struct ComputeABADerivativesBackwardStep2 |
||
240 |
: public fusion::JointUnaryVisitorBase< ComputeABADerivativesBackwardStep2<Scalar,Options,JointCollectionTpl> > |
||
241 |
{ |
||
242 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
243 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
244 |
|||
245 |
typedef boost::fusion::vector<const Model &, |
||
246 |
Data &> ArgsType; |
||
247 |
|||
248 |
template<typename JointModel> |
||
249 |
1578 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
250 |
const Model & model, |
||
251 |
Data & data) |
||
252 |
{ |
||
253 |
typedef typename Model::JointIndex JointIndex; |
||
254 |
|||
255 |
✓✗ | 1578 |
const JointIndex & i = jmodel.id(); |
256 |
1578 |
const JointIndex & parent = model.parents[i]; |
|
257 |
|||
258 |
1578 |
typename Data::RowMatrix6 & M6tmpR = data.M6tmpR; |
|
259 |
|||
260 |
1578 |
typename Data::MatrixXs & rnea_partial_dq = data.dtau_dq; |
|
261 |
1578 |
typename Data::MatrixXs & rnea_partial_dv = data.dtau_dv; |
|
262 |
|||
263 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
264 |
|||
265 |
✓✗ | 1578 |
ColsBlock J_cols = jmodel.jointCols(data.J); |
266 |
✓✗ | 1578 |
ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
267 |
✓✗ | 1578 |
ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
268 |
✓✗ | 1578 |
ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); |
269 |
✓✗ | 1578 |
ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); |
270 |
✓✗ | 1578 |
ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); |
271 |
|||
272 |
// dtau/dv |
||
273 |
✓✗ | 1578 |
motionSet::inertiaAction(data.oYcrb[i],dAdv_cols,dFdv_cols); |
274 |
✓✗✓✗ |
1578 |
dFdv_cols += data.doYcrb[i] * J_cols; |
275 |
|||
276 |
✓✗✓✗ ✓✗✓✗ |
1578 |
rnea_partial_dv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() |
277 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
3156 |
= J_cols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]); |
278 |
|||
279 |
// dtau/dq |
||
280 |
✓✗ | 1578 |
motionSet::inertiaAction(data.oYcrb[i],dAdq_cols,dFdq_cols); |
281 |
✓✓ | 1578 |
if(parent>0) |
282 |
✓✗✓✗ |
1518 |
dFdq_cols += data.doYcrb[i] * dVdq_cols; |
283 |
|||
284 |
✓✗✓✗ ✓✗✓✗ |
1578 |
rnea_partial_dq.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() |
285 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
3156 |
= J_cols.transpose()*data.dFdq.middleCols(jmodel.idx_v(),data.nvSubtree[i]); |
286 |
|||
287 |
✓✗ | 1578 |
motionSet::act<ADDTO>(J_cols,data.of[i],dFdq_cols); |
288 |
|||
289 |
✓✓ | 1578 |
if(parent > 0) |
290 |
{ |
||
291 |
✓✗✓✗ ✓✗✓✗ |
1518 |
lhsInertiaMult(data.oYcrb[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv())); |
292 |
✓✗✓✓ |
15526 |
for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) |
293 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
14008 |
rnea_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdq.col(j); |
294 |
✓✗✓✓ |
15526 |
for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) |
295 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
14008 |
rnea_partial_dv.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdv.col(j); |
296 |
|||
297 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1518 |
M6tmpR.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.doYcrb[i]; |
298 |
✓✗✓✓ |
15526 |
for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) |
299 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
14008 |
rnea_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += M6tmpR.topRows(jmodel.nv()) * data.dVdq.col(j); |
300 |
✓✗✓✓ |
15526 |
for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) |
301 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
14008 |
rnea_partial_dv.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += M6tmpR.topRows(jmodel.nv()) * data.J.col(j); |
302 |
} |
||
303 |
|||
304 |
✓✓ | 1578 |
if(parent>0) |
305 |
{ |
||
306 |
✓✗ | 1518 |
data.oYcrb[parent] += data.oYcrb[i]; |
307 |
✓✗ | 1518 |
data.doYcrb[parent] += data.doYcrb[i]; |
308 |
✓✗ | 1518 |
data.of[parent] += data.of[i]; |
309 |
} |
||
310 |
|||
311 |
// Restore the status of dAdq_cols (remove gravity) |
||
312 |
✓✗✓✗ ✗✓✗✗ |
1578 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(isZero(model.gravity.angular()), |
313 |
"The gravity must be a pure force vector, no angular part"); |
||
314 |
✓✗✓✓ |
3446 |
for(Eigen::DenseIndex k =0; k < jmodel.nv(); ++k) |
315 |
{ |
||
316 |
✓✗✓✗ |
1868 |
MotionRef<typename ColsBlock::ColXpr> m_in(J_cols.col(k)); |
317 |
✓✗✓✗ |
1868 |
MotionRef<typename ColsBlock::ColXpr> m_out(dAdq_cols.col(k)); |
318 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1868 |
m_out.linear() += model.gravity.linear().cross(m_in.angular()); |
319 |
} |
||
320 |
} |
||
321 |
|||
322 |
template<typename Min, typename Mout> |
||
323 |
1518 |
static void lhsInertiaMult(const typename Data::Inertia & Y, |
|
324 |
const Eigen::MatrixBase<Min> & J, |
||
325 |
const Eigen::MatrixBase<Mout> & F) |
||
326 |
{ |
||
327 |
1518 |
Mout & F_ = PINOCCHIO_EIGEN_CONST_CAST(Mout,F); |
|
328 |
✓✗✓✗ |
1518 |
motionSet::inertiaAction(Y,J.derived().transpose(),F_.transpose()); |
329 |
} |
||
330 |
}; |
||
331 |
|||
332 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, |
||
333 |
typename MatrixType1, typename MatrixType2, typename MatrixType3> |
||
334 |
54 |
inline void computeABADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
335 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
336 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
337 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
338 |
const Eigen::MatrixBase<TangentVectorType2> & tau, |
||
339 |
const Eigen::MatrixBase<MatrixType1> & aba_partial_dq, |
||
340 |
const Eigen::MatrixBase<MatrixType2> & aba_partial_dv, |
||
341 |
const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau) |
||
342 |
{ |
||
343 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
54 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The joint configuration vector is not of right size"); |
344 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
54 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of right size"); |
345 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
54 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv, "The joint torque vector is not of right size"); |
346 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
54 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dq.cols(), model.nv); |
347 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
54 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dq.rows(), model.nv); |
348 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
54 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.cols(), model.nv); |
349 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
54 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.rows(), model.nv); |
350 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
54 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.cols(), model.nv); |
351 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
54 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.rows(), model.nv); |
352 |
✓✗ | 54 |
assert(model.check(data) && "data is not consistent with model."); |
353 |
|||
354 |
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; |
||
355 |
|||
356 |
54 |
data.a_gf[0] = -model.gravity; |
|
357 |
54 |
data.oa_gf[0] = -model.gravity; |
|
358 |
54 |
data.u = tau; |
|
359 |
|||
360 |
54 |
MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3,aba_partial_dtau); |
|
361 |
✓✗ | 54 |
Minv_.template triangularView<Eigen::Upper>().setZero(); |
362 |
|||
363 |
/// First, compute Minv and a, the joint acceleration vector |
||
364 |
typedef ComputeABADerivativesForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1> Pass1; |
||
365 |
✓✓ | 1491 |
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) |
366 |
{ |
||
367 |
✓✗ | 1437 |
Pass1::run(model.joints[i],data.joints[i], |
368 |
typename Pass1::ArgsType(model,data,q.derived(),v.derived())); |
||
369 |
} |
||
370 |
|||
371 |
54 |
data.Fcrb[0].setZero(); |
|
372 |
typedef ComputeABADerivativesBackwardStep1<Scalar,Options,JointCollectionTpl,MatrixType3> Pass2; |
||
373 |
✓✓ | 1491 |
for(JointIndex i=(JointIndex)(model.njoints-1);i>0;--i) |
374 |
{ |
||
375 |
✓✗ | 1437 |
Pass2::run(model.joints[i],data.joints[i], |
376 |
typename Pass2::ArgsType(model,data,Minv_)); |
||
377 |
} |
||
378 |
|||
379 |
typedef ComputeABADerivativesForwardStep2<Scalar,Options,JointCollectionTpl,MatrixType3> Pass3; |
||
380 |
✓✓ | 1491 |
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) |
381 |
{ |
||
382 |
✓✗ | 1437 |
Pass3::run(model.joints[i],data.joints[i], |
383 |
typename Pass3::ArgsType(model,data,Minv_)); |
||
384 |
} |
||
385 |
|||
386 |
typedef ComputeABADerivativesBackwardStep2<Scalar,Options,JointCollectionTpl> Pass4; |
||
387 |
✓✓ | 1491 |
for(JointIndex i=(JointIndex)(model.njoints-1);i>0;--i) |
388 |
{ |
||
389 |
✓✗ | 1437 |
Pass4::run(model.joints[i], |
390 |
typename Pass4::ArgsType(model,data)); |
||
391 |
} |
||
392 |
|||
393 |
Minv_.template triangularView<Eigen::StrictlyLower>() |
||
394 |
✓✗✓✗ ✓✗ |
54 |
= Minv_.transpose().template triangularView<Eigen::StrictlyLower>(); |
395 |
|||
396 |
✓✗✓✗ ✓✗ |
54 |
PINOCCHIO_EIGEN_CONST_CAST(MatrixType1,aba_partial_dq).noalias() = -Minv_*data.dtau_dq; |
397 |
✓✗✓✗ ✓✗ |
54 |
PINOCCHIO_EIGEN_CONST_CAST(MatrixType2,aba_partial_dv).noalias() = -Minv_*data.dtau_dv; |
398 |
54 |
} |
|
399 |
|||
400 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, |
||
401 |
typename MatrixType1, typename MatrixType2, typename MatrixType3> |
||
402 |
2 |
inline void computeABADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
403 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
404 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
405 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
406 |
const Eigen::MatrixBase<TangentVectorType2> & tau, |
||
407 |
const container::aligned_vector< ForceTpl<Scalar,Options> > & fext, |
||
408 |
const Eigen::MatrixBase<MatrixType1> & aba_partial_dq, |
||
409 |
const Eigen::MatrixBase<MatrixType2> & aba_partial_dv, |
||
410 |
const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau) |
||
411 |
{ |
||
412 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The joint configuration vector is not of right size"); |
413 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of right size"); |
414 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv, "The joint torque vector is not of right size"); |
415 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(fext.size(), (size_t)model.njoints, "The external forces vector is not of right size"); |
416 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dq.cols(), model.nv); |
417 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dq.rows(), model.nv); |
418 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.cols(), model.nv); |
419 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.rows(), model.nv); |
420 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.cols(), model.nv); |
421 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.rows(), model.nv); |
422 |
✓✗ | 2 |
assert(model.check(data) && "data is not consistent with model."); |
423 |
|||
424 |
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; |
||
425 |
|||
426 |
2 |
data.a_gf[0] = -model.gravity; |
|
427 |
2 |
data.oa_gf[0] = -model.gravity; |
|
428 |
2 |
data.u = tau; |
|
429 |
|||
430 |
2 |
MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3,aba_partial_dtau); |
|
431 |
✓✗ | 2 |
Minv_.template triangularView<Eigen::Upper>().setZero(); |
432 |
|||
433 |
/// First, compute Minv and a, the joint acceleration vector |
||
434 |
typedef ComputeABADerivativesForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1> Pass1; |
||
435 |
✓✓ | 56 |
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) |
436 |
{ |
||
437 |
✓✗ | 54 |
Pass1::run(model.joints[i],data.joints[i], |
438 |
typename Pass1::ArgsType(model,data,q.derived(),v.derived())); |
||
439 |
54 |
data.f[i] -= fext[i]; |
|
440 |
} |
||
441 |
|||
442 |
2 |
data.Fcrb[0].setZero(); |
|
443 |
typedef ComputeABADerivativesBackwardStep1<Scalar,Options,JointCollectionTpl,MatrixType3> Pass2; |
||
444 |
✓✓ | 56 |
for(JointIndex i=(JointIndex)(model.njoints-1);i>0;--i) |
445 |
{ |
||
446 |
✓✗ | 54 |
Pass2::run(model.joints[i],data.joints[i], |
447 |
typename Pass2::ArgsType(model,data,Minv_)); |
||
448 |
} |
||
449 |
|||
450 |
typedef ComputeABADerivativesForwardStep2<Scalar,Options,JointCollectionTpl,MatrixType3> Pass3; |
||
451 |
✓✓ | 56 |
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) |
452 |
{ |
||
453 |
✓✗ | 54 |
Pass3::run(model.joints[i],data.joints[i], |
454 |
typename Pass3::ArgsType(model,data,Minv_)); |
||
455 |
✓✗ | 54 |
data.of[i] -= data.oMi[i].act(fext[i]); |
456 |
} |
||
457 |
|||
458 |
typedef ComputeABADerivativesBackwardStep2<Scalar,Options,JointCollectionTpl> Pass4; |
||
459 |
✓✓ | 56 |
for(JointIndex i=(JointIndex)(model.njoints-1);i>0;--i) |
460 |
{ |
||
461 |
✓✗ | 54 |
Pass4::run(model.joints[i], |
462 |
typename Pass4::ArgsType(model,data)); |
||
463 |
} |
||
464 |
|||
465 |
Minv_.template triangularView<Eigen::StrictlyLower>() |
||
466 |
✓✗✓✗ ✓✗ |
2 |
= Minv_.transpose().template triangularView<Eigen::StrictlyLower>(); |
467 |
|||
468 |
✓✗✓✗ ✓✗ |
2 |
PINOCCHIO_EIGEN_CONST_CAST(MatrixType1,aba_partial_dq).noalias() = -Minv_*data.dtau_dq; |
469 |
✓✗✓✗ ✓✗ |
2 |
PINOCCHIO_EIGEN_CONST_CAST(MatrixType2,aba_partial_dv).noalias() = -Minv_*data.dtau_dv; |
470 |
2 |
} |
|
471 |
|||
472 |
|||
473 |
} // namespace pinocchio |
||
474 |
|||
475 |
#endif // ifndef __pinocchio_algorithm_aba_derivatives_hxx__ |
| Generated by: GCOVR (Version 4.2) |