GCC Code Coverage Report

Directory: ./
File: src/relative-motion.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 59 62 95.2%
Branches: 61 94 64.9%
Line Branch Exec Source
1 // Copyright (c) 2016, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include <hpp/constraints/explicit/implicit-function.hh>
30 #include <hpp/constraints/explicit/relative-transformation.hh>
31 #include <hpp/constraints/generic-transformation.hh>
32 #include <hpp/constraints/implicit.hh>
33 #include <hpp/constraints/locked-joint.hh>
34 #include <hpp/core/config-projector.hh>
35 #include <hpp/core/constraint-set.hh>
36 #include <hpp/core/relative-motion.hh>
37 #include <hpp/pinocchio/device.hh>
38 #include <hpp/pinocchio/joint-collection.hh>
39 #include <hpp/pinocchio/joint.hh>
40 #include <pinocchio/multibody/model.hpp>
41
42 namespace hpp {
43 namespace core {
44 namespace {
45 12 inline void symSet(RelativeMotion::matrix_type& m, size_type i0, size_type i1,
46 RelativeMotion::RelativeMotionType t) {
47 12 m(i0, i1) = m(i1, i0) = t;
48 12 }
49
50 /// Considering the order Constrained(0) < Parameterized(1) < Unconstrained(2)
51 /// Change m(i0,i1) only if it decreases.
52 14 inline void symSetNoDowngrade(RelativeMotion::matrix_type& m, size_type i0,
53 size_type i1,
54 RelativeMotion::RelativeMotionType t) {
55 assert(RelativeMotion::Constrained < RelativeMotion::Parameterized &&
56 RelativeMotion::Parameterized < RelativeMotion::Unconstrained);
57
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
 14 assert(m(i0, i1) == m(i1, i0));
58
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 6 times.
 14 if (t < m(i0, i1)) m(i0, i1) = m(i1, i0) = t;
59 14 }
60 } // namespace
61
62 5 RelativeMotion::matrix_type RelativeMotion::matrix(const DevicePtr_t& dev) {
63
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
 5 assert(dev);
64 5 const size_type N = dev->model().joints.size();
65
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 matrix_type matrix(N, N);
66
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 matrix.setConstant(Unconstrained);
67
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 matrix.diagonal().setConstant(Constrained);
68 10 return matrix;
69 }
70
71 5 void RelativeMotion::fromConstraint(matrix_type& matrix,
72 const DevicePtr_t& robot,
73 const ConstraintSetPtr_t& c) {
74
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
 5 assert(robot);
75
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
 5 assert(c);
76
77 5 const size_type N = robot->model().joints.size();
78
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 5 times.
 5 if (matrix.rows() != N || matrix.cols() != N)
79 ✗ throw std::invalid_argument("Wrong RelativeMotion::matrix_type size");
80
81
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 ConfigProjectorPtr_t proj = c->configProjector();
82
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
 5 if (!proj) return;
83
84 // Loop over the constraints
85
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 const NumericalConstraints_t& ncs = proj->numericalConstraints();
86 5 for (NumericalConstraints_t::const_iterator _ncs = ncs.begin();
87
2/2
✓ Branch 3 taken 14 times.
✓ Branch 4 taken 5 times.
 19 _ncs != ncs.end(); ++_ncs) {
88 14 constraints::ImplicitConstPtr_t nc = *_ncs;
89 size_type i1, i2;
90 std::pair<JointConstPtr_t, JointConstPtr_t> joints =
91
1/2
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
 14 nc->doesConstrainRelPoseBetween(robot);
92 14 i1 = Joint::index(joints.first);
93 14 i2 = Joint::index(joints.second);
94
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 12 times.
 14 if (i1 == i2) {
95 hppDout(info, "Constraint "
96 << nc->function().name()
97 << " does not fully constrain any pair of joints.");
98 2 continue;
99 }
100
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 bool cstRHS(nc->parameterSize() == 0);
101
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 recurseSetRelMotion(matrix, i1, i2, (cstRHS ? Constrained : Parameterized));
102
4/4
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 2 times.
✓ Branch 4 taken 12 times.
✓ Branch 5 taken 2 times.
 16 }
103
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 }
104
105 12 void RelativeMotion::recurseSetRelMotion(
106 matrix_type& matrix, const size_type& i1, const size_type& i2,
107 const RelativeMotion::RelativeMotionType& type) {
108 assert(Constrained < Parameterized && Parameterized < Unconstrained);
109 12 bool param = (type == Parameterized);
110 12 RelativeMotionType t = Unconstrained;
111 // Constrained(0) < Parameterized(1) < Unconstrained(2)
112 // If the current value is more constraining, then do not change it.
113
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
 12 if (matrix(i1, i2) <= type) return;
114 12 symSet(matrix, i1, i2, type);
115
116 // i1 to i3
117
2/2
✓ Branch 1 taken 84 times.
✓ Branch 2 taken 12 times.
 96 for (size_type i3 = 0; i3 < matrix.rows(); ++i3) {
118
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 72 times.
 84 if (i3 == i1) continue;
119
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 60 times.
 72 if (i3 == i2) continue;
120
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 58 times.
 60 if (matrix(i2, i3) != Unconstrained) {
121
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 2 t = (!param && matrix(i2, i3) == Constrained) ? Constrained
122 : Parameterized;
123 2 symSetNoDowngrade(matrix, i1, i3, t);
124 }
125 }
126
2/2
✓ Branch 1 taken 84 times.
✓ Branch 2 taken 12 times.
 96 for (size_type i0 = 0; i0 < matrix.rows(); ++i0) {
127
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 72 times.
 84 if (i0 == i2) continue;
128
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 60 times.
 72 if (i0 == i1) continue;
129 // i0 to i2
130
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 54 times.
 60 if (matrix(i0, i1) != Unconstrained) {
131
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 6 t = (!param && matrix(i0, i1) == Constrained) ? Constrained
132 : Parameterized;
133 6 symSetNoDowngrade(matrix, i0, i2, t);
134 }
135
136 // from i0 to i3
137
2/2
✓ Branch 1 taken 54 times.
✓ Branch 2 taken 6 times.
 60 if (matrix(i0, i1) == Unconstrained) continue;
138
2/2
✓ Branch 1 taken 42 times.
✓ Branch 2 taken 6 times.
 48 for (size_type i3 = 0; i3 < matrix.rows(); ++i3) {
139
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 36 times.
 42 if (i3 == i2) continue;
140
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 30 times.
 36 if (i3 == i1) continue;
141
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 24 times.
 30 if (i3 == i0) continue;
142
2/2
✓ Branch 1 taken 18 times.
✓ Branch 2 taken 6 times.
 24 if (matrix(i2, i3) == Unconstrained) continue;
143 // If motion is already constrained, continue.
144 ✗ t = (!param && matrix(i0, i1) == Constrained &&
145 ✗ matrix(i2, i3) == Constrained)
146
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 6 ? Constrained
147 : Parameterized;
148 6 symSetNoDowngrade(matrix, i0, i3, t);
149 }
150 }
151 }
152 } // namespace core
153 } // namespace hpp
154