GCC Code Coverage Report

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