| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2017-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_algorithm_kinematics_derivatives_hxx__ |
||
6 |
#define __pinocchio_algorithm_kinematics_derivatives_hxx__ |
||
7 |
|||
8 |
#include "pinocchio/multibody/visitor.hpp" |
||
9 |
#include "pinocchio/algorithm/check.hpp" |
||
10 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
11 |
|||
12 |
namespace pinocchio |
||
13 |
{ |
||
14 |
|||
15 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
||
16 |
struct ForwardKinematicsDerivativesForwardStep |
||
17 |
: public fusion::JointUnaryVisitorBase< ForwardKinematicsDerivativesForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> > |
||
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 TangentVectorType1 &, |
||
26 |
const TangentVectorType2 & |
||
27 |
> ArgsType; |
||
28 |
|||
29 |
template<typename JointModel> |
||
30 |
1038 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
31 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
32 |
const Model & model, |
||
33 |
Data & data, |
||
34 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
35 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
36 |
const Eigen::MatrixBase<TangentVectorType2> & a) |
||
37 |
{ |
||
38 |
typedef typename Model::JointIndex JointIndex; |
||
39 |
typedef typename Data::SE3 SE3; |
||
40 |
typedef typename Data::Motion Motion; |
||
41 |
|||
42 |
✓✗ | 1038 |
const JointIndex & i = jmodel.id(); |
43 |
1038 |
const JointIndex & parent = model.parents[i]; |
|
44 |
1038 |
SE3 & oMi = data.oMi[i]; |
|
45 |
1038 |
Motion & vi = data.v[i]; |
|
46 |
1038 |
Motion & ai = data.a[i]; |
|
47 |
1038 |
Motion & ov = data.ov[i]; |
|
48 |
1038 |
Motion & oa = data.oa[i]; |
|
49 |
|||
50 |
✓✗ | 1038 |
jmodel.calc(jdata.derived(),q.derived(),v.derived()); |
51 |
|||
52 |
✓✗✓✓ ✗✓✗ |
1038 |
data.liMi[i] = model.jointPlacements[i]*jdata.M(); |
53 |
|||
54 |
✓✓ | 1038 |
if(parent>0) |
55 |
✓✗ | 998 |
oMi = data.oMi[parent]*data.liMi[i]; |
56 |
else |
||
57 |
✓✗ | 40 |
oMi = data.liMi[i]; |
58 |
|||
59 |
✓✗✓✗ |
1038 |
vi = jdata.v(); |
60 |
✓✓ | 1038 |
if(parent>0) |
61 |
✓✗✓✗ |
998 |
vi += data.liMi[i].actInv(data.v[parent]); |
62 |
|||
63 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1038 |
ai = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (vi ^ jdata.v()); |
64 |
✓✓ | 1038 |
if(parent>0) |
65 |
✓✗✓✗ |
998 |
ai += data.liMi[i].actInv(data.a[parent]); |
66 |
|||
67 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
68 |
✓✗ | 1038 |
ColsBlock dJcols = jmodel.jointCols(data.dJ); |
69 |
✓✗ | 1038 |
ColsBlock Jcols = jmodel.jointCols(data.J); |
70 |
|||
71 |
✓✗✓✗ ✓✗ |
1038 |
Jcols = oMi.act(jdata.S()); |
72 |
✓✗ | 1038 |
ov = oMi.act(vi); // Spatial velocity of joint i expressed in the global frame o |
73 |
✓✗ | 1038 |
motionSet::motionAction(ov,Jcols,dJcols); |
74 |
✓✗ | 1038 |
oa = oMi.act(ai); // Spatial acceleration of joint i expressed in the global frame o |
75 |
} |
||
76 |
|||
77 |
}; |
||
78 |
|||
79 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
||
80 |
23 |
inline void computeForwardKinematicsDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
81 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
82 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
83 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
84 |
const Eigen::MatrixBase<TangentVectorType2> & a) |
||
85 |
{ |
||
86 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
23 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
87 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
23 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
88 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
23 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(a.size(), model.nv, "The acceleration vector is not of right size"); |
89 |
✓✗ | 23 |
assert(model.check(data) && "data is not consistent with model."); |
90 |
|||
91 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
92 |
typedef typename Model::JointIndex JointIndex; |
||
93 |
|||
94 |
23 |
data.v[0].setZero(); |
|
95 |
23 |
data.a[0].setZero(); |
|
96 |
|||
97 |
typedef ForwardKinematicsDerivativesForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> Pass1; |
||
98 |
✓✓ | 623 |
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) |
99 |
{ |
||
100 |
✓✗ | 600 |
Pass1::run(model.joints[i],data.joints[i], |
101 |
typename Pass1::ArgsType(model,data,q.derived(),v.derived(),a.derived())); |
||
102 |
} |
||
103 |
23 |
} |
|
104 |
|||
105 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2> |
||
106 |
struct JointVelocityDerivativesBackwardStep |
||
107 |
: public fusion::JointUnaryVisitorBase< JointVelocityDerivativesBackwardStep<Scalar,Options,JointCollectionTpl,Matrix6xOut1,Matrix6xOut2> > |
||
108 |
{ |
||
109 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
110 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
111 |
|||
112 |
typedef boost::fusion::vector<const Model &, |
||
113 |
const Data &, |
||
114 |
const typename Model::JointIndex &, |
||
115 |
const ReferenceFrame &, |
||
116 |
Matrix6xOut1 &, |
||
117 |
Matrix6xOut2 & |
||
118 |
> ArgsType; |
||
119 |
|||
120 |
template<typename JointModel> |
||
121 |
190 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
122 |
const Model & model, |
||
123 |
const Data & data, |
||
124 |
const typename Model::JointIndex & jointId, |
||
125 |
const ReferenceFrame & rf, |
||
126 |
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, |
||
127 |
const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv) |
||
128 |
{ |
||
129 |
typedef typename Model::JointIndex JointIndex; |
||
130 |
typedef typename Data::SE3 SE3; |
||
131 |
typedef typename Data::Motion Motion; |
||
132 |
|||
133 |
✓✗ | 190 |
const JointIndex & i = jmodel.id(); |
134 |
190 |
const JointIndex & parent = model.parents[i]; |
|
135 |
✓✗ | 190 |
Motion vtmp; // Temporary variable |
136 |
|||
137 |
190 |
const SE3 & oMlast = data.oMi[jointId]; |
|
138 |
190 |
const Motion & vlast = data.ov[jointId]; |
|
139 |
|||
140 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::ConstType ColsBlock; |
||
141 |
✓✗ | 190 |
ColsBlock Jcols = jmodel.jointCols(data.J); |
142 |
|||
143 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut1>::Type ColsBlockOut1; |
||
144 |
190 |
Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq); |
|
145 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut2>::Type ColsBlockOut2; |
||
146 |
190 |
Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv); |
|
147 |
|||
148 |
// dvec/dv: this result is then needed by dvec/dq |
||
149 |
✓✗ | 190 |
ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); |
150 |
✓✓✓✗ |
190 |
switch(rf) |
151 |
{ |
||
152 |
68 |
case WORLD: |
|
153 |
✓✗ | 68 |
v_partial_dv_cols = Jcols; |
154 |
68 |
break; |
|
155 |
40 |
case LOCAL_WORLD_ALIGNED: |
|
156 |
✓✗ | 40 |
details::translateJointJacobian(oMlast,Jcols,v_partial_dv_cols); |
157 |
40 |
break; |
|
158 |
82 |
case LOCAL: |
|
159 |
✓✗ | 82 |
motionSet::se3ActionInverse(oMlast,Jcols,v_partial_dv_cols); |
160 |
82 |
break; |
|
161 |
default: |
||
162 |
assert(false && "This must never happened"); |
||
163 |
} |
||
164 |
|||
165 |
// dvec/dq |
||
166 |
✓✗ | 190 |
ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); |
167 |
✓✓✓✗ |
190 |
switch(rf) |
168 |
{ |
||
169 |
68 |
case WORLD: |
|
170 |
✓✓ | 68 |
if(parent > 0) |
171 |
✓✗ | 56 |
vtmp = data.ov[parent] - vlast; |
172 |
else |
||
173 |
✓✗ | 12 |
vtmp = -vlast; |
174 |
✓✗ | 68 |
motionSet::motionAction(vtmp,Jcols,v_partial_dq_cols); |
175 |
68 |
break; |
|
176 |
40 |
case LOCAL_WORLD_ALIGNED: |
|
177 |
✓✓ | 40 |
if(parent > 0) |
178 |
✓✗ | 32 |
vtmp = data.ov[parent] - vlast; |
179 |
else |
||
180 |
✓✗ | 8 |
vtmp = -vlast; |
181 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
40 |
vtmp.linear() += vtmp.angular().cross(oMlast.translation()); |
182 |
✓✗ | 40 |
motionSet::motionAction(vtmp,v_partial_dv_cols,v_partial_dq_cols); |
183 |
40 |
break; |
|
184 |
82 |
case LOCAL: |
|
185 |
✓✓ | 82 |
if(parent > 0) |
186 |
{ |
||
187 |
✓✗ | 68 |
vtmp = oMlast.actInv(data.ov[parent]); |
188 |
✓✗ | 68 |
motionSet::motionAction(vtmp,v_partial_dv_cols,v_partial_dq_cols); |
189 |
} |
||
190 |
82 |
break; |
|
191 |
default: |
||
192 |
assert(false && "This must never happened"); |
||
193 |
} |
||
194 |
|||
195 |
} |
||
196 |
|||
197 |
}; |
||
198 |
|||
199 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2> |
||
200 |
17 |
inline void getJointVelocityDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
201 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
202 |
const Model::JointIndex jointId, |
||
203 |
const ReferenceFrame rf, |
||
204 |
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, |
||
205 |
const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv) |
||
206 |
{ |
||
207 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1,Data::Matrix6x); |
||
208 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2,Data::Matrix6x); |
||
209 |
|||
210 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
17 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv); |
211 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
17 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dv.cols(), model.nv); |
212 |
✓✗ | 17 |
assert(model.check(data) && "data is not consistent with model."); |
213 |
|||
214 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
215 |
typedef typename Model::JointIndex JointIndex; |
||
216 |
|||
217 |
typedef JointVelocityDerivativesBackwardStep<Scalar,Options,JointCollectionTpl,Matrix6xOut1,Matrix6xOut2> Pass1; |
||
218 |
✓✓ | 112 |
for(JointIndex i = jointId; i > 0; i = model.parents[i]) |
219 |
{ |
||
220 |
✓✗ | 95 |
Pass1::run(model.joints[i], |
221 |
typename Pass1::ArgsType(model,data, |
||
222 |
jointId,rf, |
||
223 |
95 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq), |
|
224 |
95 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv))); |
|
225 |
} |
||
226 |
17 |
} |
|
227 |
|||
228 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4> |
||
229 |
struct JointAccelerationDerivativesBackwardStep |
||
230 |
: public fusion::JointUnaryVisitorBase< JointAccelerationDerivativesBackwardStep<Scalar,Options,JointCollectionTpl,Matrix6xOut1,Matrix6xOut2,Matrix6xOut3,Matrix6xOut4> > |
||
231 |
{ |
||
232 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
233 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
234 |
|||
235 |
typedef boost::fusion::vector<const Model &, |
||
236 |
const Data &, |
||
237 |
const typename Model::JointIndex &, |
||
238 |
const ReferenceFrame &, |
||
239 |
Matrix6xOut1 &, |
||
240 |
Matrix6xOut2 &, |
||
241 |
Matrix6xOut3 &, |
||
242 |
Matrix6xOut4 & |
||
243 |
> ArgsType; |
||
244 |
|||
245 |
template<typename JointModel> |
||
246 |
272 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
247 |
const Model & model, |
||
248 |
const Data & data, |
||
249 |
const typename Model::JointIndex & jointId, |
||
250 |
const ReferenceFrame & rf, |
||
251 |
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, |
||
252 |
const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq, |
||
253 |
const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv, |
||
254 |
const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da) |
||
255 |
{ |
||
256 |
typedef typename Model::JointIndex JointIndex; |
||
257 |
typedef typename Data::SE3 SE3; |
||
258 |
typedef typename Data::Motion Motion; |
||
259 |
|||
260 |
✓✗ | 272 |
const JointIndex & i = jmodel.id(); |
261 |
272 |
const JointIndex & parent = model.parents[i]; |
|
262 |
✓✗ | 272 |
Motion vtmp; // Temporary variable |
263 |
✓✗ | 272 |
Motion atmp; // Temporary variable |
264 |
|||
265 |
272 |
const SE3 & oMlast = data.oMi[jointId]; |
|
266 |
272 |
const Motion & vlast = data.ov[jointId]; |
|
267 |
272 |
const Motion & alast = data.oa[jointId]; |
|
268 |
|||
269 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::ConstType ColsBlock; |
||
270 |
✓✗ | 272 |
ColsBlock dJcols = jmodel.jointCols(data.dJ); |
271 |
✓✗ | 272 |
ColsBlock Jcols = jmodel.jointCols(data.J); |
272 |
|||
273 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut1>::Type ColsBlockOut1; |
||
274 |
272 |
Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq); |
|
275 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut2>::Type ColsBlockOut2; |
||
276 |
272 |
Matrix6xOut2 & a_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,a_partial_dq); |
|
277 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut3>::Type ColsBlockOut3; |
||
278 |
272 |
Matrix6xOut3 & a_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dv); |
|
279 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut4>::Type ColsBlockOut4; |
||
280 |
272 |
Matrix6xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_da); |
|
281 |
|||
282 |
✓✗ | 272 |
ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); |
283 |
✓✗ | 272 |
ColsBlockOut2 a_partial_dq_cols = jmodel.jointCols(a_partial_dq_); |
284 |
✓✗ | 272 |
ColsBlockOut3 a_partial_dv_cols = jmodel.jointCols(a_partial_dv_); |
285 |
✓✗ | 272 |
ColsBlockOut4 a_partial_da_cols = jmodel.jointCols(a_partial_da_); |
286 |
|||
287 |
// dacc/da |
||
288 |
✓✓✓✗ |
272 |
switch(rf) |
289 |
{ |
||
290 |
90 |
case WORLD: |
|
291 |
✓✗ | 90 |
a_partial_da_cols = Jcols; |
292 |
90 |
break; |
|
293 |
50 |
case LOCAL_WORLD_ALIGNED: |
|
294 |
✓✗ | 50 |
details::translateJointJacobian(oMlast,Jcols,a_partial_da_cols); |
295 |
50 |
break; |
|
296 |
132 |
case LOCAL: |
|
297 |
✓✗ | 132 |
motionSet::se3ActionInverse(oMlast,Jcols,a_partial_da_cols); |
298 |
132 |
break; |
|
299 |
} |
||
300 |
|||
301 |
// dacc/dv |
||
302 |
✓✓✓✗ |
272 |
switch(rf) |
303 |
{ |
||
304 |
90 |
case WORLD: |
|
305 |
✓✓ | 90 |
if(parent > 0) |
306 |
✓✗ | 74 |
vtmp = data.ov[parent] - vlast; |
307 |
else |
||
308 |
✓✗ | 16 |
vtmp = -vlast; |
309 |
|||
310 |
// also computes dvec/dq |
||
311 |
✓✗ | 90 |
motionSet::motionAction(vtmp,Jcols,v_partial_dq_cols); |
312 |
|||
313 |
✓✗✓✗ |
90 |
a_partial_dv_cols = v_partial_dq_cols + dJcols; |
314 |
90 |
break; |
|
315 |
50 |
case LOCAL_WORLD_ALIGNED: |
|
316 |
✓✓ | 50 |
if(parent > 0) |
317 |
✓✗ | 40 |
vtmp = data.ov[parent] - vlast; |
318 |
else |
||
319 |
✓✗ | 10 |
vtmp = -vlast; |
320 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
50 |
vtmp.linear() += vtmp.angular().cross(oMlast.translation()); |
321 |
|||
322 |
// also computes dvec/dq |
||
323 |
✓✗ | 50 |
motionSet::motionAction(vtmp,a_partial_da_cols,v_partial_dq_cols); |
324 |
|||
325 |
✓✗ | 50 |
details::translateJointJacobian(oMlast,dJcols,a_partial_dv_cols); |
326 |
// a_partial_dv_cols += v_partial_dq_cols; // dJcols is required later |
||
327 |
50 |
break; |
|
328 |
132 |
case LOCAL: |
|
329 |
// also computes dvec/dq |
||
330 |
✓✓ | 132 |
if(parent > 0) |
331 |
{ |
||
332 |
✓✗ | 110 |
vtmp = oMlast.actInv(data.ov[parent]); |
333 |
✓✗ | 110 |
motionSet::motionAction(vtmp,a_partial_da_cols,v_partial_dq_cols); |
334 |
} |
||
335 |
|||
336 |
✓✓ | 132 |
if(parent > 0) |
337 |
✓✗ | 110 |
vtmp -= data.v[jointId]; |
338 |
else |
||
339 |
✓✗ | 22 |
vtmp = -data.v[jointId]; |
340 |
|||
341 |
✓✗ | 132 |
motionSet::motionAction(vtmp,a_partial_da_cols,a_partial_dv_cols); |
342 |
✓✗ | 132 |
motionSet::se3ActionInverse<ADDTO>(oMlast,dJcols,a_partial_dv_cols); |
343 |
132 |
break; |
|
344 |
} |
||
345 |
|||
346 |
// dacc/dq |
||
347 |
✓✓✓✗ |
272 |
switch(rf) |
348 |
{ |
||
349 |
90 |
case WORLD: |
|
350 |
✓✓ | 90 |
if(parent > 0) |
351 |
✓✗ | 74 |
atmp = data.oa[parent] - alast; |
352 |
else |
||
353 |
✓✗ | 16 |
atmp = -alast; |
354 |
✓✗ | 90 |
motionSet::motionAction(atmp,Jcols,a_partial_dq_cols); |
355 |
|||
356 |
✓✓ | 90 |
if(parent >0) |
357 |
✓✗ | 74 |
motionSet::motionAction<ADDTO>(vtmp,dJcols,a_partial_dq_cols); |
358 |
90 |
break; |
|
359 |
50 |
case LOCAL_WORLD_ALIGNED: |
|
360 |
✓✓ | 50 |
if(parent > 0) |
361 |
✓✗ | 40 |
atmp = data.oa[parent] - alast; |
362 |
else |
||
363 |
✓✗ | 10 |
atmp = -alast; |
364 |
|||
365 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
50 |
atmp.linear() += atmp.angular().cross(oMlast.translation()); |
366 |
✓✗ | 50 |
motionSet::motionAction(atmp,a_partial_da_cols,a_partial_dq_cols); |
367 |
|||
368 |
✓✓ | 50 |
if(parent >0) |
369 |
✓✗ | 40 |
motionSet::motionAction<ADDTO>(vtmp,a_partial_dv_cols,a_partial_dq_cols); |
370 |
|||
371 |
✓✗ | 50 |
a_partial_dv_cols += v_partial_dq_cols; |
372 |
50 |
break; |
|
373 |
132 |
case LOCAL: |
|
374 |
✓✓ | 132 |
if(parent > 0) |
375 |
{ |
||
376 |
✓✗ | 110 |
atmp = oMlast.actInv(data.oa[parent]); |
377 |
✓✗ | 110 |
motionSet::motionAction(atmp,a_partial_da_cols,a_partial_dq_cols); |
378 |
} |
||
379 |
|||
380 |
✓✗ | 132 |
motionSet::motionAction<ADDTO>(vtmp,v_partial_dq_cols,a_partial_dq_cols); |
381 |
132 |
break; |
|
382 |
} |
||
383 |
|||
384 |
|||
385 |
} |
||
386 |
|||
387 |
}; |
||
388 |
|||
389 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4> |
||
390 |
24 |
inline void getJointAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
391 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
392 |
const Model::JointIndex jointId, |
||
393 |
const ReferenceFrame rf, |
||
394 |
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, |
||
395 |
const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq, |
||
396 |
const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv, |
||
397 |
const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da) |
||
398 |
{ |
||
399 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1,Data::Matrix6x); |
||
400 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2,Data::Matrix6x); |
||
401 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut3,Data::Matrix6x); |
||
402 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut4,Data::Matrix6x); |
||
403 |
|||
404 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
24 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv); |
405 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
24 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dq.cols(), model.nv); |
406 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
24 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dv.cols(), model.nv); |
407 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
24 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_da.cols(), model.nv); |
408 |
✓✗ | 24 |
assert(model.check(data) && "data is not consistent with model."); |
409 |
|||
410 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
411 |
typedef typename Model::JointIndex JointIndex; |
||
412 |
|||
413 |
typedef JointAccelerationDerivativesBackwardStep<Scalar,Options,JointCollectionTpl,Matrix6xOut1,Matrix6xOut2,Matrix6xOut3,Matrix6xOut4> Pass1; |
||
414 |
✓✓ | 160 |
for(JointIndex i = jointId; i > 0; i = model.parents[i]) |
415 |
{ |
||
416 |
✓✗ | 136 |
Pass1::run(model.joints[i], |
417 |
typename Pass1::ArgsType(model,data, |
||
418 |
jointId, |
||
419 |
rf, |
||
420 |
136 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq), |
|
421 |
136 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,a_partial_dq), |
|
422 |
136 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dv), |
|
423 |
136 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_da))); |
|
424 |
|||
425 |
} |
||
426 |
24 |
} |
|
427 |
|||
428 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5> |
||
429 |
inline void getJointAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
430 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
431 |
const Model::JointIndex jointId, |
||
432 |
const ReferenceFrame rf, |
||
433 |
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, |
||
434 |
const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv, |
||
435 |
const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq, |
||
436 |
const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv, |
||
437 |
const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da) |
||
438 |
{ |
||
439 |
getJointAccelerationDerivatives(model,data, |
||
440 |
jointId,rf, |
||
441 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq), |
||
442 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dq), |
||
443 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_dv), |
||
444 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5,a_partial_da)); |
||
445 |
|||
446 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv) = a_partial_da; |
||
447 |
} |
||
448 |
|||
449 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
450 |
inline void |
||
451 |
2 |
computeJointKinematicHessians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
452 |
DataTpl<Scalar,Options,JointCollectionTpl> & data) |
||
453 |
{ |
||
454 |
✓✗ | 2 |
assert(model.check(data) && "data is not consistent with model."); |
455 |
|||
456 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
457 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
458 |
|||
459 |
typedef MotionRef<const typename Data::Matrix6x::ConstColXpr> MotionIn; |
||
460 |
|||
461 |
typedef typename Data::Motion Motion; |
||
462 |
typedef Eigen::Map<typename Motion::Vector6> MapVector6; |
||
463 |
typedef MotionRef<MapVector6> MotionOut; |
||
464 |
|||
465 |
2 |
const typename Data::Matrix6x & J = data.J; |
|
466 |
2 |
typename Data::Tensor3x & kinematic_hessians = data.kinematic_hessians; |
|
467 |
2 |
const Eigen::DenseIndex slice_matrix_size = 6 * model.nv; |
|
468 |
|||
469 |
✓✓ | 56 |
for(size_t joint_id = 1; joint_id < (size_t)model.njoints; ++joint_id) |
470 |
{ |
||
471 |
54 |
const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id]; |
|
472 |
54 |
const std::vector<typename Model::JointIndex> & support = model.supports[joint_id]; |
|
473 |
|||
474 |
54 |
const int nv = model.nvs[joint_id]; |
|
475 |
54 |
const int idx_v = model.idx_vs[joint_id]; |
|
476 |
|||
477 |
✓✓ | 118 |
for(int joint_row = 0; joint_row < nv; ++joint_row) |
478 |
{ |
||
479 |
64 |
const Eigen::DenseIndex outer_row_id = idx_v + joint_row; |
|
480 |
|||
481 |
✓✓ | 350 |
for(size_t support_id = 0; support_id < support.size()-1; ++support_id) |
482 |
{ |
||
483 |
286 |
const typename Model::JointIndex joint_id_support = support[support_id]; |
|
484 |
|||
485 |
286 |
const int inner_nv = model.nvs[joint_id_support]; |
|
486 |
286 |
const int inner_idx_v = model.idx_vs[joint_id_support]; |
|
487 |
✓✓ | 768 |
for(int inner_joint_row = 0; inner_joint_row < inner_nv; ++inner_joint_row) |
488 |
{ |
||
489 |
482 |
const Eigen::DenseIndex inner_row_id = inner_idx_v + inner_joint_row; |
|
490 |
✗✓ | 482 |
assert(inner_row_id < outer_row_id); |
491 |
|||
492 |
✓✗ | 482 |
MapVector6 motion_vec_in( kinematic_hessians.data() |
493 |
✓✗ | 482 |
+ inner_row_id * slice_matrix_size |
494 |
✓✗ | 482 |
+ outer_row_id * 6); |
495 |
✓✗ | 482 |
MapVector6 motion_vec_out( kinematic_hessians.data() |
496 |
✓✗ | 482 |
+ outer_row_id * slice_matrix_size |
497 |
✓✗ | 482 |
+ inner_row_id * 6); |
498 |
|||
499 |
✓✗✓✗ |
482 |
motion_vec_out = -motion_vec_in; |
500 |
} |
||
501 |
} |
||
502 |
|||
503 |
✓✗✓✗ |
64 |
const MotionIn S1(J.col(outer_row_id)); |
504 |
|||
505 |
// Computations already done |
||
506 |
✓✓ | 94 |
for(int inner_joint_row = 0; inner_joint_row < joint_row; ++inner_joint_row) |
507 |
{ |
||
508 |
30 |
const Eigen::DenseIndex inner_row_id = idx_v + inner_joint_row; |
|
509 |
✓✗ | 30 |
MapVector6 motion_vec_in( kinematic_hessians.data() |
510 |
✓✗ | 30 |
+ inner_row_id * slice_matrix_size |
511 |
✓✗ | 30 |
+ outer_row_id * 6); |
512 |
✓✗ | 30 |
MapVector6 motion_vec_out( kinematic_hessians.data() |
513 |
✓✗ | 30 |
+ outer_row_id * slice_matrix_size |
514 |
✓✗ | 30 |
+ inner_row_id * 6); |
515 |
|||
516 |
✓✗✓✗ |
30 |
motion_vec_out = -motion_vec_in; |
517 |
} |
||
518 |
|||
519 |
✓✓ | 94 |
for(int inner_joint_row = joint_row+1; inner_joint_row < nv; ++inner_joint_row) |
520 |
{ |
||
521 |
30 |
const Eigen::DenseIndex inner_row_id = idx_v + inner_joint_row; |
|
522 |
✓✗✓✗ |
30 |
const MotionIn S2(J.col(inner_row_id)); |
523 |
|||
524 |
✓✗ | 30 |
MapVector6 motion_vec_out( kinematic_hessians.data() |
525 |
✓✗ | 30 |
+ outer_row_id * slice_matrix_size |
526 |
✓✗ | 30 |
+ inner_row_id * 6); |
527 |
✓✗✓✗ |
30 |
MotionOut S1xS2(motion_vec_out); |
528 |
|||
529 |
✓✗✓✗ |
30 |
S1xS2 = S1.cross(S2); |
530 |
} |
||
531 |
|||
532 |
✓✓ | 546 |
for(size_t subtree_id = 1; subtree_id < subtree.size(); ++subtree_id) |
533 |
{ |
||
534 |
482 |
const typename Model::JointIndex joint_id_subtree = subtree[subtree_id]; |
|
535 |
|||
536 |
482 |
const int inner_nv = model.nvs[joint_id_subtree]; |
|
537 |
482 |
const int inner_idx_v = model.idx_vs[joint_id_subtree]; |
|
538 |
✓✓ | 964 |
for(int inner_joint_row = 0; inner_joint_row < inner_nv; ++inner_joint_row) |
539 |
{ |
||
540 |
482 |
const Eigen::DenseIndex inner_row_id = inner_idx_v + inner_joint_row; |
|
541 |
✗✓ | 482 |
assert(inner_row_id > outer_row_id); |
542 |
✓✗✓✗ |
482 |
const MotionIn S2(J.col(inner_row_id)); |
543 |
|||
544 |
✓✗ | 482 |
MapVector6 motion_vec_out( kinematic_hessians.data() |
545 |
✓✗ | 482 |
+ outer_row_id * slice_matrix_size |
546 |
✓✗ | 482 |
+ inner_row_id * 6); |
547 |
✓✗✓✗ |
482 |
MotionOut S1xS2(motion_vec_out); |
548 |
|||
549 |
✓✗✓✗ |
482 |
S1xS2 = S1.cross(S2); |
550 |
} |
||
551 |
} |
||
552 |
} |
||
553 |
} |
||
554 |
2 |
} |
|
555 |
|||
556 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
557 |
inline void |
||
558 |
3 |
getJointKinematicHessian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
559 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
560 |
const JointIndex joint_id, |
||
561 |
const ReferenceFrame rf, |
||
562 |
Tensor<Scalar,3,Options> & kinematic_hessian) |
||
563 |
{ |
||
564 |
✓✗ | 3 |
assert(model.check(data) && "data is not consistent with model."); |
565 |
✓✗✓✗ |
3 |
assert(joint_id < model.joints.size() && joint_id > 0 && "joint_id is outside the valid index for a joint in model.joints"); |
566 |
|||
567 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
568 |
typedef typename Data::SE3 SE3; |
||
569 |
typedef typename Data::Motion Motion; |
||
570 |
|||
571 |
3 |
const typename Data::Matrix6x & J = data.J; |
|
572 |
3 |
const typename Data::Tensor3x & kinematic_hessians = data.kinematic_hessians; |
|
573 |
|||
574 |
// Allocate memory |
||
575 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
3 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_hessian.dimension(0), 6, "The result tensor is not of the right dimension."); |
576 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
3 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_hessian.dimension(1), model.nv, "The result tensor is not of the right dimension."); |
577 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
3 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_hessian.dimension(2), model.nv, "The result tensor is not of the right dimension."); |
578 |
|||
579 |
3 |
const int idx_vj = model.joints[joint_id].idx_v(); |
|
580 |
3 |
const int nvj = model.joints[joint_id].nv(); |
|
581 |
3 |
const Eigen::DenseIndex slice_matrix_size = 6 * model.nv; |
|
582 |
|||
583 |
typedef std::vector<int> IndexVector; |
||
584 |
3 |
const Eigen::DenseIndex last_idx = idx_vj+nvj-1; |
|
585 |
3 |
const std::vector<int> & supporting_indexes = data.supports_fromRow[(size_t)(last_idx)]; // until the last element of the joint size (nvj) |
|
586 |
|||
587 |
typedef Eigen::Map<typename Motion::Vector6> MapVector6; |
||
588 |
typedef MotionRef<MapVector6> MotionOut; |
||
589 |
typedef Eigen::Map<const typename Motion::Vector6> ConstMapVector6; |
||
590 |
typedef MotionRef<ConstMapVector6> MotionIn; |
||
591 |
|||
592 |
✓✓✓✗ |
3 |
switch(rf) |
593 |
{ |
||
594 |
1 |
case WORLD: |
|
595 |
{ |
||
596 |
✓✓ | 11 |
for(size_t i = 0; i < supporting_indexes.size(); ++i) |
597 |
{ |
||
598 |
10 |
const Eigen::DenseIndex outer_row_id = supporting_indexes[i]; |
|
599 |
|||
600 |
// Take into account parent indexes of the current joint motion subspace |
||
601 |
25 |
for(int subspace_idx = data.start_idx_v_fromRow[(size_t)outer_row_id]; |
|
602 |
✓✓ | 25 |
subspace_idx < outer_row_id; ++subspace_idx) |
603 |
{ |
||
604 |
✓✗ | 15 |
ConstMapVector6 vec_in( kinematic_hessians.data() |
605 |
✓✗ | 15 |
+ outer_row_id * slice_matrix_size |
606 |
✓✗ | 15 |
+ subspace_idx * 6); |
607 |
|||
608 |
✓✗ | 15 |
MapVector6 vec_out( kinematic_hessian.data() |
609 |
✓✗ | 15 |
+ outer_row_id * slice_matrix_size |
610 |
✓✗ | 15 |
+ subspace_idx * 6); |
611 |
|||
612 |
✓✗ | 15 |
vec_out = vec_in; |
613 |
} |
||
614 |
|||
615 |
✓✓ | 55 |
for(size_t j = i+1; j < supporting_indexes.size(); ++j) |
616 |
{ |
||
617 |
45 |
const Eigen::DenseIndex inner_row_id = supporting_indexes[j]; |
|
618 |
|||
619 |
✓✗ | 45 |
ConstMapVector6 vec_in( kinematic_hessians.data() |
620 |
✓✗ | 45 |
+ outer_row_id * slice_matrix_size |
621 |
✓✗ | 45 |
+ inner_row_id * 6); |
622 |
|||
623 |
✓✗ | 45 |
MapVector6 vec_out( kinematic_hessian.data() |
624 |
✓✗ | 45 |
+ outer_row_id * slice_matrix_size |
625 |
✓✗ | 45 |
+ inner_row_id * 6); |
626 |
|||
627 |
✓✗ | 45 |
vec_out = vec_in; |
628 |
} |
||
629 |
} |
||
630 |
1 |
break; |
|
631 |
} |
||
632 |
1 |
case LOCAL_WORLD_ALIGNED: |
|
633 |
{ |
||
634 |
typedef MotionRef<const typename Data::Matrix6x::ConstColXpr> MotionColRef; |
||
635 |
1 |
const SE3 & oMlast = data.oMi[joint_id]; |
|
636 |
|||
637 |
✓✓ | 11 |
for(size_t i = 0; i < supporting_indexes.size(); ++i) |
638 |
{ |
||
639 |
10 |
const Eigen::DenseIndex outer_row_id = supporting_indexes[i]; |
|
640 |
✓✗✓✗ |
10 |
const MotionColRef S1(J.col(outer_row_id)); |
641 |
|||
642 |
✓✗ | 40 |
for(size_t j = 0; j < supporting_indexes.size(); ++j) |
643 |
{ |
||
644 |
40 |
const Eigen::DenseIndex inner_row_id = supporting_indexes[j]; |
|
645 |
✓✓ | 40 |
if(inner_row_id >= data.start_idx_v_fromRow[(size_t)outer_row_id]) break; |
646 |
|||
647 |
✓✗✓✗ |
30 |
MotionColRef S2(J.col(inner_row_id)); |
648 |
|||
649 |
✓✗ | 30 |
ConstMapVector6 vec_in( kinematic_hessians.data() |
650 |
✓✗ | 30 |
+ outer_row_id * slice_matrix_size |
651 |
✓✗ | 30 |
+ inner_row_id * 6); |
652 |
✓✗✓✗ |
30 |
MotionIn S1xS2(vec_in); |
653 |
|||
654 |
✓✗ | 30 |
MapVector6 vec_out( kinematic_hessian.data() |
655 |
✓✗ | 30 |
+ outer_row_id * slice_matrix_size |
656 |
✓✗ | 30 |
+ inner_row_id * 6); |
657 |
✓✗✓✗ |
30 |
MotionOut m_out(vec_out); |
658 |
|||
659 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
30 |
m_out.linear() = -(S1.linear() - oMlast.translation().cross(S1.angular())).cross(S2.angular()); |
660 |
} |
||
661 |
|||
662 |
// Take into account parent indexes of the current joint motion subspace |
||
663 |
10 |
for(int inner_row_id = data.start_idx_v_fromRow[(size_t)outer_row_id]; |
|
664 |
✓✓ | 25 |
inner_row_id < outer_row_id; ++inner_row_id) |
665 |
{ |
||
666 |
✓✗✓✗ |
15 |
MotionColRef S2(J.col(inner_row_id)); |
667 |
|||
668 |
✓✗ | 15 |
ConstMapVector6 vec_in( kinematic_hessians.data() |
669 |
✓✗ | 15 |
+ outer_row_id * slice_matrix_size |
670 |
✓✗ | 15 |
+ inner_row_id * 6); |
671 |
✓✗✓✗ |
15 |
MotionIn S1xS2(vec_in); |
672 |
|||
673 |
✓✗ | 15 |
MapVector6 vec_out( kinematic_hessian.data() |
674 |
✓✗ | 15 |
+ outer_row_id * slice_matrix_size |
675 |
✓✗ | 15 |
+ inner_row_id * 6); |
676 |
✓✗✓✗ |
15 |
MotionOut m_out(vec_out); |
677 |
|||
678 |
✓✗ | 15 |
vec_out = vec_in; |
679 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
15 |
m_out.linear() -= (S1.linear() - oMlast.translation().cross(S1.angular())).cross(S2.angular()) + oMlast.translation().cross(S1xS2.angular()); |
680 |
} |
||
681 |
|||
682 |
// case: outer_row_id == inner_row_id |
||
683 |
{ |
||
684 |
✓✗ | 10 |
MapVector6 vec_out( kinematic_hessian.data() |
685 |
✓✗ | 10 |
+ outer_row_id * slice_matrix_size |
686 |
✓✗ | 10 |
+ outer_row_id * 6); |
687 |
✓✗✓✗ |
10 |
MotionOut m_out(vec_out); |
688 |
|||
689 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
10 |
m_out.linear() = -(S1.linear() - oMlast.translation().cross(S1.angular())).cross(S1.angular()); |
690 |
} |
||
691 |
|||
692 |
✓✓ | 55 |
for(size_t j = i+1; j < supporting_indexes.size(); ++j) |
693 |
{ |
||
694 |
45 |
const Eigen::DenseIndex inner_row_id = supporting_indexes[j]; |
|
695 |
✓✗✓✗ |
45 |
MotionColRef S2(J.col(inner_row_id)); |
696 |
|||
697 |
✓✗ | 45 |
ConstMapVector6 vec_in( kinematic_hessians.data() |
698 |
✓✗ | 45 |
+ outer_row_id * slice_matrix_size |
699 |
✓✗ | 45 |
+ inner_row_id * 6); |
700 |
✓✗✓✗ |
45 |
MotionIn S1xS2(vec_in); |
701 |
|||
702 |
✓✗ | 45 |
MapVector6 vec_out( kinematic_hessian.data() |
703 |
✓✗ | 45 |
+ outer_row_id * slice_matrix_size |
704 |
✓✗ | 45 |
+ inner_row_id * 6); |
705 |
✓✗✓✗ |
45 |
MotionOut m_out(vec_out); |
706 |
|||
707 |
✓✗ | 45 |
vec_out = vec_in; |
708 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
45 |
m_out.linear() -= (S1.linear() - oMlast.translation().cross(S1.angular())).cross(S2.angular()) + oMlast.translation().cross(S1xS2.angular()); |
709 |
} |
||
710 |
} |
||
711 |
1 |
break; |
|
712 |
} |
||
713 |
1 |
case LOCAL: |
|
714 |
{ |
||
715 |
1 |
const SE3 & oMlast = data.oMi[joint_id]; |
|
716 |
|||
717 |
11 |
for(IndexVector::const_reverse_iterator rit = supporting_indexes.rbegin(); |
|
718 |
✓✗✓✓ |
21 |
rit != supporting_indexes.rend(); ++rit) |
719 |
{ |
||
720 |
10 |
const Eigen::DenseIndex outer_row_id = *rit; |
|
721 |
|||
722 |
// This corresponds to the joint connected to the world, we can skip |
||
723 |
✓✓ | 10 |
if(data.parents_fromRow[(size_t)data.start_idx_v_fromRow[(size_t)outer_row_id]] < 0) |
724 |
6 |
continue; |
|
725 |
|||
726 |
// Take into account current joint motion subspace |
||
727 |
4 |
for(int subspace_idx = data.end_idx_v_fromRow[(size_t)outer_row_id]; |
|
728 |
✗✓ | 4 |
subspace_idx > outer_row_id; --subspace_idx) |
729 |
{ |
||
730 |
ConstMapVector6 vec_in( kinematic_hessians.data() |
||
731 |
+ subspace_idx * slice_matrix_size |
||
732 |
+ outer_row_id * 6); |
||
733 |
MotionIn m_in(vec_in); |
||
734 |
|||
735 |
MapVector6 vec_out( kinematic_hessian.data() |
||
736 |
+ outer_row_id * slice_matrix_size |
||
737 |
+ subspace_idx * 6); |
||
738 |
MotionOut m_out(vec_out); |
||
739 |
|||
740 |
m_out = oMlast.actInv(m_in); |
||
741 |
} |
||
742 |
|||
743 |
4 |
IndexVector::const_reverse_iterator inner_rit = rit; |
|
744 |
34 |
for(++inner_rit; |
|
745 |
✓✗✓✓ |
34 |
inner_rit != supporting_indexes.rend(); ++inner_rit) |
746 |
{ |
||
747 |
30 |
const Eigen::DenseIndex inner_row_id = *inner_rit; |
|
748 |
|||
749 |
✓✗ | 30 |
ConstMapVector6 vec_in( kinematic_hessians.data() |
750 |
✓✗ | 30 |
+ inner_row_id * slice_matrix_size |
751 |
✓✗ | 30 |
+ outer_row_id * 6); |
752 |
|||
753 |
✓✗✓✗ |
30 |
MotionIn m_in(vec_in); |
754 |
|||
755 |
✓✗ | 30 |
MapVector6 vec_out( kinematic_hessian.data() |
756 |
✓✗ | 30 |
+ outer_row_id * slice_matrix_size |
757 |
✓✗ | 30 |
+ inner_row_id * 6); |
758 |
✓✗✓✗ |
30 |
MotionOut m_out(vec_out); |
759 |
|||
760 |
✓✗✓✗ |
30 |
m_out = oMlast.actInv(m_in); |
761 |
} |
||
762 |
} |
||
763 |
|||
764 |
1 |
break; |
|
765 |
} |
||
766 |
default: |
||
767 |
assert(false && "must never happened"); |
||
768 |
break; |
||
769 |
} |
||
770 |
3 |
} |
|
771 |
|||
772 |
} // namespace pinocchio |
||
773 |
|||
774 |
#endif // ifndef __pinocchio_algorithm_kinematics_derivatives_hxx__ |
| Generated by: GCOVR (Version 4.2) |