Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/centroidal.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 164 | 176 | 93.2% |
Branches: | 208 | 552 | 37.7% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2023 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_centroidal_hxx__ | ||
6 | #define __pinocchio_algorithm_centroidal_hxx__ | ||
7 | |||
8 | #include "pinocchio/multibody/visitor.hpp" | ||
9 | #include "pinocchio/spatial/act-on-set.hpp" | ||
10 | #include "pinocchio/algorithm/kinematics.hpp" | ||
11 | #include "pinocchio/algorithm/check.hpp" | ||
12 | |||
13 | /// @cond DEV | ||
14 | |||
15 | namespace pinocchio | ||
16 | { | ||
17 | |||
18 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
19 | 4 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force & computeCentroidalMomentum( | |
20 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
21 | DataTpl<Scalar, Options, JointCollectionTpl> & data) | ||
22 | { | ||
23 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | assert(model.check(data) && "data is not consistent with model."); |
24 | |||
25 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
26 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
27 | typedef typename Model::JointIndex JointIndex; | ||
28 | |||
29 |
2/2✓ Branch 0 taken 112 times.
✓ Branch 1 taken 4 times.
|
116 | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) |
30 | { | ||
31 | 112 | const typename Data::Inertia & Y = model.inertias[i]; | |
32 | |||
33 | 112 | data.mass[i] = Y.mass(); | |
34 |
2/4✓ Branch 3 taken 112 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 112 times.
✗ Branch 8 not taken.
|
112 | data.com[i] = Y.mass() * Y.lever(); |
35 | |||
36 |
1/2✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
|
112 | data.h[i] = Y * data.v[i]; |
37 | } | ||
38 | |||
39 | 4 | data.mass[0] = 0.; | |
40 | 4 | data.com[0].setZero(); | |
41 | 4 | data.h[0].setZero(); | |
42 |
2/2✓ Branch 0 taken 112 times.
✓ Branch 1 taken 4 times.
|
116 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
43 | { | ||
44 | 112 | const JointIndex & parent = model.parents[i]; | |
45 | 112 | const typename Data::SE3 & liMi = data.liMi[i]; | |
46 | |||
47 | 112 | data.mass[parent] += data.mass[i]; | |
48 |
4/8✓ Branch 5 taken 112 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 112 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 112 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 112 times.
✗ Branch 16 not taken.
|
112 | data.com[parent] += liMi.rotation() * data.com[i] + data.mass[i] * liMi.translation(); |
49 |
1/2✓ Branch 5 taken 112 times.
✗ Branch 6 not taken.
|
112 | data.h[parent] += data.liMi[i].act(data.h[i]); |
50 | } | ||
51 | |||
52 | 4 | data.com[0] /= data.mass[0]; | |
53 | |||
54 | 4 | data.hg = data.h[0]; | |
55 |
3/6✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
|
4 | data.hg.angular() += data.hg.linear().cross(data.com[0]); |
56 | |||
57 |
3/6✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
|
4 | data.vcom[0].noalias() = data.hg.linear() / data.mass[0]; |
58 | |||
59 | 4 | return data.hg; | |
60 | } | ||
61 | |||
62 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
63 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force & | ||
64 | 109 | computeCentroidalMomentumTimeVariation( | |
65 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
66 | DataTpl<Scalar, Options, JointCollectionTpl> & data) | ||
67 | { | ||
68 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
109 | assert(model.check(data) && "data is not consistent with model."); |
69 | |||
70 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
71 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
72 | typedef typename Model::JointIndex JointIndex; | ||
73 | |||
74 |
2/2✓ Branch 0 taken 3052 times.
✓ Branch 1 taken 109 times.
|
3161 | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) |
75 | { | ||
76 | 3052 | const typename Data::Inertia & Y = model.inertias[i]; | |
77 | |||
78 | 3052 | data.mass[i] = Y.mass(); | |
79 |
2/4✓ Branch 3 taken 3052 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 3052 times.
✗ Branch 8 not taken.
|
3052 | data.com[i] = Y.mass() * Y.lever(); |
80 | |||
81 |
1/2✓ Branch 4 taken 3052 times.
✗ Branch 5 not taken.
|
3052 | data.h[i] = Y * data.v[i]; |
82 |
3/6✓ Branch 5 taken 3052 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3052 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 3052 times.
✗ Branch 13 not taken.
|
3052 | data.f[i] = Y * data.a[i] + data.v[i].cross(data.h[i]); |
83 | } | ||
84 | |||
85 | 109 | data.mass[0] = 0.; | |
86 | 109 | data.com[0].setZero(); | |
87 | 109 | data.h[0].setZero(); | |
88 | 109 | data.f[0].setZero(); | |
89 | |||
90 |
2/2✓ Branch 0 taken 3052 times.
✓ Branch 1 taken 109 times.
|
3161 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
91 | { | ||
92 | 3052 | const JointIndex & parent = model.parents[i]; | |
93 | 3052 | const typename Data::SE3 & liMi = data.liMi[i]; | |
94 | |||
95 | 3052 | data.mass[parent] += data.mass[i]; | |
96 |
4/8✓ Branch 5 taken 3052 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3052 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3052 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3052 times.
✗ Branch 16 not taken.
|
3052 | data.com[parent] += liMi.rotation() * data.com[i] + data.mass[i] * liMi.translation(); |
97 |
1/2✓ Branch 5 taken 3052 times.
✗ Branch 6 not taken.
|
3052 | data.h[parent] += data.liMi[i].act(data.h[i]); |
98 |
1/2✓ Branch 5 taken 3052 times.
✗ Branch 6 not taken.
|
3052 | data.f[parent] += data.liMi[i].act(data.f[i]); |
99 | } | ||
100 | |||
101 | 109 | data.com[0] /= data.mass[0]; | |
102 | |||
103 | 109 | data.hg = data.h[0]; | |
104 |
3/6✓ Branch 3 taken 109 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 109 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 109 times.
✗ Branch 10 not taken.
|
109 | data.hg.angular() += data.hg.linear().cross(data.com[0]); |
105 | |||
106 | 109 | data.dhg = data.f[0]; | |
107 |
3/6✓ Branch 3 taken 109 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 109 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 109 times.
✗ Branch 10 not taken.
|
109 | data.dhg.angular() += data.dhg.linear().cross(data.com[0]); |
108 | |||
109 |
3/6✓ Branch 3 taken 109 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 109 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 109 times.
✗ Branch 11 not taken.
|
109 | data.vcom[0].noalias() = data.hg.linear() / data.mass[0]; |
110 | |||
111 | 109 | return data.dhg; | |
112 | } | ||
113 | namespace impl | ||
114 | { | ||
115 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
116 | struct CcrbaBackwardStep | ||
117 | : public fusion::JointUnaryVisitorBase<CcrbaBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
118 | { | ||
119 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
120 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
121 | |||
122 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
123 | |||
124 | template<typename JointModel> | ||
125 | 2896 | static void algo( | |
126 | const JointModelBase<JointModel> & jmodel, | ||
127 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
128 | const Model & model, | ||
129 | Data & data) | ||
130 | { | ||
131 | typedef typename Model::JointIndex JointIndex; | ||
132 | typedef | ||
133 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
134 | ColsBlock; | ||
135 | |||
136 |
1/2✓ Branch 1 taken 1448 times.
✗ Branch 2 not taken.
|
2896 | const JointIndex & i = jmodel.id(); |
137 | 2896 | const JointIndex & parent = model.parents[i]; | |
138 | |||
139 |
1/2✓ Branch 1 taken 1448 times.
✗ Branch 2 not taken.
|
2896 | ColsBlock J_cols = jmodel.jointCols(data.J); |
140 |
3/6✓ Branch 2 taken 1448 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1448 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1448 times.
✗ Branch 9 not taken.
|
2896 | J_cols = data.oMi[i].act(jdata.S()); |
141 | |||
142 |
1/2✓ Branch 1 taken 1448 times.
✗ Branch 2 not taken.
|
2896 | ColsBlock Ag_cols = jmodel.jointCols(data.Ag); |
143 |
1/2✓ Branch 2 taken 1448 times.
✗ Branch 3 not taken.
|
2896 | motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); |
144 |
1/2✓ Branch 3 taken 1448 times.
✗ Branch 4 not taken.
|
2896 | data.oYcrb[parent] += data.oYcrb[i]; |
145 | } | ||
146 | |||
147 | }; // struct CcrbaBackwardStep | ||
148 | |||
149 | template< | ||
150 | typename Scalar, | ||
151 | int Options, | ||
152 | template<typename, int> | ||
153 | class JointCollectionTpl, | ||
154 | typename ConfigVectorType, | ||
155 | typename TangentVectorType> | ||
156 | 50 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & ccrba( | |
157 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
158 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
159 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
160 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
161 | { | ||
162 |
2/4✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
|
50 | assert(model.check(data) && "data is not consistent with model."); |
163 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 50 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.
|
50 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
164 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
165 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 50 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.
|
50 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
166 | |||
167 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
168 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
169 | typedef typename Model::JointIndex JointIndex; | ||
170 | |||
171 |
1/2✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
|
50 | ::pinocchio::impl::forwardKinematics(model, data, q); |
172 |
1/2✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
|
50 | data.oYcrb[0].setZero(); |
173 |
2/2✓ Branch 0 taken 1393 times.
✓ Branch 1 taken 50 times.
|
1443 | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) |
174 |
2/4✓ Branch 3 taken 1393 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1393 times.
✗ Branch 8 not taken.
|
1393 | data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
175 | |||
176 | typedef CcrbaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
177 |
2/2✓ Branch 0 taken 1393 times.
✓ Branch 1 taken 50 times.
|
1443 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
178 | { | ||
179 |
2/4✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 1393 times.
✗ Branch 7 not taken.
|
1393 | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); |
180 | } | ||
181 | |||
182 | // Express the centroidal map around the center of mass | ||
183 |
1/2✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
|
50 | data.com[0] = data.oYcrb[0].lever(); |
184 | |||
185 | typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x; | ||
186 |
1/2✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
|
50 | const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR); |
187 |
1/2✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
|
50 | Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR); |
188 |
2/2✓ Branch 0 taken 1729 times.
✓ Branch 1 taken 50 times.
|
1779 | for (Eigen::DenseIndex i = 0; i < model.nv; ++i) |
189 |
4/8✓ Branch 1 taken 1729 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1729 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1729 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1729 times.
✗ Branch 12 not taken.
|
1729 | Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]); |
190 | |||
191 |
4/8✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
|
50 | data.hg.toVector().noalias() = data.Ag * v; |
192 | |||
193 | 50 | data.Ig.mass() = data.oYcrb[0].mass(); | |
194 |
1/2✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
|
50 | data.Ig.lever().setZero(); |
195 |
1/2✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
|
50 | data.Ig.inertia() = data.oYcrb[0].inertia(); |
196 | |||
197 | 50 | return data.Ag; | |
198 | } | ||
199 | |||
200 | template< | ||
201 | typename Scalar, | ||
202 | int Options, | ||
203 | template<typename, int> | ||
204 | class JointCollectionTpl, | ||
205 | typename ConfigVectorType> | ||
206 | 2 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeCentroidalMap( | |
207 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
208 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
209 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
210 | { | ||
211 |
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."); |
212 |
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( |
213 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
214 | |||
215 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
216 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
217 | typedef typename Model::JointIndex JointIndex; | ||
218 | |||
219 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | ::pinocchio::impl::forwardKinematics(model, data, q); |
220 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | data.oYcrb[0].setZero(); |
221 |
2/2✓ Branch 0 taken 55 times.
✓ Branch 1 taken 2 times.
|
57 | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) |
222 |
2/4✓ Branch 3 taken 55 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 55 times.
✗ Branch 8 not taken.
|
55 | data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
223 | |||
224 | typedef CcrbaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
225 |
2/2✓ Branch 0 taken 55 times.
✓ Branch 1 taken 2 times.
|
57 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
226 | { | ||
227 |
2/4✓ Branch 1 taken 55 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 55 times.
✗ Branch 7 not taken.
|
55 | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); |
228 | } | ||
229 | |||
230 | // Express the centroidal map around the center of mass | ||
231 |
1/2✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | data.com[0] = data.oYcrb[0].lever(); |
232 | |||
233 | typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x; | ||
234 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR); |
235 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR); |
236 |
2/2✓ Branch 0 taken 67 times.
✓ Branch 1 taken 2 times.
|
69 | for (long i = 0; i < model.nv; ++i) |
237 |
4/8✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 67 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 67 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 67 times.
✗ Branch 12 not taken.
|
67 | Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]); |
238 | |||
239 | 2 | return data.Ag; | |
240 | } | ||
241 | |||
242 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
243 | struct DCcrbaBackwardStep | ||
244 | : public fusion::JointUnaryVisitorBase<DCcrbaBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
245 | { | ||
246 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
247 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
248 | |||
249 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
250 | |||
251 | template<typename JointModel> | ||
252 | 714 | static void algo( | |
253 | const JointModelBase<JointModel> & jmodel, | ||
254 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
255 | const Model & model, | ||
256 | Data & data) | ||
257 | { | ||
258 | typedef typename Model::JointIndex JointIndex; | ||
259 | typedef typename Data::Inertia Inertia; | ||
260 | typedef | ||
261 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
262 | ColsBlock; | ||
263 | |||
264 |
1/2✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
|
714 | const JointIndex & i = jmodel.id(); |
265 | 714 | const JointIndex & parent = model.parents[i]; | |
266 | 714 | const Inertia & Y = data.oYcrb[i]; | |
267 | 714 | const typename Inertia::Matrix6 & doYcrb = data.doYcrb[i]; | |
268 | |||
269 |
1/2✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
|
714 | ColsBlock J_cols = jmodel.jointCols(data.J); |
270 |
3/6✓ Branch 2 taken 357 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 357 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 357 times.
✗ Branch 9 not taken.
|
714 | J_cols = data.oMi[i].act(jdata.S()); |
271 | |||
272 |
1/2✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
|
714 | ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
273 |
1/2✓ Branch 2 taken 357 times.
✗ Branch 3 not taken.
|
714 | motionSet::motionAction(data.ov[i], J_cols, dJ_cols); |
274 | |||
275 |
1/2✓ Branch 2 taken 357 times.
✗ Branch 3 not taken.
|
714 | data.oYcrb[parent] += Y; |
276 |
2/2✓ Branch 0 taken 344 times.
✓ Branch 1 taken 13 times.
|
714 | if (parent > 0) |
277 |
1/2✓ Branch 2 taken 344 times.
✗ Branch 3 not taken.
|
688 | data.doYcrb[parent] += doYcrb; |
278 | |||
279 | // Calc Ag | ||
280 |
1/2✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
|
714 | ColsBlock Ag_cols = jmodel.jointCols(data.Ag); |
281 |
1/2✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
|
714 | motionSet::inertiaAction(Y, J_cols, Ag_cols); |
282 | |||
283 | // Calc dAg = Ivx + vxI | ||
284 |
1/2✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
|
714 | ColsBlock dAg_cols = jmodel.jointCols(data.dAg); |
285 |
3/6✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 357 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 357 times.
✗ Branch 8 not taken.
|
714 | dAg_cols.noalias() = doYcrb * J_cols; |
286 |
1/2✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
|
714 | motionSet::inertiaAction<ADDTO>(Y, dJ_cols, dAg_cols); |
287 | } | ||
288 | |||
289 | }; // struct DCcrbaBackwardStep | ||
290 | |||
291 | template< | ||
292 | typename Scalar, | ||
293 | int Options, | ||
294 | template<typename, int> | ||
295 | class JointCollectionTpl, | ||
296 | typename ConfigVectorType, | ||
297 | typename TangentVectorType> | ||
298 | 12 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & dccrba( | |
299 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
300 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
301 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
302 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
303 | { | ||
304 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
|
12 | assert(model.check(data) && "data is not consistent with model."); |
305 |
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( |
306 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
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(v.size(), model.nv, "The velocity vector is not of right size"); |
308 | |||
309 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
310 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
311 | typedef typename Model::JointIndex JointIndex; | ||
312 | |||
313 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | ::pinocchio::impl::forwardKinematics(model, data, q, v); |
314 |
1/2✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
|
12 | data.oYcrb[0].setZero(); |
315 |
2/2✓ Branch 0 taken 330 times.
✓ Branch 1 taken 12 times.
|
342 | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) |
316 | { | ||
317 |
2/4✓ Branch 3 taken 330 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 330 times.
✗ Branch 8 not taken.
|
330 | data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
318 |
2/4✓ Branch 3 taken 330 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 330 times.
✗ Branch 8 not taken.
|
330 | data.ov[i] = data.oMi[i].act(data.v[i]); // v_i expressed in the world frame |
319 |
1/2✓ Branch 3 taken 330 times.
✗ Branch 4 not taken.
|
330 | data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]); |
320 | } | ||
321 | |||
322 | typedef DCcrbaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
323 |
2/2✓ Branch 0 taken 330 times.
✓ Branch 1 taken 12 times.
|
342 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
324 | { | ||
325 |
2/4✓ Branch 1 taken 330 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 330 times.
✗ Branch 7 not taken.
|
330 | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); |
326 | } | ||
327 | |||
328 | // Express the centroidal map around the center of mass | ||
329 |
1/2✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
12 | data.com[0] = data.oYcrb[0].lever(); |
330 | |||
331 | typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x; | ||
332 | |||
333 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR); |
334 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR); |
335 |
2/2✓ Branch 0 taken 402 times.
✓ Branch 1 taken 12 times.
|
414 | for (Eigen::DenseIndex i = 0; i < model.nv; ++i) |
336 |
4/8✓ Branch 1 taken 402 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 402 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 402 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 402 times.
✗ Branch 12 not taken.
|
402 | Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]); |
337 | |||
338 |
4/8✓ 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.
|
12 | data.hg.toVector().noalias() = data.Ag * v; |
339 |
4/8✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
|
12 | data.vcom[0].noalias() = data.hg.linear() / data.oYcrb[0].mass(); |
340 | |||
341 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | const Block3x dAg_lin = data.dAg.template middleRows<3>(Force::LINEAR); |
342 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | Block3x dAg_ang = data.dAg.template middleRows<3>(Force::ANGULAR); |
343 |
2/2✓ Branch 0 taken 402 times.
✓ Branch 1 taken 12 times.
|
414 | for (Eigen::DenseIndex i = 0; i < model.nv; ++i) |
344 |
7/14✓ Branch 1 taken 402 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 402 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 402 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 402 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 402 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 402 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 402 times.
✗ Branch 22 not taken.
|
402 | dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]) + Ag_lin.col(i).cross(data.vcom[0]); |
345 | |||
346 | 12 | data.Ig.mass() = data.oYcrb[0].mass(); | |
347 |
1/2✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
|
12 | data.Ig.lever().setZero(); |
348 |
1/2✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
12 | data.Ig.inertia() = data.oYcrb[0].inertia(); |
349 | |||
350 | 12 | return data.dAg; | |
351 | } | ||
352 | |||
353 | template< | ||
354 | typename Scalar, | ||
355 | int Options, | ||
356 | template<typename, int> | ||
357 | class JointCollectionTpl, | ||
358 | typename ConfigVectorType, | ||
359 | typename TangentVectorType> | ||
360 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & | ||
361 | 1 | computeCentroidalMapTimeVariation( | |
362 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
363 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
364 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
365 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
366 | { | ||
367 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | assert(model.check(data) && "data is not consistent with model."); |
368 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
|
1 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
369 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
370 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
|
1 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
371 | |||
372 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
373 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
374 | typedef typename Model::JointIndex JointIndex; | ||
375 | |||
376 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | ::pinocchio::impl::forwardKinematics(model, data, q, v); |
377 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | data.oYcrb[0].setZero(); |
378 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
28 | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) |
379 | { | ||
380 |
2/4✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
|
27 | data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
381 |
2/4✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
|
27 | data.ov[i] = data.oMi[i].act(data.v[i]); // v_i expressed in the world frame |
382 |
1/2✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
|
27 | data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]); |
383 | } | ||
384 | |||
385 | typedef DCcrbaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
386 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
28 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
387 | { | ||
388 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 27 times.
✗ Branch 7 not taken.
|
27 | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); |
389 | } | ||
390 | |||
391 | // Express the centroidal map around the center of mass | ||
392 |
1/2✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | data.com[0] = data.oYcrb[0].lever(); |
393 | |||
394 | typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x; | ||
395 | |||
396 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR); |
397 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR); |
398 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
33 | for (Eigen::DenseIndex i = 0; i < model.nv; ++i) |
399 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
|
32 | Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]); |
400 | |||
401 | // Express the centroidal time derivative map around the center of mass | ||
402 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | const Block3x dAg_lin = data.dAg.template middleRows<3>(Force::LINEAR); |
403 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Block3x dAg_ang = data.dAg.template middleRows<3>(Force::ANGULAR); |
404 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
33 | for (Eigen::DenseIndex i = 0; i < model.nv; ++i) |
405 | { | ||
406 |
7/14✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 32 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 32 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
|
32 | dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]) + Ag_lin.col(i).cross(data.vcom[0]); |
407 | } | ||
408 | |||
409 | 1 | return data.dAg; | |
410 | } | ||
411 | |||
412 | template< | ||
413 | typename Scalar, | ||
414 | int Options, | ||
415 | template<typename, int> | ||
416 | class JointCollectionTpl, | ||
417 | typename ConfigVectorType, | ||
418 | typename TangentVectorType> | ||
419 | 3 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force & computeCentroidalMomentum( | |
420 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
421 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
422 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
423 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
424 | { | ||
425 | 3 | ::pinocchio::impl::forwardKinematics(model, data, q.derived(), v.derived()); | |
426 | 3 | return ::pinocchio::computeCentroidalMomentum(model, data); | |
427 | } | ||
428 | |||
429 | template< | ||
430 | typename Scalar, | ||
431 | int Options, | ||
432 | template<typename, int> | ||
433 | class JointCollectionTpl, | ||
434 | typename ConfigVectorType, | ||
435 | typename TangentVectorType1, | ||
436 | typename TangentVectorType2> | ||
437 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force & | ||
438 | 108 | computeCentroidalMomentumTimeVariation( | |
439 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
440 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
441 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
442 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
443 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
444 | { | ||
445 | 108 | ::pinocchio::impl::forwardKinematics(model, data, q, v, a); | |
446 | 108 | return computeCentroidalMomentumTimeVariation(model, data); | |
447 | } | ||
448 | } // namespace impl | ||
449 | |||
450 | template< | ||
451 | typename Scalar, | ||
452 | int Options, | ||
453 | template<typename, int> | ||
454 | class JointCollectionTpl, | ||
455 | typename ConfigVectorType, | ||
456 | typename TangentVectorType> | ||
457 | ✗ | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force & computeCentroidalMomentum( | |
458 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
459 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
460 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
461 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
462 | { | ||
463 | ✗ | return impl::computeCentroidalMomentum(model, data, make_const_ref(q), make_const_ref(v)); | |
464 | } | ||
465 | |||
466 | template< | ||
467 | typename Scalar, | ||
468 | int Options, | ||
469 | template<typename, int> | ||
470 | class JointCollectionTpl, | ||
471 | typename ConfigVectorType, | ||
472 | typename TangentVectorType1, | ||
473 | typename TangentVectorType2> | ||
474 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force & | ||
475 | ✗ | computeCentroidalMomentumTimeVariation( | |
476 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
477 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
478 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
479 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
480 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
481 | { | ||
482 | ✗ | return impl::computeCentroidalMomentumTimeVariation( | |
483 | ✗ | model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a)); | |
484 | } | ||
485 | |||
486 | template< | ||
487 | typename Scalar, | ||
488 | int Options, | ||
489 | template<typename, int> | ||
490 | class JointCollectionTpl, | ||
491 | typename ConfigVectorType, | ||
492 | typename TangentVectorType> | ||
493 | 4 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & ccrba( | |
494 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
495 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
496 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
497 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
498 | { | ||
499 |
2/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
4 | return impl::ccrba(model, data, make_const_ref(q), make_const_ref(v)); |
500 | } | ||
501 | |||
502 | template< | ||
503 | typename Scalar, | ||
504 | int Options, | ||
505 | template<typename, int> | ||
506 | class JointCollectionTpl, | ||
507 | typename ConfigVectorType> | ||
508 | ✗ | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeCentroidalMap( | |
509 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
510 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
511 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
512 | { | ||
513 | ✗ | return impl::computeCentroidalMap(model, data, make_const_ref(q)); | |
514 | } | ||
515 | |||
516 | template< | ||
517 | typename Scalar, | ||
518 | int Options, | ||
519 | template<typename, int> | ||
520 | class JointCollectionTpl, | ||
521 | typename ConfigVectorType, | ||
522 | typename TangentVectorType> | ||
523 | ✗ | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & dccrba( | |
524 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
525 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
526 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
527 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
528 | { | ||
529 | ✗ | return impl::dccrba(model, data, make_const_ref(q), make_const_ref(v)); | |
530 | } | ||
531 | |||
532 | template< | ||
533 | typename Scalar, | ||
534 | int Options, | ||
535 | template<typename, int> | ||
536 | class JointCollectionTpl, | ||
537 | typename ConfigVectorType, | ||
538 | typename TangentVectorType> | ||
539 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & | ||
540 | ✗ | computeCentroidalMapTimeVariation( | |
541 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
542 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
543 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
544 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
545 | { | ||
546 | ✗ | return impl::computeCentroidalMapTimeVariation( | |
547 | ✗ | model, data, make_const_ref(q), make_const_ref(v)); | |
548 | } | ||
549 | |||
550 | } // namespace pinocchio | ||
551 | |||
552 | /// @endcond | ||
553 | |||
554 | #endif // ifndef __pinocchio_algorithm_centroidal_hxx__ | ||
555 |