Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/kinematics-derivatives.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 502 | 521 | 96.4% |
Branches: | 644 | 1640 | 39.3% |
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/spatial/classic-acceleration.hpp" | ||
9 | #include "pinocchio/multibody/visitor.hpp" | ||
10 | #include "pinocchio/algorithm/check.hpp" | ||
11 | #include "pinocchio/algorithm/jacobian.hpp" | ||
12 | |||
13 | namespace pinocchio | ||
14 | { | ||
15 | namespace impl | ||
16 | { | ||
17 | template< | ||
18 | typename Scalar, | ||
19 | int Options, | ||
20 | template<typename, int> | ||
21 | class JointCollectionTpl, | ||
22 | typename ConfigVectorType, | ||
23 | typename TangentVectorType1, | ||
24 | typename TangentVectorType2> | ||
25 | struct ForwardKinematicsDerivativesForwardStep | ||
26 | : public fusion::JointUnaryVisitorBase<ForwardKinematicsDerivativesForwardStep< | ||
27 | Scalar, | ||
28 | Options, | ||
29 | JointCollectionTpl, | ||
30 | ConfigVectorType, | ||
31 | TangentVectorType1, | ||
32 | TangentVectorType2>> | ||
33 | { | ||
34 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
35 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
36 | |||
37 | typedef boost::fusion::vector< | ||
38 | const Model &, | ||
39 | Data &, | ||
40 | const ConfigVectorType &, | ||
41 | const TangentVectorType1 &, | ||
42 | const TangentVectorType2 &> | ||
43 | ArgsType; | ||
44 | |||
45 | template<typename JointModel> | ||
46 | 1470 | static void algo( | |
47 | const JointModelBase<JointModel> & jmodel, | ||
48 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
49 | const Model & model, | ||
50 | Data & data, | ||
51 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
52 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
53 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
54 | { | ||
55 | typedef typename Model::JointIndex JointIndex; | ||
56 | typedef typename Data::SE3 SE3; | ||
57 | typedef typename Data::Motion Motion; | ||
58 | |||
59 |
1/2✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
|
1470 | const JointIndex & i = jmodel.id(); |
60 | 1470 | const JointIndex & parent = model.parents[i]; | |
61 | 1470 | SE3 & oMi = data.oMi[i]; | |
62 | 1470 | Motion & vi = data.v[i]; | |
63 | 1470 | Motion & ai = data.a[i]; | |
64 | 1470 | Motion & ov = data.ov[i]; | |
65 | 1470 | Motion & oa = data.oa[i]; | |
66 | |||
67 |
1/2✓ Branch 4 taken 735 times.
✗ Branch 5 not taken.
|
1470 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); |
68 | |||
69 |
6/10✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 708 times.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 708 times.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 708 times.
✗ Branch 13 not taken.
|
1470 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
70 | |||
71 |
2/2✓ Branch 0 taken 707 times.
✓ Branch 1 taken 28 times.
|
1470 | if (parent > 0) |
72 |
2/4✓ Branch 3 taken 707 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 707 times.
✗ Branch 7 not taken.
|
1414 | oMi = data.oMi[parent] * data.liMi[i]; |
73 | else | ||
74 |
1/2✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
|
56 | oMi = data.liMi[i]; |
75 | |||
76 |
2/4✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 735 times.
✗ Branch 5 not taken.
|
1470 | vi = jdata.v(); |
77 |
2/2✓ Branch 0 taken 707 times.
✓ Branch 1 taken 28 times.
|
1470 | if (parent > 0) |
78 |
2/4✓ Branch 3 taken 707 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 707 times.
✗ Branch 7 not taken.
|
1414 | vi += data.liMi[i].actInv(data.v[parent]); |
79 | |||
80 |
9/18✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 735 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 735 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 735 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 735 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 735 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 735 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 735 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 735 times.
✗ Branch 26 not taken.
|
1470 | ai = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (vi ^ jdata.v()); |
81 |
2/2✓ Branch 0 taken 707 times.
✓ Branch 1 taken 28 times.
|
1470 | if (parent > 0) |
82 |
2/4✓ Branch 3 taken 707 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 707 times.
✗ Branch 7 not taken.
|
1414 | ai += data.liMi[i].actInv(data.a[parent]); |
83 | |||
84 | typedef | ||
85 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
86 | ColsBlock; | ||
87 |
1/2✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
|
1470 | ColsBlock dJcols = jmodel.jointCols(data.dJ); |
88 |
1/2✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
|
1470 | ColsBlock Jcols = jmodel.jointCols(data.J); |
89 | |||
90 |
3/6✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 735 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 735 times.
✗ Branch 8 not taken.
|
1470 | Jcols = oMi.act(jdata.S()); |
91 |
2/4✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 735 times.
✗ Branch 5 not taken.
|
1470 | ov = oMi.act(vi); // Spatial velocity of joint i expressed in the global frame o |
92 |
1/2✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
|
1470 | motionSet::motionAction(ov, Jcols, dJcols); |
93 |
2/4✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 735 times.
✗ Branch 5 not taken.
|
1470 | oa = oMi.act(ai); // Spatial acceleration of joint i expressed in the global frame o |
94 | } | ||
95 | }; | ||
96 | |||
97 | template< | ||
98 | typename Scalar, | ||
99 | int Options, | ||
100 | template<typename, int> | ||
101 | class JointCollectionTpl, | ||
102 | typename ConfigVectorType, | ||
103 | typename TangentVectorType1, | ||
104 | typename TangentVectorType2> | ||
105 | 28 | void computeForwardKinematicsDerivatives( | |
106 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
107 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
108 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
109 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
110 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
111 | { | ||
112 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 28 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
28 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
113 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
114 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 28 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
28 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
115 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 28 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
28 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
116 | a.size(), model.nv, "The acceleration vector is not of right size"); | ||
117 |
1/2✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
|
28 | assert(model.check(data) && "data is not consistent with model."); |
118 | |||
119 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
120 | typedef typename Model::JointIndex JointIndex; | ||
121 | |||
122 | 28 | data.v[0].setZero(); | |
123 | 28 | data.a[0].setZero(); | |
124 | |||
125 | typedef ForwardKinematicsDerivativesForwardStep< | ||
126 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, | ||
127 | TangentVectorType2> | ||
128 | Pass1; | ||
129 |
2/2✓ Branch 0 taken 735 times.
✓ Branch 1 taken 28 times.
|
763 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
130 | { | ||
131 |
1/2✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
|
735 | Pass1::run( |
132 | 735 | model.joints[i], data.joints[i], | |
133 | 1470 | typename Pass1::ArgsType(model, data, q.derived(), v.derived(), a.derived())); | |
134 | } | ||
135 | 28 | } | |
136 | |||
137 | template< | ||
138 | typename Scalar, | ||
139 | int Options, | ||
140 | template<typename, int> | ||
141 | class JointCollectionTpl, | ||
142 | typename Matrix6xOut1, | ||
143 | typename Matrix6xOut2> | ||
144 | struct JointVelocityDerivativesBackwardStep | ||
145 | : public fusion::JointUnaryVisitorBase<JointVelocityDerivativesBackwardStep< | ||
146 | Scalar, | ||
147 | Options, | ||
148 | JointCollectionTpl, | ||
149 | Matrix6xOut1, | ||
150 | Matrix6xOut2>> | ||
151 | { | ||
152 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
153 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
154 | |||
155 | typedef boost::fusion::vector< | ||
156 | const Model &, | ||
157 | const Data &, | ||
158 | const typename Model::JointIndex &, | ||
159 | const ReferenceFrame &, | ||
160 | Matrix6xOut1 &, | ||
161 | Matrix6xOut2 &> | ||
162 | ArgsType; | ||
163 | |||
164 | template<typename JointModel> | ||
165 | 246 | static void algo( | |
166 | const JointModelBase<JointModel> & jmodel, | ||
167 | const Model & model, | ||
168 | const Data & data, | ||
169 | const typename Model::JointIndex & jointId, | ||
170 | const ReferenceFrame & rf, | ||
171 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
172 | const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv) | ||
173 | { | ||
174 | typedef typename Model::JointIndex JointIndex; | ||
175 | typedef typename Data::SE3 SE3; | ||
176 | typedef typename Data::Motion Motion; | ||
177 | |||
178 |
1/2✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
|
246 | const JointIndex & i = jmodel.id(); |
179 | 246 | const JointIndex & parent = model.parents[i]; | |
180 |
1/2✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
|
246 | Motion vtmp; // Temporary variable |
181 | |||
182 | 246 | const SE3 & oMlast = data.oMi[jointId]; | |
183 | 246 | const Motion & vlast = data.ov[jointId]; | |
184 | |||
185 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn< | ||
186 | typename Data::Matrix6x>::ConstType ColsBlock; | ||
187 |
1/2✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
|
246 | ColsBlock Jcols = jmodel.jointCols(data.J); |
188 | |||
189 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut1>::Type | ||
190 | ColsBlockOut1; | ||
191 | 246 | Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq); | |
192 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut2>::Type | ||
193 | ColsBlockOut2; | ||
194 | 246 | Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv); | |
195 | |||
196 | // dvec/dv: this result is then needed by dvec/dq | ||
197 |
1/2✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
|
246 | ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); |
198 |
3/4✓ Branch 0 taken 34 times.
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
|
246 | switch (rf) |
199 | { | ||
200 | 68 | case WORLD: | |
201 |
1/2✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
|
68 | v_partial_dv_cols = Jcols; |
202 | 68 | break; | |
203 | 40 | case LOCAL_WORLD_ALIGNED: | |
204 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | details::translateJointJacobian(oMlast, Jcols, v_partial_dv_cols); |
205 | 40 | break; | |
206 | 138 | case LOCAL: | |
207 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
138 | motionSet::se3ActionInverse(oMlast, Jcols, v_partial_dv_cols); |
208 | 138 | break; | |
209 | ✗ | default: | |
210 | ✗ | assert(false && "This must never happened"); | |
211 | } | ||
212 | |||
213 | // dvec/dq | ||
214 |
1/2✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
|
246 | ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); |
215 |
3/4✓ Branch 0 taken 34 times.
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
|
246 | switch (rf) |
216 | { | ||
217 | 68 | case WORLD: | |
218 |
2/2✓ Branch 0 taken 28 times.
✓ Branch 1 taken 6 times.
|
68 | if (parent > 0) |
219 |
2/4✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 28 times.
✗ Branch 6 not taken.
|
56 | vtmp = data.ov[parent] - vlast; |
220 | else | ||
221 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | vtmp = -vlast; |
222 |
1/2✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
|
68 | motionSet::motionAction(vtmp, Jcols, v_partial_dq_cols); |
223 | 68 | break; | |
224 | 40 | case LOCAL_WORLD_ALIGNED: | |
225 |
2/2✓ Branch 0 taken 16 times.
✓ Branch 1 taken 4 times.
|
40 | if (parent > 0) |
226 |
2/4✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
|
32 | vtmp = data.ov[parent] - vlast; |
227 | else | ||
228 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
8 | vtmp = -vlast; |
229 |
5/10✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
|
40 | vtmp.linear() += vtmp.angular().cross(oMlast.translation()); |
230 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | motionSet::motionAction(vtmp, v_partial_dv_cols, v_partial_dq_cols); |
231 | 40 | break; | |
232 | 138 | case LOCAL: | |
233 |
2/2✓ Branch 0 taken 58 times.
✓ Branch 1 taken 11 times.
|
138 | if (parent > 0) |
234 | { | ||
235 |
2/4✓ Branch 2 taken 58 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 58 times.
✗ Branch 6 not taken.
|
116 | vtmp = oMlast.actInv(data.ov[parent]); |
236 |
1/2✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
|
116 | motionSet::motionAction(vtmp, v_partial_dv_cols, v_partial_dq_cols); |
237 | } | ||
238 | 138 | break; | |
239 | ✗ | default: | |
240 | ✗ | assert(false && "This must never happened"); | |
241 | } | ||
242 | } | ||
243 | }; | ||
244 | |||
245 | template< | ||
246 | typename Scalar, | ||
247 | int Options, | ||
248 | template<typename, int> | ||
249 | class JointCollectionTpl, | ||
250 | typename Matrix6xOut1, | ||
251 | typename Matrix6xOut2> | ||
252 | 21 | void getJointVelocityDerivatives( | |
253 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
254 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
255 | const Model::JointIndex jointId, | ||
256 | const ReferenceFrame rf, | ||
257 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
258 | const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv) | ||
259 | { | ||
260 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1, Data::Matrix6x); | ||
261 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2, Data::Matrix6x); | ||
262 | |||
263 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
264 | |||
265 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
21 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv); |
266 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
21 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dv.cols(), model.nv); |
267 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 21 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
21 | PINOCCHIO_CHECK_INPUT_ARGUMENT((int)jointId < model.njoints, "The joint id is invalid."); |
268 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
21 | assert(model.check(data) && "data is not consistent with model."); |
269 | |||
270 | typedef typename Model::JointIndex JointIndex; | ||
271 | |||
272 | typedef JointVelocityDerivativesBackwardStep< | ||
273 | Scalar, Options, JointCollectionTpl, Matrix6xOut1, Matrix6xOut2> | ||
274 | Pass1; | ||
275 |
2/2✓ Branch 0 taken 123 times.
✓ Branch 1 taken 21 times.
|
144 | for (JointIndex i = jointId; i > 0; i = model.parents[i]) |
276 | { | ||
277 |
1/2✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
|
123 | Pass1::run( |
278 | 123 | model.joints[i], | |
279 | 246 | typename Pass1::ArgsType( | |
280 | 123 | model, data, jointId, rf, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq), | |
281 | 123 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv))); | |
282 | } | ||
283 | 21 | } | |
284 | |||
285 | template< | ||
286 | typename Scalar, | ||
287 | int Options, | ||
288 | template<typename, int> | ||
289 | class JointCollectionTpl, | ||
290 | typename Matrix6xOut1, | ||
291 | typename Matrix6xOut2, | ||
292 | typename Matrix6xOut3, | ||
293 | typename Matrix6xOut4> | ||
294 | struct JointAccelerationDerivativesBackwardStep | ||
295 | : public fusion::JointUnaryVisitorBase<JointAccelerationDerivativesBackwardStep< | ||
296 | Scalar, | ||
297 | Options, | ||
298 | JointCollectionTpl, | ||
299 | Matrix6xOut1, | ||
300 | Matrix6xOut2, | ||
301 | Matrix6xOut3, | ||
302 | Matrix6xOut4>> | ||
303 | { | ||
304 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
305 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
306 | |||
307 | typedef boost::fusion::vector< | ||
308 | const Model &, | ||
309 | const Data &, | ||
310 | const typename Model::JointIndex &, | ||
311 | const ReferenceFrame &, | ||
312 | Matrix6xOut1 &, | ||
313 | Matrix6xOut2 &, | ||
314 | Matrix6xOut3 &, | ||
315 | Matrix6xOut4 &> | ||
316 | ArgsType; | ||
317 | |||
318 | template<typename JointModel> | ||
319 | 570 | static void algo( | |
320 | const JointModelBase<JointModel> & jmodel, | ||
321 | const Model & model, | ||
322 | const Data & data, | ||
323 | const typename Model::JointIndex & jointId, | ||
324 | const ReferenceFrame & rf, | ||
325 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
326 | const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq, | ||
327 | const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv, | ||
328 | const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da) | ||
329 | { | ||
330 | typedef typename Model::JointIndex JointIndex; | ||
331 | typedef typename Data::SE3 SE3; | ||
332 | typedef typename Data::Motion Motion; | ||
333 | |||
334 |
1/2✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
|
570 | const JointIndex & i = jmodel.id(); |
335 | 570 | const JointIndex & parent = model.parents[i]; | |
336 |
1/2✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
|
570 | Motion vtmp; // Temporary variable |
337 |
1/2✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
|
570 | Motion atmp; // Temporary variable |
338 | |||
339 | 570 | const SE3 & oMlast = data.oMi[jointId]; | |
340 | 570 | const Motion & vlast = data.ov[jointId]; | |
341 | 570 | const Motion & alast = data.oa[jointId]; | |
342 | |||
343 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn< | ||
344 | typename Data::Matrix6x>::ConstType ColsBlock; | ||
345 |
1/2✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
|
570 | ColsBlock dJcols = jmodel.jointCols(data.dJ); |
346 |
1/2✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
|
570 | ColsBlock Jcols = jmodel.jointCols(data.J); |
347 | |||
348 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut1>::Type | ||
349 | ColsBlockOut1; | ||
350 | 570 | Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq); | |
351 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut2>::Type | ||
352 | ColsBlockOut2; | ||
353 | 570 | Matrix6xOut2 & a_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, a_partial_dq); | |
354 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut3>::Type | ||
355 | ColsBlockOut3; | ||
356 | 570 | Matrix6xOut3 & a_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dv); | |
357 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut4>::Type | ||
358 | ColsBlockOut4; | ||
359 | 570 | Matrix6xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da); | |
360 | |||
361 |
1/2✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
|
570 | ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); |
362 |
1/2✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
|
570 | ColsBlockOut2 a_partial_dq_cols = jmodel.jointCols(a_partial_dq_); |
363 |
1/2✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
|
570 | ColsBlockOut3 a_partial_dv_cols = jmodel.jointCols(a_partial_dv_); |
364 |
1/2✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
|
570 | ColsBlockOut4 a_partial_da_cols = jmodel.jointCols(a_partial_da_); |
365 | |||
366 | // dacc/da | ||
367 |
3/4✓ Branch 0 taken 45 times.
✓ Branch 1 taken 62 times.
✓ Branch 2 taken 178 times.
✗ Branch 3 not taken.
|
570 | switch (rf) |
368 | { | ||
369 | 90 | case WORLD: | |
370 |
1/2✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
|
90 | a_partial_da_cols = Jcols; |
371 | 90 | break; | |
372 | 124 | case LOCAL_WORLD_ALIGNED: | |
373 |
1/2✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
|
124 | details::translateJointJacobian(oMlast, Jcols, a_partial_da_cols); |
374 | 124 | break; | |
375 | 356 | case LOCAL: | |
376 |
1/2✓ Branch 1 taken 178 times.
✗ Branch 2 not taken.
|
356 | motionSet::se3ActionInverse(oMlast, Jcols, a_partial_da_cols); |
377 | 356 | break; | |
378 | } | ||
379 | |||
380 | // dacc/dv | ||
381 |
3/4✓ Branch 0 taken 45 times.
✓ Branch 1 taken 62 times.
✓ Branch 2 taken 178 times.
✗ Branch 3 not taken.
|
570 | switch (rf) |
382 | { | ||
383 | 90 | case WORLD: | |
384 |
2/2✓ Branch 0 taken 37 times.
✓ Branch 1 taken 8 times.
|
90 | if (parent > 0) |
385 |
2/4✓ Branch 2 taken 37 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37 times.
✗ Branch 6 not taken.
|
74 | vtmp = data.ov[parent] - vlast; |
386 | else | ||
387 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | vtmp = -vlast; |
388 | |||
389 | // also computes dvec/dq | ||
390 |
1/2✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
|
90 | motionSet::motionAction(vtmp, Jcols, v_partial_dq_cols); |
391 | |||
392 |
2/4✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
|
90 | a_partial_dv_cols = v_partial_dq_cols + dJcols; |
393 | 90 | break; | |
394 | 124 | case LOCAL_WORLD_ALIGNED: | |
395 |
2/2✓ Branch 0 taken 52 times.
✓ Branch 1 taken 10 times.
|
124 | if (parent > 0) |
396 |
2/4✓ Branch 2 taken 52 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 52 times.
✗ Branch 6 not taken.
|
104 | vtmp = data.ov[parent] - vlast; |
397 | else | ||
398 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | vtmp = -vlast; |
399 |
5/10✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 62 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 62 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 62 times.
✗ Branch 14 not taken.
|
124 | vtmp.linear() += vtmp.angular().cross(oMlast.translation()); |
400 | |||
401 | // also computes dvec/dq | ||
402 |
1/2✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
|
124 | motionSet::motionAction(vtmp, a_partial_da_cols, v_partial_dq_cols); |
403 | |||
404 |
1/2✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
|
124 | details::translateJointJacobian(oMlast, dJcols, a_partial_dv_cols); |
405 | // a_partial_dv_cols += v_partial_dq_cols; // dJcols is required later | ||
406 | 124 | break; | |
407 | 356 | case LOCAL: | |
408 | // also computes dvec/dq | ||
409 |
2/2✓ Branch 0 taken 152 times.
✓ Branch 1 taken 26 times.
|
356 | if (parent > 0) |
410 | { | ||
411 |
2/4✓ Branch 2 taken 152 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 152 times.
✗ Branch 6 not taken.
|
304 | vtmp = oMlast.actInv(data.ov[parent]); |
412 |
1/2✓ Branch 1 taken 152 times.
✗ Branch 2 not taken.
|
304 | motionSet::motionAction(vtmp, a_partial_da_cols, v_partial_dq_cols); |
413 | } | ||
414 | |||
415 |
2/2✓ Branch 0 taken 152 times.
✓ Branch 1 taken 26 times.
|
356 | if (parent > 0) |
416 |
1/2✓ Branch 2 taken 152 times.
✗ Branch 3 not taken.
|
304 | vtmp -= data.v[jointId]; |
417 | else | ||
418 |
2/4✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
|
52 | vtmp = -data.v[jointId]; |
419 | |||
420 |
1/2✓ Branch 1 taken 178 times.
✗ Branch 2 not taken.
|
356 | motionSet::motionAction(vtmp, a_partial_da_cols, a_partial_dv_cols); |
421 |
1/2✓ Branch 1 taken 178 times.
✗ Branch 2 not taken.
|
356 | motionSet::se3ActionInverse<ADDTO>(oMlast, dJcols, a_partial_dv_cols); |
422 | 356 | break; | |
423 | } | ||
424 | |||
425 | // dacc/dq | ||
426 |
3/4✓ Branch 0 taken 45 times.
✓ Branch 1 taken 62 times.
✓ Branch 2 taken 178 times.
✗ Branch 3 not taken.
|
570 | switch (rf) |
427 | { | ||
428 | 90 | case WORLD: | |
429 |
2/2✓ Branch 0 taken 37 times.
✓ Branch 1 taken 8 times.
|
90 | if (parent > 0) |
430 |
2/4✓ Branch 2 taken 37 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37 times.
✗ Branch 6 not taken.
|
74 | atmp = data.oa[parent] - alast; |
431 | else | ||
432 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | atmp = -alast; |
433 |
1/2✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
|
90 | motionSet::motionAction(atmp, Jcols, a_partial_dq_cols); |
434 | |||
435 |
2/2✓ Branch 0 taken 37 times.
✓ Branch 1 taken 8 times.
|
90 | if (parent > 0) |
436 |
1/2✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
|
74 | motionSet::motionAction<ADDTO>(vtmp, dJcols, a_partial_dq_cols); |
437 | 90 | break; | |
438 | 124 | case LOCAL_WORLD_ALIGNED: | |
439 |
2/2✓ Branch 0 taken 52 times.
✓ Branch 1 taken 10 times.
|
124 | if (parent > 0) |
440 |
2/4✓ Branch 2 taken 52 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 52 times.
✗ Branch 6 not taken.
|
104 | atmp = data.oa[parent] - alast; |
441 | else | ||
442 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | atmp = -alast; |
443 | |||
444 |
5/10✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 62 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 62 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 62 times.
✗ Branch 14 not taken.
|
124 | atmp.linear() += atmp.angular().cross(oMlast.translation()); |
445 |
1/2✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
|
124 | motionSet::motionAction(atmp, a_partial_da_cols, a_partial_dq_cols); |
446 | |||
447 |
2/2✓ Branch 0 taken 52 times.
✓ Branch 1 taken 10 times.
|
124 | if (parent > 0) |
448 |
1/2✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
|
104 | motionSet::motionAction<ADDTO>(vtmp, a_partial_dv_cols, a_partial_dq_cols); |
449 | |||
450 |
1/2✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
|
124 | a_partial_dv_cols += v_partial_dq_cols; |
451 | 124 | break; | |
452 | 356 | case LOCAL: | |
453 |
2/2✓ Branch 0 taken 152 times.
✓ Branch 1 taken 26 times.
|
356 | if (parent > 0) |
454 | { | ||
455 |
2/4✓ Branch 2 taken 152 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 152 times.
✗ Branch 6 not taken.
|
304 | atmp = oMlast.actInv(data.oa[parent]); |
456 |
1/2✓ Branch 1 taken 152 times.
✗ Branch 2 not taken.
|
304 | motionSet::motionAction(atmp, a_partial_da_cols, a_partial_dq_cols); |
457 | |||
458 |
1/2✓ Branch 1 taken 152 times.
✗ Branch 2 not taken.
|
304 | motionSet::motionAction<ADDTO>(vtmp, v_partial_dq_cols, a_partial_dq_cols); |
459 | } | ||
460 | |||
461 | 356 | break; | |
462 | } | ||
463 | } | ||
464 | }; | ||
465 | |||
466 | template< | ||
467 | typename Scalar, | ||
468 | int Options, | ||
469 | template<typename, int> | ||
470 | class JointCollectionTpl, | ||
471 | typename Matrix6xOut1, | ||
472 | typename Matrix6xOut2, | ||
473 | typename Matrix6xOut3, | ||
474 | typename Matrix6xOut4> | ||
475 | 44 | void getJointAccelerationDerivatives( | |
476 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
477 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
478 | const Model::JointIndex jointId, | ||
479 | const ReferenceFrame rf, | ||
480 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
481 | const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq, | ||
482 | const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv, | ||
483 | const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da) | ||
484 | { | ||
485 | |||
486 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1, Data::Matrix6x); | ||
487 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2, Data::Matrix6x); | ||
488 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut3, Data::Matrix6x); | ||
489 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut4, Data::Matrix6x); | ||
490 | |||
491 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
492 | |||
493 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 44 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
44 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv); |
494 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 44 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
44 | PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dq.cols(), model.nv); |
495 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 44 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
44 | PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dv.cols(), model.nv); |
496 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 44 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
44 | PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_da.cols(), model.nv); |
497 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 44 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT((int)jointId < model.njoints, "The joint id is invalid."); |
498 |
1/2✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
|
44 | assert(model.check(data) && "data is not consistent with model."); |
499 | |||
500 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
501 | typedef typename Model::JointIndex JointIndex; | ||
502 | |||
503 | typedef JointAccelerationDerivativesBackwardStep< | ||
504 | Scalar, Options, JointCollectionTpl, Matrix6xOut1, Matrix6xOut2, Matrix6xOut3, Matrix6xOut4> | ||
505 | Pass1; | ||
506 |
2/2✓ Branch 0 taken 285 times.
✓ Branch 1 taken 44 times.
|
329 | for (JointIndex i = jointId; i > 0; i = model.parents[i]) |
507 | { | ||
508 |
1/2✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
|
285 | Pass1::run( |
509 | 285 | model.joints[i], | |
510 | 570 | typename Pass1::ArgsType( | |
511 | 285 | model, data, jointId, rf, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq), | |
512 | 285 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, a_partial_dq), | |
513 | 285 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dv), | |
514 | 285 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da))); | |
515 | } | ||
516 | 44 | } | |
517 | |||
518 | template< | ||
519 | typename Scalar, | ||
520 | int Options, | ||
521 | template<typename, int> | ||
522 | class JointCollectionTpl, | ||
523 | typename Matrix6xOut1, | ||
524 | typename Matrix6xOut2, | ||
525 | typename Matrix6xOut3, | ||
526 | typename Matrix6xOut4, | ||
527 | typename Matrix6xOut5> | ||
528 | 2 | void getJointAccelerationDerivatives( | |
529 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
530 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
531 | const Model::JointIndex jointId, | ||
532 | const ReferenceFrame rf, | ||
533 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
534 | const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv, | ||
535 | const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq, | ||
536 | const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv, | ||
537 | const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da) | ||
538 | { | ||
539 | 2 | impl::getJointAccelerationDerivatives( | |
540 | 2 | model, data, jointId, rf, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq), | |
541 | 2 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq), | |
542 | 2 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv), | |
543 | 2 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da)); | |
544 | |||
545 | 2 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv) = a_partial_da; | |
546 | 2 | } | |
547 | |||
548 | template< | ||
549 | typename Scalar, | ||
550 | int Options, | ||
551 | template<typename, int> | ||
552 | class JointCollectionTpl, | ||
553 | typename Matrix3xOut1, | ||
554 | typename Matrix3xOut2> | ||
555 | struct PointVelocityDerivativesBackwardStep | ||
556 | : public fusion::JointUnaryVisitorBase<PointVelocityDerivativesBackwardStep< | ||
557 | Scalar, | ||
558 | Options, | ||
559 | JointCollectionTpl, | ||
560 | Matrix3xOut1, | ||
561 | Matrix3xOut2>> | ||
562 | { | ||
563 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
564 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
565 | |||
566 | typedef boost::fusion::vector< | ||
567 | const Model &, | ||
568 | const Data &, | ||
569 | const typename Data::SE3 &, | ||
570 | const typename Data::Motion &, | ||
571 | const ReferenceFrame &, | ||
572 | Matrix3xOut1 &, | ||
573 | Matrix3xOut2 &> | ||
574 | ArgsType; | ||
575 | |||
576 | template<typename JointModel> | ||
577 | 28 | static void algo( | |
578 | const JointModelBase<JointModel> & jmodel, | ||
579 | const Model & model, | ||
580 | const Data & data, | ||
581 | const typename Data::SE3 & oMpoint, | ||
582 | const typename Data::Motion & spatial_point_velocity, | ||
583 | const ReferenceFrame & rf, | ||
584 | const Eigen::MatrixBase<Matrix3xOut1> & v_partial_dq, | ||
585 | const Eigen::MatrixBase<Matrix3xOut2> & v_partial_dv) | ||
586 | { | ||
587 | typedef typename Model::JointIndex JointIndex; | ||
588 | typedef typename Data::SE3 SE3; | ||
589 | typedef typename Data::Motion Motion; | ||
590 | |||
591 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
28 | const JointIndex i = jmodel.id(); |
592 | 28 | const JointIndex parent = model.parents[i]; | |
593 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
28 | Motion vtmp; // Temporary variable |
594 | |||
595 | 28 | const SE3 & oMlast = oMpoint; | |
596 | |||
597 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn< | ||
598 | typename Data::Matrix6x>::ConstType ColsBlock; | ||
599 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
28 | ColsBlock Jcols = jmodel.jointCols(data.J); |
600 | |||
601 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut1>::Type | ||
602 | ColsBlockOut1; | ||
603 | 28 | Matrix3xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1, v_partial_dq); | |
604 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut2>::Type | ||
605 | ColsBlockOut2; | ||
606 | 28 | Matrix3xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2, v_partial_dv); | |
607 | |||
608 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
28 | ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); |
609 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
28 | ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); |
610 | |||
611 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
28 | const int nv = jmodel.nv(); |
612 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
28 | Eigen::Matrix<Scalar, 6, JointModel::NV, Options> v_spatial_partial_dv_cols(6, nv); |
613 | |||
614 | #define FOR_NV() for (Eigen::DenseIndex j = 0; j < nv; ++j) | ||
615 | #define GET_LINEAR(vec6) vec6.template segment<3>(Motion::LINEAR) | ||
616 | #define GET_ANGULAR(vec6) vec6.template segment<3>(Motion::ANGULAR) | ||
617 | |||
618 | // dvec/dv | ||
619 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
28 | motionSet::se3ActionInverse(oMlast, Jcols, v_spatial_partial_dv_cols); |
620 |
2/4✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
|
28 | v_partial_dv_cols = v_spatial_partial_dv_cols.template middleRows<3>(Motion::LINEAR); |
621 | |||
622 | // dvec/dq | ||
623 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 2 times.
|
28 | if (parent > 0) |
624 | { | ||
625 |
2/4✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
|
24 | vtmp = oMlast.actInv(data.ov[parent]); |
626 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 12 times.
|
48 | FOR_NV() |
627 |
6/12✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12 times.
✗ Branch 17 not taken.
|
48 | v_partial_dq_cols.col(j).noalias() = |
628 |
3/6✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
|
24 | vtmp.angular().cross(GET_LINEAR(v_spatial_partial_dv_cols.col(j))) |
629 |
3/6✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
|
48 | + vtmp.linear().cross(GET_ANGULAR(v_spatial_partial_dv_cols.col(j))); |
630 | } | ||
631 | else | ||
632 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | v_partial_dq_cols.setZero(); |
633 | |||
634 |
2/2✓ Branch 0 taken 7 times.
✓ Branch 1 taken 7 times.
|
28 | if (rf == LOCAL_WORLD_ALIGNED) |
635 | { | ||
636 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 7 times.
|
38 | FOR_NV() |
637 |
6/12✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12 times.
✗ Branch 17 not taken.
|
48 | v_partial_dq_cols.col(j) = oMlast.rotation() |
638 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | * (v_partial_dq_cols.col(j) |
639 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
24 | + GET_ANGULAR(v_spatial_partial_dv_cols.col(j)) |
640 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
48 | .cross(spatial_point_velocity.linear())); |
641 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 7 times.
|
38 | FOR_NV() |
642 |
5/10✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
|
24 | v_partial_dv_cols.col(j) = oMlast.rotation() * v_partial_dv_cols.col(j); |
643 | } | ||
644 | |||
645 | #undef FOR_NV | ||
646 | #undef GET_LINEAR | ||
647 | #undef GET_ANGULAR | ||
648 | } | ||
649 | }; | ||
650 | |||
651 | template< | ||
652 | typename Scalar, | ||
653 | int Options, | ||
654 | template<typename, int> | ||
655 | class JointCollectionTpl, | ||
656 | typename Matrix3xOut1, | ||
657 | typename Matrix3xOut2> | ||
658 | 2 | void getPointVelocityDerivatives( | |
659 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
660 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
661 | const Model::JointIndex joint_id, | ||
662 | const SE3Tpl<Scalar, Options> & placement, | ||
663 | const ReferenceFrame rf, | ||
664 | const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq, | ||
665 | const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv) | ||
666 | { | ||
667 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut1, Data::Matrix3x); | ||
668 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut2, Data::Matrix3x); | ||
669 | |||
670 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
671 | typedef typename Model::JointIndex JointIndex; | ||
672 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
673 | |||
674 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
2 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v_point_partial_dq.cols(), model.nv); |
675 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
2 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v_point_partial_dv.cols(), model.nv); |
676 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | PINOCCHIO_CHECK_INPUT_ARGUMENT((int)joint_id < model.njoints, "The joint id is invalid."); |
677 |
3/6✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
2 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
678 | rf == LOCAL || rf == LOCAL_WORLD_ALIGNED, | ||
679 | "The reference frame is not valid, expected LOCAL or LOCAL_WORLD_ALIGNED"); | ||
680 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
2 | assert(model.check(data) && "data is not consistent with model."); |
681 | |||
682 | typedef typename Data::SE3 SE3; | ||
683 | typedef typename Data::Motion Motion; | ||
684 | |||
685 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMpoint = data.oMi[joint_id] * placement; |
686 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | const Motion spatial_velocity = oMpoint.actInv(data.ov[joint_id]); |
687 | |||
688 | typedef PointVelocityDerivativesBackwardStep< | ||
689 | Scalar, Options, JointCollectionTpl, Matrix3xOut1, Matrix3xOut2> | ||
690 | Pass1; | ||
691 |
2/2✓ Branch 0 taken 14 times.
✓ Branch 1 taken 2 times.
|
16 | for (JointIndex i = joint_id; i > 0; i = model.parents[i]) |
692 | { | ||
693 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
14 | Pass1::run( |
694 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
28 | model.joints[i], typename Pass1::ArgsType( |
695 | model, data, oMpoint, spatial_velocity, rf, | ||
696 | 14 | PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1, v_point_partial_dq), | |
697 | 14 | PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2, v_point_partial_dv))); | |
698 | } | ||
699 | 2 | } | |
700 | |||
701 | template< | ||
702 | typename Scalar, | ||
703 | int Options, | ||
704 | template<typename, int> | ||
705 | class JointCollectionTpl, | ||
706 | typename Matrix3xOut1, | ||
707 | typename Matrix3xOut2, | ||
708 | typename Matrix3xOut3, | ||
709 | typename Matrix3xOut4> | ||
710 | struct PointClassicAccelerationDerivativesBackwardStep | ||
711 | : public fusion::JointUnaryVisitorBase<PointClassicAccelerationDerivativesBackwardStep< | ||
712 | Scalar, | ||
713 | Options, | ||
714 | JointCollectionTpl, | ||
715 | Matrix3xOut1, | ||
716 | Matrix3xOut2, | ||
717 | Matrix3xOut3, | ||
718 | Matrix3xOut4>> | ||
719 | { | ||
720 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
721 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
722 | |||
723 | typedef boost::fusion::vector< | ||
724 | const Model &, | ||
725 | const Data &, | ||
726 | const typename Data::SE3 &, | ||
727 | const typename Data::Motion &, | ||
728 | const typename Data::Motion::Vector3 &, | ||
729 | const ReferenceFrame &, | ||
730 | Matrix3xOut1 &, | ||
731 | Matrix3xOut2 &, | ||
732 | Matrix3xOut3 &, | ||
733 | Matrix3xOut4 &> | ||
734 | ArgsType; | ||
735 | |||
736 | template<typename JointModel> | ||
737 | 396 | static void algo( | |
738 | const JointModelBase<JointModel> & jmodel, | ||
739 | const Model & model, | ||
740 | const Data & data, | ||
741 | const typename Data::SE3 & oMpoint, | ||
742 | const typename Data::Motion & spatial_point_velocity, | ||
743 | const typename Data::Motion::Vector3 & point_classic_acceleration, | ||
744 | const ReferenceFrame & rf, | ||
745 | const Eigen::MatrixBase<Matrix3xOut1> & v_partial_dq, | ||
746 | const Eigen::MatrixBase<Matrix3xOut2> & a_partial_dq, | ||
747 | const Eigen::MatrixBase<Matrix3xOut3> & a_partial_dv, | ||
748 | const Eigen::MatrixBase<Matrix3xOut4> & a_partial_da) | ||
749 | { | ||
750 | typedef typename Model::JointIndex JointIndex; | ||
751 | typedef typename Data::SE3 SE3; | ||
752 | typedef typename Data::Motion Motion; | ||
753 | |||
754 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | const JointIndex i = jmodel.id(); |
755 | 396 | const JointIndex parent = model.parents[i]; | |
756 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | Motion vtmp; // Temporary variable |
757 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | Motion atmp; // Temporary variable |
758 | |||
759 | 396 | const SE3 & oMlast = oMpoint; | |
760 | 396 | const Motion & v_last = spatial_point_velocity; | |
761 | |||
762 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn< | ||
763 | typename Data::Matrix6x>::ConstType ColsBlock; | ||
764 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | ColsBlock dJcols = jmodel.jointCols(data.dJ); |
765 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | ColsBlock Jcols = jmodel.jointCols(data.J); |
766 | |||
767 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut1>::Type | ||
768 | ColsBlockOut1; | ||
769 | 396 | Matrix3xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1, v_partial_dq); | |
770 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut2>::Type | ||
771 | ColsBlockOut2; | ||
772 | 396 | Matrix3xOut2 & a_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2, a_partial_dq); | |
773 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut3>::Type | ||
774 | ColsBlockOut3; | ||
775 | 396 | Matrix3xOut3 & a_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut3, a_partial_dv); | |
776 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut4>::Type | ||
777 | ColsBlockOut4; | ||
778 | 396 | Matrix3xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut4, a_partial_da); | |
779 | |||
780 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); |
781 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | ColsBlockOut2 a_partial_dq_cols = jmodel.jointCols(a_partial_dq_); |
782 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | ColsBlockOut3 a_partial_dv_cols = jmodel.jointCols(a_partial_dv_); |
783 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | ColsBlockOut4 a_partial_da_cols = jmodel.jointCols(a_partial_da_); |
784 | |||
785 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | const int nv = jmodel.nv(); |
786 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | Eigen::Matrix<Scalar, 6, JointModel::NV, Options> a_spatial_partial_da_cols(6, nv); |
787 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | Eigen::Matrix<Scalar, 6, JointModel::NV, Options> v_spatial_partial_dq_cols(6, nv); |
788 | |||
789 | #define FOR_NV() for (Eigen::DenseIndex j = 0; j < nv; ++j) | ||
790 | #define GET_LINEAR(vec6) vec6.template segment<3>(Motion::LINEAR) | ||
791 | #define GET_ANGULAR(vec6) vec6.template segment<3>(Motion::ANGULAR) | ||
792 | |||
793 | // dacc/da | ||
794 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | motionSet::se3ActionInverse(oMlast, Jcols, a_spatial_partial_da_cols); |
795 |
2/4✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198 times.
✗ Branch 5 not taken.
|
396 | a_partial_da_cols = a_spatial_partial_da_cols.template middleRows<3>(Motion::LINEAR); |
796 | |||
797 | // dacc/dv | ||
798 | // also computes dvec/dq | ||
799 |
2/2✓ Branch 0 taken 171 times.
✓ Branch 1 taken 27 times.
|
396 | if (parent > 0) |
800 | { | ||
801 |
2/4✓ Branch 2 taken 171 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 171 times.
✗ Branch 6 not taken.
|
342 | vtmp = oMlast.actInv(data.ov[parent]); |
802 |
1/2✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
|
342 | motionSet::motionAction(vtmp, a_spatial_partial_da_cols, v_spatial_partial_dq_cols); |
803 |
2/4✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
|
342 | v_partial_dq_cols = v_spatial_partial_dq_cols.template middleRows<3>(Motion::LINEAR); |
804 | } | ||
805 | else | ||
806 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | v_partial_dq_cols.setZero(); |
807 | |||
808 |
2/2✓ Branch 0 taken 171 times.
✓ Branch 1 taken 27 times.
|
396 | if (parent > 0) |
809 |
1/2✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
|
342 | vtmp -= v_last; |
810 | else | ||
811 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
54 | vtmp = -v_last; |
812 | |||
813 | // motionSet::motionAction(vtmp,a_partial_da_cols,a_partial_dv_cols); | ||
814 |
2/2✓ Branch 0 taken 333 times.
✓ Branch 1 taken 198 times.
|
1062 | FOR_NV() |
815 |
6/12✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 333 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 333 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 333 times.
✗ Branch 17 not taken.
|
1332 | a_partial_dv_cols.col(j).noalias() = |
816 |
3/6✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
|
666 | vtmp.angular().cross(GET_LINEAR(a_spatial_partial_da_cols.col(j))) |
817 |
3/6✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
|
1332 | + vtmp.linear().cross(GET_ANGULAR(a_spatial_partial_da_cols.col(j))); |
818 | // motionSet::se3ActionInverse<ADDTO>(oMlast,dJcols,a_partial_dv_cols); | ||
819 |
2/2✓ Branch 0 taken 333 times.
✓ Branch 1 taken 198 times.
|
1062 | FOR_NV() |
820 |
4/8✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 333 times.
✗ Branch 11 not taken.
|
1332 | a_partial_dv_cols.col(j) += |
821 |
2/4✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
|
666 | oMlast.rotation().transpose() |
822 |
6/12✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 333 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 333 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 333 times.
✗ Branch 17 not taken.
|
1332 | * (GET_LINEAR(dJcols.col(j)) + GET_ANGULAR(dJcols.col(j)).cross(oMlast.translation())); |
823 | // wxv | ||
824 |
2/2✓ Branch 0 taken 333 times.
✓ Branch 1 taken 198 times.
|
1062 | FOR_NV() |
825 |
5/10✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 333 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 333 times.
✗ Branch 14 not taken.
|
1332 | a_partial_dv_cols.col(j) += |
826 |
3/6✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
|
666 | v_last.angular().cross(GET_LINEAR(a_spatial_partial_da_cols.col(j))) |
827 |
3/6✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
|
1332 | - v_last.linear().cross(GET_ANGULAR(a_spatial_partial_da_cols.col(j))); |
828 | |||
829 | // dacc/dq | ||
830 |
2/2✓ Branch 0 taken 171 times.
✓ Branch 1 taken 27 times.
|
396 | if (parent > 0) |
831 | { | ||
832 |
2/4✓ Branch 2 taken 171 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 171 times.
✗ Branch 6 not taken.
|
342 | atmp = oMlast.actInv(data.oa[parent]); |
833 | // motionSet::motionAction(atmp,a_partial_da_cols,a_partial_dq_cols); | ||
834 |
2/2✓ Branch 0 taken 171 times.
✓ Branch 1 taken 171 times.
|
684 | FOR_NV() |
835 |
6/12✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 171 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 171 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 171 times.
✗ Branch 17 not taken.
|
684 | a_partial_dq_cols.col(j).noalias() = |
836 |
3/6✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
|
342 | atmp.angular().cross(GET_LINEAR(a_spatial_partial_da_cols.col(j))) |
837 |
3/6✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
|
684 | + atmp.linear().cross(GET_ANGULAR(a_spatial_partial_da_cols.col(j))); |
838 | |||
839 | // motionSet::motionAction<ADDTO>(vtmp,v_partial_dq_cols,a_partial_dq_cols); | ||
840 |
2/2✓ Branch 0 taken 171 times.
✓ Branch 1 taken 171 times.
|
684 | FOR_NV() |
841 |
5/10✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 171 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 171 times.
✗ Branch 14 not taken.
|
684 | a_partial_dq_cols.col(j) += |
842 |
3/6✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
|
342 | vtmp.angular().cross(GET_LINEAR(v_spatial_partial_dq_cols.col(j))) |
843 |
3/6✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
|
684 | + vtmp.linear().cross(GET_ANGULAR(v_spatial_partial_dq_cols.col(j))); |
844 | |||
845 | // wxv | ||
846 |
2/2✓ Branch 0 taken 171 times.
✓ Branch 1 taken 171 times.
|
684 | FOR_NV() |
847 |
5/10✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 171 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 171 times.
✗ Branch 14 not taken.
|
684 | a_partial_dq_cols.col(j) += |
848 |
3/6✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
|
342 | v_last.angular().cross(GET_LINEAR(v_spatial_partial_dq_cols.col(j))) |
849 |
3/6✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
|
684 | - v_last.linear().cross(GET_ANGULAR(v_spatial_partial_dq_cols.col(j))); |
850 | } | ||
851 | else | ||
852 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | a_partial_dq_cols.setZero(); |
853 | |||
854 |
2/2✓ Branch 0 taken 53 times.
✓ Branch 1 taken 145 times.
|
396 | if (rf == LOCAL_WORLD_ALIGNED) |
855 | { | ||
856 | |||
857 |
2/2✓ Branch 0 taken 88 times.
✓ Branch 1 taken 53 times.
|
282 | FOR_NV() |
858 |
6/12✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 88 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 88 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 88 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 88 times.
✗ Branch 17 not taken.
|
352 | v_partial_dq_cols.col(j) = oMlast.rotation() |
859 |
1/2✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
|
176 | * (v_partial_dq_cols.col(j) |
860 |
2/4✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
|
176 | + GET_ANGULAR(a_spatial_partial_da_cols.col(j)) |
861 |
1/2✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
|
352 | .cross(spatial_point_velocity.linear())); |
862 |
2/2✓ Branch 0 taken 88 times.
✓ Branch 1 taken 53 times.
|
282 | FOR_NV() |
863 |
4/8✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 88 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 88 times.
✗ Branch 11 not taken.
|
528 | a_partial_dq_cols.col(j) = |
864 |
2/4✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
|
176 | oMlast.rotation() |
865 |
3/6✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 88 times.
✗ Branch 8 not taken.
|
528 | * (a_partial_dq_cols.col(j) + GET_ANGULAR(a_spatial_partial_da_cols.col(j)).cross(point_classic_acceleration)); |
866 |
2/2✓ Branch 0 taken 88 times.
✓ Branch 1 taken 53 times.
|
282 | FOR_NV() |
867 |
5/10✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 88 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 88 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 88 times.
✗ Branch 14 not taken.
|
176 | a_partial_dv_cols.col(j) = oMlast.rotation() * a_partial_dv_cols.col(j); |
868 |
2/2✓ Branch 0 taken 88 times.
✓ Branch 1 taken 53 times.
|
282 | FOR_NV() |
869 |
5/10✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 88 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 88 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 88 times.
✗ Branch 14 not taken.
|
176 | a_partial_da_cols.col(j) = oMlast.rotation() * a_partial_da_cols.col(j); |
870 | } | ||
871 | |||
872 | #undef FOR_NV | ||
873 | #undef GET_LINEAR | ||
874 | #undef GET_ANGULAR | ||
875 | } | ||
876 | }; | ||
877 | |||
878 | template< | ||
879 | typename Scalar, | ||
880 | int Options, | ||
881 | template<typename, int> | ||
882 | class JointCollectionTpl, | ||
883 | typename Matrix3xOut1, | ||
884 | typename Matrix3xOut2, | ||
885 | typename Matrix3xOut3, | ||
886 | typename Matrix3xOut4> | ||
887 | 27 | void getPointClassicAccelerationDerivatives( | |
888 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
889 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
890 | const Model::JointIndex joint_id, | ||
891 | const SE3Tpl<Scalar, Options> & placement, | ||
892 | const ReferenceFrame rf, | ||
893 | const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq, | ||
894 | const Eigen::MatrixBase<Matrix3xOut2> & a_point_partial_dq, | ||
895 | const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dv, | ||
896 | const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_da) | ||
897 | { | ||
898 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut1, Data::Matrix3x); | ||
899 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut2, Data::Matrix3x); | ||
900 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut3, Data::Matrix3x); | ||
901 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut4, Data::Matrix3x); | ||
902 | |||
903 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
904 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
905 | typedef typename Data::SE3 SE3; | ||
906 | typedef typename Data::Motion Motion; | ||
907 | typedef typename SE3::Vector3 Vector3; | ||
908 | typedef typename Model::JointIndex JointIndex; | ||
909 | |||
910 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 27 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
27 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v_point_partial_dq.cols(), model.nv); |
911 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 27 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
27 | PINOCCHIO_CHECK_ARGUMENT_SIZE(a_point_partial_dq.cols(), model.nv); |
912 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 27 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
27 | PINOCCHIO_CHECK_ARGUMENT_SIZE(a_point_partial_dv.cols(), model.nv); |
913 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 27 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
27 | PINOCCHIO_CHECK_ARGUMENT_SIZE(a_point_partial_da.cols(), model.nv); |
914 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 27 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
27 | PINOCCHIO_CHECK_INPUT_ARGUMENT((int)joint_id < model.njoints, "The joint id is invalid."); |
915 |
3/6✓ Branch 0 taken 7 times.
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
27 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
916 | rf == LOCAL || rf == LOCAL_WORLD_ALIGNED, | ||
917 | "The reference frame is not valid, expected LOCAL or LOCAL_WORLD_ALIGNED"); | ||
918 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
|
27 | assert(model.check(data) && "data is not consistent with model."); |
919 | |||
920 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
27 | const SE3 oMpoint = data.oMi[joint_id] * placement; |
921 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
27 | const Motion spatial_velocity = oMpoint.actInv(data.ov[joint_id]); |
922 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
27 | const Motion spatial_acceleration = oMpoint.actInv(data.oa[joint_id]); |
923 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
27 | const Vector3 point_acc = classicAcceleration(spatial_velocity, spatial_acceleration); |
924 | |||
925 | typedef PointClassicAccelerationDerivativesBackwardStep< | ||
926 | Scalar, Options, JointCollectionTpl, Matrix3xOut1, Matrix3xOut2, Matrix3xOut3, Matrix3xOut4> | ||
927 | Pass1; | ||
928 |
2/2✓ Branch 0 taken 198 times.
✓ Branch 1 taken 27 times.
|
225 | for (JointIndex i = joint_id; i > 0; i = model.parents[i]) |
929 | { | ||
930 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
198 | Pass1::run( |
931 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
396 | model.joints[i], typename Pass1::ArgsType( |
932 | model, data, oMpoint, spatial_velocity, point_acc, rf, | ||
933 | 198 | PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1, v_point_partial_dq), | |
934 | 198 | PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2, a_point_partial_dq), | |
935 | 198 | PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut3, a_point_partial_dv), | |
936 | 198 | PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut4, a_point_partial_da))); | |
937 | } | ||
938 | 27 | } | |
939 | |||
940 | template< | ||
941 | typename Scalar, | ||
942 | int Options, | ||
943 | template<typename, int> | ||
944 | class JointCollectionTpl, | ||
945 | typename Matrix3xOut1, | ||
946 | typename Matrix3xOut2, | ||
947 | typename Matrix3xOut3, | ||
948 | typename Matrix3xOut4, | ||
949 | typename Matrix3xOut5> | ||
950 | 3 | void getPointClassicAccelerationDerivatives( | |
951 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
952 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
953 | const Model::JointIndex joint_id, | ||
954 | const SE3Tpl<Scalar, Options> & placement, | ||
955 | const ReferenceFrame rf, | ||
956 | const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq, | ||
957 | const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv, | ||
958 | const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dq, | ||
959 | const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_dv, | ||
960 | const Eigen::MatrixBase<Matrix3xOut5> & a_point_partial_da) | ||
961 | { | ||
962 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut2, Data::Matrix3x); | ||
963 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v_point_partial_dv.cols(), model.nv); |
964 | 3 | impl::getPointClassicAccelerationDerivatives( | |
965 | model, data, joint_id, placement, rf, | ||
966 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1, v_point_partial_dq), | |
967 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut3, a_point_partial_dq), | |
968 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut4, a_point_partial_dv), | |
969 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut5, a_point_partial_da)); | |
970 | |||
971 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2, v_point_partial_dv) = a_point_partial_da; | |
972 | 3 | } | |
973 | |||
974 | } // namespace impl | ||
975 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
976 | 2 | void computeJointKinematicHessians( | |
977 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
978 | DataTpl<Scalar, Options, JointCollectionTpl> & data) | ||
979 | { | ||
980 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | assert(model.check(data) && "data is not consistent with model."); |
981 | |||
982 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
983 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
984 | |||
985 | typedef MotionRef<const typename Data::Matrix6x::ConstColXpr> MotionIn; | ||
986 | |||
987 | typedef typename Data::Motion Motion; | ||
988 | typedef Eigen::Map<typename Motion::Vector6> MapVector6; | ||
989 | typedef MotionRef<MapVector6> MotionOut; | ||
990 | |||
991 | 2 | const typename Data::Matrix6x & J = data.J; | |
992 | 2 | typename Data::Tensor3x & kinematic_hessians = data.kinematic_hessians; | |
993 | 2 | const Eigen::DenseIndex slice_matrix_size = 6 * model.nv; | |
994 | |||
995 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
|
56 | for (size_t joint_id = 1; joint_id < (size_t)model.njoints; ++joint_id) |
996 | { | ||
997 | 54 | const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id]; | |
998 | 54 | const std::vector<typename Model::JointIndex> & support = model.supports[joint_id]; | |
999 | |||
1000 | 54 | const int nv = model.nvs[joint_id]; | |
1001 | 54 | const int idx_v = model.idx_vs[joint_id]; | |
1002 | |||
1003 |
2/2✓ Branch 0 taken 64 times.
✓ Branch 1 taken 54 times.
|
118 | for (int joint_row = 0; joint_row < nv; ++joint_row) |
1004 | { | ||
1005 | 64 | const Eigen::DenseIndex outer_row_id = idx_v + joint_row; | |
1006 | |||
1007 |
2/2✓ Branch 1 taken 286 times.
✓ Branch 2 taken 64 times.
|
350 | for (size_t support_id = 0; support_id < support.size() - 1; ++support_id) |
1008 | { | ||
1009 | 286 | const typename Model::JointIndex joint_id_support = support[support_id]; | |
1010 | |||
1011 | 286 | const int inner_nv = model.nvs[joint_id_support]; | |
1012 | 286 | const int inner_idx_v = model.idx_vs[joint_id_support]; | |
1013 |
2/2✓ Branch 0 taken 482 times.
✓ Branch 1 taken 286 times.
|
768 | for (int inner_joint_row = 0; inner_joint_row < inner_nv; ++inner_joint_row) |
1014 | { | ||
1015 | 482 | const Eigen::DenseIndex inner_row_id = inner_idx_v + inner_joint_row; | |
1016 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 482 times.
|
482 | assert(inner_row_id < outer_row_id); |
1017 | |||
1018 |
1/2✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
|
482 | MapVector6 motion_vec_in( |
1019 |
2/4✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
|
482 | kinematic_hessians.data() + inner_row_id * slice_matrix_size + outer_row_id * 6); |
1020 |
1/2✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
|
482 | MapVector6 motion_vec_out( |
1021 |
2/4✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
|
482 | kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6); |
1022 | |||
1023 |
2/4✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
|
482 | motion_vec_out = -motion_vec_in; |
1024 | } | ||
1025 | } | ||
1026 | |||
1027 |
2/4✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✗ Branch 5 not taken.
|
64 | const MotionIn S1(J.col(outer_row_id)); |
1028 | |||
1029 | // Computations already done | ||
1030 |
2/2✓ Branch 0 taken 30 times.
✓ Branch 1 taken 64 times.
|
94 | for (int inner_joint_row = 0; inner_joint_row < joint_row; ++inner_joint_row) |
1031 | { | ||
1032 | 30 | const Eigen::DenseIndex inner_row_id = idx_v + inner_joint_row; | |
1033 |
1/2✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
|
30 | MapVector6 motion_vec_in( |
1034 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | kinematic_hessians.data() + inner_row_id * slice_matrix_size + outer_row_id * 6); |
1035 |
1/2✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
|
30 | MapVector6 motion_vec_out( |
1036 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6); |
1037 | |||
1038 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | motion_vec_out = -motion_vec_in; |
1039 | } | ||
1040 | |||
1041 |
2/2✓ Branch 0 taken 30 times.
✓ Branch 1 taken 64 times.
|
94 | for (int inner_joint_row = joint_row + 1; inner_joint_row < nv; ++inner_joint_row) |
1042 | { | ||
1043 | 30 | const Eigen::DenseIndex inner_row_id = idx_v + inner_joint_row; | |
1044 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | const MotionIn S2(J.col(inner_row_id)); |
1045 | |||
1046 |
1/2✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
|
30 | MapVector6 motion_vec_out( |
1047 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6); |
1048 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | MotionOut S1xS2(motion_vec_out); |
1049 | |||
1050 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | S1xS2 = S1.cross(S2); |
1051 | } | ||
1052 | |||
1053 |
2/2✓ Branch 1 taken 482 times.
✓ Branch 2 taken 64 times.
|
546 | for (size_t subtree_id = 1; subtree_id < subtree.size(); ++subtree_id) |
1054 | { | ||
1055 | 482 | const typename Model::JointIndex joint_id_subtree = subtree[subtree_id]; | |
1056 | |||
1057 | 482 | const int inner_nv = model.nvs[joint_id_subtree]; | |
1058 | 482 | const int inner_idx_v = model.idx_vs[joint_id_subtree]; | |
1059 |
2/2✓ Branch 0 taken 482 times.
✓ Branch 1 taken 482 times.
|
964 | for (int inner_joint_row = 0; inner_joint_row < inner_nv; ++inner_joint_row) |
1060 | { | ||
1061 | 482 | const Eigen::DenseIndex inner_row_id = inner_idx_v + inner_joint_row; | |
1062 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 482 times.
|
482 | assert(inner_row_id > outer_row_id); |
1063 |
2/4✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
|
482 | const MotionIn S2(J.col(inner_row_id)); |
1064 | |||
1065 |
1/2✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
|
482 | MapVector6 motion_vec_out( |
1066 |
2/4✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
|
482 | kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6); |
1067 |
2/4✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
|
482 | MotionOut S1xS2(motion_vec_out); |
1068 | |||
1069 |
2/4✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
|
482 | S1xS2 = S1.cross(S2); |
1070 | } | ||
1071 | } | ||
1072 | } | ||
1073 | } | ||
1074 | 2 | } | |
1075 | |||
1076 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
1077 | 3 | void getJointKinematicHessian( | |
1078 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
1079 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
1080 | const JointIndex joint_id, | ||
1081 | const ReferenceFrame rf, | ||
1082 | Tensor<Scalar, 3, Options> & kinematic_hessian) | ||
1083 | { | ||
1084 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | assert(model.check(data) && "data is not consistent with model."); |
1085 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | assert( |
1086 | joint_id < model.joints.size() && joint_id > 0 | ||
1087 | && "joint_id is outside the valid index for a joint in model.joints"); | ||
1088 | |||
1089 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
1090 | typedef typename Data::SE3 SE3; | ||
1091 | typedef typename Data::Motion Motion; | ||
1092 | |||
1093 | 3 | const typename Data::Matrix6x & J = data.J; | |
1094 | 3 | const typename Data::Tensor3x & kinematic_hessians = data.kinematic_hessians; | |
1095 | |||
1096 | // Allocate memory | ||
1097 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
1098 | kinematic_hessian.dimension(0), 6, "The result tensor is not of the right dimension."); | ||
1099 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
1100 | kinematic_hessian.dimension(1), model.nv, "The result tensor is not of the right dimension."); | ||
1101 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
1102 | kinematic_hessian.dimension(2), model.nv, "The result tensor is not of the right dimension."); | ||
1103 | |||
1104 | 3 | const int idx_vj = model.joints[joint_id].idx_v(); | |
1105 | 3 | const int nvj = model.joints[joint_id].nv(); | |
1106 | 3 | const Eigen::DenseIndex slice_matrix_size = 6 * model.nv; | |
1107 | |||
1108 | typedef std::vector<int> IndexVector; | ||
1109 | 3 | const Eigen::DenseIndex last_idx = idx_vj + nvj - 1; | |
1110 | const std::vector<int> & supporting_indexes = | ||
1111 | 3 | data.supports_fromRow[(size_t)(last_idx)]; // until the last element of the joint size (nvj) | |
1112 | |||
1113 | typedef Eigen::Map<typename Motion::Vector6> MapVector6; | ||
1114 | typedef MotionRef<MapVector6> MotionOut; | ||
1115 | typedef Eigen::Map<const typename Motion::Vector6> ConstMapVector6; | ||
1116 | typedef MotionRef<ConstMapVector6> MotionIn; | ||
1117 | |||
1118 |
3/4✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
3 | switch (rf) |
1119 | { | ||
1120 | 1 | case WORLD: { | |
1121 |
2/2✓ Branch 1 taken 10 times.
✓ Branch 2 taken 1 times.
|
11 | for (size_t i = 0; i < supporting_indexes.size(); ++i) |
1122 | { | ||
1123 | 10 | const Eigen::DenseIndex outer_row_id = supporting_indexes[i]; | |
1124 | |||
1125 | // Take into account parent indexes of the current joint motion subspace | ||
1126 | 25 | for (int subspace_idx = data.start_idx_v_fromRow[(size_t)outer_row_id]; | |
1127 |
2/2✓ Branch 0 taken 15 times.
✓ Branch 1 taken 10 times.
|
25 | subspace_idx < outer_row_id; ++subspace_idx) |
1128 | { | ||
1129 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | ConstMapVector6 vec_in( |
1130 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
15 | kinematic_hessians.data() + outer_row_id * slice_matrix_size + subspace_idx * 6); |
1131 | |||
1132 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | MapVector6 vec_out( |
1133 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
15 | kinematic_hessian.data() + outer_row_id * slice_matrix_size + subspace_idx * 6); |
1134 | |||
1135 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | vec_out = vec_in; |
1136 | } | ||
1137 | |||
1138 |
2/2✓ Branch 1 taken 45 times.
✓ Branch 2 taken 10 times.
|
55 | for (size_t j = i + 1; j < supporting_indexes.size(); ++j) |
1139 | { | ||
1140 | 45 | const Eigen::DenseIndex inner_row_id = supporting_indexes[j]; | |
1141 | |||
1142 |
1/2✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
|
45 | ConstMapVector6 vec_in( |
1143 |
2/4✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
|
45 | kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6); |
1144 | |||
1145 |
1/2✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
|
45 | MapVector6 vec_out( |
1146 |
2/4✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
|
45 | kinematic_hessian.data() + outer_row_id * slice_matrix_size + inner_row_id * 6); |
1147 | |||
1148 |
1/2✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
|
45 | vec_out = vec_in; |
1149 | } | ||
1150 | } | ||
1151 | 1 | break; | |
1152 | } | ||
1153 | 1 | case LOCAL_WORLD_ALIGNED: { | |
1154 | typedef MotionRef<const typename Data::Matrix6x::ConstColXpr> MotionColRef; | ||
1155 | 1 | const SE3 & oMlast = data.oMi[joint_id]; | |
1156 | |||
1157 |
2/2✓ Branch 1 taken 10 times.
✓ Branch 2 taken 1 times.
|
11 | for (size_t i = 0; i < supporting_indexes.size(); ++i) |
1158 | { | ||
1159 | 10 | const Eigen::DenseIndex outer_row_id = supporting_indexes[i]; | |
1160 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | const MotionColRef S1(J.col(outer_row_id)); |
1161 | |||
1162 |
1/2✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
|
40 | for (size_t j = 0; j < supporting_indexes.size(); ++j) |
1163 | { | ||
1164 | 40 | const Eigen::DenseIndex inner_row_id = supporting_indexes[j]; | |
1165 |
2/2✓ Branch 1 taken 10 times.
✓ Branch 2 taken 30 times.
|
40 | if (inner_row_id >= data.start_idx_v_fromRow[(size_t)outer_row_id]) |
1166 | 10 | break; | |
1167 | |||
1168 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | MotionColRef S2(J.col(inner_row_id)); |
1169 | |||
1170 |
1/2✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
|
30 | ConstMapVector6 vec_in( |
1171 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6); |
1172 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | MotionIn S1xS2(vec_in); |
1173 | |||
1174 |
1/2✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
|
30 | MapVector6 vec_out( |
1175 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | kinematic_hessian.data() + outer_row_id * slice_matrix_size + inner_row_id * 6); |
1176 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | MotionOut m_out(vec_out); |
1177 | |||
1178 |
6/12✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 30 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 30 times.
✗ Branch 17 not taken.
|
90 | m_out.linear() = |
1179 |
4/8✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30 times.
✗ Branch 11 not taken.
|
120 | -(S1.linear() - oMlast.translation().cross(S1.angular())).cross(S2.angular()); |
1180 | } | ||
1181 | |||
1182 | // Take into account parent indexes of the current joint motion subspace | ||
1183 | 10 | for (int inner_row_id = data.start_idx_v_fromRow[(size_t)outer_row_id]; | |
1184 |
2/2✓ Branch 0 taken 15 times.
✓ Branch 1 taken 10 times.
|
25 | inner_row_id < outer_row_id; ++inner_row_id) |
1185 | { | ||
1186 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
15 | MotionColRef S2(J.col(inner_row_id)); |
1187 | |||
1188 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | ConstMapVector6 vec_in( |
1189 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
15 | kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6); |
1190 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
15 | MotionIn S1xS2(vec_in); |
1191 | |||
1192 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | MapVector6 vec_out( |
1193 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
15 | kinematic_hessian.data() + outer_row_id * slice_matrix_size + inner_row_id * 6); |
1194 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
15 | MotionOut m_out(vec_out); |
1195 | |||
1196 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | vec_out = vec_in; |
1197 |
7/14✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 15 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 15 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 15 times.
✗ Branch 20 not taken.
|
60 | m_out.linear() -= |
1198 |
4/8✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
|
60 | (S1.linear() - oMlast.translation().cross(S1.angular())).cross(S2.angular()) |
1199 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
15 | + oMlast.translation().cross(S1xS2.angular()); |
1200 | } | ||
1201 | |||
1202 | // case: outer_row_id == inner_row_id | ||
1203 | { | ||
1204 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | MapVector6 vec_out( |
1205 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | kinematic_hessian.data() + outer_row_id * slice_matrix_size + outer_row_id * 6); |
1206 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | MotionOut m_out(vec_out); |
1207 | |||
1208 |
6/12✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
|
30 | m_out.linear() = |
1209 |
4/8✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
|
30 | -(S1.linear() - oMlast.translation().cross(S1.angular())).cross(S1.angular()); |
1210 | } | ||
1211 | |||
1212 |
2/2✓ Branch 1 taken 45 times.
✓ Branch 2 taken 10 times.
|
55 | for (size_t j = i + 1; j < supporting_indexes.size(); ++j) |
1213 | { | ||
1214 | 45 | const Eigen::DenseIndex inner_row_id = supporting_indexes[j]; | |
1215 |
2/4✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
|
45 | MotionColRef S2(J.col(inner_row_id)); |
1216 | |||
1217 |
1/2✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
|
45 | ConstMapVector6 vec_in( |
1218 |
2/4✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
|
45 | kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6); |
1219 |
2/4✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
|
45 | MotionIn S1xS2(vec_in); |
1220 | |||
1221 |
1/2✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
|
45 | MapVector6 vec_out( |
1222 |
2/4✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
|
45 | kinematic_hessian.data() + outer_row_id * slice_matrix_size + inner_row_id * 6); |
1223 |
2/4✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
|
45 | MotionOut m_out(vec_out); |
1224 | |||
1225 |
1/2✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
|
45 | vec_out = vec_in; |
1226 |
7/14✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 45 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 45 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 45 times.
✗ Branch 20 not taken.
|
180 | m_out.linear() -= |
1227 |
4/8✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
|
180 | (S1.linear() - oMlast.translation().cross(S1.angular())).cross(S2.angular()) |
1228 |
2/4✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
|
45 | + oMlast.translation().cross(S1xS2.angular()); |
1229 | } | ||
1230 | } | ||
1231 | 1 | break; | |
1232 | } | ||
1233 | 1 | case LOCAL: { | |
1234 | 1 | const SE3 & oMlast = data.oMi[joint_id]; | |
1235 | |||
1236 | 1 | for (IndexVector::const_reverse_iterator rit = supporting_indexes.rbegin(); | |
1237 |
3/4✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 10 times.
✓ Branch 6 taken 1 times.
|
11 | rit != supporting_indexes.rend(); ++rit) |
1238 | { | ||
1239 | 10 | const Eigen::DenseIndex outer_row_id = *rit; | |
1240 | |||
1241 | // This corresponds to the joint connected to the world, we can skip | ||
1242 |
2/2✓ Branch 2 taken 6 times.
✓ Branch 3 taken 4 times.
|
10 | if (data.parents_fromRow[(size_t)data.start_idx_v_fromRow[(size_t)outer_row_id]] < 0) |
1243 | 6 | continue; | |
1244 | |||
1245 | // Take into account current joint motion subspace | ||
1246 | 4 | for (int subspace_idx = data.end_idx_v_fromRow[(size_t)outer_row_id]; | |
1247 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
|
4 | subspace_idx > outer_row_id; --subspace_idx) |
1248 | { | ||
1249 | ✗ | ConstMapVector6 vec_in( | |
1250 | ✗ | kinematic_hessians.data() + subspace_idx * slice_matrix_size + outer_row_id * 6); | |
1251 | ✗ | MotionIn m_in(vec_in); | |
1252 | |||
1253 | ✗ | MapVector6 vec_out( | |
1254 | ✗ | kinematic_hessian.data() + outer_row_id * slice_matrix_size + subspace_idx * 6); | |
1255 | ✗ | MotionOut m_out(vec_out); | |
1256 | |||
1257 | ✗ | m_out = oMlast.actInv(m_in); | |
1258 | } | ||
1259 | |||
1260 | 4 | IndexVector::const_reverse_iterator inner_rit = rit; | |
1261 |
3/4✓ Branch 3 taken 34 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 30 times.
✓ Branch 6 taken 4 times.
|
34 | for (++inner_rit; inner_rit != supporting_indexes.rend(); ++inner_rit) |
1262 | { | ||
1263 | 30 | const Eigen::DenseIndex inner_row_id = *inner_rit; | |
1264 | |||
1265 |
1/2✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
|
30 | ConstMapVector6 vec_in( |
1266 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | kinematic_hessians.data() + inner_row_id * slice_matrix_size + outer_row_id * 6); |
1267 | |||
1268 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | MotionIn m_in(vec_in); |
1269 | |||
1270 |
1/2✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
|
30 | MapVector6 vec_out( |
1271 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | kinematic_hessian.data() + outer_row_id * slice_matrix_size + inner_row_id * 6); |
1272 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | MotionOut m_out(vec_out); |
1273 | |||
1274 |
2/4✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
|
30 | m_out = oMlast.actInv(m_in); |
1275 | } | ||
1276 | } | ||
1277 | |||
1278 | 1 | break; | |
1279 | } | ||
1280 | ✗ | default: | |
1281 | ✗ | assert(false && "must never happened"); | |
1282 | break; | ||
1283 | } | ||
1284 | 3 | } | |
1285 | |||
1286 | template< | ||
1287 | typename Scalar, | ||
1288 | int Options, | ||
1289 | template<typename, int> | ||
1290 | class JointCollectionTpl, | ||
1291 | typename ConfigVectorType, | ||
1292 | typename TangentVectorType1, | ||
1293 | typename TangentVectorType2> | ||
1294 | 3 | void computeForwardKinematicsDerivatives( | |
1295 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
1296 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
1297 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
1298 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
1299 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
1300 | { | ||
1301 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
3 | impl::computeForwardKinematicsDerivatives( |
1302 | model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a)); | ||
1303 | 3 | } | |
1304 | |||
1305 | template< | ||
1306 | typename Scalar, | ||
1307 | int Options, | ||
1308 | template<typename, int> | ||
1309 | class JointCollectionTpl, | ||
1310 | typename Matrix6xOut1, | ||
1311 | typename Matrix6xOut2> | ||
1312 | 14 | void getJointVelocityDerivatives( | |
1313 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
1314 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
1315 | const Model::JointIndex jointId, | ||
1316 | const ReferenceFrame rf, | ||
1317 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
1318 | const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv) | ||
1319 | { | ||
1320 |
2/4✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
|
14 | impl::getJointVelocityDerivatives( |
1321 | model, data, jointId, rf, make_ref(v_partial_dq), make_ref(v_partial_dv)); | ||
1322 | 14 | } | |
1323 | |||
1324 | template< | ||
1325 | typename Scalar, | ||
1326 | int Options, | ||
1327 | template<typename, int> | ||
1328 | class JointCollectionTpl, | ||
1329 | typename Matrix6xOut1, | ||
1330 | typename Matrix6xOut2, | ||
1331 | typename Matrix6xOut3, | ||
1332 | typename Matrix6xOut4> | ||
1333 | 29 | void getJointAccelerationDerivatives( | |
1334 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
1335 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
1336 | const Model::JointIndex jointId, | ||
1337 | const ReferenceFrame rf, | ||
1338 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
1339 | const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq, | ||
1340 | const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv, | ||
1341 | const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da) | ||
1342 | { | ||
1343 |
4/8✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 29 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 29 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 29 times.
✗ Branch 12 not taken.
|
29 | impl::getJointAccelerationDerivatives( |
1344 | model, data, jointId, rf, make_ref(v_partial_dq), make_ref(a_partial_dq), | ||
1345 | make_ref(a_partial_dv), make_ref(a_partial_da)); | ||
1346 | 29 | } | |
1347 | |||
1348 | template< | ||
1349 | typename Scalar, | ||
1350 | int Options, | ||
1351 | template<typename, int> | ||
1352 | class JointCollectionTpl, | ||
1353 | typename Matrix6xOut1, | ||
1354 | typename Matrix6xOut2, | ||
1355 | typename Matrix6xOut3, | ||
1356 | typename Matrix6xOut4, | ||
1357 | typename Matrix6xOut5> | ||
1358 | ✗ | void getJointAccelerationDerivatives( | |
1359 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
1360 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
1361 | const Model::JointIndex jointId, | ||
1362 | const ReferenceFrame rf, | ||
1363 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
1364 | const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv, | ||
1365 | const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq, | ||
1366 | const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv, | ||
1367 | const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da) | ||
1368 | { | ||
1369 | ✗ | impl::getJointAccelerationDerivatives( | |
1370 | model, data, jointId, rf, make_ref(v_partial_dq), make_ref(v_partial_dv), | ||
1371 | make_ref(a_partial_dq), make_ref(a_partial_dv), make_ref(a_partial_da)); | ||
1372 | } | ||
1373 | |||
1374 | template< | ||
1375 | typename Scalar, | ||
1376 | int Options, | ||
1377 | template<typename, int> | ||
1378 | class JointCollectionTpl, | ||
1379 | typename Matrix3xOut1, | ||
1380 | typename Matrix3xOut2> | ||
1381 | ✗ | void getPointVelocityDerivatives( | |
1382 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
1383 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
1384 | const Model::JointIndex joint_id, | ||
1385 | const SE3Tpl<Scalar, Options> & placement, | ||
1386 | const ReferenceFrame rf, | ||
1387 | const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq, | ||
1388 | const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv) | ||
1389 | { | ||
1390 | ✗ | impl::getPointVelocityDerivatives( | |
1391 | model, data, joint_id, placement, rf, make_ref(v_point_partial_dq), | ||
1392 | make_ref(v_point_partial_dv)); | ||
1393 | } | ||
1394 | |||
1395 | template< | ||
1396 | typename Scalar, | ||
1397 | int Options, | ||
1398 | template<typename, int> | ||
1399 | class JointCollectionTpl, | ||
1400 | typename Matrix3xOut1, | ||
1401 | typename Matrix3xOut2, | ||
1402 | typename Matrix3xOut3, | ||
1403 | typename Matrix3xOut4> | ||
1404 | 20 | void getPointClassicAccelerationDerivatives( | |
1405 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
1406 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
1407 | const Model::JointIndex joint_id, | ||
1408 | const SE3Tpl<Scalar, Options> & placement, | ||
1409 | const ReferenceFrame rf, | ||
1410 | const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq, | ||
1411 | const Eigen::MatrixBase<Matrix3xOut2> & a_point_partial_dq, | ||
1412 | const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dv, | ||
1413 | const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_da) | ||
1414 | { | ||
1415 |
4/8✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 20 times.
✗ Branch 12 not taken.
|
20 | impl::getPointClassicAccelerationDerivatives( |
1416 | model, data, joint_id, placement, rf, make_ref(v_point_partial_dq), | ||
1417 | make_ref(a_point_partial_dq), make_ref(a_point_partial_dv), make_ref(a_point_partial_da)); | ||
1418 | 20 | } | |
1419 | |||
1420 | template< | ||
1421 | typename Scalar, | ||
1422 | int Options, | ||
1423 | template<typename, int> | ||
1424 | class JointCollectionTpl, | ||
1425 | typename Matrix3xOut1, | ||
1426 | typename Matrix3xOut2, | ||
1427 | typename Matrix3xOut3, | ||
1428 | typename Matrix3xOut4, | ||
1429 | typename Matrix3xOut5> | ||
1430 | ✗ | void getPointClassicAccelerationDerivatives( | |
1431 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
1432 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
1433 | const Model::JointIndex joint_id, | ||
1434 | const SE3Tpl<Scalar, Options> & placement, | ||
1435 | const ReferenceFrame rf, | ||
1436 | const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq, | ||
1437 | const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv, | ||
1438 | const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dq, | ||
1439 | const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_dv, | ||
1440 | const Eigen::MatrixBase<Matrix3xOut5> & a_point_partial_da) | ||
1441 | { | ||
1442 | ✗ | impl::getPointClassicAccelerationDerivatives( | |
1443 | model, data, joint_id, placement, rf, make_ref(v_point_partial_dq), | ||
1444 | make_ref(v_point_partial_dv), make_ref(a_point_partial_dq), make_ref(a_point_partial_dv), | ||
1445 | make_ref(a_point_partial_da)); | ||
1446 | } | ||
1447 | |||
1448 | } // namespace pinocchio | ||
1449 | |||
1450 | #endif // ifndef __pinocchio_algorithm_kinematics_derivatives_hxx__ | ||
1451 |