GCC Code Coverage Report

Directory: ./
File: src/path-optimization/simple-time-parameterization.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 8 127 6.3%
Branches: 15 496 3.0%
Line Branch Exec Source
1 // Copyright (c) 2017, 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/core/interpolated-path.hh>
30 #include <hpp/core/path-optimization/simple-time-parameterization.hh>
31 #include <hpp/core/path-vector.hh>
32 #include <hpp/core/problem.hh>
33 #include <hpp/core/straight-path.hh>
34 #include <hpp/core/time-parameterization/polynomial.hh>
35 #include <hpp/pinocchio/configuration.hh>
36 #include <hpp/pinocchio/device.hh>
37 #include <hpp/pinocchio/liegroup.hh>
38 #include <limits>
39 #include <pinocchio/multibody/model.hpp>
40
41 namespace hpp {
42 namespace core {
43 namespace pathOptimization {
44 using std::isfinite;
45 using timeParameterization::Polynomial;
46
47 namespace {
48 ✗ TimeParameterizationPtr_t computeTimeParameterizationFirstOrder(
49 const value_type& s0, const value_type& s1, const value_type& B,
50 value_type& T) {
51 ✗ vector_t a(2);
52 ✗ T = (s1 - s0) / B;
53 ✗ a[0] = s0;
54 ✗ a[1] = B;
55 hppDout(info, "Time parametrization returned " << a.transpose() << ", " << T);
56 ✗ if (!isfinite(T) || !a.allFinite() || a.hasNaN()) {
57 ✗ HPP_THROW(std::logic_error,
58 "Invalid time parameterization "
59 "coefficients: "
60 << a.transpose());
61 }
62 ✗ return TimeParameterizationPtr_t(new Polynomial(a));
63 }
64
65 ✗ TimeParameterizationPtr_t computeTimeParameterizationThirdOrder(
66 const value_type& s0, const value_type& s1, const value_type& B,
67 value_type& T) {
68 ✗ vector_t a(4);
69 ✗ T = 3 * (s1 - s0) / (2 * B);
70 ✗ a[0] = s0;
71 ✗ a[1] = 0;
72 ✗ a[2] = 3 * (s1 - s0) / (T * T);
73 ✗ a[3] = -2 * a[2] / (3 * T);
74 hppDout(info, "Time parametrization returned " << a.transpose() << ", " << T);
75 ✗ if (!isfinite(T) || !a.allFinite() || a.hasNaN()) {
76 ✗ HPP_THROW(std::logic_error,
77 "Invalid time parameterization "
78 "coefficients: "
79 << a.transpose());
80 }
81 ✗ return TimeParameterizationPtr_t(new Polynomial(a));
82 }
83
84 ✗ TimeParameterizationPtr_t computeTimeParameterizationFifthOrder(
85 const value_type& s0, const value_type& s1, const value_type& B,
86 const value_type& C, value_type& T) {
87 ✗ vector_t a(6);
88 ✗ if (C > 0) {
89 ✗ T = std::max(
90 ✗ 15 * (s1 - s0) / (8 * B), // Velocity limit
91 ✗ sqrt((10. / std::sqrt(3))) * sqrt((s1 - s0) / C) // Acceleration limit
92 );
93 } else {
94 ✗ T = 15 * (s1 - s0) / (8 * B);
95 }
96
97 ✗ value_type Tpow = T * T * T;
98 ✗ a[0] = s0;
99 ✗ a[1] = 0;
100 ✗ a[2] = 0;
101 ✗ a[3] = 10 * (s1 - s0) / Tpow;
102 ✗ Tpow *= T;
103 ✗ a[4] = -15 * (s1 - s0) / Tpow;
104 ✗ Tpow *= T;
105 ✗ a[5] = 6 * (s1 - s0) / Tpow;
106 hppDout(info, "Time parametrization returned " << a.transpose() << ", " << T);
107 ✗ if (!isfinite(T) || !a.allFinite() || a.hasNaN()) {
108 ✗ HPP_THROW(std::logic_error,
109 "Invalid time parameterization "
110 "coefficients: "
111 << a.transpose());
112 }
113 ✗ return TimeParameterizationPtr_t(new Polynomial(a));
114 }
115
116 ✗ void checkTimeParameterization(const TimeParameterizationPtr_t tp,
117 const size_type order, const interval_t sr,
118 const value_type B, const value_type& C,
119 const value_type T) {
120 using std::fabs;
121 ✗ const value_type thr =
122 std::sqrt(Eigen::NumTraits<value_type>::dummy_precision());
123 ✗ if (fabs(tp->value(0) - sr.first) >= thr ||
124 ✗ fabs(tp->value(T) - sr.second) >= thr) {
125 ✗ HPP_THROW(std::logic_error,
126 "Interval of TimeParameterization result"
127 " is not correct. Expected "
128 << sr.first << ", " << sr.second << ". Got " << tp->value(0)
129 << ", " << tp->value(T));
130 }
131 ✗ if (order >= 1 &&
132 ✗ (fabs(tp->derivative(0, 1)) > thr || fabs(tp->derivative(T, 1)) > thr ||
133 ✗ ((C <= 0) && fabs(tp->derivative(T / 2, 1) - B) > thr))) {
134 ✗ HPP_THROW(
135 std::logic_error,
136 "Derivative of TimeParameterization are not correct:"
137 << "\ntp->derivative(0, 1) = " << tp->derivative(0, 1)
138 << "\ntp->derivative(T, 1) = " << tp->derivative(T, 1)
139 << "\ntp->derivative(T/2, 1) - B = " << tp->derivative(T / 2, 1) - B
140 << "\nT = " << T << "\nB = " << B << "\nC = " << C);
141 }
142 ✗ if (order >= 2 &&
143 ✗ (fabs(tp->derivative(0, 2)) > thr || fabs(tp->derivative(T, 2)) > thr)) {
144 ✗ HPP_THROW(std::logic_error,
145 "Derivative of TimeParameterization are not correct:"
146 << "\ntp->derivative(0, 2) = " << tp->derivative(0, 2)
147 << "\ntp->derivative(T, 2) = " << tp->derivative(T, 2)
148 << "\nT = " << T);
149 }
150 }
151 } // namespace
152
153 ✗ SimpleTimeParameterizationPtr_t SimpleTimeParameterization::create(
154 const ProblemConstPtr_t& problem) {
155 ✗ SimpleTimeParameterizationPtr_t ptr(new SimpleTimeParameterization(problem));
156 ✗ return ptr;
157 }
158
159 ✗ PathVectorPtr_t SimpleTimeParameterization::optimize(
160 const PathVectorPtr_t& path) {
161 ✗ if (path->length() == 0) {
162 ✗ return path;
163 }
164 ✗ const value_type infinity = std::numeric_limits<value_type>::infinity();
165
166 const value_type safety =
167 ✗ problem()->getParameter("SimpleTimeParameterization/safety").floatValue();
168 const size_type order =
169 ✗ problem()->getParameter("SimpleTimeParameterization/order").intValue();
170 const value_type maxAcc =
171 ✗ problem()
172 ✗ ->getParameter("SimpleTimeParameterization/maxAcceleration")
173 ✗ .floatValue();
174 ✗ if (order <= 1 && maxAcc > 0) {
175 ✗ throw std::invalid_argument(
176 "Maximum acceleration cannot be set when order is <= to 1. Please set "
177 "parameter SimpleTimeParameterization/maxAcceleration to a negative "
178 ✗ "value.");
179 }
180
181 // Retrieve velocity limits
182 ✗ const DevicePtr_t& robot = problem()->robot();
183 ✗ vector_t ub(robot->model().velocityLimit), lb(-robot->model().velocityLimit),
184 ✗ cb((ub + lb) / 2);
185 ✗ assert(cb.size() + robot->extraConfigSpace().dimension() ==
186 robot->numberDof());
187
188 // The velocity must be in [lb, ub]
189 ✗ ub = cb + safety * (ub - cb);
190 ✗ lb = cb + safety * (lb - cb);
191
192 // When ub or lb are NaN, set them to infinity.
193 ✗ ub = (ub.array() == ub.array()).select(ub, infinity);
194 ✗ lb = (lb.array() == lb.array()).select(lb, -infinity);
195
196 ✗ for (size_type i = 0; i < cb.size(); ++i) {
197 ✗ if (std::isnan(lb[i]) || std::isnan(ub[i])) {
198 ✗ HPP_THROW(
199 std::runtime_error,
200 "in SimpleTimeParameterization::optimize:\n"
201 << " the velocities of the input device should be bounded\n"
202 << " velocity bounds at rank " << i << " are [" << lb[i] << ", "
203 << ub[i] << "].");
204 }
205 }
206
207 hppDout(info, "Lower velocity bound :" << lb.transpose());
208 hppDout(info, "Upper velocity bound :" << ub.transpose());
209
210 ✗ if ((ub.array() <= 0).any() && (lb.array() >= 0).any())
211 ✗ throw std::invalid_argument(
212 "The case where zero is not an admissible velocity is not "
213 ✗ "implemented.");
214
215 PathVectorPtr_t input =
216 ✗ PathVector::create(path->outputSize(), path->outputDerivativeSize());
217 PathVectorPtr_t output =
218 ✗ PathVector::create(path->outputSize(), path->outputDerivativeSize());
219 ✗ path->flatten(input);
220
221 ✗ vector_t v(robot->numberDof());
222 ✗ vector_t v_inv(robot->numberDof());
223 ✗ for (std::size_t i = 0; i < input->numberPaths(); ++i) {
224 ✗ PathPtr_t p = input->pathAtRank(i);
225 ✗ interval_t paramRange = p->paramRange();
226 // Skip sub path of 0 length.
227 ✗ if (paramRange.first == paramRange.second) continue;
228 ✗ p->timeParameterization(TimeParameterizationPtr_t(), paramRange);
229
230 ✗ PathPtr_t pp = p->copy();
231 // if (p->length() > 0) {
232 // output->appendPath (pp);
233 // continue;
234 // }
235 // Compute B
236 ✗ p->velocityBound(v, paramRange.first, paramRange.second);
237 ✗ v_inv = (v.array() == 0).select(infinity, v.cwiseInverse());
238 ✗ const value_type B = std::min((ub.cwiseProduct(v_inv)).minCoeff(),
239 ✗ (-lb.cwiseProduct(v_inv)).minCoeff());
240 ✗ if (B <= 0 || B != B || !isfinite(B)) {
241 ✗ HPP_THROW(std::runtime_error,
242 "Invalid parametrization derivative "
243 "velocity bound: "
244 << B);
245 }
246
247 // Compute the polynom and total time
248 value_type T;
249 ✗ TimeParameterizationPtr_t tp;
250 ✗ switch (order) {
251 ✗ case 0:
252 ✗ tp = computeTimeParameterizationFirstOrder(paramRange.first,
253 ✗ paramRange.second, B, T);
254 ✗ break;
255 ✗ case 1:
256 ✗ tp = computeTimeParameterizationThirdOrder(paramRange.first,
257 ✗ paramRange.second, B, T);
258 ✗ break;
259 ✗ case 2:
260 ✗ tp = computeTimeParameterizationFifthOrder(
261 ✗ paramRange.first, paramRange.second, B, maxAcc, T);
262 ✗ break;
263 ✗ default:
264 ✗ throw std::invalid_argument(
265 "Parameter SimpleTimeParameterization/order should be in { 0, 1, 2 "
266 ✗ "}");
267 break;
268 }
269
270 ✗ checkTimeParameterization(tp, order, paramRange, B, maxAcc, T);
271
272 ✗ pp->timeParameterization(tp, interval_t(0, T));
273 ✗ output->appendPath(pp);
274 }
275 ✗ return output;
276 }
277
278 ✗ SimpleTimeParameterization::SimpleTimeParameterization(
279 ✗ const ProblemConstPtr_t& problem)
280 ✗ : PathOptimizer(problem) {}
281
282 // ----------- Declare parameters ------------------------------------- //
283
284 18 HPP_START_PARAMETER_DECLARATION(SimpleTimeParameterization)
285
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
286 Parameter::FLOAT, "SimpleTimeParameterization/safety",
287
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 "A scaling factor for the joint bounds.", Parameter(1.)));
288
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
289 Parameter::INT, "SimpleTimeParameterization/order",
290
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 "The desired continuity order.", Parameter((size_type)0)));
291
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
292 Parameter::FLOAT, "SimpleTimeParameterization/maxAcceleration",
293 "The maximum acceleration for each degree of freedom. Not considered if "
294 "negative.",
295
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter((value_type)-1)));
296 18 HPP_END_PARAMETER_DECLARATION(SimpleTimeParameterization)
297 } // namespace pathOptimization
298 } // namespace core
299 } // namespace hpp
300