GCC Code Coverage Report

Directory: ./
File: src/joint.cc
Date: 2025-05-04 12:09:19
Exec Total Coverage
Lines: 121 281 43.1%
Branches: 82 299 27.4%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016 CNRS
3 // Author: NMansard
4 //
5 //
6
7 // Redistribution and use in source and binary forms, with or without
8 // modification, are permitted provided that the following conditions are
9 // met:
10 //
11 // 1. Redistributions of source code must retain the above copyright
12 // notice, this list of conditions and the following disclaimer.
13 //
14 // 2. Redistributions in binary form must reproduce the above copyright
15 // notice, this list of conditions and the following disclaimer in the
16 // documentation and/or other materials provided with the distribution.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
21 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
22 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
23 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
24 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
29 // DAMAGE.
30
31 #include <boost/serialization/vector.hpp>
32 #include <hpp/pinocchio/body.hh>
33 #include <hpp/pinocchio/device-data.hh>
34 #include <hpp/pinocchio/device.hh>
35 #include <hpp/pinocchio/frame.hh>
36 #include <hpp/pinocchio/joint-collection.hh>
37 #include <hpp/pinocchio/joint.hh>
38 #include <hpp/pinocchio/liegroup-space.hh>
39 #include <hpp/pinocchio/serialization.hh>
40 #include <hpp/util/serialization.hh>
41 #include <limits>
42 #include <pinocchio/algorithm/jacobian.hpp>
43
44 #include "joint/bound.hh"
45
46 #define CALL_JOINT(method) model().joints[jointIndex].method()
47
48 namespace hpp {
49 namespace pinocchio {
50 using ::pinocchio::LOCAL;
51 using ::pinocchio::WORLD;
52
53 138 JointPtr_t Joint::create(DeviceWkPtr_t device, JointIndex indexInJointList) {
54
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 138 times.
 138 if (indexInJointList == 0)
55 ✗ return JointPtr_t();
56 else
57
3/6
✓ Branch 2 taken 138 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 138 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 138 times.
✗ Branch 9 not taken.
 138 return JointPtr_t(new Joint(device, indexInJointList));
58 }
59
60 1333 Joint::Joint(DeviceWkPtr_t device, JointIndex indexInJointList)
61 1333 : maximalDistanceToParent_(-1),
62 1333 devicePtr(device),
63 1333 jointIndex(indexInJointList) {
64
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1333 times.
 1333 assert(devicePtr.lock());
65
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 1333 times.
 1333 assert(robot()->modelPtr());
66
2/4
✓ Branch 1 taken 1333 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1333 times.
 1333 assert(std::size_t(jointIndex) < model().joints.size());
67
1/2
✓ Branch 1 taken 1333 times.
✗ Branch 2 not taken.
 1333 setChildList();
68 1333 }
69
70 1340 void Joint::setChildList() {
71
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 1340 times.
 1340 assert(robot()->modelPtr());
72
2/4
✓ Branch 3 taken 1340 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1340 times.
 1340 assert(robot()->dataPtr());
73
1/2
✓ Branch 3 taken 1340 times.
✗ Branch 4 not taken.
 1340 const Data& data = robot()->data();
74 1340 children.clear();
75 6758 for (JointIndex child = jointIndex + 1;
76
2/2
✓ Branch 1 taken 5418 times.
✓ Branch 2 taken 1340 times.
 6758 int(child) <= data.lastChild[jointIndex]; ++child)
77
4/6
✓ Branch 1 taken 5418 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1246 times.
✓ Branch 5 taken 4172 times.
✓ Branch 7 taken 1246 times.
✗ Branch 8 not taken.
 5418 if (model().parents[child] == jointIndex) children.push_back(child);
78 1340 }
79
80 10368 inline void Joint::selfAssert() const {
81 10368 DevicePtr_t device = devicePtr.lock();
82
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 10368 times.
 10368 assert(device);
83
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 10368 times.
 10368 assert(device->modelPtr());
84
2/4
✓ Branch 2 taken 10368 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 10368 times.
 10368 assert(device->dataPtr());
85
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 10368 times.
 10368 assert(device->model().joints.size() > std::size_t(jointIndex));
86
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 10368 times.
 10368 assert(jointIndex > 0);
87 10368 }
88
89 6973 Model& Joint::model() {
90 6973 selfAssert();
91 6973 return robot()->model();
92 }
93 1660 const Model& Joint::model() const {
94 1660 selfAssert();
95 1660 return robot()->model();
96 }
97
98 ✗ JointPtr_t Joint::parentJoint() const {
99 ✗ selfAssert();
100 ✗ JointIndex idParent = model().parents[jointIndex];
101 ✗ if (idParent == 0)
102 ✗ return JointPtr_t();
103 else
104 ✗ return JointPtr_t(new Joint(devicePtr, idParent));
105 }
106
107 1 const std::string& Joint::name() const {
108 1 selfAssert();
109 1 return model().names[jointIndex];
110 }
111
112 ✗ const Transform3s& Joint::currentTransformation(const DeviceData& d) const {
113 ✗ selfAssert();
114 ✗ return d.data_->oMi[jointIndex];
115 }
116
117 4 size_type Joint::numberDof() const {
118 4 selfAssert();
119 4 return CALL_JOINT(nv);
120 }
121
122 268 size_type Joint::configSize() const {
123 268 selfAssert();
124 268 return CALL_JOINT(nq);
125 }
126
127 66 size_type Joint::rankInConfiguration() const {
128 66 selfAssert();
129 66 return CALL_JOINT(idx_q);
130 }
131
132 ✗ size_type Joint::rankInVelocity() const {
133 ✗ selfAssert();
134 ✗ return CALL_JOINT(idx_v);
135 }
136
137 ✗ std::size_t Joint::numberChildJoints() const { return children.size(); }
138
139 ✗ JointPtr_t Joint::childJoint(std::size_t rank) const {
140 ✗ selfAssert();
141 ✗ assert(rank < children.size());
142 ✗ return JointPtr_t(new Joint(devicePtr, children[rank]));
143 }
144
145 ✗ const Transform3s& Joint::positionInParentFrame() const {
146 ✗ selfAssert();
147 ✗ return model().jointPlacements[jointIndex];
148 }
149
150 ✗ void Joint::isBounded(size_type rank, bool bounded) {
151 ✗ const size_type idx = rankInConfiguration() + rank;
152 ✗ assert(rank < configSize());
153 ✗ if (!bounded) {
154 ✗ const value_type& inf = std::numeric_limits<value_type>::infinity();
155 ✗ model().lowerPositionLimit[idx] = -inf;
156 ✗ model().upperPositionLimit[idx] = inf;
157 } else {
158 ✗ assert(false &&
159 "This function can only unset bounds. "
160 "Use lowerBound and upperBound to set the bounds.");
161 }
162 }
163 ✗ bool Joint::isBounded(size_type rank) const {
164 ✗ const size_type idx = rankInConfiguration() + rank;
165 ✗ assert(rank < configSize());
166 ✗ return !std::isinf(model().lowerPositionLimit[idx]) &&
167 ✗ !std::isinf(model().upperPositionLimit[idx]);
168 }
169 33 value_type Joint::lowerBound(size_type rank) const {
170 33 const size_type idx = rankInConfiguration() + rank;
171
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 33 times.
 33 assert(rank < configSize());
172 33 return model().lowerPositionLimit[idx];
173 }
174 33 value_type Joint::upperBound(size_type rank) const {
175 33 const size_type idx = rankInConfiguration() + rank;
176
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 33 times.
 33 assert(rank < configSize());
177 33 return model().upperPositionLimit[idx];
178 }
179 ✗ void Joint::lowerBound(size_type rank, value_type lowerBound) {
180 ✗ const size_type idx = rankInConfiguration() + rank;
181 ✗ assert(rank < configSize());
182 ✗ model().lowerPositionLimit[idx] = lowerBound;
183 }
184 ✗ void Joint::upperBound(size_type rank, value_type upperBound) {
185 ✗ const size_type idx = rankInConfiguration() + rank;
186 ✗ assert(rank < configSize());
187 ✗ model().upperPositionLimit[idx] = upperBound;
188 }
189 30 void Joint::lowerBounds(vectorIn_t lowerBounds) {
190 30 selfAssert();
191
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 SetBoundStep::run(jointModel(), SetBoundStep::ArgsType(
192 30 lowerBounds, model().lowerPositionLimit));
193 30 }
194 30 void Joint::upperBounds(vectorIn_t upperBounds) {
195 30 selfAssert();
196
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 SetBoundStep::run(jointModel(), SetBoundStep::ArgsType(
197 30 upperBounds, model().upperPositionLimit));
198 30 }
199
200 /* --- MAX DISTANCE ------------------------------------------------------*/
201 /* --- MAX DISTANCE ------------------------------------------------------*/
202 /* --- MAX DISTANCE ------------------------------------------------------*/
203
204 template <bool X, bool Y, bool Z, typename Derived>
205 6 value_type computeMaximalDistanceToParentForAlignedTranslation(
206 const Eigen::MatrixBase<Derived>& lower,
207 const Eigen::MatrixBase<Derived>& upper, const SE3& placement) {
208
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 3 times.
 6 if (!lower.allFinite() || !upper.allFinite())
209 ✗ return std::numeric_limits<value_type>::infinity();
210
211 6 value_type d = 0;
212 6 const size_type iX = 0;
213 6 const size_type iY = (X ? 1 : 0);
214 6 const size_type iZ = iY + (Y ? 1 : 0);
215
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 6 vector3_t p(vector3_t::Zero());
216
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
 12 for (size_type i = 0; i < (X ? 1 : 2); ++i) {
217
3/8
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
 6 if (X) p[0] = (i == 0 ? lower[iX] : upper[iX]);
218
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
 12 for (size_type j = 0; j < (Y ? 1 : 2); ++j) {
219
3/8
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
 6 if (Y) p[1] = (j == 0 ? lower[iY] : upper[iY]);
220
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
 12 for (size_type k = 0; k < (Z ? 1 : 2); ++k) {
221
3/8
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
 6 if (Z) p[1] = (k == 0 ? lower[iZ] : upper[iZ]);
222
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 6 d = std::max(d, placement.act(p).norm());
223 }
224 }
225 }
226 6 return d;
227 }
228
229 template <typename Joint>
230 ✗ value_type computeMaximalDistanceToParent(
231 const Model& /*model*/, const ::pinocchio::JointModelBase<Joint>&,
232 const SE3& /*jointPlacement*/) {
233 ✗ assert(false &&
234 "The function <maximalDistance> as not been implemented for this "
235 "class of joint");
236 return 0.0;
237 }
238
239 3 value_type computeMaximalDistanceToParent(
240 const Model& model, const ::pinocchio::JointModelFreeFlyer& jmodel,
241 const SE3& jointPlacement) {
242
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 const size_type& i = jmodel.idx_q();
243
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 return computeMaximalDistanceToParentForAlignedTranslation<true, true, true>(
244
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 model.lowerPositionLimit.segment< ::pinocchio::JointModelTranslation::NQ>(
245 i),
246
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 model.upperPositionLimit.segment< ::pinocchio::JointModelTranslation::NQ>(
247 i),
248 6 jointPlacement);
249 }
250
251 template <typename Scalar, int Options, int Axis>
252 156 value_type computeMaximalDistanceToParent(
253 const Model& /*model*/,
254 const ::pinocchio::JointModelRevoluteTpl<Scalar, Options, Axis>&,
255 const SE3& jointPlacement) {
256 156 return jointPlacement.translation().norm();
257 }
258
259 template <typename Scalar, int Options>
260 ✗ value_type computeMaximalDistanceToParent(
261 const Model& /*model*/,
262 const ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options>&,
263 const SE3& jointPlacement) {
264 ✗ return jointPlacement.translation().norm();
265 }
266
267 template <typename Scalar, int Options, int Axis>
268 ✗ value_type computeMaximalDistanceToParent(
269 const Model& /*model*/,
270 const ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar, Options, Axis>&,
271 const SE3& jointPlacement) {
272 ✗ return jointPlacement.translation().norm();
273 }
274
275 template <typename Scalar, int Options, int Axis>
276 ✗ value_type computeMaximalDistanceToParent(
277 const Model& model,
278 const ::pinocchio::JointModelPrismaticTpl<Scalar, Options, Axis>& jmodel,
279 const SE3& jointPlacement) {
280 return computeMaximalDistanceToParentForAlignedTranslation < Axis == 0,
281 Axis == 1,
282 ✗ Axis == 2 > (model.lowerPositionLimit.segment<1>(jmodel.idx_q()),
283 ✗ model.upperPositionLimit.segment<1>(jmodel.idx_q()),
284 ✗ jointPlacement);
285 }
286
287 template <typename Scalar, int Options>
288 ✗ value_type computeMaximalDistanceToParent(
289 const Model& model,
290 const ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options>& jmodel,
291 const SE3& jointPlacement) {
292 ✗ if (std::isinf(model.lowerPositionLimit[jmodel.idx_q()]) ||
293 ✗ std::isinf(model.upperPositionLimit[jmodel.idx_q()]))
294 ✗ return std::numeric_limits<value_type>::infinity();
295
296 ✗ Eigen::Vector3d pmin = jmodel.axis * model.lowerPositionLimit[jmodel.idx_q()];
297 ✗ Eigen::Vector3d pmax = jmodel.axis * model.upperPositionLimit[jmodel.idx_q()];
298
299 ✗ return std::max(jointPlacement.act(pmin).norm(),
300 ✗ jointPlacement.act(pmax).norm());
301 }
302
303 template <typename Scalar, int Options>
304 ✗ value_type computeMaximalDistanceToParent(
305 const Model& /*model*/,
306 const ::pinocchio::JointModelSphericalTpl<Scalar, Options>&,
307 const SE3& jointPlacement) {
308 ✗ return jointPlacement.translation().norm();
309 }
310
311 template <typename Scalar, int Options>
312 ✗ value_type computeMaximalDistanceToParent(
313 const Model& /*model*/,
314 const ::pinocchio::JointModelSphericalZYXTpl<Scalar, Options>&,
315 const SE3& jointPlacement) {
316 ✗ return jointPlacement.translation().norm();
317 }
318
319 template <typename Scalar, int Options>
320 ✗ value_type computeMaximalDistanceToParent(
321 const Model& model,
322 const ::pinocchio::JointModelTranslationTpl<Scalar, Options>& jmodel,
323 const SE3& jointPlacement) {
324 ✗ const size_type& i = jmodel.idx_q();
325 ✗ return computeMaximalDistanceToParentForAlignedTranslation<true, true, true>(
326 ✗ model.lowerPositionLimit.segment< ::pinocchio::JointModelTranslation::NQ>(
327 i),
328 ✗ model.upperPositionLimit.segment< ::pinocchio::JointModelTranslation::NQ>(
329 i),
330 ✗ jointPlacement);
331 }
332 // TODO (really?): handle the case where the translation is bounded.
333
334 template <typename Scalar, int Options>
335 ✗ value_type computeMaximalDistanceToParent(
336 const Model& model,
337 const ::pinocchio::JointModelPlanarTpl<Scalar, Options>& jmodel,
338 const SE3& jointPlacement) {
339 ✗ const size_type& i = jmodel.idx_q();
340 ✗ return computeMaximalDistanceToParentForAlignedTranslation<true, true, false>(
341 ✗ model.lowerPositionLimit.segment<2>(i),
342 ✗ model.upperPositionLimit.segment<2>(i), jointPlacement);
343 }
344
345 struct VisitMaximalDistanceToParent : public boost::static_visitor<value_type> {
346 const Model& model;
347 const SE3& jointPlacement;
348 81 VisitMaximalDistanceToParent(const Model& model, const SE3& jointPlacement)
349 81 : model(model), jointPlacement(jointPlacement) {}
350
351 template <typename Joint>
352 162 value_type operator()(const ::pinocchio::JointModelBase<Joint>& jmodel) {
353 162 return computeMaximalDistanceToParent(model, jmodel.derived(),
354 162 jointPlacement);
355 }
356 };
357
358 81 void Joint::computeMaximalDistanceToParent() {
359
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 selfAssert();
360
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 VisitMaximalDistanceToParent visitor(model(),
361
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 162 model().jointPlacements[jointIndex]);
362
2/4
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 81 times.
✗ Branch 5 not taken.
 81 maximalDistanceToParent_ = boost::apply_visitor(visitor, jointModel());
363 81 }
364
365 /* --- MAX VEL -----------------------------------------------------------*/
366 /* --- MAX VEL -----------------------------------------------------------*/
367 /* --- MAX VEL -----------------------------------------------------------*/
368
369 /* --- LINEAR VELOCITY ---------------------------------------------------*/
370 template <typename D>
371 ✗ value_type upperBoundLinearVelocity(const ::pinocchio::JointModelBase<D>&) {
372 ✗ assert(false &&
373 "The function <upperBoundLinearVel> as not been implemented for this "
374 "class of joint");
375 return 0.0;
376 }
377 template <typename Scalar, int Options>
378 ✗ value_type upperBoundLinearVelocity(
379 const ::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>&) {
380 ✗ return 1.0;
381 }
382 template <typename Scalar, int Options, int AXIS>
383 ✗ value_type upperBoundLinearVelocity(
384 const ::pinocchio::JointModelRevoluteTpl<Scalar, Options, AXIS>&) {
385 ✗ return 0.0;
386 }
387 template <typename Scalar, int Options>
388 ✗ value_type upperBoundLinearVelocity(
389 const ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options>&) {
390 ✗ return 0.0;
391 }
392 template <typename Scalar, int Options, int AXIS>
393 ✗ value_type upperBoundLinearVelocity(
394 const ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar, Options, AXIS>&) {
395 ✗ return 0.0;
396 }
397 template <typename Scalar, int Options, int AXIS>
398 ✗ value_type upperBoundLinearVelocity(
399 const ::pinocchio::JointModelPrismaticTpl<Scalar, Options, AXIS>&) {
400 ✗ return 1.0;
401 }
402 template <typename Scalar, int Options>
403 ✗ value_type upperBoundLinearVelocity(
404 const ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options>&) {
405 ✗ return 1.0;
406 }
407 template <typename Scalar, int Options>
408 ✗ value_type upperBoundLinearVelocity(
409 const ::pinocchio::JointModelSphericalTpl<Scalar, Options>&) {
410 ✗ return 0.0;
411 }
412 template <typename Scalar, int Options>
413 ✗ value_type upperBoundLinearVelocity(
414 const ::pinocchio::JointModelSphericalZYXTpl<Scalar, Options>&) {
415 ✗ return 0.0;
416 }
417 template <typename Scalar, int Options>
418 ✗ value_type upperBoundLinearVelocity(
419 const ::pinocchio::JointModelTranslationTpl<Scalar, Options>&) {
420 ✗ return 1.0;
421 }
422 template <typename Scalar, int Options>
423 ✗ value_type upperBoundLinearVelocity(
424 const ::pinocchio::JointModelPlanarTpl<Scalar, Options>&) {
425 ✗ return 1.0;
426 }
427
428 struct VisitUpperBoundLinearVelocity
429 : public boost::static_visitor<value_type> {
430 template <typename Joint>
431 ✗ value_type operator()(
432 const ::pinocchio::JointModelBase<Joint>& jmodel) const {
433 ✗ return upperBoundLinearVelocity(jmodel.derived());
434 }
435 };
436
437 ✗ value_type Joint::upperBoundLinearVelocity() const {
438 ✗ selfAssert();
439 VisitUpperBoundLinearVelocity visitor;
440 ✗ return boost::apply_visitor(visitor, jointModel());
441 }
442
443 /* --- ANGULAR VELOCITY -------------------------------------------------- */
444 template <typename D>
445 ✗ value_type upperBoundAngularVelocity(const ::pinocchio::JointModelBase<D>&) {
446 ✗ assert(false &&
447 "The function <upperBoundAngularVel> as not been implemented for this "
448 "class of joint");
449 return 0.0;
450 }
451 template <typename Scalar, int Options>
452 ✗ value_type upperBoundAngularVelocity(
453 const ::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>&) {
454 ✗ return 1.0;
455 }
456 template <typename Scalar, int Options, int AXIS>
457 ✗ value_type upperBoundAngularVelocity(
458 const ::pinocchio::JointModelRevoluteTpl<Scalar, Options, AXIS>&) {
459 ✗ return 1.0;
460 }
461 template <typename Scalar, int Options>
462 ✗ value_type upperBoundAngularVelocity(
463 const ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options>&) {
464 ✗ return 1.0;
465 }
466 template <typename Scalar, int Options, int AXIS>
467 ✗ value_type upperBoundAngularVelocity(
468 const ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar, Options, AXIS>&) {
469 ✗ return 1.0;
470 }
471 template <typename Scalar, int Options, int AXIS>
472 ✗ value_type upperBoundAngularVelocity(
473 const ::pinocchio::JointModelPrismaticTpl<Scalar, Options, AXIS>&) {
474 ✗ return 0.0;
475 }
476 template <typename Scalar, int Options>
477 ✗ value_type upperBoundAngularVelocity(
478 const ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options>&) {
479 ✗ return 0.0;
480 }
481 template <typename Scalar, int Options>
482 ✗ value_type upperBoundAngularVelocity(
483 const ::pinocchio::JointModelSphericalTpl<Scalar, Options>&) {
484 ✗ return 1.0;
485 }
486 template <typename Scalar, int Options>
487 ✗ value_type upperBoundAngularVelocity(
488 const ::pinocchio::JointModelSphericalZYXTpl<Scalar, Options>&) {
489 ✗ return 1.0;
490 }
491 template <typename Scalar, int Options>
492 ✗ value_type upperBoundAngularVelocity(
493 const ::pinocchio::JointModelTranslationTpl<Scalar, Options>&) {
494 ✗ return 0.0;
495 }
496 template <typename Scalar, int Options>
497 ✗ value_type upperBoundAngularVelocity(
498 const ::pinocchio::JointModelPlanarTpl<Scalar, Options>&) {
499 ✗ return 1.0;
500 }
501
502 struct VisitUpperBoundAngularVelocity
503 : public boost::static_visitor<value_type> {
504 template <typename Joint>
505 ✗ value_type operator()(
506 const ::pinocchio::JointModelBase<Joint>& jmodel) const {
507 ✗ return upperBoundAngularVelocity(jmodel.derived());
508 }
509 };
510
511 ✗ value_type Joint::upperBoundAngularVelocity() const {
512 ✗ selfAssert();
513 VisitUpperBoundAngularVelocity visitor;
514 ✗ return boost::apply_visitor(visitor, jointModel());
515 }
516
517 ✗ JointJacobian_t& Joint::jacobian(DeviceData& d, const bool local) const {
518 ✗ selfAssert();
519 ✗ if (robot()->computationFlag() & JACOBIAN) {
520 ✗ std::logic_error(
521 "Robot computation flag should contain JACOBIAN in "
522 "order to retrieve the joint jacobians");
523 }
524 ✗ assert(jointIndex > 0 && jointIndex - 1 < d.jointJacobians_.size());
525 ✗ JointJacobian_t& jacobian(d.jointJacobians_[jointIndex - 1]);
526 ✗ if (jacobian.cols() != model().nv)
527 ✗ jacobian = JointJacobian_t::Zero(6, model().nv);
528 ✗ if (local)
529 ✗ ::pinocchio::getJointJacobian(model(), *d.data_, jointIndex, LOCAL,
530 jacobian);
531 else
532 ✗ ::pinocchio::getJointJacobian(model(), *d.data_, jointIndex, WORLD,
533 jacobian);
534 ✗ return jacobian;
535 }
536
537 ✗ BodyPtr_t Joint::linkedBody() const {
538 ✗ return BodyPtr_t(new Body(devicePtr.lock(), jointIndex));
539 }
540
541 ✗ std::ostream& Joint::display(std::ostream& os) const {
542 ✗ os << "Joint " << jointIndex << "(nq=" << configSize()
543 ✗ << ",nv=" << numberDof() << ")" << std::endl;
544
545 ✗ os << "\"" << name() << "\""
546 ✗ << "[shape=box label=\"" << name() << "\\n";
547 ✗ if (configSize() != 0)
548 ✗ os << "Rank in configuration: " << rankInConfiguration() << "\\n";
549 else
550 ✗ os << "Anchor joint\\n";
551 ✗ os << "Current transformation: " << currentTransformation();
552 ✗ os << "\\n";
553 ✗ os << "\"]" << std::endl;
554 ✗ for (unsigned int iChild = 0; iChild < numberChildJoints(); iChild++) {
555 // for (hpp::model::JointVector_t::const_iterator it =
556 // children_.begin (); it != children_.end (); ++it) {
557
558 // write edges to children joints
559 ✗ os << "\"" << name() << "\"->\"" << childJoint(iChild)->name() << "\""
560 ✗ << std::endl;
561 }
562 ✗ return os;
563 }
564
565 template <typename LieGroupMap_t>
566 struct ConfigSpaceVisitor : public ::pinocchio::fusion::JointUnaryVisitorBase<
567 ConfigSpaceVisitor<LieGroupMap_t> > {
568 typedef boost::fusion::vector<LiegroupSpace&> ArgsType;
569
570 template <typename JointModel>
571 2228 static void algo(const ::pinocchio::JointModelBase<JointModel>& jmodel,
572 LiegroupSpace& space) {
573 2228 _algo(jmodel.derived(), space);
574 }
575
576 template <typename JointModel>
577 2228 static void _algo(const ::pinocchio::JointModelBase<JointModel>&,
578 LiegroupSpace& space) {
579 typedef typename LieGroupMap_t::template operation<JointModel>::type LG_t;
580
6/9
✓ Branch 2 taken 48 times.
✓ Branch 3 taken 1066 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 48 times.
✓ Branch 6 taken 1066 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 48 times.
✓ Branch 10 taken 1066 times.
✗ Branch 11 not taken.
 2228 space *= LiegroupSpace::create(LG_t());
581 }
582 };
583
584 557 LiegroupSpacePtr_t Joint::configurationSpace() const {
585
1/2
✓ Branch 1 taken 557 times.
✗ Branch 2 not taken.
 557 LiegroupSpacePtr_t res = LiegroupSpace::empty();
586
1/2
✓ Branch 2 taken 557 times.
✗ Branch 3 not taken.
 557 ConfigSpaceVisitor<DefaultLieGroupMap>::ArgsType args(*res);
587
3/6
✓ Branch 1 taken 557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 557 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 557 times.
✗ Branch 8 not taken.
 557 ConfigSpaceVisitor<DefaultLieGroupMap>::run(jointModel(), args);
588 1114 return res;
589 }
590
591 557 LiegroupSpacePtr_t Joint::RnxSOnConfigurationSpace() const {
592
1/2
✓ Branch 1 taken 557 times.
✗ Branch 2 not taken.
 557 LiegroupSpacePtr_t res = LiegroupSpace::empty();
593
1/2
✓ Branch 2 taken 557 times.
✗ Branch 3 not taken.
 557 ConfigSpaceVisitor<RnxSOnLieGroupMap>::ArgsType args(*res);
594
3/6
✓ Branch 1 taken 557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 557 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 557 times.
✗ Branch 8 not taken.
 557 ConfigSpaceVisitor<RnxSOnLieGroupMap>::run(jointModel(), args);
595 1114 return res;
596 }
597
598 1255 const JointModel& Joint::jointModel() const {
599 1255 selfAssert();
600 1255 return model().joints[index()];
601 }
602
603 ✗ DeviceData& Joint::data() const { return devicePtr.lock()->d(); }
604
605 template <class Archive>
606 28 void Joint::serialize(Archive& ar, const unsigned int version) {
607 (void)version;
608
1/2
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
 28 ar& BOOST_SERIALIZATION_NVP(devicePtr);
609
1/2
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
 28 ar& BOOST_SERIALIZATION_NVP(jointIndex);
610 if (!Archive::is_saving::value) {
611 14 maximalDistanceToParent_ = -1;
612 14 setChildList();
613 }
614 }
615
616 HPP_SERIALIZATION_IMPLEMENT(Joint);
617 } // namespace pinocchio
618 } // namespace hpp
619