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
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
//! This example uses [`copp3_socp`] to convert an analytic path into a
//! third-order convex-objective 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::copp3_socp::{
ClarabelOptionsBuilder, Copp3ProblemBuilder, CoppObjective, copp3_socp, s_to_t_topp3,
t_to_s_topp3,
};
use copp::solver::topp2_ra::{ReachSet2OptionsBuilder, Topp2ProblemBuilder, topp2_ra};
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 COPP3-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)?;
// Here we use a hybrid objective: 1.0 * time + 0.1 * thermal energy.
// We use `usize` as a trivial point-mass robot model (inverse dynamics: `tau = ddq`) in this example.
// The user should replace this with their real robot model where traits `RobotBasic` and `RobotTorque` are implemented.
let objectives = [
CoppObjective::Time(1.0),
CoppObjective::ThermalEnergy(0.1, &[1.0; DIM]),
];
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 copp3_problem = Copp3ProblemBuilder::new(
&mut robot,
&objectives,
idx_s_interval.0,
&a_ra0,
(0.0, 0.0),
(0.0, 0.0),
)
.build_with_linearization()?;
copp3_socp(&copp3_problem, &options_socp)?
};
// 5) Post-process COPP3-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 COPP3. It can be directly used for control or as a reference for further optimization unless a more optimal profile is required.
println!("COPP3-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 COPP3-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 copp3_problem = Copp3ProblemBuilder::new(
&mut robot,
&objectives,
idx_s_interval.0,
&profile_qp1.a,
(0.0, 0.0),
(0.0, 0.0),
)
.build_with_linearization()?;
copp3_socp(&copp3_problem, &options_socp)?
};
// 8) Post-process COPP3-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 COPP3 compared with profile_qp1.
println!("---------\nCOPP3-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(())
}