GCC Code Coverage Report

Directory: ./
File: src/explicit/input-configurations.hh
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 73 80 91.2%
Branches: 83 170 48.8%
Line Branch Exec Source
1 // Copyright (c) 2020, Airbus SAS and CNRS
2 // Authors: Florent Lamiraux
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 #ifndef HPP_CONSTRAINTS_SRC_EXPLICIT_CONFIGURATION_HH
30 #define HPP_CONSTRAINTS_SRC_EXPLICIT_CONFIGURATION_HH
31
32 #include <hpp/constraints/matrix-view.hh>
33 #include <hpp/pinocchio/device.hh>
34 #include <hpp/pinocchio/joint.hh>
35
36 namespace hpp {
37 namespace constraints {
38 namespace explicit_ {
39 namespace {
40 using Eigen::BlockIndex;
41 20 inline BlockIndex::segments_t vectorOfBoolToIntervals(std::vector<bool>& v) {
42 20 BlockIndex::segments_t ret;
43
2/2
✓ Branch 1 taken 338 times.
✓ Branch 2 taken 20 times.
 358 for (std::size_t i = 0; i < v.size(); ++i)
44
4/6
✓ Branch 1 taken 338 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✓ Branch 5 taken 234 times.
✓ Branch 8 taken 104 times.
✗ Branch 9 not taken.
 338 if (v[i]) ret.push_back(BlockIndex::segment_t(i, 1));
45
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 BlockIndex::shrink(ret);
46 20 return ret;
47 }
48
49 // This function computes the configuration variables of
50 // - joint1 and its parents up to the common ancestor with joint2,
51 // - joint2 parent and its parents up to the common ancestor with
52 // joint1.
53 // Note that configuration variables of joint2 do not belong to
54 // the resulting set.
55 12 inline std::vector<bool> relPoseConfVariables(const DevicePtr_t& robot,
56 JointConstPtr_t joint1,
57 JointConstPtr_t joint2) {
58
2/4
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
 12 std::vector<bool> conf(robot->configSize(), false);
59
5/6
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 12 times.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 10 times.
✓ Branch 8 taken 12 times.
 22 while (joint1 && joint1->index() != 0) {
60
3/4
✓ Branch 2 taken 80 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 70 times.
✓ Branch 5 taken 10 times.
 80 for (size_type i = 0; i < joint1->configSize(); ++i)
61
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 conf[joint1->rankInConfiguration() + i] =
62
2/4
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
 140 !conf[joint1->rankInConfiguration() + i];
63 hppDout(info, "Adding joint1 " << joint1->name() << " as input variable.");
64
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 joint1 = joint1->parentJoint();
65 }
66
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 joint2 = joint2->parentJoint();
67
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 12 times.
 12 while (joint2 && joint2->index() != 0) {
68 ✗ for (size_type i = 0; i < joint2->configSize(); ++i)
69 ✗ conf[joint2->rankInConfiguration() + i] =
70 ✗ !conf[joint2->rankInConfiguration() + i];
71 hppDout(info, "Adding joint2 " << joint2->name() << " as input variable.");
72 ✗ joint2 = joint2->parentJoint();
73 }
74 12 return conf;
75 }
76
77 // This function computes the velocity variables of
78 // - joint1 and its parents up to the root joint,
79 // - joint2 parent and its parent up to root joint.
80 // Note that velocity variables of joint2 do not belong to
81 // the resulting set.
82 12 inline std::vector<bool> relPoseVelVariables(const DevicePtr_t& robot,
83 JointConstPtr_t joint1,
84 JointConstPtr_t joint2) {
85
2/4
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
 12 std::vector<bool> vel(robot->numberDof(), false);
86
5/6
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 12 times.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 10 times.
✓ Branch 8 taken 12 times.
 22 while (joint1 && joint1->index() != 0) {
87
3/4
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 60 times.
✓ Branch 5 taken 10 times.
 70 for (size_type i = 0; i < joint1->numberDof(); ++i)
88
4/8
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 60 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 60 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 60 times.
✗ Branch 14 not taken.
 60 vel[joint1->rankInVelocity() + i] = !vel[joint1->rankInVelocity() + i];
89 hppDout(info, "Adding joint1 " << joint1->name() << " as input variable.");
90
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 joint1 = joint1->parentJoint();
91 }
92
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 joint2 = joint2->parentJoint();
93
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 12 times.
 12 while (joint2 && joint2->index() != 0) {
94 ✗ for (size_type i = 0; i < joint2->numberDof(); ++i)
95 ✗ vel[joint2->rankInVelocity() + i] = !vel[joint2->rankInVelocity() + i];
96 hppDout(info, "Adding joint2 " << joint2->name() << " as input variable.");
97 ✗ joint2 = joint2->parentJoint();
98 }
99 12 return vel;
100 }
101 } // namespace
102 namespace relativePose {
103 // This function computes the configuration variables of
104 // - joint1 and its parents up to the common ancestor with joint2,
105 // - joint2 parent and its parents up to the common ancestor with
106 // joint1.
107 // Note that configuration variables of joint2 do not belong to
108 // the resulting set.
109 8 inline BlockIndex::segments_t inputConfVariables(const DevicePtr_t& robot,
110 JointConstPtr_t joint1,
111 JointConstPtr_t joint2) {
112
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 16 std::vector<bool> conf(relPoseConfVariables(robot, joint1, joint2));
113
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 return vectorOfBoolToIntervals(conf);
114 8 }
115
116 // This function computes the velocity variables of
117 // - joint1 and its parents up to the root joint,
118 // - joint2 parent and its parent up to root joint.
119 // Note that velocity variables of joint2 do not belong to
120 // the resulting set.
121 8 inline BlockIndex::segments_t inputVelocityVariables(const DevicePtr_t& robot,
122 JointConstPtr_t joint1,
123 JointConstPtr_t joint2) {
124
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 16 std::vector<bool> vel(relPoseVelVariables(robot, joint1, joint2));
125
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 return vectorOfBoolToIntervals(vel);
126 8 }
127
128 // Return configuration variables of a joint
129 9 inline BlockIndex::segments_t jointConfInterval(JointConstPtr_t j) {
130 return BlockIndex::segments_t(
131
3/6
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
 18 1, BlockIndex::segment_t(j->rankInConfiguration(), j->configSize()));
132 }
133 // Return velocity variables of a joint
134 9 inline BlockIndex::segments_t jointVelInterval(JointConstPtr_t j) {
135 return BlockIndex::segments_t(
136
3/6
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
 18 1, BlockIndex::segment_t(j->rankInVelocity(), j->numberDof()));
137 }
138 } // namespace relativePose
139 namespace contact {
140 // Compute input configuration variables of
141 // explicit_::ConvexShapeContact.
142 2 inline BlockIndex::segments_t inputConfVariables(
143 const DevicePtr_t& robot, const JointAndShapes_t& floorSurfaces,
144 const JointAndShapes_t& objectSurfaces) {
145
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 std::vector<bool> res(robot->configSize(), false);
146
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(objectSurfaces.size() > 0);
147 2 JointPtr_t objectJoint = objectSurfaces.front().first;
148 2 for (JointAndShapes_t::const_iterator it(floorSurfaces.begin());
149
2/2
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 2 times.
 6 it != floorSurfaces.end(); ++it) {
150 4 JointPtr_t joint1(it->first);
151 4 JointPtr_t joint2(objectJoint);
152
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 8 std::vector<bool> conf(relPoseConfVariables(robot, joint1, joint2));
153
2/2
✓ Branch 1 taken 56 times.
✓ Branch 2 taken 4 times.
 60 for (std::size_t i = 0; i < res.size(); ++i) {
154
4/6
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✓ Branch 5 taken 28 times.
✓ Branch 7 taken 28 times.
✗ Branch 8 not taken.
 56 if (conf[i]) res[i] = true;
155 }
156 4 }
157
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 return vectorOfBoolToIntervals(res);
158 2 }
159
160 // Compute size of input configuration variables of
161 // explicit_::ConvexShapeContact.
162 1 inline size_type inputSize(const DevicePtr_t& robot,
163 const JointAndShapes_t& floorSurfaces,
164 const JointAndShapes_t& objectSurfaces) {
165 BlockIndex::segments_t variables(
166
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 inputConfVariables(robot, floorSurfaces, objectSurfaces));
167
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 return (size_type)BlockIndex::cardinal(variables);
168 1 }
169 // Compute input configuration variables of
170 // explicit_::ConvexShapeContact.
171 2 inline BlockIndex::segments_t inputVelocityVariables(
172 const DevicePtr_t& robot, const JointAndShapes_t& floorSurfaces,
173 const JointAndShapes_t& objectSurfaces) {
174
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 std::vector<bool> res(robot->numberDof(), false);
175
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(objectSurfaces.size() > 0);
176 2 JointPtr_t objectJoint = objectSurfaces.front().first;
177 2 for (JointAndShapes_t::const_iterator it(floorSurfaces.begin());
178
2/2
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 2 times.
 6 it != floorSurfaces.end(); ++it) {
179 4 JointPtr_t joint1(it->first);
180 4 JointPtr_t joint2(objectJoint);
181
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 8 std::vector<bool> conf(relPoseVelVariables(robot, joint1, joint2));
182
2/2
✓ Branch 1 taken 48 times.
✓ Branch 2 taken 4 times.
 52 for (std::size_t i = 0; i < res.size(); ++i) {
183
4/6
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✓ Branch 5 taken 24 times.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 if (conf[i]) res[i] = true;
184 }
185 4 }
186
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 return vectorOfBoolToIntervals(res);
187 2 }
188 1 inline size_type inputDerivSize(const DevicePtr_t& robot,
189 const JointAndShapes_t& floorSurfaces,
190 const JointAndShapes_t& objectSurfaces) {
191 BlockIndex::segments_t variables(
192
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 inputVelocityVariables(robot, floorSurfaces, objectSurfaces));
193
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 return (size_type)BlockIndex::cardinal(variables);
194 1 }
195 } // namespace contact
196 } // namespace explicit_
197 } // namespace constraints
198 } // namespace hpp
199
200 #endif // HPP_CONSTRAINTS_SRC_EXPLICIT_CONFIGURATION_HH
201