1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
//! This example uses [`topp3_socp`] to convert an analytic path into a
//! third-order time-optimal trajectory whose axial velocity, acceleration, and
//! jerk all stay within `[-1, 1]`.
use copp::InterpolationMode;
use copp::diag::CoppError;
use copp::path::{Jet3, Path, sin};
use copp::robot::Robot;
use copp::solver::topp2_ra::{ReachSet2OptionsBuilder, Topp2ProblemBuilder, topp2_ra};
use copp::solver::topp3_socp::{
ClarabelOptionsBuilder, Topp3ProblemBuilder, s_to_t_topp3, t_to_s_topp3, topp3_socp,
};
use std::f64::consts::PI;
fn main() -> Result<(), CoppError> {
// 1) Deterministic 3-axis Lissajous path q(s), s in [0, 1]
let path = Path::from_parametric(
|s: Jet3| {
vec![
sin(2.0 * PI * s + 0.0),
sin(3.0 * PI * s + 0.3),
sin(5.0 * PI * s + 0.7),
]
},
0.0,
1.0,
)?;
// `n` is the number of path samples (s_i) to build robot constraints on.
let n = 1001;
let s: Vec<f64> = (0..n).map(|j| j as f64 / (n - 1) as f64).collect();
// 2) Build robot constraints (3-axis), then apply symmetric limits vel/acc/jerk = 1
const DIM: usize = 3;
let mut robot = Robot::with_capacity(DIM, n);
// The axial velocity is -1 <= vel <= 1 for each axis in this example
let vel_max = vec![1.0; DIM];
let vel_min = vec![-1.0; DIM];
// The axial acceleration is -1 <= acc <= 1 for each axis in this example.
let acc_max = vec![1.0; DIM];
let acc_min = vec![-1.0; DIM];
// The axial jerk is -1 <= jerk <= 1 for each axis in this example.
let jerk_max = vec![1.0; DIM];
let jerk_min = vec![-1.0; DIM];
robot
.with_s(s.as_slice())?
.with_q_from_path_3rd(&path, 0, n)?
.with_axial_velocity((vel_max.as_slice(), n), (vel_min.as_slice(), n), 0)?
.with_axial_acceleration((acc_max.as_slice(), n), (acc_min.as_slice(), n), 0)?
.with_axial_jerk((jerk_max.as_slice(), n), (jerk_min.as_slice(), n), 0)?;
// 3) Build a reference a(s) profile for third-order linearization.
// Jerk-level constraints are linearized around a speed-squared profile.
// TOPP2-RA is a cheap and usually feasible initial guess.
let idx_s_interval = (0, n - 1); // 0 <= k <= n-1
let a_boundary = (0.0, 0.0); // a(0) = 0, a(1) = 0
let a_ra0 = {
let topp2_problem = Topp2ProblemBuilder::new(&robot, idx_s_interval, a_boundary).build()?;
let options = ReachSet2OptionsBuilder::new().build()?;
topp2_ra(&topp2_problem, &options)?
};
// 4) Solve TOPP3-SOCP with the TOPP2-RA reference profile.
// Optional: use the TOPP2-RA profile as the first-order upper bound for
// the first linearized third-order solve. Skip this to keep the original
// `amax` bounds.
robot.constraints.amax_substitute(&a_ra0, 0)?;
let options_socp = ClarabelOptionsBuilder::new()
.allow_almost_solved(true)
.build()?;
let profile_qp1 = {
// build_with_linearization() converts nonlinear jerk rows into cached
// affine rows in `robot.constraints`, so the builder takes `&mut robot`.
// Rebuild the problem whenever the linearization profile changes.
let topp3_problem =
Topp3ProblemBuilder::new(&mut robot, idx_s_interval.0, &a_ra0, (0.0, 0.0), (0.0, 0.0))
.build_with_linearization()?;
topp3_socp(&topp3_problem, &options_socp)?
};
// 5) Post-process TOPP3-SOCP profile: (profile,s) -> t(s) -> s(t)
// t_final is the traversal time of the path.
// t_s[i] is the time at which the path parameter s_i is reached.
let (t_final1, t_s1) = s_to_t_topp3(&s, profile_qp1.as_parts(), 0.0)?;
// s_t is a uniform time grid of s(t) with dt = 1e-3s. This is useful for plotting and downstream control.
let dt = 1e-3;
let s_t1 = t_to_s_topp3(
&s,
profile_qp1.as_parts(),
&t_s1,
InterpolationMode::UniformTimeGrid(0.0, dt, true),
)?;
// 6) Print some results. More detailed results and plots can be achieved by the user.
// profile_qp1 is a feasible but possibly suboptimal profile for TOPP3. It can be directly used for control or as a reference for further optimization unless a more optimal profile is required.
println!("TOPP3-SOCP done. (The first-iteration)");
println!("dim = {DIM}, N = {n}");
println!("t_final = {t_final1:.6} s");
println!("a_profile.len() = {}", profile_qp1.a.len());
println!("b_profile.len() = {}", profile_qp1.b.len());
println!("s(t) samples = {}", s_t1.len());
// 7) Rebuild TOPP3-SOCP around the first solution and solve one SCP refinement.
let profile_qp2 = {
// The linearization point changes from `a_ra0` to `profile_qp1.a`.
// Rebuilding refreshes the cached affine jerk rows before the second solve.
let topp3_problem = Topp3ProblemBuilder::new(
&mut robot,
idx_s_interval.0,
&profile_qp1.a,
(0.0, 0.0),
(0.0, 0.0),
)
.build_with_linearization()?;
topp3_socp(&topp3_problem, &options_socp)?
};
// 8) Post-process TOPP3-SOCP profile: (profile,s) -> t(s) -> s(t)
// t_final is the traversal time of the path.
// t_s[i] is the time at which the path parameter s_i is reached.
let (t_final2, t_s2) = s_to_t_topp3(&s, profile_qp2.as_parts(), 0.0)?;
// s_t is a uniform time grid of s(t) with dt = 1e-3s. This is useful for plotting and downstream control.
let dt = 1e-3;
let s_t2 = t_to_s_topp3(
&s,
profile_qp2.as_parts(),
&t_s2,
InterpolationMode::UniformTimeGrid(0.0, dt, true),
)?;
// 9) Print some results. More detailed results and plots can be achieved by the user.
// profile_qp2 is a less conservative and more optimal profile for TOPP3 compared with profile_qp1.
println!("---------\nTOPP3-SOCP done. (The second-iteration)");
println!("dim = {DIM}, N = {n}");
println!("t_final = {t_final2:.6} s <= {t_final1:.6} s");
println!("a_profile.len() = {}", profile_qp2.a.len());
println!("b_profile.len() = {}", profile_qp2.b.len());
println!("s(t) samples = {}", s_t2.len());
Ok(())
}