Skip to content

Commit 1db3ea9

Browse files
calcmogulcalcmogul
committed
[trajoptlib] Use geometry types in Rust FFI
1 parent dd343ba commit 1db3ea9

13 files changed

Lines changed: 452 additions & 546 deletions

File tree

‎src-core/src/generation/transformers/constraints.rs‎

Lines changed: 52 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@ use super::{
44
DifferentialGenerationTransformer, FeatureLockedTransformer, GenerationContext,
55
SwerveGenerationTransformer,
66
};
7+
use trajoptlib::Translation2d;
78

89
fn fix_scope(idx: usize, removed_idxs: &[usize]) -> usize {
910
let mut to_subtract: usize = 0;
@@ -108,8 +109,10 @@ impl SwerveGenerationTransformer for ConstraintSetter {
108109
tolerance,
109110
flip,
110111
} => match to_opt {
111-
None => generator.wpt_point_at(from, x, y, tolerance, flip),
112-
Some(to) => generator.sgmt_point_at(from, to, x, y, tolerance, flip),
112+
None => generator.wpt_point_at(from, Translation2d { x, y }, tolerance, flip),
113+
Some(to) => {
114+
generator.sgmt_point_at(from, to, Translation2d { x, y }, tolerance, flip)
115+
}
113116
},
114117
ConstraintData::MaxVelocity { max } => match to_opt {
115118
None => generator.wpt_linear_velocity_max_magnitude(from, max),
@@ -130,15 +133,19 @@ impl SwerveGenerationTransformer for ConstraintSetter {
130133
}
131134
}
132135
ConstraintData::KeepInCircle { x, y, r } => match to_opt {
133-
None => generator.wpt_keep_in_circle(from, x, y, r),
134-
Some(to) => generator.sgmt_keep_in_circle(from, to, x, y, r),
136+
None => generator.wpt_keep_in_circle(from, Translation2d { x, y }, r),
137+
Some(to) => generator.sgmt_keep_in_circle(from, to, Translation2d { x, y }, r),
135138
},
136139
ConstraintData::KeepInRectangle { x, y, w, h } => {
137-
let xs = vec![x, x + w, x + w, x];
138-
let ys = vec![y, y, y + h, y + h];
140+
let field_points = vec![
141+
Translation2d { x, y },
142+
Translation2d { x: x + w, y },
143+
Translation2d { x: x + w, y: y + h },
144+
Translation2d { x, y: y + h },
145+
];
139146
match to_opt {
140-
None => generator.wpt_keep_in_polygon(from, xs, ys),
141-
Some(to) => generator.sgmt_keep_in_polygon(from, to, xs, ys),
147+
None => generator.wpt_keep_in_polygon(from, field_points),
148+
Some(to) => generator.sgmt_keep_in_polygon(from, to, field_points),
142149
}
143150
}
144151
ConstraintData::KeepInLane { tolerance } => {
@@ -147,13 +154,23 @@ impl SwerveGenerationTransformer for ConstraintSetter {
147154
&& let Some(wpt_to) = self.waypoint_idx.get(idx_to)
148155
{
149156
generator.sgmt_keep_in_lane(
150-
from, idx_to, wpt_from.x, wpt_from.y, wpt_to.x, wpt_to.y, tolerance,
157+
from,
158+
idx_to,
159+
Translation2d {
160+
x: wpt_from.x,
161+
y: wpt_from.y,
162+
},
163+
Translation2d {
164+
x: wpt_to.x,
165+
y: wpt_to.y,
166+
},
167+
tolerance,
151168
);
152169
}
153170
}
154171
ConstraintData::KeepOutCircle { x, y, r } => match to_opt {
155-
None => generator.wpt_keep_out_circle(from, x, y, r),
156-
Some(to) => generator.sgmt_keep_out_circle(from, to, x, y, r),
172+
None => generator.wpt_keep_out_circle(from, Translation2d { x, y }, r),
173+
Some(to) => generator.sgmt_keep_out_circle(from, to, Translation2d { x, y }, r),
157174
},
158175
};
159176
}
@@ -177,7 +194,7 @@ impl DifferentialGenerationTransformer for ConstraintSetter {
177194
flip,
178195
} => {
179196
if to_opt.is_none() {
180-
generator.wpt_point_at(from, x, y, tolerance, flip)
197+
generator.wpt_point_at(from, Translation2d { x, y }, tolerance, flip)
181198
}
182199
}
183200
ConstraintData::MaxVelocity { max } => match to_opt {
@@ -199,15 +216,19 @@ impl DifferentialGenerationTransformer for ConstraintSetter {
199216
}
200217
}
201218
ConstraintData::KeepInCircle { x, y, r } => match to_opt {
202-
None => generator.wpt_keep_in_circle(from, x, y, r),
203-
Some(to) => generator.sgmt_keep_in_circle(from, to, x, y, r),
219+
None => generator.wpt_keep_in_circle(from, Translation2d { x, y }, r),
220+
Some(to) => generator.sgmt_keep_in_circle(from, to, Translation2d { x, y }, r),
204221
},
205222
ConstraintData::KeepInRectangle { x, y, w, h } => {
206-
let xs = vec![x, x + w, x + w, x];
207-
let ys = vec![y, y, y + h, y + h];
223+
let field_points = vec![
224+
Translation2d { x, y },
225+
Translation2d { x: x + w, y },
226+
Translation2d { x: x + w, y: y + h },
227+
Translation2d { x, y: y + h },
228+
];
208229
match to_opt {
209-
None => generator.wpt_keep_in_polygon(from, xs, ys),
210-
Some(to) => generator.sgmt_keep_in_polygon(from, to, xs, ys),
230+
None => generator.wpt_keep_in_polygon(from, field_points),
231+
Some(to) => generator.sgmt_keep_in_polygon(from, to, field_points),
211232
}
212233
}
213234
ConstraintData::KeepInLane { tolerance } => {
@@ -216,13 +237,23 @@ impl DifferentialGenerationTransformer for ConstraintSetter {
216237
&& let Some(wpt_to) = self.waypoint_idx.get(idx_to)
217238
{
218239
generator.sgmt_keep_in_lane(
219-
from, idx_to, wpt_from.x, wpt_from.y, wpt_to.x, wpt_to.y, tolerance,
240+
from,
241+
idx_to,
242+
Translation2d {
243+
x: wpt_from.x,
244+
y: wpt_from.y,
245+
},
246+
Translation2d {
247+
x: wpt_to.x,
248+
y: wpt_to.y,
249+
},
250+
tolerance,
220251
);
221252
}
222253
}
223254
ConstraintData::KeepOutCircle { x, y, r } => match to_opt {
224-
None => generator.wpt_keep_out_circle(from, x, y, r),
225-
Some(to) => generator.sgmt_keep_out_circle(from, to, x, y, r),
255+
None => generator.wpt_keep_out_circle(from, Translation2d { x, y }, r),
256+
Some(to) => generator.sgmt_keep_out_circle(from, to, Translation2d { x, y }, r),
226257
},
227258
};
228259
}

