Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/rnea.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 220 | 228 | 96.5% |
Branches: | 330 | 917 | 36.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2022 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_rnea_hxx__ | ||
6 | #define __pinocchio_algorithm_rnea_hxx__ | ||
7 | |||
8 | /// @cond DEV | ||
9 | |||
10 | #include "pinocchio/multibody/visitor.hpp" | ||
11 | #include "pinocchio/algorithm/check.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 RneaForwardStep | ||
26 | : public fusion::JointUnaryVisitorBase<RneaForwardStep< | ||
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 | 19738 | 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 | |||
57 | 19738 | const JointIndex i = jmodel.id(); | |
58 | 19738 | const JointIndex parent = model.parents[i]; | |
59 | |||
60 | 19738 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); | |
61 | |||
62 |
3/5✓ Branch 4 taken 9491 times.
✓ Branch 5 taken 378 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9491 times.
✗ Branch 9 not taken.
|
19738 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
63 | |||
64 | 19738 | data.v[i] = jdata.v(); | |
65 |
2/2✓ Branch 0 taken 9478 times.
✓ Branch 1 taken 391 times.
|
19738 | if (parent > 0) |
66 |
1/2✓ Branch 5 taken 9478 times.
✗ Branch 6 not taken.
|
18956 | data.v[i] += data.liMi[i].actInv(data.v[parent]); |
67 | |||
68 |
4/8✓ Branch 4 taken 9869 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 9862 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
|
19738 | data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); |
69 |
3/6✓ Branch 2 taken 9869 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9869 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 9869 times.
✗ Branch 10 not taken.
|
19738 | data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a); |
70 |
1/2✓ Branch 5 taken 9869 times.
✗ Branch 6 not taken.
|
19738 | data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); |
71 | // | ||
72 | // data.f[i] = model.inertias[i]*data.a_gf[i];// + model.inertias[i].vxiv(data.v[i]); | ||
73 | // // -f_ext data.h[i] = model.inertias[i]*data.v[i]; | ||
74 | 19738 | model.inertias[i].__mult__(data.v[i], data.h[i]); | |
75 | 19738 | model.inertias[i].__mult__(data.a_gf[i], data.f[i]); | |
76 |
1/2✓ Branch 5 taken 9869 times.
✗ Branch 6 not taken.
|
19738 | data.f[i] += data.v[i].cross(data.h[i]); |
77 | // data.h[i].motionAction(data.v[i],data.f[i]); | ||
78 | // data.f[i] = model.inertias[i].vxiv(data.v[i]); | ||
79 | // data.f[i].setZero(); | ||
80 | } | ||
81 | }; | ||
82 | |||
83 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
84 | struct RneaBackwardStep | ||
85 | : public fusion::JointUnaryVisitorBase<RneaBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
86 | { | ||
87 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
88 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
89 | |||
90 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
91 | |||
92 | template<typename JointModel> | ||
93 | 19720 | static void algo( | |
94 | const JointModelBase<JointModel> & jmodel, | ||
95 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
96 | const Model & model, | ||
97 | Data & data) | ||
98 | { | ||
99 | typedef typename Model::JointIndex JointIndex; | ||
100 | |||
101 | 19720 | const JointIndex i = jmodel.id(); | |
102 | 19720 | const JointIndex parent = model.parents[i]; | |
103 | |||
104 |
3/6✓ Branch 4 taken 9860 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9860 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9860 times.
✗ Branch 11 not taken.
|
19720 | jmodel.jointVelocitySelector(data.tau) = jdata.S().transpose() * data.f[i]; |
105 | |||
106 |
2/2✓ Branch 0 taken 9478 times.
✓ Branch 1 taken 382 times.
|
19720 | if (parent > 0) |
107 |
1/2✓ Branch 5 taken 9478 times.
✗ Branch 6 not taken.
|
18956 | data.f[parent] += data.liMi[i].act(data.f[i]); |
108 | } | ||
109 | }; | ||
110 | |||
111 | template< | ||
112 | typename Scalar, | ||
113 | int Options, | ||
114 | template<typename, int> | ||
115 | class JointCollectionTpl, | ||
116 | typename ConfigVectorType, | ||
117 | typename TangentVectorType1, | ||
118 | typename TangentVectorType2> | ||
119 | 287 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & rnea( | |
120 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
121 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
122 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
123 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
124 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
125 | { | ||
126 |
2/4✓ Branch 1 taken 287 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 287 times.
✗ Branch 4 not taken.
|
287 | assert(model.check(data) && "data is not consistent with model."); |
127 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 287 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.
|
287 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
128 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
129 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 287 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.
|
287 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
130 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 287 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.
|
287 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
131 | a.size(), model.nv, "The acceleration vector is not of right size"); | ||
132 | |||
133 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
134 | typedef typename Model::JointIndex JointIndex; | ||
135 | |||
136 |
1/2✓ Branch 2 taken 287 times.
✗ Branch 3 not taken.
|
287 | data.v[0].setZero(); |
137 |
2/4✓ Branch 1 taken 287 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 287 times.
✗ Branch 6 not taken.
|
287 | data.a_gf[0] = -model.gravity; |
138 | |||
139 | typedef RneaForwardStep< | ||
140 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, | ||
141 | TangentVectorType2> | ||
142 | Pass1; | ||
143 |
1/2✓ Branch 4 taken 287 times.
✗ Branch 5 not taken.
|
287 | typename Pass1::ArgsType arg1(model, data, q.derived(), v.derived(), a.derived()); |
144 |
2/2✓ Branch 0 taken 7075 times.
✓ Branch 1 taken 287 times.
|
7362 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
145 | { | ||
146 |
2/4✓ Branch 1 taken 7075 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 7075 times.
✗ Branch 7 not taken.
|
7075 | Pass1::run(model.joints[i], data.joints[i], arg1); |
147 | } | ||
148 | |||
149 | typedef RneaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
150 |
1/2✓ Branch 1 taken 287 times.
✗ Branch 2 not taken.
|
287 | typename Pass2::ArgsType arg2(model, data); |
151 |
2/2✓ Branch 0 taken 7075 times.
✓ Branch 1 taken 287 times.
|
7362 | for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) |
152 | { | ||
153 |
2/4✓ Branch 1 taken 7075 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 7075 times.
✗ Branch 7 not taken.
|
7075 | Pass2::run(model.joints[i], data.joints[i], arg2); |
154 | } | ||
155 | |||
156 | // Add rotorinertia contribution | ||
157 |
5/10✓ Branch 1 taken 287 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 287 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 287 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 287 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 287 times.
✗ Branch 14 not taken.
|
287 | data.tau.array() += model.armature.array() * a.array(); // Check if there is memory allocation |
158 | |||
159 | 287 | return data.tau; | |
160 | } | ||
161 | |||
162 | template< | ||
163 | typename Scalar, | ||
164 | int Options, | ||
165 | template<typename, int> | ||
166 | class JointCollectionTpl, | ||
167 | typename ConfigVectorType, | ||
168 | typename TangentVectorType1, | ||
169 | typename TangentVectorType2, | ||
170 | typename ForceDerived> | ||
171 | 104 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & rnea( | |
172 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
173 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
174 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
175 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
176 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
177 | const container::aligned_vector<ForceDerived> & fext) | ||
178 | { | ||
179 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 104 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 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.
|
104 | PINOCCHIO_CHECK_ARGUMENT_SIZE(fext.size(), model.joints.size()); |
180 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | assert(model.check(data) && "data is not consistent with model."); |
181 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 104 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.
|
104 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
182 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
183 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 104 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.
|
104 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
184 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 104 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.
|
104 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
185 | a.size(), model.nv, "The acceleration vector is not of right size"); | ||
186 | |||
187 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
188 | typedef typename Model::JointIndex JointIndex; | ||
189 | |||
190 | 104 | data.v[0].setZero(); | |
191 |
1/2✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
|
104 | data.a_gf[0] = -model.gravity; |
192 | |||
193 | typedef RneaForwardStep< | ||
194 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, | ||
195 | TangentVectorType2> | ||
196 | Pass1; | ||
197 |
2/2✓ Branch 0 taken 2794 times.
✓ Branch 1 taken 104 times.
|
2898 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
198 | { | ||
199 |
1/2✓ Branch 1 taken 2794 times.
✗ Branch 2 not taken.
|
2794 | Pass1::run( |
200 | 2794 | model.joints[i], data.joints[i], | |
201 | 2794 | typename Pass1::ArgsType(model, data, q.derived(), v.derived(), a.derived())); | |
202 | 2794 | data.f[i] -= fext[i]; | |
203 | } | ||
204 | |||
205 | typedef RneaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
206 |
2/2✓ Branch 0 taken 2794 times.
✓ Branch 1 taken 104 times.
|
2898 | for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) |
207 | { | ||
208 |
1/2✓ Branch 4 taken 2794 times.
✗ Branch 5 not taken.
|
2794 | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); |
209 | } | ||
210 | |||
211 | // Add armature contribution | ||
212 |
3/6✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 104 times.
✗ Branch 8 not taken.
|
104 | data.tau.array() += |
213 |
1/2✓ Branch 2 taken 104 times.
✗ Branch 3 not taken.
|
104 | model.armature.array() * a.array(); // TODO: check if there is memory allocation |
214 | |||
215 | 104 | return data.tau; | |
216 | } | ||
217 | |||
218 | template< | ||
219 | typename Scalar, | ||
220 | int Options, | ||
221 | template<typename, int> | ||
222 | class JointCollectionTpl, | ||
223 | typename ConfigVectorType, | ||
224 | typename TangentVectorType> | ||
225 | struct NLEForwardStep | ||
226 | : public fusion::JointUnaryVisitorBase< | ||
227 | NLEForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>> | ||
228 | { | ||
229 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
230 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
231 | |||
232 | typedef boost::fusion:: | ||
233 | vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &> | ||
234 | ArgsType; | ||
235 | |||
236 | template<typename JointModel> | ||
237 | 648 | static void algo( | |
238 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
239 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
240 | const Model & model, | ||
241 | Data & data, | ||
242 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
243 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
244 | { | ||
245 | typedef typename Model::JointIndex JointIndex; | ||
246 | |||
247 |
1/2✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
|
648 | const JointIndex & i = jmodel.id(); |
248 | 648 | const JointIndex & parent = model.parents[i]; | |
249 | |||
250 |
1/2✓ Branch 4 taken 324 times.
✗ Branch 5 not taken.
|
648 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); |
251 | |||
252 |
6/10✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 312 times.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 312 times.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 312 times.
✗ Branch 13 not taken.
|
648 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
253 | |||
254 |
2/4✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 324 times.
✗ Branch 6 not taken.
|
648 | data.v[i] = jdata.v(); |
255 |
2/2✓ Branch 0 taken 312 times.
✓ Branch 1 taken 12 times.
|
648 | if (parent > 0) |
256 |
2/4✓ Branch 3 taken 312 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 312 times.
✗ Branch 8 not taken.
|
624 | data.v[i] += data.liMi[i].actInv(data.v[parent]); |
257 | |||
258 |
4/12✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 324 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 324 times.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 324 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
|
648 | data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); |
259 |
2/4✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 324 times.
✗ Branch 8 not taken.
|
648 | data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); |
260 | |||
261 |
4/8✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 324 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 324 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 324 times.
✗ Branch 16 not taken.
|
648 | data.f[i] = model.inertias[i] * data.a_gf[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext |
262 | } | ||
263 | }; | ||
264 | |||
265 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
266 | struct NLEBackwardStep | ||
267 | : public fusion::JointUnaryVisitorBase<NLEBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
268 | { | ||
269 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
270 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
271 | |||
272 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
273 | |||
274 | template<typename JointModel> | ||
275 | 648 | static void algo( | |
276 | const JointModelBase<JointModel> & jmodel, | ||
277 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
278 | const Model & model, | ||
279 | Data & data) | ||
280 | { | ||
281 | typedef typename Model::JointIndex JointIndex; | ||
282 | |||
283 |
1/2✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
|
648 | const JointIndex & i = jmodel.id(); |
284 | 648 | const JointIndex & parent = model.parents[i]; | |
285 | |||
286 |
8/13✓ Branch 2 taken 324 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 312 times.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 312 times.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 312 times.
✓ Branch 12 taken 12 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 312 times.
✗ Branch 15 not taken.
|
648 | jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose() * data.f[i]; |
287 |
2/2✓ Branch 0 taken 312 times.
✓ Branch 1 taken 12 times.
|
648 | if (parent > 0) |
288 |
2/4✓ Branch 3 taken 312 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 312 times.
✗ Branch 8 not taken.
|
624 | data.f[parent] += data.liMi[i].act(data.f[i]); |
289 | } | ||
290 | }; | ||
291 | |||
292 | template< | ||
293 | typename Scalar, | ||
294 | int Options, | ||
295 | template<typename, int> | ||
296 | class JointCollectionTpl, | ||
297 | typename ConfigVectorType, | ||
298 | typename TangentVectorType> | ||
299 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & | ||
300 | 12 | nonLinearEffects( | |
301 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
302 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
303 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
304 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
305 | { | ||
306 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | assert(model.check(data) && "data is not consistent with model."); |
307 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 12 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.
|
12 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
308 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
309 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 12 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.
|
12 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
310 | |||
311 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
312 | typedef typename Model::JointIndex JointIndex; | ||
313 | |||
314 | 12 | data.v[0].setZero(); | |
315 |
1/2✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
|
12 | data.a_gf[0] = -model.gravity; |
316 | |||
317 | typedef NLEForwardStep< | ||
318 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType> | ||
319 | Pass1; | ||
320 |
2/2✓ Branch 0 taken 324 times.
✓ Branch 1 taken 12 times.
|
336 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
321 | { | ||
322 |
1/2✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
|
324 | Pass1::run( |
323 | 324 | model.joints[i], data.joints[i], | |
324 | 648 | typename Pass1::ArgsType(model, data, q.derived(), v.derived())); | |
325 | } | ||
326 | |||
327 | typedef NLEBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
328 |
2/2✓ Branch 0 taken 324 times.
✓ Branch 1 taken 12 times.
|
336 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
329 | { | ||
330 |
1/2✓ Branch 4 taken 324 times.
✗ Branch 5 not taken.
|
324 | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); |
331 | } | ||
332 | |||
333 | 12 | return data.nle; | |
334 | } | ||
335 | |||
336 | template< | ||
337 | typename Scalar, | ||
338 | int Options, | ||
339 | template<typename, int> | ||
340 | class JointCollectionTpl, | ||
341 | typename ConfigVectorType> | ||
342 | struct ComputeGeneralizedGravityForwardStep | ||
343 | : public fusion::JointUnaryVisitorBase< | ||
344 | ComputeGeneralizedGravityForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>> | ||
345 | { | ||
346 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
347 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
348 | |||
349 | typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType; | ||
350 | |||
351 | template<typename JointModel> | ||
352 | 3888 | static void algo( | |
353 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
354 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
355 | const Model & model, | ||
356 | Data & data, | ||
357 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
358 | { | ||
359 | typedef typename Model::JointIndex JointIndex; | ||
360 | |||
361 |
1/2✓ Branch 1 taken 1944 times.
✗ Branch 2 not taken.
|
3888 | const JointIndex & i = jmodel.id(); |
362 | 3888 | const JointIndex & parent = model.parents[i]; | |
363 | |||
364 |
1/2✓ Branch 3 taken 1944 times.
✗ Branch 4 not taken.
|
3888 | jmodel.calc(jdata.derived(), q.derived()); |
365 | |||
366 |
6/10✓ Branch 1 taken 1944 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1872 times.
✓ Branch 5 taken 72 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1872 times.
✓ Branch 9 taken 72 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1872 times.
✗ Branch 13 not taken.
|
3888 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
367 | |||
368 |
2/4✓ Branch 3 taken 1944 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1944 times.
✗ Branch 8 not taken.
|
3888 | data.a_gf[i] = data.liMi[i].actInv(data.a_gf[(size_t)parent]); |
369 |
2/4✓ Branch 3 taken 1944 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1944 times.
✗ Branch 8 not taken.
|
3888 | data.f[i] = model.inertias[i] * data.a_gf[i]; |
370 | } | ||
371 | }; | ||
372 | |||
373 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
374 | struct ComputeGeneralizedGravityBackwardStep | ||
375 | : public fusion::JointUnaryVisitorBase< | ||
376 | ComputeGeneralizedGravityBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
377 | { | ||
378 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
379 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
380 | |||
381 | typedef boost::fusion::vector<const Model &, Data &, typename Data::VectorXs &> ArgsType; | ||
382 | |||
383 | template<typename JointModel> | ||
384 | 3888 | static void algo( | |
385 | const JointModelBase<JointModel> & jmodel, | ||
386 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
387 | const Model & model, | ||
388 | Data & data, | ||
389 | typename Data::VectorXs & g) | ||
390 | { | ||
391 | typedef typename Model::JointIndex JointIndex; | ||
392 | |||
393 |
1/2✓ Branch 1 taken 1944 times.
✗ Branch 2 not taken.
|
3888 | const JointIndex & i = jmodel.id(); |
394 | 3888 | const JointIndex & parent = model.parents[i]; | |
395 | |||
396 |
8/13✓ Branch 2 taken 1944 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1872 times.
✓ Branch 6 taken 72 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1872 times.
✓ Branch 9 taken 72 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1872 times.
✓ Branch 12 taken 72 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 1872 times.
✗ Branch 15 not taken.
|
3888 | jmodel.jointVelocitySelector(g) = jdata.S().transpose() * data.f[i]; |
397 |
2/2✓ Branch 0 taken 1872 times.
✓ Branch 1 taken 72 times.
|
3888 | if (parent > 0) |
398 |
2/4✓ Branch 3 taken 1872 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1872 times.
✗ Branch 8 not taken.
|
3744 | data.f[(size_t)parent] += data.liMi[i].act(data.f[i]); |
399 | } | ||
400 | }; | ||
401 | |||
402 | template< | ||
403 | typename Scalar, | ||
404 | int Options, | ||
405 | template<typename, int> | ||
406 | class JointCollectionTpl, | ||
407 | typename ConfigVectorType> | ||
408 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & | ||
409 | 38 | computeGeneralizedGravity( | |
410 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
411 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
412 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
413 | { | ||
414 |
1/2✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
|
38 | assert(model.check(data) && "data is not consistent with model."); |
415 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 38 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.
|
38 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
416 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
417 | |||
418 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
419 | typedef typename Model::JointIndex JointIndex; | ||
420 | |||
421 |
1/2✓ Branch 3 taken 38 times.
✗ Branch 4 not taken.
|
38 | data.a_gf[0] = -model.gravity; |
422 | |||
423 | typedef ComputeGeneralizedGravityForwardStep< | ||
424 | Scalar, Options, JointCollectionTpl, ConfigVectorType> | ||
425 | Pass1; | ||
426 |
2/2✓ Branch 0 taken 1026 times.
✓ Branch 1 taken 38 times.
|
1064 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
427 | { | ||
428 |
1/2✓ Branch 1 taken 1026 times.
✗ Branch 2 not taken.
|
1026 | Pass1::run( |
429 | 2052 | model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); | |
430 | } | ||
431 | |||
432 | typedef ComputeGeneralizedGravityBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
433 |
2/2✓ Branch 0 taken 1026 times.
✓ Branch 1 taken 38 times.
|
1064 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
434 | { | ||
435 |
1/2✓ Branch 4 taken 1026 times.
✗ Branch 5 not taken.
|
1026 | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data, data.g)); |
436 | } | ||
437 | |||
438 | 38 | return data.g; | |
439 | } | ||
440 | |||
441 | template< | ||
442 | typename Scalar, | ||
443 | int Options, | ||
444 | template<typename, int> | ||
445 | class JointCollectionTpl, | ||
446 | typename ConfigVectorType> | ||
447 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & | ||
448 | 34 | computeStaticTorque( | |
449 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
450 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
451 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
452 | const container::aligned_vector<ForceTpl<Scalar, Options>> & fext) | ||
453 | { | ||
454 |
1/2✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
|
34 | assert(model.check(data) && "data is not consistent with model."); |
455 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 34 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.
|
34 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
456 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
457 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 34 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.
|
34 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
458 | fext.size(), (size_t)model.njoints, "The size of the external forces is not of right size"); | ||
459 | |||
460 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
461 | typedef typename Model::JointIndex JointIndex; | ||
462 | |||
463 |
1/2✓ Branch 3 taken 34 times.
✗ Branch 4 not taken.
|
34 | data.a_gf[0] = -model.gravity; |
464 | |||
465 | typedef ComputeGeneralizedGravityForwardStep< | ||
466 | Scalar, Options, JointCollectionTpl, ConfigVectorType> | ||
467 | Pass1; | ||
468 |
2/2✓ Branch 0 taken 918 times.
✓ Branch 1 taken 34 times.
|
952 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
469 | { | ||
470 |
1/2✓ Branch 1 taken 918 times.
✗ Branch 2 not taken.
|
918 | Pass1::run( |
471 | 918 | model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); | |
472 | 918 | data.f[i] -= fext[i]; | |
473 | } | ||
474 | |||
475 | typedef ComputeGeneralizedGravityBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
476 |
2/2✓ Branch 0 taken 918 times.
✓ Branch 1 taken 34 times.
|
952 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
477 | { | ||
478 |
1/2✓ Branch 1 taken 918 times.
✗ Branch 2 not taken.
|
918 | Pass2::run( |
479 | 1836 | model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data, data.tau)); | |
480 | } | ||
481 | |||
482 | 34 | return data.tau; | |
483 | } | ||
484 | |||
485 | template< | ||
486 | typename Scalar, | ||
487 | int Options, | ||
488 | template<typename, int> | ||
489 | class JointCollectionTpl, | ||
490 | typename ConfigVectorType, | ||
491 | typename TangentVectorType> | ||
492 | struct CoriolisMatrixForwardStep | ||
493 | : public fusion::JointUnaryVisitorBase<CoriolisMatrixForwardStep< | ||
494 | Scalar, | ||
495 | Options, | ||
496 | JointCollectionTpl, | ||
497 | ConfigVectorType, | ||
498 | TangentVectorType>> | ||
499 | { | ||
500 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
501 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
502 | |||
503 | typedef boost::fusion:: | ||
504 | vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &> | ||
505 | ArgsType; | ||
506 | |||
507 | template<typename JointModel> | ||
508 | 162 | static void algo( | |
509 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
510 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
511 | const Model & model, | ||
512 | Data & data, | ||
513 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
514 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
515 | { | ||
516 | typedef typename Model::JointIndex JointIndex; | ||
517 | |||
518 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | const JointIndex & i = jmodel.id(); |
519 | 162 | const JointIndex & parent = model.parents[i]; | |
520 | |||
521 |
1/2✓ Branch 4 taken 81 times.
✗ Branch 5 not taken.
|
162 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); |
522 | |||
523 |
6/10✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78 times.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 78 times.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 78 times.
✗ Branch 13 not taken.
|
162 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
524 |
2/2✓ Branch 0 taken 78 times.
✓ Branch 1 taken 3 times.
|
162 | if (parent > 0) |
525 |
2/4✓ Branch 3 taken 78 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 78 times.
✗ Branch 8 not taken.
|
156 | data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
526 | else | ||
527 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
6 | data.oMi[i] = data.liMi[i]; |
528 | |||
529 | // express quantities in the world frame | ||
530 |
2/4✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 81 times.
✗ Branch 8 not taken.
|
162 | data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
531 | |||
532 |
2/4✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
|
162 | data.v[i] = jdata.v(); |
533 |
2/2✓ Branch 0 taken 78 times.
✓ Branch 1 taken 3 times.
|
162 | if (parent > 0) |
534 |
2/4✓ Branch 3 taken 78 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 78 times.
✗ Branch 8 not taken.
|
156 | data.v[i] += data.liMi[i].actInv(data.v[parent]); |
535 |
2/4✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 81 times.
✗ Branch 8 not taken.
|
162 | data.ov[i] = data.oMi[i].act(data.v[i]); |
536 |
2/4✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 81 times.
✗ Branch 8 not taken.
|
162 | data.oh[i] = data.oYcrb[i] * data.ov[i]; |
537 | |||
538 | // computes S expressed at the world frame | ||
539 | typedef | ||
540 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
541 | ColsBlock; | ||
542 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock J_cols = jmodel.jointCols(data.J); |
543 |
3/6✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 81 times.
✗ Branch 9 not taken.
|
162 | J_cols = data.oMi[i].act(jdata.S()); // collection of S expressed at the world frame |
544 | |||
545 | // computes vxS expressed at the world frame | ||
546 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
547 |
1/2✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
|
162 | motionSet::motionAction(data.ov[i], J_cols, dJ_cols); |
548 | |||
549 |
2/4✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 81 times.
✗ Branch 7 not taken.
|
162 | data.B[i] = data.oYcrb[i].variation(Scalar(0.5) * data.ov[i]); |
550 |
2/4✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 81 times.
✗ Branch 7 not taken.
|
162 | addForceCrossMatrix(Scalar(0.5) * data.oh[i], data.B[i]); |
551 | } | ||
552 | |||
553 | template<typename ForceDerived, typename M6> | ||
554 | static void | ||
555 | 216 | addForceCrossMatrix(const ForceDense<ForceDerived> & f, const Eigen::MatrixBase<M6> & mout) | |
556 | { | ||
557 | 216 | M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout); | |
558 |
2/4✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
|
216 | addSkew( |
559 |
1/2✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
|
216 | -f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR)); |
560 |
2/4✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
|
216 | addSkew( |
561 |
1/2✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
|
216 | -f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::LINEAR)); |
562 |
2/4✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
|
216 | addSkew( |
563 |
1/2✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
|
216 | -f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR)); |
564 | 216 | } | |
565 | }; | ||
566 | |||
567 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
568 | struct CoriolisMatrixBackwardStep | ||
569 | : public fusion::JointUnaryVisitorBase< | ||
570 | CoriolisMatrixBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
571 | { | ||
572 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
573 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
574 | |||
575 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
576 | |||
577 | template<typename JointModel> | ||
578 | 162 | static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data) | |
579 | { | ||
580 | typedef typename Model::JointIndex JointIndex; | ||
581 | typedef Eigen::Matrix< | ||
582 | Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV, | ||
583 | 6> | ||
584 | MatrixNV6; | ||
585 | |||
586 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | const JointIndex i = jmodel.id(); |
587 | 162 | const JointIndex parent = model.parents[i]; | |
588 | |||
589 |
2/4✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 81 times.
✗ Branch 5 not taken.
|
162 | typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nv(), 6); |
590 | |||
591 | typedef | ||
592 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
593 | ColsBlock; | ||
594 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
595 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock J_cols = jmodel.jointCols(data.J); |
596 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock Ag_cols = jmodel.jointCols(data.Ag); |
597 | |||
598 |
2/4✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
|
162 | motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointCols(data.dFdv)); |
599 |
4/8✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 81 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 81 times.
✗ Branch 12 not taken.
|
162 | jmodel.jointCols(data.dFdv).noalias() += data.B[i] * J_cols; |
600 | |||
601 |
6/12✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 81 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 81 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 81 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 81 times.
✗ Branch 18 not taken.
|
162 | data.C.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = |
602 |
4/8✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 81 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 81 times.
✗ Branch 12 not taken.
|
162 | J_cols.transpose() * data.dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]); |
603 | |||
604 |
1/2✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
|
162 | motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); |
605 |
3/4✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 723 times.
✓ Branch 5 taken 81 times.
|
1608 | for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; |
606 | 1446 | j = data.parents_fromRow[(JointIndex)j]) | |
607 |
6/12✓ Branch 1 taken 723 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 723 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 723 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 723 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 723 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 723 times.
✗ Branch 17 not taken.
|
1446 | data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = |
608 |
3/6✓ Branch 1 taken 723 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 723 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 723 times.
✗ Branch 8 not taken.
|
2892 | Ag_cols.transpose() * data.dJ.col(j); |
609 | |||
610 |
6/12✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 81 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 81 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 81 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 81 times.
✗ Branch 18 not taken.
|
162 | Mat_tmp.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.B[i]; |
611 |
3/4✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 723 times.
✓ Branch 5 taken 81 times.
|
1608 | for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; |
612 | 1446 | j = data.parents_fromRow[(JointIndex)j]) | |
613 |
6/12✓ Branch 1 taken 723 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 723 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 723 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 723 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 723 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 723 times.
✗ Branch 17 not taken.
|
1446 | data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() += |
614 |
2/4✓ Branch 1 taken 723 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 723 times.
✗ Branch 5 not taken.
|
2892 | Mat_tmp * data.J.col(j); |
615 | |||
616 |
2/2✓ Branch 0 taken 78 times.
✓ Branch 1 taken 3 times.
|
162 | if (parent > 0) |
617 | { | ||
618 |
1/2✓ Branch 3 taken 78 times.
✗ Branch 4 not taken.
|
156 | data.oYcrb[parent] += data.oYcrb[i]; |
619 |
1/2✓ Branch 3 taken 78 times.
✗ Branch 4 not taken.
|
156 | data.B[parent] += data.B[i]; |
620 | } | ||
621 | } | ||
622 | }; | ||
623 | |||
624 | template< | ||
625 | typename Scalar, | ||
626 | int Options, | ||
627 | template<typename, int> | ||
628 | class JointCollectionTpl, | ||
629 | typename ConfigVectorType, | ||
630 | typename TangentVectorType> | ||
631 | 3 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & computeCoriolisMatrix( | |
632 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
633 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
634 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
635 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
636 | { | ||
637 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | assert(model.check(data) && "data is not consistent with model."); |
638 |
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(q.size(), model.nq); |
639 |
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.size(), model.nv); |
640 | |||
641 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
642 | typedef typename Model::JointIndex JointIndex; | ||
643 | |||
644 | typedef CoriolisMatrixForwardStep< | ||
645 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType> | ||
646 | Pass1; | ||
647 |
2/2✓ Branch 0 taken 81 times.
✓ Branch 1 taken 3 times.
|
84 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
648 | { | ||
649 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
81 | Pass1::run( |
650 | 81 | model.joints[i], data.joints[i], | |
651 | 162 | typename Pass1::ArgsType(model, data, q.derived(), v.derived())); | |
652 | } | ||
653 | |||
654 | typedef CoriolisMatrixBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
655 |
2/2✓ Branch 0 taken 81 times.
✓ Branch 1 taken 3 times.
|
84 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
656 | { | ||
657 |
1/2✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
|
81 | Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); |
658 | } | ||
659 | |||
660 | 3 | return data.C; | |
661 | } | ||
662 | } // namespace impl | ||
663 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
664 | struct GetCoriolisMatrixBackwardStep | ||
665 | : public fusion::JointUnaryVisitorBase< | ||
666 | GetCoriolisMatrixBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
667 | { | ||
668 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
669 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
670 | |||
671 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
672 | |||
673 | template<typename JointModel> | ||
674 | 54 | static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data) | |
675 | { | ||
676 | typedef typename Model::JointIndex JointIndex; | ||
677 | typedef Eigen::Matrix< | ||
678 | Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV, | ||
679 | 6> | ||
680 | MatrixNV6; | ||
681 | |||
682 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | const JointIndex i = jmodel.id(); |
683 | 54 | const JointIndex parent = model.parents[i]; | |
684 | |||
685 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
54 | typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nv(), 6); |
686 | |||
687 | typedef | ||
688 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
689 | ColsBlock; | ||
690 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
691 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | ColsBlock J_cols = jmodel.jointCols(data.J); |
692 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | ColsBlock Ag_cols = jmodel.jointCols(data.Ag); |
693 | 54 | typename Data::Matrix6x & dFdv = data.Fcrb[0]; | |
694 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | ColsBlock dFdv_cols = jmodel.jointCols(dFdv); |
695 | |||
696 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
54 | motionSet::inertiaAction(data.oYcrb[i], dJ_cols, dFdv_cols); |
697 |
3/6✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
|
54 | dFdv_cols.noalias() += data.B[i] * J_cols; |
698 | |||
699 |
6/12✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 27 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
|
54 | data.C.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = |
700 |
4/8✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
|
54 | J_cols.transpose() * dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]); |
701 | |||
702 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
54 | motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); |
703 |
3/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✓ Branch 5 taken 27 times.
|
536 | for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; |
704 | 482 | j = data.parents_fromRow[(JointIndex)j]) | |
705 |
6/12✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 241 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 241 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 241 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 241 times.
✗ Branch 17 not taken.
|
482 | data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = |
706 |
3/6✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 241 times.
✗ Branch 8 not taken.
|
964 | Ag_cols.transpose() * data.dJ.col(j); |
707 | |||
708 |
6/12✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 27 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
|
54 | Mat_tmp.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.B[i]; |
709 |
3/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✓ Branch 5 taken 27 times.
|
536 | for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; |
710 | 482 | j = data.parents_fromRow[(JointIndex)j]) | |
711 |
7/14✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 241 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 241 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 241 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 241 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 241 times.
✗ Branch 20 not taken.
|
482 | data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j) += Mat_tmp * data.J.col(j); |
712 | |||
713 |
2/2✓ Branch 0 taken 26 times.
✓ Branch 1 taken 1 times.
|
54 | if (parent > 0) |
714 |
1/2✓ Branch 3 taken 26 times.
✗ Branch 4 not taken.
|
52 | data.B[parent] += data.B[i]; |
715 | } | ||
716 | }; | ||
717 | |||
718 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
719 | 1 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & getCoriolisMatrix( | |
720 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
721 | DataTpl<Scalar, Options, JointCollectionTpl> & data) | ||
722 | { | ||
723 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | assert(model.check(data) && "data is not consistent with model."); |
724 | |||
725 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
726 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
727 | typedef typename Model::JointIndex JointIndex; | ||
728 | typedef typename Model::ConfigVectorType ConfigVectorType; | ||
729 | typedef typename Model::TangentVectorType TangentVectorType; | ||
730 | |||
731 | typedef impl::CoriolisMatrixForwardStep< | ||
732 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType> | ||
733 | Pass1; | ||
734 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
28 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
735 | { | ||
736 | typedef typename Data::Inertia Inertia; | ||
737 |
1/2✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
|
27 | const Inertia oY = data.oMi[i].act(model.inertias[i]); |
738 |
2/4✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
|
27 | data.B[i] = oY.variation(Scalar(0.5) * data.ov[i]); |
739 |
2/4✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 27 times.
✗ Branch 7 not taken.
|
27 | Pass1::addForceCrossMatrix(Scalar(0.5) * data.oh[i], data.B[i]); |
740 | } | ||
741 | |||
742 | typedef GetCoriolisMatrixBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
743 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
28 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
744 | { | ||
745 |
1/2✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
|
27 | Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); |
746 | } | ||
747 | |||
748 | 1 | return data.C; | |
749 | } | ||
750 | |||
751 | template< | ||
752 | typename Scalar, | ||
753 | int Options, | ||
754 | template<typename, int> | ||
755 | class JointCollectionTpl, | ||
756 | typename ConfigVectorType, | ||
757 | typename TangentVectorType1, | ||
758 | typename TangentVectorType2> | ||
759 | 4 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & rnea( | |
760 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
761 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
762 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
763 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
764 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
765 | { | ||
766 |
3/6✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
|
4 | return impl::rnea(model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a)); |
767 | } | ||
768 | |||
769 | template< | ||
770 | typename Scalar, | ||
771 | int Options, | ||
772 | template<typename, int> | ||
773 | class JointCollectionTpl, | ||
774 | typename ConfigVectorType, | ||
775 | typename TangentVectorType1, | ||
776 | typename TangentVectorType2, | ||
777 | typename ForceDerived> | ||
778 | 2 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & rnea( | |
779 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
780 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
781 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
782 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
783 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
784 | const container::aligned_vector<ForceDerived> & fext) | ||
785 | { | ||
786 |
3/6✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
|
2 | return impl::rnea(model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), fext); |
787 | } | ||
788 | |||
789 | template< | ||
790 | typename Scalar, | ||
791 | int Options, | ||
792 | template<typename, int> | ||
793 | class JointCollectionTpl, | ||
794 | typename ConfigVectorType, | ||
795 | typename TangentVectorType> | ||
796 | ✗ | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & nonLinearEffects( | |
797 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
798 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
799 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
800 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
801 | { | ||
802 | ✗ | return impl::nonLinearEffects(model, data, make_const_ref(q), make_const_ref(v)); | |
803 | } | ||
804 | |||
805 | template< | ||
806 | typename Scalar, | ||
807 | int Options, | ||
808 | template<typename, int> | ||
809 | class JointCollectionTpl, | ||
810 | typename ConfigVectorType> | ||
811 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & | ||
812 | ✗ | computeGeneralizedGravity( | |
813 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
814 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
815 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
816 | { | ||
817 | ✗ | return impl::computeGeneralizedGravity(model, data, make_const_ref(q)); | |
818 | } | ||
819 | |||
820 | template< | ||
821 | typename Scalar, | ||
822 | int Options, | ||
823 | template<typename, int> | ||
824 | class JointCollectionTpl, | ||
825 | typename ConfigVectorType> | ||
826 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & | ||
827 | ✗ | computeStaticTorque( | |
828 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
829 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
830 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
831 | const container::aligned_vector<ForceTpl<Scalar, Options>> & fext) | ||
832 | { | ||
833 | ✗ | return impl::computeStaticTorque(model, data, make_const_ref(q), fext); | |
834 | } | ||
835 | |||
836 | template< | ||
837 | typename Scalar, | ||
838 | int Options, | ||
839 | template<typename, int> | ||
840 | class JointCollectionTpl, | ||
841 | typename ConfigVectorType, | ||
842 | typename TangentVectorType> | ||
843 | ✗ | const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & computeCoriolisMatrix( | |
844 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
845 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
846 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
847 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
848 | { | ||
849 | ✗ | return impl::computeCoriolisMatrix(model, data, make_const_ref(q), make_const_ref(v)); | |
850 | } | ||
851 | } // namespace pinocchio | ||
852 | |||
853 | /// @endcond | ||
854 | |||
855 | #endif // ifndef __pinocchio_algorithm_rnea_hxx__ | ||
856 |