Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/joint/joint-composite.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 118 | 212 | 55.7% |
Branches: | 86 | 268 | 32.1% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2016-2021 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_multibody_joint_composite_hpp__ | ||
6 | #define __pinocchio_multibody_joint_composite_hpp__ | ||
7 | |||
8 | #include "pinocchio/multibody/joint/fwd.hpp" | ||
9 | #include "pinocchio/multibody/joint/joint-collection.hpp" | ||
10 | #include "pinocchio/multibody/joint/joint-basic-visitors.hpp" | ||
11 | #include "pinocchio/container/aligned-vector.hpp" | ||
12 | #include "pinocchio/spatial/act-on-set.hpp" | ||
13 | |||
14 | #include "pinocchio/serialization/fwd.hpp" | ||
15 | |||
16 | namespace pinocchio | ||
17 | { | ||
18 | |||
19 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
20 | struct JointCompositeTpl; | ||
21 | |||
22 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
23 | struct traits<JointCompositeTpl<_Scalar, _Options, JointCollectionTpl>> | ||
24 | { | ||
25 | typedef _Scalar Scalar; | ||
26 | |||
27 | enum | ||
28 | { | ||
29 | Options = _Options, | ||
30 | NQ = Eigen::Dynamic, | ||
31 | NV = Eigen::Dynamic | ||
32 | }; | ||
33 | |||
34 | typedef JointCollectionTpl<Scalar, Options> JointCollection; | ||
35 | typedef JointDataCompositeTpl<Scalar, Options, JointCollectionTpl> JointDataDerived; | ||
36 | typedef JointModelCompositeTpl<Scalar, Options, JointCollectionTpl> JointModelDerived; | ||
37 | typedef JointMotionSubspaceTpl<Eigen::Dynamic, Scalar, Options> Constraint_t; | ||
38 | typedef SE3Tpl<Scalar, Options> Transformation_t; | ||
39 | typedef MotionTpl<Scalar, Options> Motion_t; | ||
40 | typedef MotionTpl<Scalar, Options> Bias_t; | ||
41 | |||
42 | // [ABA] | ||
43 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> U_t; | ||
44 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> D_t; | ||
45 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> UD_t; | ||
46 | |||
47 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> ConfigVector_t; | ||
48 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> TangentVector_t; | ||
49 | |||
50 | PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE | ||
51 | }; | ||
52 | |||
53 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
54 | struct traits<JointModelCompositeTpl<_Scalar, _Options, JointCollectionTpl>> | ||
55 | { | ||
56 | typedef JointCompositeTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
57 | typedef _Scalar Scalar; | ||
58 | }; | ||
59 | |||
60 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
61 | struct traits<JointDataCompositeTpl<_Scalar, _Options, JointCollectionTpl>> | ||
62 | { | ||
63 | typedef JointCompositeTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
64 | typedef _Scalar Scalar; | ||
65 | }; | ||
66 | |||
67 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
68 | struct JointDataCompositeTpl | ||
69 | : public JointDataBase<JointDataCompositeTpl<_Scalar, _Options, JointCollectionTpl>> | ||
70 | { | ||
71 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
72 | |||
73 | typedef JointDataBase<JointDataCompositeTpl> Base; | ||
74 | typedef JointCompositeTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
75 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
76 | 4 | PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR | |
77 | |||
78 | typedef JointCollectionTpl<Scalar, Options> JointCollection; | ||
79 | typedef JointDataTpl<Scalar, Options, JointCollectionTpl> JointDataVariant; | ||
80 | |||
81 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointDataVariant) JointDataVector; | ||
82 | |||
83 | // JointDataComposite() {} // can become necessary if we want a vector of JointDataComposite ? | ||
84 | |||
85 | 20 | JointDataCompositeTpl() | |
86 | 20 | : joints() | |
87 |
1/2✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
|
20 | , iMlast(0) |
88 |
1/2✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
|
20 | , pjMi(0) |
89 |
2/4✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
|
20 | , joint_q(ConfigVector_t::Zero(0)) |
90 |
2/4✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
|
20 | , joint_v(TangentVector_t::Zero(0)) |
91 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | , S(0) |
92 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | , M(Transformation_t::Identity()) |
93 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | , v(Motion_t::Zero()) |
94 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | , c(Motion_t::Zero()) |
95 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | , U(6, 0) |
96 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | , Dinv(0, 0) |
97 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | , UDinv(6, 0) |
98 |
1/2✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
|
40 | , StU(0, 0) |
99 | { | ||
100 | 20 | } | |
101 | |||
102 | 6 | JointDataCompositeTpl(const JointDataVector & joint_data, const int nq, const int nv) | |
103 | 6 | : joints(joint_data) | |
104 |
1/2✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
6 | , iMlast(joint_data.size()) |
105 |
1/2✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
6 | , pjMi(joint_data.size()) |
106 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | , joint_q(ConfigVector_t::Zero(nq)) |
107 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | , joint_v(TangentVector_t::Zero(nv)) |
108 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | , S(Constraint_t::Zero(nv)) |
109 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | , M(Transformation_t::Identity()) |
110 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | , v(Motion_t::Zero()) |
111 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | , c(Motion_t::Zero()) |
112 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | , U(U_t::Zero(6, nv)) |
113 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | , Dinv(D_t::Zero(nv, nv)) |
114 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | , UDinv(UD_t::Zero(6, nv)) |
115 |
2/4✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
|
12 | , StU(D_t::Zero(nv, nv)) |
116 | { | ||
117 | 6 | } | |
118 | |||
119 | /// \brief Vector of joints | ||
120 | JointDataVector joints; | ||
121 | |||
122 | /// \brief Transforms from previous joint to last joint | ||
123 | PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) iMlast; | ||
124 | |||
125 | /// \brief Transforms from previous joint to joint i | ||
126 | PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) pjMi; | ||
127 | |||
128 | ConfigVector_t joint_q; | ||
129 | TangentVector_t joint_v; | ||
130 | |||
131 | Constraint_t S; | ||
132 | Transformation_t M; | ||
133 | Motion_t v; | ||
134 | Bias_t c; | ||
135 | |||
136 | // // [ABA] specific data | ||
137 | U_t U; | ||
138 | D_t Dinv; | ||
139 | UD_t UDinv; | ||
140 | D_t StU; | ||
141 | |||
142 | 40 | static std::string classname() | |
143 | { | ||
144 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | return std::string("JointDataComposite"); |
145 | } | ||
146 | ✗ | std::string shortname() const | |
147 | { | ||
148 | ✗ | return classname(); | |
149 | } | ||
150 | }; | ||
151 | |||
152 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
153 | ✗ | inline std::ostream & operator<<( | |
154 | std::ostream & os, const JointDataCompositeTpl<Scalar, Options, JointCollectionTpl> & jdata) | ||
155 | { | ||
156 | typedef typename JointDataCompositeTpl<Scalar, Options, JointCollectionTpl>::JointDataVector | ||
157 | JointDataVector; | ||
158 | |||
159 | ✗ | os << "JointDataComposite containing following models:\n"; | |
160 | ✗ | for (typename JointDataVector::const_iterator it = jdata.joints.begin(); | |
161 | ✗ | it != jdata.joints.end(); ++it) | |
162 | ✗ | os << " " << shortname(*it) << std::endl; | |
163 | ✗ | return os; | |
164 | } | ||
165 | |||
166 | template< | ||
167 | typename NewScalar, | ||
168 | typename Scalar, | ||
169 | int Options, | ||
170 | template<typename S, int O> | ||
171 | class JointCollectionTpl> | ||
172 | struct CastType<NewScalar, JointModelCompositeTpl<Scalar, Options, JointCollectionTpl>> | ||
173 | { | ||
174 | typedef JointModelCompositeTpl<NewScalar, Options, JointCollectionTpl> type; | ||
175 | }; | ||
176 | |||
177 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
178 | struct JointModelCompositeTpl | ||
179 | : public JointModelBase<JointModelCompositeTpl<_Scalar, _Options, JointCollectionTpl>> | ||
180 | { | ||
181 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
182 | |||
183 | typedef JointModelBase<JointModelCompositeTpl> Base; | ||
184 | typedef JointCompositeTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
185 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
186 | |||
187 | typedef JointCollectionTpl<Scalar, Options> JointCollection; | ||
188 | typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModelVariant; | ||
189 | |||
190 | typedef SE3Tpl<Scalar, Options> SE3; | ||
191 | typedef MotionTpl<Scalar, Options> Motion; | ||
192 | typedef InertiaTpl<Scalar, Options> Inertia; | ||
193 | |||
194 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModelVariant) JointModelVector; | ||
195 | |||
196 | using Base::id; | ||
197 | using Base::idx_q; | ||
198 | using Base::idx_v; | ||
199 | using Base::nq; | ||
200 | using Base::nv; | ||
201 | using Base::setIndexes; | ||
202 | |||
203 | /// \brief Default contructor | ||
204 | 102 | JointModelCompositeTpl() | |
205 | 102 | : joints() | |
206 |
1/2✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
|
102 | , jointPlacements() |
207 | 102 | , m_nq(0) | |
208 | 102 | , m_nv(0) | |
209 | 204 | , njoints(0) | |
210 | { | ||
211 | 102 | } | |
212 | |||
213 | /// \brief Default contructor with a defined size | ||
214 | ✗ | JointModelCompositeTpl(const size_t size) | |
215 | ✗ | : joints() | |
216 | ✗ | , jointPlacements() | |
217 | ✗ | , m_nq(0) | |
218 | ✗ | , m_nv(0) | |
219 | ✗ | , njoints(0) | |
220 | { | ||
221 | ✗ | joints.reserve(size); | |
222 | ✗ | jointPlacements.reserve(size); | |
223 | ✗ | m_idx_q.reserve(size); | |
224 | ✗ | m_idx_v.reserve(size); | |
225 | ✗ | m_nqs.reserve(size); | |
226 | ✗ | m_nvs.reserve(size); | |
227 | } | ||
228 | |||
229 | /// | ||
230 | /// \brief Constructor with one joint. | ||
231 | /// | ||
232 | /// \param jmodel Model of the first joint. | ||
233 | /// \param placement Placement of the first joint w.r.t. the joint origin. | ||
234 | /// | ||
235 | template<typename JointModel> | ||
236 | 1 | JointModelCompositeTpl( | |
237 | const JointModelBase<JointModel> & jmodel, const SE3 & placement = SE3::Identity()) | ||
238 |
2/4✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
1 | : joints(1, (JointModelVariant)jmodel.derived()) |
239 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | , jointPlacements(1, placement) |
240 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | , m_nq(jmodel.nq()) |
241 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | , m_nv(jmodel.nv()) |
242 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | , m_idx_q(1, 0) |
243 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | , m_nqs(1, jmodel.nq()) |
244 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | , m_idx_v(1, 0) |
245 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | , m_nvs(1, jmodel.nv()) |
246 | 1 | , njoints(1) | |
247 | { | ||
248 | 1 | } | |
249 | |||
250 | /// | ||
251 | /// \brief Copy constructor. | ||
252 | /// | ||
253 | /// \param other JointModel to copy. | ||
254 | /// | ||
255 | 58 | JointModelCompositeTpl(const JointModelCompositeTpl & other) | |
256 | : Base(other) | ||
257 | 58 | , joints(other.joints) | |
258 |
1/2✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
|
58 | , jointPlacements(other.jointPlacements) |
259 | 58 | , m_nq(other.m_nq) | |
260 | 58 | , m_nv(other.m_nv) | |
261 |
1/2✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
|
58 | , m_idx_q(other.m_idx_q) |
262 |
1/2✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
|
58 | , m_nqs(other.m_nqs) |
263 |
1/2✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
|
58 | , m_idx_v(other.m_idx_v) |
264 |
1/2✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
|
58 | , m_nvs(other.m_nvs) |
265 | 58 | , njoints(other.njoints) | |
266 | { | ||
267 | 58 | } | |
268 | |||
269 | /// | ||
270 | /// \brief Add a joint to the vector of joints. | ||
271 | /// | ||
272 | /// \param jmodel Model of the joint to add. | ||
273 | /// \param placement Placement of the joint relatively to its predecessor. | ||
274 | /// | ||
275 | /// \return A reference to *this | ||
276 | /// | ||
277 | template<typename JointModel> | ||
278 | JointModelDerived & | ||
279 | 43 | addJoint(const JointModelBase<JointModel> & jmodel, const SE3 & placement = SE3::Identity()) | |
280 | { | ||
281 |
1/2✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
|
43 | joints.push_back((JointModelVariant)jmodel.derived()); |
282 | 43 | jointPlacements.push_back(placement); | |
283 | |||
284 | 43 | m_nq += jmodel.nq(); | |
285 | 43 | m_nv += jmodel.nv(); | |
286 | |||
287 | 43 | updateJointIndexes(); | |
288 | 43 | njoints++; | |
289 | |||
290 | 43 | return *this; | |
291 | } | ||
292 | |||
293 | 6 | JointDataDerived createData() const | |
294 | { | ||
295 |
1/2✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
6 | typename JointDataDerived::JointDataVector jdata(joints.size()); |
296 |
2/2✓ Branch 1 taken 15 times.
✓ Branch 2 taken 6 times.
|
21 | for (int i = 0; i < (int)joints.size(); ++i) |
297 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
|
15 | jdata[(size_t)i] = |
298 | 15 | ::pinocchio::createData<Scalar, Options, JointCollectionTpl>(joints[(size_t)i]); | |
299 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
12 | return JointDataDerived(jdata, nq(), nv()); |
300 | 6 | } | |
301 | |||
302 | ✗ | const std::vector<bool> hasConfigurationLimit() const | |
303 | { | ||
304 | ✗ | std::vector<bool> vec; | |
305 | ✗ | for (size_t i = 0; i < joints.size(); ++i) | |
306 | { | ||
307 | ✗ | const std::vector<bool> & joint_cf_limit = joints[i].hasConfigurationLimit(); | |
308 | ✗ | vec.insert(vec.end(), joint_cf_limit.begin(), joint_cf_limit.end()); | |
309 | } | ||
310 | ✗ | return vec; | |
311 | } | ||
312 | |||
313 | ✗ | const std::vector<bool> hasConfigurationLimitInTangent() const | |
314 | { | ||
315 | ✗ | std::vector<bool> vec; | |
316 | ✗ | for (size_t i = 0; i < joints.size(); ++i) | |
317 | { | ||
318 | ✗ | const std::vector<bool> & joint_cf_limit = joints[i].hasConfigurationLimitInTangent(); | |
319 | ✗ | vec.insert(vec.end(), joint_cf_limit.begin(), joint_cf_limit.end()); | |
320 | } | ||
321 | ✗ | return vec; | |
322 | } | ||
323 | |||
324 | template<typename, int, template<typename S, int O> class, typename> | ||
325 | friend struct JointCompositeCalcZeroOrderStep; | ||
326 | |||
327 | template<typename ConfigVectorType> | ||
328 | void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const; | ||
329 | |||
330 | template<typename, int, template<typename S, int O> class, typename, typename> | ||
331 | friend struct JointCompositeCalcFirstOrderStep; | ||
332 | |||
333 | template<typename ConfigVectorType, typename TangentVectorType> | ||
334 | void calc( | ||
335 | JointDataDerived & data, | ||
336 | const Eigen::MatrixBase<ConfigVectorType> & qs, | ||
337 | const Eigen::MatrixBase<TangentVectorType> & vs) const; | ||
338 | |||
339 | template<typename TangentVectorType> | ||
340 | void calc( | ||
341 | JointDataDerived & data, | ||
342 | const Blank blank, | ||
343 | const Eigen::MatrixBase<TangentVectorType> & vs) const; | ||
344 | |||
345 | template<typename VectorLike, typename Matrix6Like> | ||
346 | ✗ | void calc_aba( | |
347 | JointDataDerived & data, | ||
348 | const Eigen::MatrixBase<VectorLike> & armature, | ||
349 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
350 | const bool update_I) const | ||
351 | { | ||
352 | ✗ | data.U.noalias() = I * data.S.matrix(); | |
353 | ✗ | data.StU.noalias() = data.S.matrix().transpose() * data.U; | |
354 | ✗ | data.StU.diagonal() += armature; | |
355 | |||
356 | ✗ | internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv); | |
357 | ✗ | data.UDinv.noalias() = data.U * data.Dinv; | |
358 | |||
359 | ✗ | if (update_I) | |
360 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose(); | |
361 | } | ||
362 | |||
363 | 197 | int nv_impl() const | |
364 | { | ||
365 | 197 | return m_nv; | |
366 | } | ||
367 | 105 | int nq_impl() const | |
368 | { | ||
369 | 105 | return m_nq; | |
370 | } | ||
371 | |||
372 | /** | ||
373 | * @brief Update the indexes of subjoints in the stack | ||
374 | */ | ||
375 | 11 | void setIndexes_impl(JointIndex id, int q, int v) | |
376 | { | ||
377 | 11 | Base::setIndexes_impl(id, q, v); | |
378 | 11 | updateJointIndexes(); | |
379 | 11 | } | |
380 | |||
381 | 43 | static std::string classname() | |
382 | { | ||
383 |
1/2✓ Branch 2 taken 43 times.
✗ Branch 3 not taken.
|
43 | return std::string("JointModelComposite"); |
384 | } | ||
385 | 3 | std::string shortname() const | |
386 | { | ||
387 | 3 | return classname(); | |
388 | } | ||
389 | |||
390 | ✗ | JointModelCompositeTpl & operator=(const JointModelCompositeTpl & other) | |
391 | { | ||
392 | ✗ | Base::operator=(other); | |
393 | ✗ | m_nq = other.m_nq; | |
394 | ✗ | m_nv = other.m_nv; | |
395 | ✗ | m_idx_q = other.m_idx_q; | |
396 | ✗ | m_idx_v = other.m_idx_v; | |
397 | ✗ | m_nqs = other.m_nqs; | |
398 | ✗ | m_nvs = other.m_nvs; | |
399 | ✗ | joints = other.joints; | |
400 | ✗ | jointPlacements = other.jointPlacements; | |
401 | ✗ | njoints = other.njoints; | |
402 | |||
403 | ✗ | return *this; | |
404 | } | ||
405 | |||
406 | using Base::isEqual; | ||
407 | 4 | bool isEqual(const JointModelCompositeTpl & other) const | |
408 | { | ||
409 |
5/10✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 4 times.
✗ Branch 13 not taken.
|
8 | return Base::isEqual(other) && internal::comparison_eq(nq(), other.nq()) |
410 |
4/8✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
|
4 | && internal::comparison_eq(nv(), other.nv()) |
411 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | && internal::comparison_eq(m_idx_q, other.m_idx_q) |
412 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | && internal::comparison_eq(m_idx_v, other.m_idx_v) |
413 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | && internal::comparison_eq(m_nqs, other.m_nqs) |
414 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | && internal::comparison_eq(m_nvs, other.m_nvs) |
415 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | && internal::comparison_eq(joints, other.joints) |
416 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | && internal::comparison_eq(jointPlacements, other.jointPlacements) |
417 |
3/6✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
8 | && internal::comparison_eq(njoints, other.njoints); |
418 | } | ||
419 | |||
420 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
421 | template<typename NewScalar> | ||
422 | ✗ | JointModelCompositeTpl<NewScalar, Options, JointCollectionTpl> cast() const | |
423 | { | ||
424 | typedef JointModelCompositeTpl<NewScalar, Options, JointCollectionTpl> ReturnType; | ||
425 | ✗ | ReturnType res((size_t)njoints); | |
426 | ✗ | res.setIndexes(id(), idx_q(), idx_v()); | |
427 | ✗ | res.m_nq = m_nq; | |
428 | ✗ | res.m_nv = m_nv; | |
429 | ✗ | res.m_idx_q = m_idx_q; | |
430 | ✗ | res.m_idx_v = m_idx_v; | |
431 | ✗ | res.m_nqs = m_nqs; | |
432 | ✗ | res.m_nvs = m_nvs; | |
433 | ✗ | res.njoints = njoints; | |
434 | |||
435 | ✗ | res.joints.resize(joints.size()); | |
436 | ✗ | res.jointPlacements.resize(jointPlacements.size()); | |
437 | ✗ | for (size_t k = 0; k < jointPlacements.size(); ++k) | |
438 | { | ||
439 | ✗ | res.joints[k] = joints[k].template cast<NewScalar>(); | |
440 | ✗ | res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>(); | |
441 | } | ||
442 | |||
443 | ✗ | return res; | |
444 | } | ||
445 | |||
446 | /// \brief Vector of joints contained in the joint composite. | ||
447 | JointModelVector joints; | ||
448 | /// \brief Vector of joint placements. Those placements correspond to the origin of the joint | ||
449 | /// relatively to their parent. | ||
450 | PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements; | ||
451 | |||
452 | template<typename D> | ||
453 | typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType | ||
454 | ✗ | jointConfigSelector(const Eigen::MatrixBase<D> & a) const | |
455 | { | ||
456 | ✗ | return a.segment(Base::i_q, nq()); | |
457 | } | ||
458 | template<typename D> | ||
459 | typename SizeDepType<NQ>::template SegmentReturn<D>::Type | ||
460 | ✗ | jointConfigSelector(Eigen::MatrixBase<D> & a) const | |
461 | { | ||
462 | ✗ | return a.segment(Base::i_q, nq()); | |
463 | } | ||
464 | |||
465 | template<typename D> | ||
466 | typename SizeDepType<NV>::template SegmentReturn<D>::ConstType | ||
467 | ✗ | jointVelocitySelector(const Eigen::MatrixBase<D> & a) const | |
468 | { | ||
469 | ✗ | return a.segment(Base::i_v, nv()); | |
470 | } | ||
471 | template<typename D> | ||
472 | typename SizeDepType<NV>::template SegmentReturn<D>::Type | ||
473 | ✗ | jointVelocitySelector(Eigen::MatrixBase<D> & a) const | |
474 | { | ||
475 | ✗ | return a.segment(Base::i_v, nv()); | |
476 | } | ||
477 | |||
478 | template<typename D> | ||
479 | typename SizeDepType<NV>::template ColsReturn<D>::ConstType | ||
480 | ✗ | jointCols(const Eigen::MatrixBase<D> & A) const | |
481 | { | ||
482 | ✗ | return A.middleCols(Base::i_v, nv()); | |
483 | } | ||
484 | template<typename D> | ||
485 | ✗ | typename SizeDepType<NV>::template ColsReturn<D>::Type jointCols(Eigen::MatrixBase<D> & A) const | |
486 | { | ||
487 | ✗ | return A.middleCols(Base::i_v, nv()); | |
488 | } | ||
489 | |||
490 | template<typename D> | ||
491 | typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType | ||
492 | ✗ | jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const | |
493 | { | ||
494 | ✗ | return a.segment(Base::i_q, nq()); | |
495 | } | ||
496 | template<typename D> | ||
497 | typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type | ||
498 | jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const | ||
499 | { | ||
500 | return a.segment(Base::i_q, nq()); | ||
501 | } | ||
502 | template<typename D> | ||
503 | typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType | ||
504 | 3 | jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const | |
505 | { | ||
506 | 3 | return a.segment(Base::i_v, nv()); | |
507 | } | ||
508 | template<typename D> | ||
509 | typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type | ||
510 | ✗ | jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const | |
511 | { | ||
512 | ✗ | return a.segment(Base::i_v, nv()); | |
513 | } | ||
514 | |||
515 | template<typename D> | ||
516 | typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType | ||
517 | ✗ | jointCols_impl(const Eigen::MatrixBase<D> & A) const | |
518 | { | ||
519 | ✗ | return A.middleCols(Base::i_v, nv()); | |
520 | } | ||
521 | template<typename D> | ||
522 | typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type | ||
523 | ✗ | jointCols_impl(Eigen::MatrixBase<D> & A) const | |
524 | { | ||
525 | ✗ | return A.middleCols(Base::i_v, nv()); | |
526 | } | ||
527 | |||
528 | protected: | ||
529 | friend struct Serialize<JointModelCompositeTpl>; | ||
530 | |||
531 | template<typename, int, template<typename, int> class> | ||
532 | friend struct JointModelCompositeTpl; | ||
533 | |||
534 | /// \brief Update the indexes of the joints contained in the composition according | ||
535 | /// to the position of the joint composite. | ||
536 | 33 | void updateJointIndexes() | |
537 | { | ||
538 | 33 | int idx_q = this->idx_q(); | |
539 | 33 | int idx_v = this->idx_v(); | |
540 | |||
541 | 33 | m_idx_q.resize(joints.size()); | |
542 | 33 | m_idx_v.resize(joints.size()); | |
543 | 33 | m_nqs.resize(joints.size()); | |
544 | 33 | m_nvs.resize(joints.size()); | |
545 | |||
546 |
2/2✓ Branch 1 taken 58 times.
✓ Branch 2 taken 33 times.
|
91 | for (size_t i = 0; i < joints.size(); ++i) |
547 | { | ||
548 | 58 | JointModelVariant & joint = joints[i]; | |
549 | |||
550 | 58 | m_idx_q[i] = idx_q; | |
551 | 58 | m_idx_v[i] = idx_v; | |
552 | 58 | ::pinocchio::setIndexes(joint, i, idx_q, idx_v); | |
553 | 58 | m_nqs[i] = ::pinocchio::nq(joint); | |
554 | 58 | m_nvs[i] = ::pinocchio::nv(joint); | |
555 | 58 | idx_q += m_nqs[i]; | |
556 | 58 | idx_v += m_nvs[i]; | |
557 | } | ||
558 | 33 | } | |
559 | |||
560 | /// \brief Dimensions of the config and tangent space of the composite joint. | ||
561 | int m_nq, m_nv; | ||
562 | |||
563 | /// Keep information of both the dimension and the position of the joints in the composition. | ||
564 | |||
565 | /// \brief Index in the config vector | ||
566 | std::vector<int> m_idx_q; | ||
567 | /// \brief Dimension of the segment in the config vector | ||
568 | std::vector<int> m_nqs; | ||
569 | /// \brief Index in the tangent vector | ||
570 | std::vector<int> m_idx_v; | ||
571 | /// \brief Dimension of the segment in the tangent vector | ||
572 | std::vector<int> m_nvs; | ||
573 | |||
574 | public: | ||
575 | /// \brief Number of joints contained in the JointModelComposite | ||
576 | int njoints; | ||
577 | }; | ||
578 | |||
579 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
580 | ✗ | inline std::ostream & operator<<( | |
581 | std::ostream & os, const JointModelCompositeTpl<Scalar, Options, JointCollectionTpl> & jmodel) | ||
582 | { | ||
583 | typedef typename JointModelCompositeTpl<Scalar, Options, JointCollectionTpl>::JointModelVector | ||
584 | JointModelVector; | ||
585 | |||
586 | ✗ | os << "JointModelComposite containing following models:\n"; | |
587 | ✗ | for (typename JointModelVector::const_iterator it = jmodel.joints.begin(); | |
588 | ✗ | it != jmodel.joints.end(); ++it) | |
589 | ✗ | os << " " << shortname(*it) << std::endl; | |
590 | |||
591 | ✗ | return os; | |
592 | } | ||
593 | |||
594 | } // namespace pinocchio | ||
595 | |||
596 | #include <boost/type_traits.hpp> | ||
597 | |||
598 | namespace boost | ||
599 | { | ||
600 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
601 | struct has_nothrow_constructor< | ||
602 | ::pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollectionTpl>> | ||
603 | : public integral_constant<bool, true> | ||
604 | { | ||
605 | }; | ||
606 | |||
607 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
608 | struct has_nothrow_copy<::pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollectionTpl>> | ||
609 | : public integral_constant<bool, true> | ||
610 | { | ||
611 | }; | ||
612 | |||
613 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
614 | struct has_nothrow_constructor< | ||
615 | ::pinocchio::JointDataCompositeTpl<Scalar, Options, JointCollectionTpl>> | ||
616 | : public integral_constant<bool, true> | ||
617 | { | ||
618 | }; | ||
619 | |||
620 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
621 | struct has_nothrow_copy<::pinocchio::JointDataCompositeTpl<Scalar, Options, JointCollectionTpl>> | ||
622 | : public integral_constant<bool, true> | ||
623 | { | ||
624 | }; | ||
625 | } // namespace boost | ||
626 | |||
627 | /* --- Details -------------------------------------------------------------- */ | ||
628 | /* --- Details -------------------------------------------------------------- */ | ||
629 | /* --- Details -------------------------------------------------------------- */ | ||
630 | #include "pinocchio/multibody/joint/joint-composite.hxx" | ||
631 | |||
632 | #endif // ifndef __pinocchio_multibody_joint_composite_hpp__ | ||
633 |