‎src-core/src/generation/transformers/interval_count.rs‎

Lines changed: 43 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
use trajoptlib::Pose2d;
1+
use trajoptlib::{Pose2d, Translation2d};
22

33
use crate::spec::trajectory::Waypoint;
44

@@ -43,11 +43,29 @@ impl SwerveGenerationTransformer for IntervalCountSetter {
4343
}
4444
guess_points_after_waypoint.clear();
4545
if wpt.fix_heading && wpt.fix_translation {
46-
generator.pose_wpt(wpt_cnt, wpt.x, wpt.y, wpt.heading);
46+
generator.pose_wpt(
47+
wpt_cnt,
48+
Pose2d {
49+
x: wpt.x,
50+
y: wpt.y,
51+
heading: wpt.heading,
52+
},
53+
);
4754
} else if wpt.fix_translation {
48-
generator.translation_wpt(wpt_cnt, wpt.x, wpt.y, wpt.heading);
55+
generator.translation_wpt(
56+
wpt_cnt,
57+
Translation2d { x: wpt.x, y: wpt.y },
58+
wpt.heading,
59+
);
4960
} else {
50-
generator.empty_wpt(wpt_cnt, wpt.x, wpt.y, wpt.heading);
61+
generator.empty_wpt(
62+
wpt_cnt,
63+
Pose2d {
64+
x: wpt.x,
65+
y: wpt.y,
66+
heading: wpt.heading,
67+
},
68+
);
5169
}
5270
wpt_cnt += 1;
5371
if i != waypoints.len() - 1 {
@@ -92,11 +110,29 @@ impl DifferentialGenerationTransformer for IntervalCountSetter {
92110
}
93111
guess_points_after_waypoint.clear();
94112
if wpt.fix_heading && wpt.fix_translation {
95-
generator.pose_wpt(wpt_cnt, wpt.x, wpt.y, wpt.heading);
113+
generator.pose_wpt(
114+
wpt_cnt,
115+
Pose2d {
116+
x: wpt.x,
117+
y: wpt.y,
118+
heading: wpt.heading,
119+
},
120+
);
96121
} else if wpt.fix_translation {
97-
generator.translation_wpt(wpt_cnt, wpt.x, wpt.y, wpt.heading);
122+
generator.translation_wpt(
123+
wpt_cnt,
124+
Translation2d { x: wpt.x, y: wpt.y },
125+
wpt.heading,
126+
);
98127
} else {
99-
generator.empty_wpt(wpt_cnt, wpt.x, wpt.y, wpt.heading);
128+
generator.empty_wpt(
129+
wpt_cnt,
130+
Pose2d {
131+
x: wpt.x,
132+
y: wpt.y,
133+
heading: wpt.heading,
134+
},
135+
);
100136
}
101137
wpt_cnt += 1;
102138
if i != waypoints.len() - 1 {

‎trajoptlib/examples/differential.rs‎

Lines changed: 18 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
use std::f64;
22

3-
use trajoptlib::{DifferentialDrivetrain, DifferentialTrajectoryGenerator};
3+
use trajoptlib::{DifferentialDrivetrain, DifferentialTrajectoryGenerator, Pose2d, Translation2d};
44

55
fn main() {
66
let drivetrain = DifferentialDrivetrain {
@@ -26,12 +26,26 @@ fn main() {
2626
generator.set_drivetrain(&drivetrain);
2727
generator.set_bumpers(0.65, 0.65, 0.65, 0.65);
2828

29-
generator.pose_wpt(0, 0.0, 0.0, 0.0);
30-
generator.pose_wpt(1, 1.0, 0.0, 0.0);
29+
generator.pose_wpt(
30+
0,
31+
Pose2d {
32+
x: 0.0,
33+
y: 0.0,
34+
heading: 0.0,
35+
},
36+
);
37+
generator.pose_wpt(
38+
1,
39+
Pose2d {
40+
x: 1.0,
41+
y: 0.0,
42+
heading: 0.0,
43+
},
44+
);
3145

3246
generator.wpt_angular_velocity_max_magnitude(0, 0.0);
3347
generator.wpt_angular_velocity_max_magnitude(1, 0.0);
34-
// generator.sgmt_keep_out_circle(0, 1, 0.5, 0.1, 0.2);
48+
generator.sgmt_keep_out_circle(0, 1, Translation2d { x: 0.5, y: 0.1 }, 0.2);
3549

3650
generator.set_control_interval_counts(vec![40]);
3751

‎trajoptlib/examples/differential/src/main.cpp‎

Lines changed: 16 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -37,8 +37,8 @@ int main() {
3737
{
3838
trajopt::DifferentialPathBuilder path;
3939
path.set_drivetrain(differential_drivetrain);
40-
path.pose_wpt(0, 0.0, 0.0, 0.0);
41-
path.pose_wpt(1, 1.0, 0.0, 0.0);
40+
path.pose_wpt(0, {0.0, 0.0, 0.0});
41+
path.pose_wpt(1, {1.0, 0.0, 0.0});
4242
path.wpt_linear_velocity_max_magnitude(0, 0.0);
4343
path.wpt_linear_velocity_max_magnitude(1, 0.0);
4444
path.set_control_interval_counts({40});
@@ -54,8 +54,8 @@ int main() {
5454
{
5555
trajopt::DifferentialPathBuilder path;
5656
path.set_drivetrain(differential_drivetrain);
57-
path.pose_wpt(0, 1.0, 1.0, -std::numbers::pi / 2);
58-
path.pose_wpt(1, 2.0, 0.0, 0.0);
57+
path.pose_wpt(0, {1.0, 1.0, -std::numbers::pi / 2});
58+
path.pose_wpt(1, {2.0, 0.0, 0.0});
5959
path.wpt_linear_velocity_max_magnitude(0, 0.0);
6060
path.wpt_linear_velocity_max_magnitude(1, 0.0);
6161
path.set_control_interval_counts({40});
@@ -71,9 +71,9 @@ int main() {
7171
{
7272
trajopt::DifferentialPathBuilder path;
7373
path.set_drivetrain(differential_drivetrain);
74-
path.pose_wpt(0, 0.0, 0.0, std::numbers::pi / 2);
75-
path.pose_wpt(1, 1.0, 1.0, 0.0);
76-
path.pose_wpt(2, 2.0, 0.0, std::numbers::pi / 2);
74+
path.pose_wpt(0, {0.0, 0.0, std::numbers::pi / 2});
75+
path.pose_wpt(1, {1.0, 1.0, 0.0});
76+
path.pose_wpt(2, {2.0, 0.0, std::numbers::pi / 2});
7777
path.wpt_linear_velocity_max_magnitude(0, 0.0);
7878
path.wpt_linear_velocity_max_magnitude(1, 0.0);
7979
path.set_control_interval_counts({50, 50});
@@ -89,8 +89,8 @@ int main() {
8989
{
9090
trajopt::DifferentialPathBuilder path;
9191
path.set_drivetrain(differential_drivetrain);
92-
path.pose_wpt(0, 0.0, 0.0, 0.0);
93-
path.pose_wpt(1, 0.0, 1.0, 0.0);
92+
path.pose_wpt(0, {0.0, 0.0, 0.0});
93+
path.pose_wpt(1, {0.0, 1.0, 0.0});
9494
path.wpt_linear_velocity_max_magnitude(0, 0.0);
9595
path.set_control_interval_counts({40});
9696

@@ -106,9 +106,9 @@ int main() {
106106
trajopt::DifferentialPathBuilder path;
107107
path.set_drivetrain(differential_drivetrain);
108108
path.set_bumpers(0.65, 0.65, 0.65, 0.65);
109-
path.pose_wpt(0, 0.0, 0.0, 0.0);
109+
path.pose_wpt(0, {0.0, 0.0, 0.0});
110110
path.sgmt_keep_out_circle(0, 1, {0.5, 0.5}, 0.1);
111-
path.pose_wpt(1, 1.0, 0.0, 0.0);
111+
path.pose_wpt(1, {1.0, 0.0, 0.0});
112112
path.wpt_linear_velocity_max_magnitude(0, 0.0);
113113
path.wpt_linear_velocity_max_magnitude(1, 0.0);
114114
path.set_control_interval_counts({40});
@@ -126,24 +126,24 @@ int main() {
126126
path.set_drivetrain(differential_drivetrain);
127127

128128
// Starting position
129-
path.pose_wpt(0, 0.0, 0.0, 0.0);
129+
path.pose_wpt(0, {0.0, 0.0, 0.0});
130130

131131
// Align towards the station one meter behind
132-
path.pose_wpt(1, 1.0, 1.0, std::numbers::pi / 2);
132+
path.pose_wpt(1, {1.0, 1.0, std::numbers::pi / 2});
133133
path.wpt_angular_velocity_max_magnitude(1, 0.0);
134134
path.wpt_linear_velocity_direction(1, std::numbers::pi / 2);
135135

136136
// Go up to the station. In practice, the optimizer will still end up
137137
// aligning the heading without the pose constraint since it's most optimal.
138-
path.translation_wpt(2, 1.0, 2.0);
138+
path.translation_wpt(2, {1.0, 2.0});
139139

140140
// Realign behind the station
141-
path.pose_wpt(3, 1.0, 1.0, std::numbers::pi / 2);
141+
path.pose_wpt(3, {1.0, 1.0, std::numbers::pi / 2});
142142
path.wpt_angular_velocity_max_magnitude(3, 0.0);
143143
path.wpt_linear_velocity_direction(3, std::numbers::pi / 2);
144144

145145
// Ending position
146-
path.pose_wpt(4, 2.0, 0.0, std::numbers::pi);
146+
path.pose_wpt(4, {2.0, 0.0, std::numbers::pi});
147147

148148
path.wpt_linear_velocity_max_magnitude(0, 0.0);
149149
path.wpt_linear_velocity_max_magnitude(4, 0.0);

‎trajoptlib/examples/swerve.rs‎

Lines changed: 18 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
use trajoptlib::{SwerveDrivetrain, SwerveTrajectoryGenerator, Translation2d};
1+
use trajoptlib::{Pose2d, SwerveDrivetrain, SwerveTrajectoryGenerator, Translation2d};
22

33
fn main() {
44
let drivetrain = SwerveDrivetrain {
@@ -29,12 +29,26 @@ fn main() {
2929
generator.set_drivetrain(&drivetrain);
3030
generator.set_bumpers(0.65, 0.65, 0.65, 0.65);
3131

32-
generator.pose_wpt(0, 0.0, 0.0, 0.0);
33-
generator.pose_wpt(1, 1.0, 0.0, 0.0);
32+
generator.pose_wpt(
33+
0,
34+
Pose2d {
35+
x: 0.0,
36+
y: 0.0,
37+
heading: 0.0,
38+
},
39+
);
40+
generator.pose_wpt(
41+
1,
42+
Pose2d {
43+
x: 1.0,
44+
y: 0.0,
45+
heading: 0.0,
46+
},
47+
);
3448

3549
generator.wpt_angular_velocity_max_magnitude(0, 0.0);
3650
generator.wpt_angular_velocity_max_magnitude(1, 0.0);
37-
generator.sgmt_keep_out_circle(0, 1, 0.5, 0.1, 0.2);
51+
generator.sgmt_keep_out_circle(0, 1, Translation2d { x: 0.5, y: 0.1 }, 0.2);
3852

3953
generator.set_control_interval_counts(vec![40]);
4054

0 commit comments

Comments
 (0)