GCC Code Coverage Report

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