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