GCC Code Coverage Report

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