GCC Code Coverage Report

Directory: ./
File: src/configuration.cc
Date: 2025-05-04 12:09:19
Exec Total Coverage
Lines: 76 97 78.4%
Branches: 87 214 40.7%
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/pinocchio/configuration.hh>
30 #include <hpp/pinocchio/device.hh>
31 #include <hpp/pinocchio/joint-collection.hh>
32 #include <hpp/pinocchio/liegroup.hh>
33 #include <hpp/pinocchio/util.hh>
34 #include <hpp/util/indent.hh>
35 #include <pinocchio/algorithm/joint-configuration.hpp>
36 #include <pinocchio/multibody/liegroup/liegroup.hpp>
37
38 namespace hpp {
39 namespace pinocchio {
40 24030 void saturate(const DevicePtr_t& robot, ConfigurationOut_t configuration) {
41 24030 const Model& model = robot->model();
42
1/2
✓ Branch 1 taken 24029 times.
✗ Branch 2 not taken.
 24030 configuration.head(model.nq) =
43
3/6
✓ Branch 1 taken 24030 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24030 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24030 times.
✗ Branch 8 not taken.
 48059 model.upperPositionLimit.cwiseMin(configuration.head(model.nq));
44
1/2
✓ Branch 1 taken 24030 times.
✗ Branch 2 not taken.
 24030 configuration.head(model.nq) =
45
3/6
✓ Branch 1 taken 24030 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24030 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24029 times.
✗ Branch 8 not taken.
 48060 model.lowerPositionLimit.cwiseMax(configuration.head(model.nq));
46
47 24029 const ExtraConfigSpace& ecs = robot->extraConfigSpace();
48 24030 const size_type& d = ecs.dimension();
49
4/8
✓ Branch 2 taken 24030 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24029 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24030 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 24029 times.
✗ Branch 12 not taken.
 24030 configuration.tail(d) = ecs.upper().cwiseMin(configuration.tail(d));
50
4/8
✓ Branch 2 taken 24030 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24029 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24030 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 24029 times.
✗ Branch 12 not taken.
 24029 configuration.tail(d) = ecs.lower().cwiseMax(configuration.tail(d));
51 24029 }
52
53 6 bool saturate(const DevicePtr_t& robot, ConfigurationOut_t configuration,
54 ArrayXb& saturation) {
55 6 bool ret = false;
56 6 const Model& model = robot->model();
57
58
2/2
✓ Branch 1 taken 98 times.
✓ Branch 2 taken 6 times.
 104 for (std::size_t i = 1; i < model.joints.size(); ++i) {
59
1/2
✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
 98 const size_type nq = model.joints[i].nq();
60
1/2
✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
 98 const size_type nv = model.joints[i].nv();
61
1/2
✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
 98 const size_type idx_q = model.joints[i].idx_q();
62
1/2
✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
 98 const size_type idx_v = model.joints[i].idx_v();
63
2/2
✓ Branch 0 taken 116 times.
✓ Branch 1 taken 98 times.
 214 for (size_type j = 0; j < nq; ++j) {
64 116 const size_type iq = idx_q + j;
65 116 const size_type iv = idx_v + std::min(j, nv - 1);
66
4/6
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 51 times.
✓ Branch 7 taken 65 times.
 116 if (configuration[iq] > model.upperPositionLimit[iq]) {
67
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 saturation[iv] = true;
68
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 configuration[iq] = model.upperPositionLimit[iq];
69 51 ret = true;
70
4/6
✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 65 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 50 times.
✓ Branch 7 taken 15 times.
 65 } else if (configuration[iq] < model.lowerPositionLimit[iq]) {
71
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 saturation[iv] = true;
72
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 configuration[iq] = model.lowerPositionLimit[iq];
73 50 ret = true;
74 } else
75
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 saturation[iv] = false;
76 }
77 }
78
79 6 const ExtraConfigSpace& ecs = robot->extraConfigSpace();
80 6 const size_type& d = ecs.dimension();
81
82
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
 6 for (size_type k = 0; k < d; ++k) {
83 ✗ const size_type iq = model.nq + k;
84 ✗ const size_type iv = model.nv + k;
85 ✗ if (configuration[iq] > ecs.upper(k)) {
86 ✗ saturation[iv] = true;
87 ✗ configuration[iq] = ecs.upper(k);
88 ✗ ret = true;
89 ✗ } else if (configuration[iq] < ecs.lower(k)) {
90 ✗ saturation[iv] = true;
91 ✗ configuration[iq] = ecs.lower(k);
92 ✗ ret = true;
93 } else
94 ✗ saturation[iv] = false;
95 }
96 6 return ret;
97 }
98
99 template <bool saturateConfig, typename LieGroup>
100 48120 void integrate(const DevicePtr_t& robot, ConfigurationIn_t configuration,
101 vectorIn_t velocity, ConfigurationOut_t result) {
102 48120 const Model& model = robot->model();
103
3/6
✓ Branch 1 taken 24060 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24060 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24059 times.
✗ Branch 8 not taken.
 48120 result.head(model.nq) =
104 ::pinocchio::integrate<LieGroup>(model, configuration, velocity);
105 48120 const size_type& dim = robot->extraConfigSpace().dimension();
106
5/10
✓ Branch 1 taken 24060 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24060 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24059 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24060 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 24060 times.
✗ Branch 14 not taken.
 48120 result.tail(dim) = configuration.tail(dim) + velocity.tail(dim);
107
2/4
✓ Branch 1 taken 24024 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24024 times.
✗ Branch 5 not taken.
 48048 if (saturateConfig) saturate(robot, result);
108 48120 }
109
110 template void integrate<true, RnxSOnLieGroupMap>(
111 const DevicePtr_t& robot, ConfigurationIn_t configuration,
112 vectorIn_t velocity, ConfigurationOut_t result);
113 template void integrate<false, RnxSOnLieGroupMap>(
114 const DevicePtr_t& robot, ConfigurationIn_t configuration,
115 vectorIn_t velocity, ConfigurationOut_t result);
116 template void integrate<true, DefaultLieGroupMap>(
117 const DevicePtr_t& robot, ConfigurationIn_t configuration,
118 vectorIn_t velocity, ConfigurationOut_t result);
119 template void integrate<false, DefaultLieGroupMap>(
120 const DevicePtr_t& robot, ConfigurationIn_t configuration,
121 vectorIn_t velocity, ConfigurationOut_t result);
122
123 ✗ void integrate(const DevicePtr_t& robot, ConfigurationIn_t configuration,
124 vectorIn_t velocity, ConfigurationOut_t result) {
125 ✗ integrate<true, DefaultLieGroupMap>(robot, configuration, velocity, result);
126 }
127
128 template <typename LieGroup>
129 144 void interpolate(const DevicePtr_t& robot, ConfigurationIn_t q0,
130 ConfigurationIn_t q1, const value_type& u,
131 ConfigurationOut_t result) {
132 144 const Model& model = robot->model();
133
3/6
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
 144 result.head(model.nq) = ::pinocchio::interpolate<LieGroup>(model, q0, q1, u);
134 144 const size_type& dim = robot->extraConfigSpace().dimension();
135
7/14
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 72 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 72 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 72 times.
✗ Branch 20 not taken.
 144 result.tail(dim) = u * q1.tail(dim) + (1 - u) * q0.tail(dim);
136 }
137
138 template void interpolate<DefaultLieGroupMap>(const DevicePtr_t& robot,
139 ConfigurationIn_t q0,
140 ConfigurationIn_t q1,
141 const value_type& u,
142 ConfigurationOut_t result);
143 template void interpolate<RnxSOnLieGroupMap>(const DevicePtr_t& robot,
144 ConfigurationIn_t q0,
145 ConfigurationIn_t q1,
146 const value_type& u,
147 ConfigurationOut_t result);
148
149 // TODO remove me. This is kept for backward compatibility
150 template void interpolate< ::pinocchio::LieGroupMap>(const DevicePtr_t& robot,
151 ConfigurationIn_t q0,
152 ConfigurationIn_t q1,
153 const value_type& u,
154 ConfigurationOut_t result);
155
156 ✗ void interpolate(const DevicePtr_t& robot, ConfigurationIn_t q0,
157 ConfigurationIn_t q1, const value_type& u,
158 ConfigurationOut_t result) {
159 ✗ interpolate<RnxSOnLieGroupMap>(robot, q0, q1, u, result);
160 }
161
162 template <typename LieGroup>
163 48120 void difference(const DevicePtr_t& robot, ConfigurationIn_t q1,
164 ConfigurationIn_t q2, vectorOut_t result) {
165 48120 const Model& model = robot->model();
166
3/6
✓ Branch 1 taken 24060 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24060 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24060 times.
✗ Branch 8 not taken.
 48120 result.head(model.nv) = ::pinocchio::difference<LieGroup>(model, q2, q1);
167 48120 const size_type& dim = robot->extraConfigSpace().dimension();
168
5/10
✓ Branch 1 taken 24060 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24060 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24059 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24060 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 24058 times.
✗ Branch 14 not taken.
 48120 result.tail(dim) = q1.tail(dim) - q2.tail(dim);
169 }
170
171 template void difference<DefaultLieGroupMap>(const DevicePtr_t& robot,
172 ConfigurationIn_t q1,
173 ConfigurationIn_t q2,
174 vectorOut_t result);
175 // TODO remove me. This is kept for backward compatibility
176 template void difference< ::pinocchio::LieGroupMap>(const DevicePtr_t& robot,
177 ConfigurationIn_t q1,
178 ConfigurationIn_t q2,
179 vectorOut_t result);
180
181 template void difference<RnxSOnLieGroupMap>(const DevicePtr_t& robot,
182 ConfigurationIn_t q1,
183 ConfigurationIn_t q2,
184 vectorOut_t result);
185
186 ✗ void difference(const DevicePtr_t& robot, ConfigurationIn_t q1,
187 ConfigurationIn_t q2, vectorOut_t result) {
188 ✗ difference<RnxSOnLieGroupMap>(robot, q1, q2, result);
189 }
190
191 48 bool isApprox(const DevicePtr_t& robot, ConfigurationIn_t q1,
192 ConfigurationIn_t q2, value_type eps) {
193
2/4
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 48 times.
 48 if (!::pinocchio::isSameConfiguration< ::pinocchio::LieGroupMap>(
194 48 robot->model(), q1, q2, eps))
195 ✗ return false;
196 48 const size_type& dim = robot->extraConfigSpace().dimension();
197
3/6
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 48 times.
✗ Branch 8 not taken.
 48 return q2.tail(dim).isApprox(q1.tail(dim), eps);
198 }
199
200 108 value_type distance(const DevicePtr_t& robot, ConfigurationIn_t q1,
201 ConfigurationIn_t q2) {
202 vector_t dist = ::pinocchio::squaredDistance< ::pinocchio::LieGroupMap>(
203
1/2
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
 108 robot->model(), q1, q2);
204 108 const size_type& dim = robot->extraConfigSpace().dimension();
205
1/2
✓ Branch 0 taken 108 times.
✗ Branch 1 not taken.
 108 if (dim == 0)
206
1/2
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
 108 return sqrt(dist.sum());
207 else
208 ✗ return sqrt(dist.sum() + (q2.tail(dim) - q1.tail(dim)).squaredNorm());
209 108 }
210
211 6 void normalize(const DevicePtr_t& robot, Configuration_t& q) {
212 6 ::pinocchio::normalize(robot->model(), q);
213 6 }
214
215 struct IsNormalizedStep
216 : public ::pinocchio::fusion::JointUnaryVisitorBase<IsNormalizedStep> {
217 typedef boost::fusion::vector<ConfigurationIn_t, const value_type&, bool&>
218 ArgsType;
219
220 template <typename JointModel>
221 1574326 static void algo(const ::pinocchio::JointModelBase<JointModel>& jmodel,
222 ConfigurationIn_t q, const value_type& eps, bool& ret) {
223 typedef typename RnxSOnLieGroupMap::operation<JointModel>::type LG_t;
224
5/8
✓ Branch 0 taken 787163 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 787196 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 787198 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 32135 times.
✓ Branch 9 taken 2 times.
 1574326 ret = ret && LG_t::isNormalized(jmodel.jointConfigSelector(q), eps);
225 }
226 };
227
228 48203 bool isNormalized(const DevicePtr_t& robot, ConfigurationIn_t q,
229 const value_type& eps) {
230 48203 bool ret = true;
231 48203 const Model& model = robot->model();
232
2/2
✓ Branch 0 taken 786983 times.
✓ Branch 1 taken 48200 times.
 835183 for (std::size_t i = 1; i < (std::size_t)model.njoints; ++i) {
233
1/2
✓ Branch 2 taken 787087 times.
✗ Branch 3 not taken.
 786323 IsNormalizedStep::run(model.joints[i],
234
1/2
✓ Branch 1 taken 786323 times.
✗ Branch 2 not taken.
 1574070 IsNormalizedStep::ArgsType(q, eps, ret));
235
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 787045 times.
 787047 if (!ret) return false;
236 }
237 48200 return true;
238 }
239
240 ✗ std::ostream& display(std::ostream& os, const SE3& m) {
241 ✗ return os << pretty_print(m);
242 }
243 } // namespace pinocchio
244 } // namespace hpp
245