COPP C++ API
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 };
104
105} // namespace copp
106
107namespace copp::interpolation
108{
109
114 struct TimeProfile
115 {
116 double t_final = 0.0;
117 std::vector<double> t_s;
118 };
119
124 struct TimeGridOptions
125 {
126 double t0 = 0.0;
127 bool include_final = true;
128 };
129
139 COPP_API std::vector<double> a_to_b_topp2(Span<const double> s, Span<const double> a);
140 inline Expected<std::vector<double>> a_to_b_topp2(
141 Span<const double> s,
142 Span<const double> a,
143 NoThrowTag)
144 {
145 return detail::expected_from([&] { return a_to_b_topp2(s, a); });
146 }
147
170 COPP_API TimeProfile s_to_t_topp2(Span<const double> s, Span<const double> a, double t0 = 0.0);
171 inline Expected<TimeProfile> s_to_t_topp2(
172 Span<const double> s,
173 Span<const double> a,
174 double t0,
175 NoThrowTag)
176 {
177 return detail::expected_from([&] { return s_to_t_topp2(s, a, t0); });
178 }
179
180 inline Expected<TimeProfile> s_to_t_topp2(
181 Span<const double> s,
182 Span<const double> a,
183 NoThrowTag tag)
184 {
185 return s_to_t_topp2(s, a, 0.0, tag);
186 }
187
194 COPP_API std::vector<double> t_to_s_topp2_uniform(
195 Span<const double> s,
196 Span<const double> a,
197 Span<const double> t_s,
198 double dt,
199 TimeGridOptions options = {});
200 inline Expected<std::vector<double>> t_to_s_topp2_uniform(
201 Span<const double> s,
202 Span<const double> a,
203 Span<const double> t_s,
204 double dt,
205 TimeGridOptions options,
206 NoThrowTag)
207 {
208 return detail::expected_from([&] { return t_to_s_topp2_uniform(s, a, t_s, dt, options); });
209 }
210
211 inline Expected<std::vector<double>> t_to_s_topp2_uniform(
212 Span<const double> s,
213 Span<const double> a,
214 Span<const double> t_s,
215 double dt,
216 NoThrowTag tag)
217 {
218 return t_to_s_topp2_uniform(s, a, t_s, dt, TimeGridOptions{}, tag);
219 }
220
225 COPP_API std::vector<double> t_to_s_topp2_samples(
226 Span<const double> s,
227 Span<const double> a,
228 Span<const double> t_s,
229 Span<const double> t_sample);
230 inline Expected<std::vector<double>> t_to_s_topp2_samples(
231 Span<const double> s,
232 Span<const double> a,
233 Span<const double> t_s,
234 Span<const double> t_sample,
235 NoThrowTag)
236 {
237 return detail::expected_from([&] { return t_to_s_topp2_samples(s, a, t_s, t_sample); });
238 }
239
258 COPP_API TimeProfile s_to_t_topp3(
259 Span<const double> s,
260 const Profile3rd &profile,
261 double t0 = 0.0);
262 COPP_API TimeProfile s_to_t_topp3(
263 Span<const double> s,
264 Profile3rdRef profile,
265 double t0 = 0.0);
266 inline Expected<TimeProfile> s_to_t_topp3(
267 Span<const double> s,
268 const Profile3rd &profile,
269 double t0,
270 NoThrowTag)
271 {
272 return detail::expected_from([&] { return s_to_t_topp3(s, profile, t0); });
273 }
274
275 inline Expected<TimeProfile> s_to_t_topp3(
276 Span<const double> s,
277 const Profile3rd &profile,
278 NoThrowTag tag)
279 {
280 return s_to_t_topp3(s, profile, 0.0, tag);
281 }
282
283 inline Expected<TimeProfile> s_to_t_topp3(
284 Span<const double> s,
285 Profile3rdRef profile,
286 double t0,
287 NoThrowTag)
288 {
289 return detail::expected_from([&] { return s_to_t_topp3(s, profile, t0); });
290 }
291
292 inline Expected<TimeProfile> s_to_t_topp3(
293 Span<const double> s,
294 Profile3rdRef profile,
295 NoThrowTag tag)
296 {
297 return s_to_t_topp3(s, profile, 0.0, tag);
298 }
299
304 COPP_API std::vector<double> t_to_s_topp3_uniform(
305 Span<const double> s,
306 const Profile3rd &profile,
307 Span<const double> t_s,
308 double dt,
309 TimeGridOptions options = {});
310 COPP_API std::vector<double> t_to_s_topp3_uniform(
311 Span<const double> s,
312 Profile3rdRef profile,
313 Span<const double> t_s,
314 double dt,
315 TimeGridOptions options = {});
316 inline Expected<std::vector<double>> t_to_s_topp3_uniform(
317 Span<const double> s,
318 const Profile3rd &profile,
319 Span<const double> t_s,
320 double dt,
321 TimeGridOptions options,
322 NoThrowTag)
323 {
324 return detail::expected_from(
325 [&] { return t_to_s_topp3_uniform(s, profile, t_s, dt, options); });
326 }
327
328 inline Expected<std::vector<double>> t_to_s_topp3_uniform(
329 Span<const double> s,
330 const Profile3rd &profile,
331 Span<const double> t_s,
332 double dt,
333 NoThrowTag tag)
334 {
335 return t_to_s_topp3_uniform(s, profile, t_s, dt, TimeGridOptions{}, tag);
336 }
337
338 inline Expected<std::vector<double>> t_to_s_topp3_uniform(
339 Span<const double> s,
340 Profile3rdRef profile,
341 Span<const double> t_s,
342 double dt,
343 TimeGridOptions options,
344 NoThrowTag)
345 {
346 return detail::expected_from(
347 [&] { return t_to_s_topp3_uniform(s, profile, t_s, dt, options); });
348 }
349
350 inline Expected<std::vector<double>> t_to_s_topp3_uniform(
351 Span<const double> s,
352 Profile3rdRef profile,
353 Span<const double> t_s,
354 double dt,
355 NoThrowTag tag)
356 {
357 return t_to_s_topp3_uniform(s, profile, t_s, dt, TimeGridOptions{}, tag);
358 }
359
368 COPP_API std::vector<double> t_to_s_topp3_samples(
369 Span<const double> s,
370 const Profile3rd &profile,
371 Span<const double> t_s,
372 Span<const double> t_sample);
373 COPP_API std::vector<double> t_to_s_topp3_samples(
374 Span<const double> s,
375 Profile3rdRef profile,
376 Span<const double> t_s,
377 Span<const double> t_sample);
378 inline Expected<std::vector<double>> t_to_s_topp3_samples(
379 Span<const double> s,
380 const Profile3rd &profile,
381 Span<const double> t_s,
382 Span<const double> t_sample,
383 NoThrowTag)
384 {
385 return detail::expected_from([&] { return t_to_s_topp3_samples(s, profile, t_s, t_sample); });
386 }
387
388 inline Expected<std::vector<double>> t_to_s_topp3_samples(
389 Span<const double> s,
390 Profile3rdRef profile,
391 Span<const double> t_s,
392 Span<const double> t_sample,
393 NoThrowTag)
394 {
395 return detail::expected_from([&] { return t_to_s_topp3_samples(s, profile, t_s, t_sample); });
396 }
397
398} // 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::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:125
copp::interpolation::TimeProfile
Cumulative time profile returned by s_to_t_topp2.
Definition interpolation.hpp:115