| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2018-2019 INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_algorithm_centroidal_derivatives_hxx__ |
||
6 |
#define __pinocchio_algorithm_centroidal_derivatives_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 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, |
||
18 |
typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
||
19 |
struct CentroidalDynDerivativesForwardStep |
||
20 |
: public fusion::JointUnaryVisitorBase<CentroidalDynDerivativesForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> > |
||
21 |
{ |
||
22 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
23 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
24 |
|||
25 |
typedef typename Model::JointIndex JointIndex; |
||
26 |
|||
27 |
typedef boost::fusion::vector<const Model &, |
||
28 |
Data &, |
||
29 |
const ConfigVectorType &, |
||
30 |
const TangentVectorType1 &, |
||
31 |
const TangentVectorType2 & |
||
32 |
> ArgsType; |
||
33 |
|||
34 |
template<typename JointModel> |
||
35 |
110 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
36 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
37 |
const Model & model, |
||
38 |
Data & data, |
||
39 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
40 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
41 |
const Eigen::MatrixBase<TangentVectorType2> & a) |
||
42 |
{ |
||
43 |
typedef typename Model::JointIndex JointIndex; |
||
44 |
typedef typename Data::Motion Motion; |
||
45 |
|||
46 |
✓✗ | 110 |
const JointIndex & i = jmodel.id(); |
47 |
110 |
const JointIndex & parent = model.parents[i]; |
|
48 |
110 |
Motion & ov = data.ov[i]; |
|
49 |
110 |
Motion & oa = data.oa[i]; |
|
50 |
|||
51 |
✓✗ | 110 |
jmodel.calc(jdata.derived(),q.derived(),v.derived()); |
52 |
|||
53 |
✓✗✓✓ ✗✓✗ |
110 |
data.liMi[i] = model.jointPlacements[i]*jdata.M(); |
54 |
|||
55 |
✓✗✓✗ |
110 |
data.v[i] = jdata.v(); |
56 |
|||
57 |
✓✓ | 110 |
if(parent > 0) |
58 |
{ |
||
59 |
✓✗ | 106 |
data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
60 |
✓✗✓✗ |
106 |
data.v[i] += data.liMi[i].actInv(data.v[parent]); |
61 |
} |
||
62 |
else |
||
63 |
✓✗ | 4 |
data.oMi[i] = data.liMi[i]; |
64 |
|||
65 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
110 |
data.a[i] = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); |
66 |
✓✓ | 110 |
if(parent > 0) |
67 |
{ |
||
68 |
✓✗✓✗ |
106 |
data.a[i] += data.liMi[i].actInv(data.a[parent]); |
69 |
} |
||
70 |
|||
71 |
✓✗✓✗ |
110 |
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
72 |
✓✗ | 110 |
ov = data.oMi[i].act(data.v[i]); |
73 |
✓✗ | 110 |
oa = data.oMi[i].act(data.a[i]); |
74 |
|||
75 |
✓✗✓✗ |
110 |
data.oh[i] = data.oYcrb[i] * ov; |
76 |
✓✗✓✗ ✓✗✓✗ |
110 |
data.of[i] = data.oYcrb[i] * oa + ov.cross(data.oh[i]); |
77 |
|||
78 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
79 |
✓✗ | 110 |
ColsBlock J_cols = jmodel.jointCols(data.J); |
80 |
✓✗ | 110 |
ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
81 |
✓✗ | 110 |
ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
82 |
✓✗ | 110 |
ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
83 |
✓✗ | 110 |
ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); |
84 |
|||
85 |
✓✗✓✗ ✓✗ |
110 |
J_cols = data.oMi[i].act(jdata.S()); |
86 |
✓✗ | 110 |
motionSet::motionAction(ov,J_cols,dJ_cols); |
87 |
✓✗ | 110 |
motionSet::motionAction(data.oa[parent],J_cols,dAdq_cols); |
88 |
✓✗ | 110 |
dAdv_cols = dJ_cols; |
89 |
✓✓ | 110 |
if(parent > 0) |
90 |
{ |
||
91 |
✓✗ | 106 |
motionSet::motionAction(data.ov[parent],J_cols,dVdq_cols); |
92 |
✓✗ | 106 |
motionSet::motionAction<ADDTO>(data.ov[parent],dVdq_cols,dAdq_cols); |
93 |
✓✗✓✗ |
106 |
dAdv_cols.noalias() += dVdq_cols; |
94 |
} |
||
95 |
else |
||
96 |
{ |
||
97 |
✓✗ | 4 |
dVdq_cols.setZero(); |
98 |
} |
||
99 |
|||
100 |
// computes variation of inertias |
||
101 |
✓✗ | 110 |
data.doYcrb[i] = data.oYcrb[i].variation(ov); |
102 |
|||
103 |
✓✗ | 110 |
addForceCrossMatrix(data.oh[i],data.doYcrb[i]); |
104 |
} |
||
105 |
|||
106 |
template<typename ForceDerived, typename M6> |
||
107 |
55 |
static void addForceCrossMatrix(const ForceDense<ForceDerived> & f, |
|
108 |
const Eigen::MatrixBase<M6> & mout) |
||
109 |
{ |
||
110 |
55 |
M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,mout); |
|
111 |
✓✗✓✗ ✓✗ |
55 |
addSkew(-f.linear(),mout_.template block<3,3>(ForceDerived::LINEAR,ForceDerived::ANGULAR)); |
112 |
✓✗✓✗ ✓✗ |
55 |
addSkew(-f.linear(),mout_.template block<3,3>(ForceDerived::ANGULAR,ForceDerived::LINEAR)); |
113 |
✓✗✓✗ ✓✗ |
55 |
addSkew(-f.angular(),mout_.template block<3,3>(ForceDerived::ANGULAR,ForceDerived::ANGULAR)); |
114 |
55 |
} |
|
115 |
|||
116 |
}; // struct CentroidalDynDerivativesForwardStep |
||
117 |
|||
118 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
119 |
struct CentroidalDynDerivativesBackwardStep |
||
120 |
: public fusion::JointUnaryVisitorBase<CentroidalDynDerivativesBackwardStep<Scalar,Options,JointCollectionTpl> > |
||
121 |
{ |
||
122 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
123 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
124 |
|||
125 |
typedef boost::fusion::vector<const Model &, |
||
126 |
Data & |
||
127 |
> ArgsType; |
||
128 |
|||
129 |
template<typename JointModel> |
||
130 |
110 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
131 |
const Model & model, |
||
132 |
Data & data) |
||
133 |
{ |
||
134 |
typedef typename Model::JointIndex JointIndex; |
||
135 |
|||
136 |
✓✗ | 110 |
const JointIndex & i = jmodel.id(); |
137 |
110 |
const JointIndex & parent = model.parents[i]; |
|
138 |
|||
139 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
140 |
|||
141 |
✓✗ | 110 |
ColsBlock J_cols = jmodel.jointCols(data.J); |
142 |
✓✗ | 110 |
ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
143 |
✓✗ | 110 |
ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
144 |
✓✗ | 110 |
ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); |
145 |
✓✗ | 110 |
ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq); |
146 |
✓✗ | 110 |
ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); |
147 |
✓✗ | 110 |
ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); |
148 |
✓✗ | 110 |
ColsBlock dFda_cols = jmodel.jointCols(data.dFda); |
149 |
|||
150 |
// tau |
||
151 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
110 |
jmodel.jointVelocitySelector(data.tau).noalias() = J_cols.transpose()*data.of[i].toVector(); |
152 |
|||
153 |
// dtau/da similar to data.M |
||
154 |
✓✗ | 110 |
motionSet::inertiaAction(data.oYcrb[i],J_cols,dFda_cols); |
155 |
|||
156 |
// dtau/dv |
||
157 |
✓✗✓✗ ✓✗ |
110 |
dFdv_cols.noalias() = data.doYcrb[i] * J_cols; |
158 |
✓✗ | 110 |
motionSet::inertiaAction<ADDTO>(data.oYcrb[i],dAdv_cols,dFdv_cols); |
159 |
|||
160 |
// dtau/dq |
||
161 |
✓✓ | 110 |
if(parent>0) |
162 |
{ |
||
163 |
✓✗✓✗ ✓✗ |
106 |
dFdq_cols.noalias() = data.doYcrb[i] * dVdq_cols; |
164 |
✓✗ | 106 |
motionSet::inertiaAction<ADDTO>(data.oYcrb[i],dAdq_cols,dFdq_cols); |
165 |
} |
||
166 |
else |
||
167 |
✓✗ | 4 |
motionSet::inertiaAction(data.oYcrb[i],dAdq_cols,dFdq_cols); |
168 |
|||
169 |
✓✗ | 110 |
motionSet::act<ADDTO>(J_cols,data.of[i],dFdq_cols); |
170 |
|||
171 |
✓✗ | 110 |
data.oYcrb[parent] += data.oYcrb[i]; |
172 |
✓✗ | 110 |
data.doYcrb[parent] += data.doYcrb[i]; |
173 |
✓✗ | 110 |
data.oh[parent] += data.oh[i]; |
174 |
✓✗ | 110 |
data.of[parent] += data.of[i]; |
175 |
|||
176 |
✓✗ | 110 |
motionSet::act(J_cols, data.oh[i], dHdq_cols); |
177 |
✓✗ | 110 |
motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dVdq_cols, dHdq_cols); |
178 |
} |
||
179 |
|||
180 |
template<typename Min, typename Mout> |
||
181 |
static void lhsInertiaMult(const typename Data::Inertia & Y, |
||
182 |
const Eigen::MatrixBase<Min> & J, |
||
183 |
const Eigen::MatrixBase<Mout> & F) |
||
184 |
{ |
||
185 |
Mout & F_ = PINOCCHIO_EIGEN_CONST_CAST(Mout,F); |
||
186 |
motionSet::inertiaAction(Y,J.derived().transpose(),F_.transpose()); |
||
187 |
} |
||
188 |
}; // struct CentroidalDynDerivativesBackwardStep |
||
189 |
|||
190 |
namespace |
||
191 |
{ |
||
192 |
// TODO: should be moved to ForceSet |
||
193 |
template<typename Matrix6xLikeIn, typename Vector3Like, typename Matrix6xLikeOut> |
||
194 |
12 |
inline void translateForceSet(const Eigen::MatrixBase<Matrix6xLikeIn> & Fin, |
|
195 |
const Eigen::MatrixBase<Vector3Like> & v3, |
||
196 |
const Eigen::MatrixBase<Matrix6xLikeOut> & Fout) |
||
197 |
{ |
||
198 |
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLikeIn,6,Eigen::Dynamic) |
||
199 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3) |
||
200 |
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLikeOut,6,Eigen::Dynamic) |
||
201 |
|||
202 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
12 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(Fin.cols(), Fout.cols(), "Fin and Fout do not have the same number of columns"); |
203 |
|||
204 |
✓✓ | 408 |
for(Eigen::DenseIndex k = 0; k < Fin.cols(); ++k) |
205 |
{ |
||
206 |
typedef ForceRef<typename Matrix6xLikeIn::ColXpr> ForceTypeIn; |
||
207 |
typedef ForceRef<typename Matrix6xLikeOut::ColXpr> ForceTypeOut; |
||
208 |
✓✗✓✗ |
396 |
ForceTypeOut fout(PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut,Fout).col(k)); |
209 |
✓✗✓✗ |
396 |
const ForceTypeIn fin(PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeIn,Fin).col(k)); |
210 |
✓✗✓✗ ✓✗ |
396 |
fout.linear() = fin.linear(); |
211 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
396 |
fout.angular().noalias() = fin.angular() - v3.cross(fin.linear()); |
212 |
} |
||
213 |
12 |
} |
|
214 |
} // internal namespace |
||
215 |
|||
216 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, |
||
217 |
typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, |
||
218 |
typename Matrix6xLike0, typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3> |
||
219 |
inline void |
||
220 |
2 |
computeCentroidalDynamicsDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
221 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
222 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
223 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
224 |
const Eigen::MatrixBase<TangentVectorType2> & a, |
||
225 |
const Eigen::MatrixBase<Matrix6xLike0> & dh_dq, |
||
226 |
const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq, |
||
227 |
const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, |
||
228 |
const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da) |
||
229 |
{ |
||
230 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The joint configuration vector is not of right size"); |
231 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of right size"); |
232 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(a.size(), model.nv, "The joint acceleration vector is not of right size"); |
233 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dh_dq.cols(), model.nv); |
234 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dh_dq.rows(), 6); |
235 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dq.cols(), model.nv); |
236 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dq.rows(), 6); |
237 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dv.cols(), model.nv); |
238 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dv.rows(), 6); |
239 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.cols(), model.nv); |
240 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.rows(), 6); |
241 |
✓✗ | 2 |
assert(model.check(data) && "data is not consistent with model."); |
242 |
|||
243 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
244 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
245 |
typedef typename Model::JointIndex JointIndex; |
||
246 |
|||
247 |
typedef CentroidalDynDerivativesForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> Pass1; |
||
248 |
✓✓ | 57 |
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) |
249 |
{ |
||
250 |
✓✗ | 55 |
Pass1::run(model.joints[i],data.joints[i], |
251 |
typename Pass1::ArgsType(model,data,q.derived(),v.derived(),a.derived())); |
||
252 |
} |
||
253 |
|||
254 |
2 |
data.oYcrb[0].setZero(); |
|
255 |
2 |
data.oh[0].setZero(); |
|
256 |
2 |
data.of[0].setZero(); |
|
257 |
typedef CentroidalDynDerivativesBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
258 |
✓✓ | 57 |
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) |
259 |
{ |
||
260 |
✓✗ | 55 |
Pass2::run(model.joints[i],typename Pass2::ArgsType(model,data)); |
261 |
} |
||
262 |
|||
263 |
// expressed all the quantities around the center of mass |
||
264 |
typedef typename Data::Inertia Inertia; |
||
265 |
|||
266 |
2 |
const Inertia & Ytot = data.oYcrb[0]; |
|
267 |
2 |
const typename Inertia::Vector3 & com = Ytot.lever(); |
|
268 |
|||
269 |
// Mass of the system |
||
270 |
2 |
data.mass[0] = Ytot.mass(); |
|
271 |
|||
272 |
// Center of mass of the system |
||
273 |
2 |
data.com[0] = Ytot.lever(); |
|
274 |
|||
275 |
// Compute the centroidal quantities |
||
276 |
2 |
data.hg = data.oh[0]; |
|
277 |
✓✗✓✗ ✓✗ |
2 |
data.hg.angular() += data.hg.linear().cross(com); |
278 |
|||
279 |
2 |
data.dhg = data.of[0]; |
|
280 |
✓✗✓✗ ✓✗ |
2 |
data.dhg.angular() += data.dhg.linear().cross(com); |
281 |
|||
282 |
// Compute centroidal inertia |
||
283 |
2 |
data.Ig.mass() = Ytot.mass(); |
|
284 |
2 |
data.Ig.lever().setZero(); |
|
285 |
2 |
data.Ig.inertia() = Ytot.inertia(); |
|
286 |
|||
287 |
// Compute the partial derivatives |
||
288 |
2 |
translateForceSet(data.dHdq,com,dh_dq.const_cast_derived()); |
|
289 |
2 |
Matrix6xLike0 & dh_dq_ = dh_dq.const_cast_derived(); |
|
290 |
✓✓ | 69 |
for(Eigen::DenseIndex k = 0; k < model.nv; ++k) |
291 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
67 |
dh_dq_.col(k).template segment<3>(Force::ANGULAR) += data.hg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR))/Ytot.mass(); |
292 |
|||
293 |
2 |
translateForceSet(data.dFdq,com,dhdot_dq.const_cast_derived()); |
|
294 |
2 |
Matrix6xLike1 & dhdot_dq_ = dhdot_dq.const_cast_derived(); |
|
295 |
✓✓ | 69 |
for(Eigen::DenseIndex k = 0; k < model.nv; ++k) |
296 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
67 |
dhdot_dq_.col(k).template segment<3>(Force::ANGULAR) += data.dhg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR))/Ytot.mass(); |
297 |
|||
298 |
2 |
translateForceSet(data.dFdv,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv)); |
|
299 |
2 |
translateForceSet(data.dFda,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da)); |
|
300 |
2 |
} |
|
301 |
|||
302 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
303 |
struct GetCentroidalDynDerivativesBackwardStep |
||
304 |
: public fusion::JointUnaryVisitorBase<GetCentroidalDynDerivativesBackwardStep<Scalar,Options,JointCollectionTpl> > |
||
305 |
{ |
||
306 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
307 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
308 |
|||
309 |
typedef boost::fusion::vector<const Model &, |
||
310 |
Data & |
||
311 |
> ArgsType; |
||
312 |
|||
313 |
template<typename JointModel> |
||
314 |
54 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
315 |
const Model & model, |
||
316 |
Data & data) |
||
317 |
{ |
||
318 |
typedef typename Model::JointIndex JointIndex; |
||
319 |
typedef typename Data::Vector3 Vector3; |
||
320 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
321 |
|||
322 |
✓✗ | 54 |
const JointIndex & i = jmodel.id(); |
323 |
54 |
const JointIndex & parent = model.parents[i]; |
|
324 |
|||
325 |
54 |
typename Data::Motion & vtmp = data.v[0]; |
|
326 |
54 |
typename Data::Matrix6x & Ftmp = data.Fcrb[0]; |
|
327 |
|||
328 |
✓✗ | 54 |
ColsBlock J_cols = jmodel.jointCols(data.J); |
329 |
✓✗ | 54 |
ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
330 |
✓✗ | 54 |
ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq); |
331 |
✓✗ | 54 |
ColsBlock Ftmp_cols = jmodel.jointCols(Ftmp); |
332 |
|||
333 |
✓✗✓✗ ✓✗ |
54 |
const Vector3 mg = data.oYcrb[i].mass() * model.gravity.linear(); |
334 |
✓✗✓✓ |
118 |
for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k) |
335 |
{ |
||
336 |
✓✗✓✗ |
64 |
MotionRef<typename ColsBlock::ColXpr> mref(J_cols.col(k)); |
337 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
vtmp.linear() = mref.linear() + mref.angular().cross(data.oYcrb[i].lever()); |
338 |
|||
339 |
✓✗✓✗ |
64 |
ForceRef<typename ColsBlock::ColXpr> fout(Ftmp_cols.col(k)); |
340 |
✓✗✓✗ ✓✗✓✗ |
64 |
fout.angular() += vtmp.linear().cross(mg); |
341 |
} |
||
342 |
|||
343 |
✓✗ | 54 |
data.oh[parent] += data.oh[i]; |
344 |
✓✓ | 54 |
if(parent == 0) |
345 |
{ |
||
346 |
✓✗ | 2 |
data.of[0] += data.of[i]; |
347 |
✓✗ | 2 |
data.oYcrb[0] += data.oYcrb[i]; |
348 |
} |
||
349 |
|||
350 |
✓✗ | 54 |
motionSet::act(J_cols, data.oh[i], dHdq_cols); |
351 |
✓✗ | 54 |
motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dVdq_cols, dHdq_cols); |
352 |
} |
||
353 |
}; // struct GetCentroidalDynDerivativesBackwardStep |
||
354 |
|||
355 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, |
||
356 |
typename Matrix6xLike0, typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3> |
||
357 |
inline void |
||
358 |
1 |
getCentroidalDynamicsDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
359 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
360 |
const Eigen::MatrixBase<Matrix6xLike0> & dh_dq, |
||
361 |
const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq, |
||
362 |
const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, |
||
363 |
const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da) |
||
364 |
{ |
||
365 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dq.cols(), model.nv); |
366 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dq.rows(), 6); |
367 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dv.cols(), model.nv); |
368 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dv.rows(), 6); |
369 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.cols(), model.nv); |
370 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.rows(), 6); |
371 |
✓✗ | 1 |
assert(model.check(data) && "data is not consistent with model."); |
372 |
|||
373 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
374 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
375 |
typedef typename Model::JointIndex JointIndex; |
||
376 |
|||
377 |
// compute first data.oh[0] and data.of[0] |
||
378 |
1 |
data.oh[0].setZero(); data.of[0].setZero(); data.oYcrb[0].setZero(); |
|
379 |
|||
380 |
1 |
typename Data::Matrix6x & Ftmp = data.Fcrb[0]; |
|
381 |
1 |
Ftmp = data.dFdq; |
|
382 |
|||
383 |
typedef GetCentroidalDynDerivativesBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
384 |
✓✓ | 28 |
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) |
385 |
{ |
||
386 |
✓✗ | 27 |
Pass2::run(model.joints[i],typename Pass2::ArgsType(model,data)); |
387 |
} |
||
388 |
|||
389 |
typedef typename Data::Inertia Inertia; |
||
390 |
|||
391 |
1 |
const Inertia & Ytot = data.oYcrb[0]; |
|
392 |
1 |
const typename Inertia::Vector3 & com = Ytot.lever(); |
|
393 |
|||
394 |
// Center of mass of the system |
||
395 |
1 |
data.com[0] = com; |
|
396 |
1 |
data.mass[0] = Ytot.mass(); |
|
397 |
|||
398 |
// Remove the gravity contribution |
||
399 |
✓✗ | 1 |
data.of[0] += Ytot * model.gravity; |
400 |
|||
401 |
// Compute the centroidal quantities |
||
402 |
1 |
data.hg = data.oh[0]; |
|
403 |
✓✗✓✗ ✓✗ |
1 |
data.hg.angular() += data.hg.linear().cross(com); |
404 |
|||
405 |
1 |
data.dhg = data.of[0]; |
|
406 |
✓✗✓✗ ✓✗ |
1 |
data.dhg.angular() += data.dhg.linear().cross(com); |
407 |
|||
408 |
// Compute centroidal inertia |
||
409 |
1 |
data.Ig.mass() = Ytot.mass(); |
|
410 |
1 |
data.Ig.lever().setZero(); |
|
411 |
1 |
data.Ig.inertia() = Ytot.inertia(); |
|
412 |
|||
413 |
// Retrieve the partial derivatives from RNEA derivatives |
||
414 |
1 |
translateForceSet(data.dHdq,com,dh_dq.const_cast_derived()); |
|
415 |
1 |
Matrix6xLike0 & dh_dq_ = dh_dq.const_cast_derived(); |
|
416 |
✓✓ | 33 |
for(Eigen::DenseIndex k = 0; k < model.nv; ++k) |
417 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
32 |
dh_dq_.col(k).template segment<3>(Force::ANGULAR) += data.hg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR))/Ytot.mass(); |
418 |
|||
419 |
1 |
translateForceSet(Ftmp,com,dhdot_dq.const_cast_derived()); |
|
420 |
1 |
Matrix6xLike1 & dhdot_dq_ = dhdot_dq.const_cast_derived(); |
|
421 |
✓✓ | 33 |
for(Eigen::DenseIndex k = 0; k < model.nv; ++k) |
422 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
32 |
dhdot_dq_.col(k).template segment<3>(Force::ANGULAR) += data.dhg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR))/Ytot.mass(); |
423 |
|||
424 |
1 |
translateForceSet(data.dFdv,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv)); |
|
425 |
1 |
translateForceSet(data.dFda,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da)); |
|
426 |
1 |
} |
|
427 |
|||
428 |
} // namespace pinocchio |
||
429 |
|||
430 |
/// @endcond |
||
431 |
|||
432 |
#endif // ifndef __pinocchio_algorithm_centroidal_derivatives_hxx__ |
||
433 |
| Generated by: GCOVR (Version 4.2) |