COPP C++ API 0.2.3
C++ interface for COPP trajectory optimization
Loading...
Searching...
No Matches
interpolation.hpp
Go to the documentation of this file.
1#pragma once
2
3#include <utility>
4#include <vector>
5
6#include "copp/core.hpp"
7
8namespace copp
9{
10
21 struct COPP_API Profile3rdRef
22 {
23 Span<const double> a;
24 Span<const double> b;
25 std::size_t num_stationary_start = 0;
26 std::size_t num_stationary_end = 0;
27
28 Profile3rdRef() = default;
29 Profile3rdRef(Span<const double> a_values, Span<const double> b_values);
30 Profile3rdRef(
31 Span<const double> a_values,
32 Span<const double> b_values,
33 std::pair<std::size_t, std::size_t> num_stationary);
34 Profile3rdRef(
35 Span<const double> a_values,
36 Span<const double> b_values,
37 std::size_t stationary_start,
38 std::size_t stationary_end);
39
40 std::size_t len() const noexcept { return a.size(); }
41 std::size_t size() const noexcept { return a.size(); }
42 bool empty() const noexcept { return a.empty(); }
43 std::pair<std::size_t, std::size_t> num_stationary() const noexcept
44 {
45 return {num_stationary_start, num_stationary_end};
46 }
47
55 Profile3rdRef slice(std::size_t idx_s_start, std::size_t idx_s_final) const;
56
58 Profile3rdRef segment(std::size_t idx_s) const;
59 };
60
68 struct COPP_API Profile3rd
69 {
70 std::vector<double> a;
71 std::vector<double> b;
72 std::size_t num_stationary_start = 0;
73 std::size_t num_stationary_end = 0;
74
75 Profile3rd() = default;
76 Profile3rd(std::vector<double> a_values, std::vector<double> b_values);
77 Profile3rd(
78 std::vector<double> a_values,
79 std::vector<double> b_values,
80 std::pair<std::size_t, std::size_t> num_stationary);
81 Profile3rd(
82 std::vector<double> a_values,
83 std::vector<double> b_values,
84 std::size_t stationary_start,
85 std::size_t stationary_end);
86
87 std::size_t len() const noexcept { return a.size(); }
88 std::size_t size() const noexcept { return a.size(); }
89 bool empty() const noexcept { return a.empty(); }
90 std::pair<std::size_t, std::size_t> num_stationary() const noexcept
91 {
92 return {num_stationary_start, num_stationary_end};
93 }
94
96 Profile3rdRef ref() const;
97
99 Profile3rdRef slice(std::size_t idx_s_start, std::size_t idx_s_final) const;
100
102 Profile3rdRef segment(std::size_t idx_s) const;
103
124 bool force_positive_a(Span<const double> s, double a_min = 1.0e-12);
125
131 Expected<bool> force_positive_a(Span<const double> s, double a_min, NoThrowTag);
132 Expected<bool> force_positive_a(Span<const double> s, NoThrowTag);
133 };
134
135 inline Expected<bool> Profile3rd::force_positive_a(
136 Span<const double> s,
137 double a_min,
138 NoThrowTag)
139 {
140 return detail::expected_from([&] { return force_positive_a(s, a_min); });
141 }
142
143 inline Expected<bool> Profile3rd::force_positive_a(Span<const double> s, NoThrowTag tag)
144 {
145 return force_positive_a(s, 1.0e-12, tag);
146 }
147
148} // namespace copp
149
150namespace copp::interpolation
151{
152
157 struct TimeProfile
158 {
159 double t_final = 0.0;
160 std::vector<double> t_s;
161 };
162
167 struct TimeGridOptions
168 {
169 double t0 = 0.0;
170 bool include_final = true;
171 };
172
182 COPP_API std::vector<double> a_to_b_topp2(Span<const double> s, Span<const double> a);
183 inline Expected<std::vector<double>> a_to_b_topp2(
184 Span<const double> s,
185 Span<const double> a,
186 NoThrowTag)
187 {
188 return detail::expected_from([&] { return a_to_b_topp2(s, a); });
189 }
190
213 COPP_API TimeProfile s_to_t_topp2(Span<const double> s, Span<const double> a, double t0 = 0.0);
214 inline Expected<TimeProfile> s_to_t_topp2(
215 Span<const double> s,
216 Span<const double> a,
217 double t0,
218 NoThrowTag)
219 {
220 return detail::expected_from([&] { return s_to_t_topp2(s, a, t0); });
221 }
222
223 inline Expected<TimeProfile> s_to_t_topp2(
224 Span<const double> s,
225 Span<const double> a,
226 NoThrowTag tag)
227 {
228 return s_to_t_topp2(s, a, 0.0, tag);
229 }
230
237 COPP_API std::vector<double> t_to_s_topp2_uniform(
238 Span<const double> s,
239 Span<const double> a,
240 Span<const double> t_s,
241 double dt,
242 TimeGridOptions options = {});
243 inline Expected<std::vector<double>> t_to_s_topp2_uniform(
244 Span<const double> s,
245 Span<const double> a,
246 Span<const double> t_s,
247 double dt,
248 TimeGridOptions options,
249 NoThrowTag)
250 {
251 return detail::expected_from([&] { return t_to_s_topp2_uniform(s, a, t_s, dt, options); });
252 }
253
254 inline Expected<std::vector<double>> t_to_s_topp2_uniform(
255 Span<const double> s,
256 Span<const double> a,
257 Span<const double> t_s,
258 double dt,
259 NoThrowTag tag)
260 {
261 return t_to_s_topp2_uniform(s, a, t_s, dt, TimeGridOptions{}, tag);
262 }
263
268 COPP_API std::vector<double> t_to_s_topp2_samples(
269 Span<const double> s,
270 Span<const double> a,
271 Span<const double> t_s,
272 Span<const double> t_sample);
273 inline Expected<std::vector<double>> t_to_s_topp2_samples(
274 Span<const double> s,
275 Span<const double> a,
276 Span<const double> t_s,
277 Span<const double> t_sample,
278 NoThrowTag)
279 {
280 return detail::expected_from([&] { return t_to_s_topp2_samples(s, a, t_s, t_sample); });
281 }
282
301 COPP_API TimeProfile s_to_t_topp3(
302 Span<const double> s,
303 const Profile3rd &profile,
304 double t0 = 0.0);
305 COPP_API TimeProfile s_to_t_topp3(
306 Span<const double> s,
307 Profile3rdRef profile,
308 double t0 = 0.0);
309 inline Expected<TimeProfile> s_to_t_topp3(
310 Span<const double> s,
311 const Profile3rd &profile,
312 double t0,
313 NoThrowTag)
314 {
315 return detail::expected_from([&] { return s_to_t_topp3(s, profile, t0); });
316 }
317
318 inline Expected<TimeProfile> s_to_t_topp3(
319 Span<const double> s,
320 const Profile3rd &profile,
321 NoThrowTag tag)
322 {
323 return s_to_t_topp3(s, profile, 0.0, tag);
324 }
325
326 inline Expected<TimeProfile> s_to_t_topp3(
327 Span<const double> s,
328 Profile3rdRef profile,
329 double t0,
330 NoThrowTag)
331 {
332 return detail::expected_from([&] { return s_to_t_topp3(s, profile, t0); });
333 }
334
335 inline Expected<TimeProfile> s_to_t_topp3(
336 Span<const double> s,
337 Profile3rdRef profile,
338 NoThrowTag tag)
339 {
340 return s_to_t_topp3(s, profile, 0.0, tag);
341 }
342
347 COPP_API std::vector<double> t_to_s_topp3_uniform(
348 Span<const double> s,
349 const Profile3rd &profile,
350 Span<const double> t_s,
351 double dt,
352 TimeGridOptions options = {});
353 COPP_API std::vector<double> t_to_s_topp3_uniform(
354 Span<const double> s,
355 Profile3rdRef profile,
356 Span<const double> t_s,
357 double dt,
358 TimeGridOptions options = {});
359 inline Expected<std::vector<double>> t_to_s_topp3_uniform(
360 Span<const double> s,
361 const Profile3rd &profile,
362 Span<const double> t_s,
363 double dt,
364 TimeGridOptions options,
365 NoThrowTag)
366 {
367 return detail::expected_from(
368 [&] { return t_to_s_topp3_uniform(s, profile, t_s, dt, options); });
369 }
370
371 inline Expected<std::vector<double>> t_to_s_topp3_uniform(
372 Span<const double> s,
373 const Profile3rd &profile,
374 Span<const double> t_s,
375 double dt,
376 NoThrowTag tag)
377 {
378 return t_to_s_topp3_uniform(s, profile, t_s, dt, TimeGridOptions{}, tag);
379 }
380
381 inline Expected<std::vector<double>> t_to_s_topp3_uniform(
382 Span<const double> s,
383 Profile3rdRef profile,
384 Span<const double> t_s,
385 double dt,
386 TimeGridOptions options,
387 NoThrowTag)
388 {
389 return detail::expected_from(
390 [&] { return t_to_s_topp3_uniform(s, profile, t_s, dt, options); });
391 }
392
393 inline Expected<std::vector<double>> t_to_s_topp3_uniform(
394 Span<const double> s,
395 Profile3rdRef profile,
396 Span<const double> t_s,
397 double dt,
398 NoThrowTag tag)
399 {
400 return t_to_s_topp3_uniform(s, profile, t_s, dt, TimeGridOptions{}, tag);
401 }
402
411 COPP_API std::vector<double> t_to_s_topp3_samples(
412 Span<const double> s,
413 const Profile3rd &profile,
414 Span<const double> t_s,
415 Span<const double> t_sample);
416 COPP_API std::vector<double> t_to_s_topp3_samples(
417 Span<const double> s,
418 Profile3rdRef profile,
419 Span<const double> t_s,
420 Span<const double> t_sample);
421 inline Expected<std::vector<double>> t_to_s_topp3_samples(
422 Span<const double> s,
423 const Profile3rd &profile,
424 Span<const double> t_s,
425 Span<const double> t_sample,
426 NoThrowTag)
427 {
428 return detail::expected_from([&] { return t_to_s_topp3_samples(s, profile, t_s, t_sample); });
429 }
430
431 inline Expected<std::vector<double>> t_to_s_topp3_samples(
432 Span<const double> s,
433 Profile3rdRef profile,
434 Span<const double> t_s,
435 Span<const double> t_sample,
436 NoThrowTag)
437 {
438 return detail::expected_from([&] { return t_to_s_topp3_samples(s, profile, t_s, t_sample); });
439 }
440
441} // namespace copp::interpolation
copp::Expected
Small C++17 expected-like result used by no-throw overloads.
Definition core.hpp:527
copp::Span
Non-owning view of a contiguous one-dimensional array.
Definition core.hpp:139
COPP_API
#define COPP_API
Definition core.hpp:23
copp::detail::expected_from
auto expected_from(Fn &&fn) -> Expected< typename std::decay< decltype(fn())>::type >
Definition core.hpp:580
copp::interpolation
Time-conversion helpers for TOPP2/TOPP3 profiles, including s -> t, uniform t -> s,...
copp::interpolation::t_to_s_topp2_uniform
COPP_API std::vector< double > t_to_s_topp2_uniform(Span< const double > s, Span< const double > a, Span< const double > t_s, double dt, TimeGridOptions options={})
Sample the inverse TOPP2 mapping s(t) on a uniform time grid.
copp::interpolation::a_to_b_topp2
COPP_API std::vector< double > a_to_b_topp2(Span< const double > s, Span< const double > a)
Compute segment profile b from node profile a.
copp::interpolation::s_to_t_topp2
COPP_API TimeProfile s_to_t_topp2(Span< const double > s, Span< const double > a, double t0=0.0)
Convert a second-order path profile a(s) = ds/dt squared to t(s).
copp::interpolation::t_to_s_topp3_samples
COPP_API std::vector< double > t_to_s_topp3_samples(Span< const double > s, const Profile3rd &profile, Span< const double > t_s, Span< const double > t_sample)
Sample the inverse TOPP3 mapping s(t) at explicit query times.
copp::interpolation::t_to_s_topp2_samples
COPP_API std::vector< double > t_to_s_topp2_samples(Span< const double > s, Span< const double > a, Span< const double > t_s, Span< const double > t_sample)
Sample the inverse TOPP2 mapping s(t) at explicit query times.
copp::interpolation::s_to_t_topp3
COPP_API TimeProfile s_to_t_topp3(Span< const double > s, const Profile3rd &profile, double t0=0.0)
Convert a third-order profile to cumulative time t(s).
copp::interpolation::t_to_s_topp3_uniform
COPP_API std::vector< double > t_to_s_topp3_uniform(Span< const double > s, const Profile3rd &profile, Span< const double > t_s, double dt, TimeGridOptions options={})
Sample the inverse TOPP3 mapping s(t) on a uniform time grid.
copp
Root namespace for the C++ facade: paths, robots, constraints, matrices, profiles,...
copp::NoThrowTag
Tag object selecting no-throw overloads.
Definition core.hpp:608
copp::Profile3rd
Owned third-order TOPP/COPP profile.
Definition interpolation.hpp:69
copp::Profile3rd::force_positive_a
bool force_positive_a(Span< const double > s, double a_min=1.0e-12)
Adjust a and b in place so interpolated a(s) stays strictly positive on every interval.
copp::Profile3rd::Profile3rd
Profile3rd()=default
copp::Profile3rd::b
std::vector< double > b
Definition interpolation.hpp:71
copp::Profile3rd::size
std::size_t size() const noexcept
Definition interpolation.hpp:88
copp::Profile3rd::a
std::vector< double > a
Definition interpolation.hpp:70
copp::Profile3rd::Profile3rd
Profile3rd(std::vector< double > a_values, std::vector< double > b_values)
copp::Profile3rd::empty
bool empty() const noexcept
Definition interpolation.hpp:89
copp::Profile3rd::num_stationary_start
std::size_t num_stationary_start
Definition interpolation.hpp:72
copp::Profile3rd::num_stationary
std::pair< std::size_t, std::size_t > num_stationary() const noexcept
Definition interpolation.hpp:90
copp::Profile3rd::Profile3rd
Profile3rd(std::vector< double > a_values, std::vector< double > b_values, std::size_t stationary_start, std::size_t stationary_end)
copp::Profile3rd::len
std::size_t len() const noexcept
Definition interpolation.hpp:87
copp::Profile3rd::slice
Profile3rdRef slice(std::size_t idx_s_start, std::size_t idx_s_final) const
Borrow a closed station subrange idx_s_start..=idx_s_final.
copp::Profile3rd::segment
Profile3rdRef segment(std::size_t idx_s) const
Borrow one segment as the two-node profile idx_s..=idx_s+1.
copp::Profile3rd::num_stationary_end
std::size_t num_stationary_end
Definition interpolation.hpp:73
copp::Profile3rd::ref
Profile3rdRef ref() const
Borrow this owned profile without copying arrays.
copp::Profile3rd::Profile3rd
Profile3rd(std::vector< double > a_values, std::vector< double > b_values, std::pair< std::size_t, std::size_t > num_stationary)
copp::Profile3rdRef
Borrowed third-order TOPP/COPP profile view.
Definition interpolation.hpp:22
copp::Profile3rdRef::num_stationary_start
std::size_t num_stationary_start
Definition interpolation.hpp:25
copp::Profile3rdRef::empty
bool empty() const noexcept
Definition interpolation.hpp:42
copp::Profile3rdRef::num_stationary
std::pair< std::size_t, std::size_t > num_stationary() const noexcept
Definition interpolation.hpp:43
copp::Profile3rdRef::b
Span< const double > b
Definition interpolation.hpp:24
copp::Profile3rdRef::Profile3rdRef
Profile3rdRef()=default
copp::Profile3rdRef::Profile3rdRef
Profile3rdRef(Span< const double > a_values, Span< const double > b_values, std::size_t stationary_start, std::size_t stationary_end)
copp::Profile3rdRef::Profile3rdRef
Profile3rdRef(Span< const double > a_values, Span< const double > b_values, std::pair< std::size_t, std::size_t > num_stationary)
copp::Profile3rdRef::a
Span< const double > a
Definition interpolation.hpp:23
copp::Profile3rdRef::slice
Profile3rdRef slice(std::size_t idx_s_start, std::size_t idx_s_final) const
Borrow a closed station subrange idx_s_start..=idx_s_final.
copp::Profile3rdRef::len
std::size_t len() const noexcept
Definition interpolation.hpp:40
copp::Profile3rdRef::size
std::size_t size() const noexcept
Definition interpolation.hpp:41
copp::Profile3rdRef::num_stationary_end
std::size_t num_stationary_end
Definition interpolation.hpp:26
copp::Profile3rdRef::segment
Profile3rdRef segment(std::size_t idx_s) const
Borrow one segment as the two-node profile idx_s..=idx_s+1.
copp::Profile3rdRef::Profile3rdRef
Profile3rdRef(Span< const double > a_values, Span< const double > b_values)
copp::interpolation::TimeGridOptions
Options for generating a uniform s(t) sampling grid.
Definition interpolation.hpp:168
copp::interpolation::TimeProfile
Cumulative time profile returned by s_to_t_topp2.
Definition interpolation.hpp:158