trajoptlib/lib.rs
1#![deny(missing_docs)]
2#![doc = include_str!("../README.md")]
3
4// cxx generates `Clone` impls for the FFI structs below; nightly clippy flags
5// the derive-expanded field copies as `clone_on_copy` false positives.
6#[allow(clippy::clone_on_copy)]
7#[cxx::bridge(namespace = "trajopt::rsffi")]
8mod ffi {
9 #[derive(Debug, Deserialize, Serialize, Clone)]
10 /// Represents a translation in 2D space.
11 struct Translation2d {
12 /// The x component of the translation.
13 x: f64,
14 /// The y component of the translation.
15 y: f64,
16 }
17
18 #[derive(Debug, Deserialize, Serialize, Clone)]
19 /// Represents a 2D pose with translation and rotation.
20 struct Pose2d {
21 /// The x component of the translational component of the pose.
22 x: f64,
23 /// The y component of the translational component of the pose.
24 y: f64,
25 /// The rotational component of the pose.
26 heading: f64,
27 }
28
29 #[derive(Debug, Deserialize, Serialize, Clone)]
30 /// A swerve drivetrain physical model.
31 struct SwerveDrivetrain {
32 /// The mass of the robot (kg).
33 mass: f64,
34 /// The moment of inertia of the robot about the origin (kg−m²).
35 moi: f64,
36 /// Radius of the wheels (m).
37 wheel_radius: f64,
38 /// Maximum angular velocity of each wheel (rad/s).
39 wheel_max_angular_velocity: f64,
40 /// Maximum torque applied to each wheel (N−m).
41 wheel_max_torque: f64,
42 /// The Coefficient of Friction (CoF) of the wheels.
43 wheel_cof: f64,
44 /// Translation of each swerve module from the origin of the robot
45 /// coordinate system to the center of the module (m). There's
46 /// usually one in each corner.
47 modules: Vec<Translation2d>,
48 }
49
50 #[derive(Debug, Deserialize, Serialize, Clone)]
51 /// Swerve trajectory sample.
52 struct SwerveTrajectorySample {
53 /// The timestamp.
54 timestamp: f64,
55 /// The x coordinate.
56 x: f64,
57 /// The y coordinate.
58 y: f64,
59 /// The heading.
60 heading: f64,
61 /// The velocity's x component.
62 velocity_x: f64,
63 /// The velocity's y component.
64 velocity_y: f64,
65 /// The angular velocity.
66 angular_velocity: f64,
67 /// The acceleration's x component.
68 acceleration_x: f64,
69 /// The acceleration's y component.
70 acceleration_y: f64,
71 /// The angular acceleration.
72 angular_acceleration: f64,
73 /// The force on each module in the X direction.
74 module_forces_x: Vec<f64>,
75 /// The force on each module in the Y direction.
76 module_forces_y: Vec<f64>,
77 }
78
79 #[derive(Debug, Deserialize, Serialize, Clone)]
80 /// Swerve trajectory.
81 struct SwerveTrajectory {
82 /// The samples that make up the trajectory.
83 samples: Vec<SwerveTrajectorySample>,
84 }
85
86 #[derive(Debug, Deserialize, Serialize, Clone)]
87 /// A differential drivetrain physical model.
88 struct DifferentialDrivetrain {
89 /// The mass of the robot (kg).
90 mass: f64,
91 /// The moment of inertia of the robot about the origin (kg−m²).
92 moi: f64,
93 /// Radius of the wheels (m).
94 wheel_radius: f64,
95 /// Maximum angular velocity of the wheels (rad/s).
96 wheel_max_angular_velocity: f64,
97 /// Maximum torque applied to the wheels (N−m).
98 wheel_max_torque: f64,
99 /// The Coefficient of Friction (CoF) of the wheels.
100 wheel_cof: f64,
101 /// Distance between the two driverails (m).
102 trackwidth: f64,
103 }
104
105 #[derive(Debug, Deserialize, Serialize, Clone)]
106 /// Differential trajectory sample.
107 struct DifferentialTrajectorySample {
108 /// The timestamp.
109 timestamp: f64,
110 /// The x coordinate.
111 x: f64,
112 /// The y coordinate.
113 y: f64,
114 /// The heading.
115 heading: f64,
116 /// The left wheel's velocity.
117 velocity_l: f64,
118 /// The right wheel's velocity.
119 velocity_r: f64,
120 /// The chassis angular velocity.
121 angular_velocity: f64,
122 /// The left wheel's acceleration.
123 acceleration_l: f64,
124 /// The right wheel's acceleration.
125 acceleration_r: f64,
126 /// The chassis angular acceleration.
127 angular_acceleration: f64,
128 /// The left wheel's force.
129 force_l: f64,
130 /// The right wheel's force.
131 force_r: f64,
132 }
133
134 #[derive(Debug, Deserialize, Serialize, Clone)]
135 /// Differential trajectory.
136 struct DifferentialTrajectory {
137 /// The samples that make up the trajectory.
138 samples: Vec<DifferentialTrajectorySample>,
139 }
140
141 unsafe extern "C++" {
142 include!("rust_ffi.hpp");
143
144 type SwerveTrajectoryGenerator;
145
146 fn swerve_trajectory_generator_new() -> UniquePtr<SwerveTrajectoryGenerator>;
147
148 fn set_drivetrain(self: Pin<&mut SwerveTrajectoryGenerator>, drivetrain: &SwerveDrivetrain);
149
150 fn set_bumpers(
151 self: Pin<&mut SwerveTrajectoryGenerator>,
152 front: f64,
153 left: f64,
154 right: f64,
155 back: f64,
156 );
157
158 fn set_control_interval_counts(
159 self: Pin<&mut SwerveTrajectoryGenerator>,
160 counts: Vec<usize>,
161 );
162
163 // Pose constraints
164
165 fn pose_wpt(self: Pin<&mut SwerveTrajectoryGenerator>, index: usize, pose: Pose2d);
166
167 fn translation_wpt(
168 self: Pin<&mut SwerveTrajectoryGenerator>,
169 index: usize,
170 translation: Translation2d,
171 heading_guess: f64,
172 );
173
174 fn empty_wpt(self: Pin<&mut SwerveTrajectoryGenerator>, index: usize, pose_guess: Pose2d);
175
176 // Segment initial guess points setter
177
178 fn sgmt_initial_guess_points(
179 self: Pin<&mut SwerveTrajectoryGenerator>,
180 from_index: usize,
181 guess_points: &Vec<Pose2d>,
182 );
183
184 // Constraints with waypoint scope
185
186 fn wpt_linear_velocity_direction(
187 self: Pin<&mut SwerveTrajectoryGenerator>,
188 index: usize,
189 angle: f64,
190 );
191
192 fn wpt_linear_velocity_max_magnitude(
193 self: Pin<&mut SwerveTrajectoryGenerator>,
194 index: usize,
195 magnitude: f64,
196 );
197
198 fn wpt_angular_velocity_max_magnitude(
199 self: Pin<&mut SwerveTrajectoryGenerator>,
200 index: usize,
201 magnitude: f64,
202 );
203
204 fn wpt_linear_acceleration_max_magnitude(
205 self: Pin<&mut SwerveTrajectoryGenerator>,
206 index: usize,
207 magnitude: f64,
208 );
209
210 fn wpt_point_at(
211 self: Pin<&mut SwerveTrajectoryGenerator>,
212 index: usize,
213 field_point: Translation2d,
214 heading_tolerance: f64,
215 flip: bool,
216 );
217
218 fn wpt_keep_in_circle(
219 self: Pin<&mut SwerveTrajectoryGenerator>,
220 index: usize,
221 center: Translation2d,
222 radius: f64,
223 );
224
225 fn wpt_keep_in_polygon(
226 self: Pin<&mut SwerveTrajectoryGenerator>,
227 index: usize,
228 field_points: Vec<Translation2d>,
229 );
230
231 fn wpt_keep_in_lane(
232 self: Pin<&mut SwerveTrajectoryGenerator>,
233 index: usize,
234 center_line_start: Translation2d,
235 center_line_end: Translation2d,
236 tolerance: f64,
237 );
238
239 fn wpt_keep_out_circle(
240 self: Pin<&mut SwerveTrajectoryGenerator>,
241 index: usize,
242 center: Translation2d,
243 radius: f64,
244 );
245
246 // Constraints with segment scope
247
248 fn sgmt_linear_velocity_direction(
249 self: Pin<&mut SwerveTrajectoryGenerator>,
250 from_index: usize,
251 to_index: usize,
252 angle: f64,
253 );
254
255 fn sgmt_linear_velocity_max_magnitude(
256 self: Pin<&mut SwerveTrajectoryGenerator>,
257 from_index: usize,
258 to_index: usize,
259 magnitude: f64,
260 );
261
262 fn sgmt_angular_velocity_max_magnitude(
263 self: Pin<&mut SwerveTrajectoryGenerator>,
264 from_index: usize,
265 to_index: usize,
266 angular_velocity: f64,
267 );
268
269 fn sgmt_linear_acceleration_max_magnitude(
270 self: Pin<&mut SwerveTrajectoryGenerator>,
271 from_index: usize,
272 to_index: usize,
273 magnitude: f64,
274 );
275
276 fn sgmt_point_at(
277 self: Pin<&mut SwerveTrajectoryGenerator>,
278 from_index: usize,
279 to_index: usize,
280 field_point: Translation2d,
281 heading_tolerance: f64,
282 flip: bool,
283 );
284
285 fn sgmt_keep_in_circle(
286 self: Pin<&mut SwerveTrajectoryGenerator>,
287 from_index: usize,
288 to_index: usize,
289 center: Translation2d,
290 radius: f64,
291 );
292
293 fn sgmt_keep_in_polygon(
294 self: Pin<&mut SwerveTrajectoryGenerator>,
295 from_index: usize,
296 to_index: usize,
297 field_points: Vec<Translation2d>,
298 );
299
300 #[allow(clippy::too_many_arguments)]
301 fn sgmt_keep_in_lane(
302 self: Pin<&mut SwerveTrajectoryGenerator>,
303 from_index: usize,
304 to_index: usize,
305 center_line_start: Translation2d,
306 center_line_end: Translation2d,
307 tolerance: f64,
308 );
309
310 fn sgmt_keep_out_circle(
311 self: Pin<&mut SwerveTrajectoryGenerator>,
312 from_index: usize,
313 to_index: usize,
314 center: Translation2d,
315 radius: f64,
316 );
317
318 // Trajectory generator functions
319
320 fn add_callback(
321 self: Pin<&mut SwerveTrajectoryGenerator>,
322 callback: fn(SwerveTrajectory, i64),
323 );
324
325 fn generate(
326 self: &SwerveTrajectoryGenerator,
327 diagnostics: bool,
328 uuid: i64,
329 ) -> Result<SwerveTrajectory>;
330
331 type DifferentialTrajectoryGenerator;
332
333 fn differential_trajectory_generator_new() -> UniquePtr<DifferentialTrajectoryGenerator>;
334
335 fn set_drivetrain(
336 self: Pin<&mut DifferentialTrajectoryGenerator>,
337 drivetrain: &DifferentialDrivetrain,
338 );
339
340 fn set_bumpers(
341 self: Pin<&mut DifferentialTrajectoryGenerator>,
342 front: f64,
343 left: f64,
344 right: f64,
345 back: f64,
346 );
347
348 fn set_control_interval_counts(
349 self: Pin<&mut DifferentialTrajectoryGenerator>,
350 counts: Vec<usize>,
351 );
352
353 // Pose constraints
354
355 fn pose_wpt(self: Pin<&mut DifferentialTrajectoryGenerator>, index: usize, pose: Pose2d);
356
357 fn translation_wpt(
358 self: Pin<&mut DifferentialTrajectoryGenerator>,
359 index: usize,
360 translation: Translation2d,
361 heading_guess: f64,
362 );
363
364 fn empty_wpt(
365 self: Pin<&mut DifferentialTrajectoryGenerator>,
366 index: usize,
367 pose_guess: Pose2d,
368 );
369
370 // Segment initial guess points setter
371
372 fn sgmt_initial_guess_points(
373 self: Pin<&mut DifferentialTrajectoryGenerator>,
374 from_index: usize,
375 guess_points: &Vec<Pose2d>,
376 );
377
378 // Constraints with waypoint scope
379
380 fn wpt_linear_velocity_direction(
381 self: Pin<&mut DifferentialTrajectoryGenerator>,
382 index: usize,
383 angle: f64,
384 );
385
386 fn wpt_linear_velocity_max_magnitude(
387 self: Pin<&mut DifferentialTrajectoryGenerator>,
388 index: usize,
389 magnitude: f64,
390 );
391
392 fn wpt_angular_velocity_max_magnitude(
393 self: Pin<&mut DifferentialTrajectoryGenerator>,
394 index: usize,
395 angular_velocity: f64,
396 );
397
398 fn wpt_linear_acceleration_max_magnitude(
399 self: Pin<&mut DifferentialTrajectoryGenerator>,
400 index: usize,
401 magnitude: f64,
402 );
403
404 fn wpt_point_at(
405 self: Pin<&mut DifferentialTrajectoryGenerator>,
406 index: usize,
407 field_point: Translation2d,
408 heading_tolerance: f64,
409 flip: bool,
410 );
411
412 fn wpt_keep_in_circle(
413 self: Pin<&mut DifferentialTrajectoryGenerator>,
414 index: usize,
415 center: Translation2d,
416 radius: f64,
417 );
418
419 fn wpt_keep_in_polygon(
420 self: Pin<&mut DifferentialTrajectoryGenerator>,
421 index: usize,
422 field_points: Vec<Translation2d>,
423 );
424
425 fn wpt_keep_in_lane(
426 self: Pin<&mut DifferentialTrajectoryGenerator>,
427 index: usize,
428 center_line_start: Translation2d,
429 center_line_end: Translation2d,
430 tolerance: f64,
431 );
432
433 fn wpt_keep_out_circle(
434 self: Pin<&mut DifferentialTrajectoryGenerator>,
435 index: usize,
436 center: Translation2d,
437 radius: f64,
438 );
439
440 // Constraints with waypoint scope
441
442 fn sgmt_linear_velocity_direction(
443 self: Pin<&mut DifferentialTrajectoryGenerator>,
444 from_index: usize,
445 to_index: usize,
446 angle: f64,
447 );
448
449 fn sgmt_linear_velocity_max_magnitude(
450 self: Pin<&mut DifferentialTrajectoryGenerator>,
451 from_index: usize,
452 to_index: usize,
453 magnitude: f64,
454 );
455
456 fn sgmt_angular_velocity_max_magnitude(
457 self: Pin<&mut DifferentialTrajectoryGenerator>,
458 from_index: usize,
459 to_index: usize,
460 angular_velocity: f64,
461 );
462
463 fn sgmt_linear_acceleration_max_magnitude(
464 self: Pin<&mut DifferentialTrajectoryGenerator>,
465 from_index: usize,
466 to_index: usize,
467 magnitude: f64,
468 );
469
470 fn sgmt_keep_in_circle(
471 self: Pin<&mut DifferentialTrajectoryGenerator>,
472 from_index: usize,
473 to_index: usize,
474 center: Translation2d,
475 radius: f64,
476 );
477
478 fn sgmt_keep_in_polygon(
479 self: Pin<&mut DifferentialTrajectoryGenerator>,
480 from_index: usize,
481 to_index: usize,
482 field_points: Vec<Translation2d>,
483 );
484
485 #[allow(clippy::too_many_arguments)]
486 fn sgmt_keep_in_lane(
487 self: Pin<&mut DifferentialTrajectoryGenerator>,
488 from_index: usize,
489 to_index: usize,
490 center_line_start: Translation2d,
491 center_line_end: Translation2d,
492 tolerance: f64,
493 );
494
495 fn sgmt_keep_out_circle(
496 self: Pin<&mut DifferentialTrajectoryGenerator>,
497 from_index: usize,
498 to_index: usize,
499 center: Translation2d,
500 radius: f64,
501 );
502
503 // Trajectory generator
504
505 fn add_callback(
506 self: Pin<&mut DifferentialTrajectoryGenerator>,
507 callback: fn(DifferentialTrajectory, i64),
508 );
509
510 fn generate(
511 self: &DifferentialTrajectoryGenerator,
512 diagnostics: bool,
513 uuid: i64,
514 ) -> Result<DifferentialTrajectory>;
515
516 // Cancel all generators
517
518 fn cancel_all();
519 }
520}
521
522/// This trajectory generator class contains functions to generate
523/// time-optimal trajectories for several drivetrain types.
524pub struct SwerveTrajectoryGenerator {
525 generator: cxx::UniquePtr<crate::ffi::SwerveTrajectoryGenerator>,
526}
527
528impl Default for SwerveTrajectoryGenerator {
529 fn default() -> Self {
530 Self::new()
531 }
532}
533
534impl SwerveTrajectoryGenerator {
535 /// Constructs a new swerve trajectory optimization problem.
536 pub fn new() -> SwerveTrajectoryGenerator {
537 SwerveTrajectoryGenerator {
538 generator: crate::ffi::swerve_trajectory_generator_new(),
539 }
540 }
541
542 /// Sets the Drivetrain object.
543 ///
544 /// * `drivetrain` - The drivetrain.
545 pub fn set_drivetrain(&mut self, drivetrain: &crate::ffi::SwerveDrivetrain) {
546 crate::ffi::SwerveTrajectoryGenerator::set_drivetrain(self.generator.pin_mut(), drivetrain);
547 }
548
549 /// Adds a rectangular bumper to a list used when applying
550 /// keep-out constraints.
551 ///
552 /// * `front` - Distance in meters from center to front bumper edge.
553 /// * `left` - Distance in meters from center to left bumper edge.
554 /// * `right` - Distance in meters from center to right bumper edge.
555 /// * `back` - Distance in meters from center to back bumper edge.
556 pub fn set_bumpers(&mut self, front: f64, left: f64, right: f64, back: f64) {
557 crate::ffi::SwerveTrajectoryGenerator::set_bumpers(
558 self.generator.pin_mut(),
559 front,
560 left,
561 right,
562 back,
563 );
564 }
565
566 /// Sets the number of discrete samples for each trajectory segment.
567 ///
568 /// * `counts` - The sequence of control interval counts per segment. Length
569 /// is number of waypoints - 1.
570 pub fn set_control_interval_counts(&mut self, counts: Vec<usize>) {
571 crate::ffi::SwerveTrajectoryGenerator::set_control_interval_counts(
572 self.generator.pin_mut(),
573 counts,
574 );
575 }
576
577 // Pose constraints
578
579 /// Applies a pose constraint to a waypoint, and adds an initial guess with
580 /// the same pose.
581 ///
582 /// * `index` - The waypoint's index.
583 /// * `pose` - The pose.
584 pub fn pose_wpt(&mut self, index: usize, pose: Pose2d) {
585 crate::ffi::SwerveTrajectoryGenerator::pose_wpt(self.generator.pin_mut(), index, pose);
586 }
587
588 /// Applies a translation constraint to a waypoint, and adds an initial
589 /// guess point with the same translation.
590 ///
591 /// * `index` - The waypoint's index.
592 /// * `translation` - The translation.
593 /// * `heading_guess` - The heading initial guess.
594 pub fn translation_wpt(
595 &mut self,
596 index: usize,
597 translation: Translation2d,
598 heading_guess: f64,
599 ) {
600 crate::ffi::SwerveTrajectoryGenerator::translation_wpt(
601 self.generator.pin_mut(),
602 index,
603 translation,
604 heading_guess,
605 );
606 }
607
608 /// Sets a waypoint's pose initial guess.
609 ///
610 /// * `index` - The waypoint's index.
611 /// * `pose_guess` - The pose initial guess.
612 pub fn empty_wpt(&mut self, index: usize, pose_guess: Pose2d) {
613 crate::ffi::SwerveTrajectoryGenerator::empty_wpt(
614 self.generator.pin_mut(),
615 index,
616 pose_guess,
617 );
618 }
619
620 // Segment initial guess points setter
621
622 /// Adds a sequence of initial guess points between two waypoints.
623 ///
624 /// The points are inserted between the waypoints at fromIndex and
625 /// fromIndex + 1. Linear interpolation between the waypoint initial guess
626 /// points and these segment initial guess points is used as the initial
627 /// guess of the robot's pose over the trajectory.
628 ///
629 /// * `from_index` - The index of the waypoint the initial guess point comes
630 /// after.
631 /// * `guess_points` - The sequence of initial guess points.
632 pub fn sgmt_initial_guess_points(
633 &mut self,
634 from_index: usize,
635 guess_points: &Vec<crate::ffi::Pose2d>,
636 ) {
637 crate::ffi::SwerveTrajectoryGenerator::sgmt_initial_guess_points(
638 self.generator.pin_mut(),
639 from_index,
640 guess_points,
641 );
642 }
643
644 // Constraints with waypoint scope
645
646 /// Applies a linear velocity direction constraint to a waypoint.
647 ///
648 /// * `index` - The waypoint's index.
649 /// * `angle` - The angle in radians.
650 pub fn wpt_linear_velocity_direction(&mut self, index: usize, angle: f64) {
651 crate::ffi::SwerveTrajectoryGenerator::wpt_linear_velocity_direction(
652 self.generator.pin_mut(),
653 index,
654 angle,
655 );
656 }
657
658 /// Applies a linear velocity max magnitude constraint to a waypoint.
659 ///
660 /// * `index` - The waypoint's index.
661 /// * `magnitude` - The magnitude.
662 pub fn wpt_linear_velocity_max_magnitude(&mut self, index: usize, magnitude: f64) {
663 crate::ffi::SwerveTrajectoryGenerator::wpt_linear_velocity_max_magnitude(
664 self.generator.pin_mut(),
665 index,
666 magnitude,
667 );
668 }
669
670 /// Applies an angular velocity max magnitude constraint to a waypoint.
671 ///
672 /// * `index` - The waypoint's index.
673 /// * `magnitude` - The magnitude.
674 pub fn wpt_angular_velocity_max_magnitude(&mut self, index: usize, magnitude: f64) {
675 crate::ffi::SwerveTrajectoryGenerator::wpt_angular_velocity_max_magnitude(
676 self.generator.pin_mut(),
677 index,
678 magnitude,
679 );
680 }
681
682 /// Applies a linear acceleration max magnitude constraint to a waypoint.
683 ///
684 /// * `index` - The waypoint's index.
685 /// * `magnitude` - The magnitude.
686 pub fn wpt_linear_acceleration_max_magnitude(&mut self, index: usize, magnitude: f64) {
687 crate::ffi::SwerveTrajectoryGenerator::wpt_linear_acceleration_max_magnitude(
688 self.generator.pin_mut(),
689 index,
690 magnitude,
691 );
692 }
693
694 /// Applies a point-at constraint to a waypoint.
695 ///
696 /// * `index` - The waypoint's index.
697 /// * `field_point` - The field point to point at.
698 /// * `heading_tolerance` - The heading tolerance.
699 /// * `flip` - Whether to point away from the field point.
700 pub fn wpt_point_at(
701 &mut self,
702 index: usize,
703 field_point: Translation2d,
704 heading_tolerance: f64,
705 flip: bool,
706 ) {
707 crate::ffi::SwerveTrajectoryGenerator::wpt_point_at(
708 self.generator.pin_mut(),
709 index,
710 field_point,
711 heading_tolerance,
712 flip,
713 )
714 }
715
716 /// Applies a keep-in circle constraint to a waypoint.
717 ///
718 /// Applies to the robot bumpers.
719 ///
720 /// * `index` - The waypoint's index.
721 /// * `center` - The circle's center.
722 /// * `radius` - The circle's radius.
723 pub fn wpt_keep_in_circle(&mut self, index: usize, center: Translation2d, radius: f64) {
724 crate::ffi::SwerveTrajectoryGenerator::wpt_keep_in_circle(
725 self.generator.pin_mut(),
726 index,
727 center,
728 radius,
729 )
730 }
731
732 /// Applies a keep-in polygon constraint to a waypoint.
733 ///
734 /// Applies to the robot bumpers.
735 ///
736 /// * `index` - The waypoint's index.
737 /// * `field_points` - The points defining the keep-in polygon (must wind
738 /// counterclockwise).
739 pub fn wpt_keep_in_polygon(&mut self, index: usize, field_points: Vec<Translation2d>) {
740 crate::ffi::SwerveTrajectoryGenerator::wpt_keep_in_polygon(
741 self.generator.pin_mut(),
742 index,
743 field_points,
744 );
745 }
746
747 /// Applies a keep-in lane constraint to a waypoint.
748 ///
749 /// Applies to the robot center.
750 ///
751 /// * `index` - The waypoint's index.
752 /// * `center_line_start` - The center line's start point.
753 /// * `center_line_end` - The center line's end point.
754 /// * `tolerance` - The distance from the center line to each lane edge.
755 pub fn wpt_keep_in_lane(
756 &mut self,
757 index: usize,
758 center_line_start: Translation2d,
759 center_line_end: Translation2d,
760 tolerance: f64,
761 ) {
762 crate::ffi::SwerveTrajectoryGenerator::wpt_keep_in_lane(
763 self.generator.pin_mut(),
764 index,
765 center_line_start,
766 center_line_end,
767 tolerance,
768 );
769 }
770
771 /// Applies a keep-out circle constraint to a waypoint.
772 ///
773 /// Applies to the robot bumpers.
774 ///
775 /// * `index` - The waypoint's index.
776 /// * `center` - The circle's center.
777 /// * `radius` - The circle's radius.
778 pub fn wpt_keep_out_circle(&mut self, index: usize, center: Translation2d, radius: f64) {
779 crate::ffi::SwerveTrajectoryGenerator::wpt_keep_out_circle(
780 self.generator.pin_mut(),
781 index,
782 center,
783 radius,
784 )
785 }
786
787 // Constraints with segment scope
788
789 /// Applies a linear velocity direction constraint between two waypoints.
790 ///
791 /// * `from_index` - The first waypoint's index.
792 /// * `to_index` - The second waypoint's index.
793 /// * `angle` - The angle in radians.
794 pub fn sgmt_linear_velocity_direction(
795 &mut self,
796 from_index: usize,
797 to_index: usize,
798 angle: f64,
799 ) {
800 crate::ffi::SwerveTrajectoryGenerator::sgmt_linear_velocity_direction(
801 self.generator.pin_mut(),
802 from_index,
803 to_index,
804 angle,
805 );
806 }
807
808 /// Applies a linear velocity max magnitude constraint between two
809 /// waypoints.
810 ///
811 /// * `from_index` - The first waypoint's index.
812 /// * `to_index` - The second waypoint's index.
813 /// * `magnitude` - The magnitude.
814 pub fn sgmt_linear_velocity_max_magnitude(
815 &mut self,
816 from_index: usize,
817 to_index: usize,
818 magnitude: f64,
819 ) {
820 crate::ffi::SwerveTrajectoryGenerator::sgmt_linear_velocity_max_magnitude(
821 self.generator.pin_mut(),
822 from_index,
823 to_index,
824 magnitude,
825 );
826 }
827
828 /// Applies an angular velocity max magnitude constraint between two
829 /// waypoints.
830 ///
831 /// * `from_index` - The first waypoint's index.
832 /// * `to_index` - The second waypoint's index.
833 /// * `magnitude` - The magnitude.
834 pub fn sgmt_angular_velocity_max_magnitude(
835 &mut self,
836 from_index: usize,
837 to_index: usize,
838 magnitude: f64,
839 ) {
840 crate::ffi::SwerveTrajectoryGenerator::sgmt_angular_velocity_max_magnitude(
841 self.generator.pin_mut(),
842 from_index,
843 to_index,
844 magnitude,
845 );
846 }
847
848 /// Applies a linear acceleration max magnitude constraint between two
849 /// waypoints.
850 ///
851 /// * `from_index` - The first waypoint's index.
852 /// * `to_index` - The second waypoint's index.
853 /// * `magnitude` - The magnitude.
854 pub fn sgmt_linear_acceleration_max_magnitude(
855 &mut self,
856 from_index: usize,
857 to_index: usize,
858 magnitude: f64,
859 ) {
860 crate::ffi::SwerveTrajectoryGenerator::sgmt_linear_acceleration_max_magnitude(
861 self.generator.pin_mut(),
862 from_index,
863 to_index,
864 magnitude,
865 );
866 }
867
868 /// Applies a point-at constraint between two waypoints.
869 ///
870 /// * `from_index` - The first waypoint's index.
871 /// * `to_index` - The second waypoint's index.
872 /// * `field_point` - The field point to point at.
873 /// * `heading_tolerance` - The heading tolerance.
874 /// * `flip` - Whether to point away from the field point.
875 pub fn sgmt_point_at(
876 &mut self,
877 from_index: usize,
878 to_index: usize,
879 field_point: Translation2d,
880 heading_tolerance: f64,
881 flip: bool,
882 ) {
883 crate::ffi::SwerveTrajectoryGenerator::sgmt_point_at(
884 self.generator.pin_mut(),
885 from_index,
886 to_index,
887 field_point,
888 heading_tolerance,
889 flip,
890 )
891 }
892
893 /// Applies a keep-in circle constraint between two waypoints.
894 ///
895 /// Applies to the robot bumpers.
896 ///
897 /// * `from_index` - The first waypoint's index.
898 /// * `to_index` - The second waypoint's index.
899 /// * `center` - The circle's center.
900 /// * `radius` - The circle's radius.
901 pub fn sgmt_keep_in_circle(
902 &mut self,
903 from_index: usize,
904 to_index: usize,
905 center: Translation2d,
906 radius: f64,
907 ) {
908 crate::ffi::SwerveTrajectoryGenerator::sgmt_keep_in_circle(
909 self.generator.pin_mut(),
910 from_index,
911 to_index,
912 center,
913 radius,
914 )
915 }
916
917 /// Applies a keep-in polygon constraint between two waypoints.
918 ///
919 /// Applies to the robot bumpers.
920 ///
921 /// * `from_index` - The first waypoint's index.
922 /// * `to_index` - The second waypoint's index.
923 /// * `field_points` - The points defining the keep-in polygon (must wind
924 /// counterclockwise).
925 pub fn sgmt_keep_in_polygon(
926 &mut self,
927 from_index: usize,
928 to_index: usize,
929 field_points: Vec<Translation2d>,
930 ) {
931 crate::ffi::SwerveTrajectoryGenerator::sgmt_keep_in_polygon(
932 self.generator.pin_mut(),
933 from_index,
934 to_index,
935 field_points,
936 );
937 }
938
939 /// Applies a keep-in lane constraint between two waypoints.
940 ///
941 /// Applies to the robot center.
942 ///
943 /// * `from_index` - The first waypoint's index.
944 /// * `to_index` - The second waypoint's index.
945 /// * `center_line_start` - The center line's start point.
946 /// * `center_line_end` - The center line's end point.
947 /// * `tolerance` - The distance from the center line to each lane edge.
948 #[allow(clippy::too_many_arguments)]
949 pub fn sgmt_keep_in_lane(
950 &mut self,
951 from_index: usize,
952 to_index: usize,
953 center_line_start: Translation2d,
954 center_line_end: Translation2d,
955 tolerance: f64,
956 ) {
957 crate::ffi::SwerveTrajectoryGenerator::sgmt_keep_in_lane(
958 self.generator.pin_mut(),
959 from_index,
960 to_index,
961 center_line_start,
962 center_line_end,
963 tolerance,
964 )
965 }
966
967 /// Applies a keep-out circle constraint between two waypoints.
968 ///
969 /// Applies to the robot bumpers.
970 ///
971 /// * `from_index` - The first waypoint's index.
972 /// * `to_index` - The second waypoint's index.
973 /// * `center` - The circle's center.
974 /// * `radius` - The circle's radius.
975 pub fn sgmt_keep_out_circle(
976 &mut self,
977 from_index: usize,
978 to_index: usize,
979 center: Translation2d,
980 radius: f64,
981 ) {
982 crate::ffi::SwerveTrajectoryGenerator::sgmt_keep_out_circle(
983 self.generator.pin_mut(),
984 from_index,
985 to_index,
986 center,
987 radius,
988 );
989 }
990
991 /// Adds a callback that will be called on each iteration of the solver.
992 ///
993 /// * `callback` - a `fn` (not a closure) to be executed. The callback's
994 /// first parameter will be a `trajopt::SwerveTrajectory`, and the second
995 /// parameter will be an `i64` equal to the handle passed in `generate()`
996 ///
997 /// This function can be called multiple times to add multiple callbacks.
998 pub fn add_callback(&mut self, callback: fn(SwerveTrajectory, i64)) {
999 crate::ffi::SwerveTrajectoryGenerator::add_callback(self.generator.pin_mut(), callback);
1000 }
1001
1002 /// Generates the trajectory.
1003 ///
1004 /// * `diagnostics` - If true, prints per-iteration details of the solver to
1005 /// stdout.
1006 /// * `handle` - A number used to identify results from this generation in
1007 /// the `add_callback` callback. If `add_callback` has not been called,
1008 /// this value has no significance.
1009 ///
1010 /// Returns a result with either the final `trajopt::SwerveTrajectory`,
1011 /// or a TrajoptError if generation failed.
1012 pub fn generate(
1013 &self,
1014 diagnostics: bool,
1015 handle: i64,
1016 ) -> Result<SwerveTrajectory, TrajoptError> {
1017 match self.generator.generate(diagnostics, handle) {
1018 Ok(trajectory) => Ok(trajectory),
1019 Err(msg) => {
1020 let what = msg.what();
1021 Err(TrajoptError::from(
1022 what.parse::<i8>()
1023 .map_err(|_| TrajoptError::Unparsable(Box::from(what)))?,
1024 ))
1025 }
1026 }
1027 }
1028}
1029
1030/// This trajectory generator class contains functions to generate
1031/// time-optimal trajectories for differential drivetrain types.
1032pub struct DifferentialTrajectoryGenerator {
1033 generator: cxx::UniquePtr<crate::ffi::DifferentialTrajectoryGenerator>,
1034}
1035
1036impl Default for DifferentialTrajectoryGenerator {
1037 fn default() -> Self {
1038 Self::new()
1039 }
1040}
1041
1042impl DifferentialTrajectoryGenerator {
1043 /// Constructs a new differential trajectory optimization problem.
1044 pub fn new() -> DifferentialTrajectoryGenerator {
1045 DifferentialTrajectoryGenerator {
1046 generator: crate::ffi::differential_trajectory_generator_new(),
1047 }
1048 }
1049
1050 /// Sets the Drivetrain object.
1051 ///
1052 /// * `drivetrain` - The drivetrain.
1053 pub fn set_drivetrain(&mut self, drivetrain: &crate::ffi::DifferentialDrivetrain) {
1054 crate::ffi::DifferentialTrajectoryGenerator::set_drivetrain(
1055 self.generator.pin_mut(),
1056 drivetrain,
1057 );
1058 }
1059
1060 /// Adds a rectangular bumper to a list used when applying
1061 /// keep-out constraints.
1062 ///
1063 /// * `front` - Distance in meters from center to front bumper edge.
1064 /// * `left` - Distance in meters from center to left bumper edge.
1065 /// * `right` - Distance in meters from center to right bumper edge.
1066 /// * `back` - Distance in meters from center to back bumper edge.
1067 pub fn set_bumpers(&mut self, front: f64, left: f64, right: f64, back: f64) {
1068 crate::ffi::DifferentialTrajectoryGenerator::set_bumpers(
1069 self.generator.pin_mut(),
1070 front,
1071 left,
1072 right,
1073 back,
1074 );
1075 }
1076
1077 /// Sets the number of discrete samples for each trajectory segment.
1078 ///
1079 /// * `counts` - The sequence of control interval counts per segment. Length
1080 /// is number of waypoints - 1.
1081 pub fn set_control_interval_counts(&mut self, counts: Vec<usize>) {
1082 crate::ffi::DifferentialTrajectoryGenerator::set_control_interval_counts(
1083 self.generator.pin_mut(),
1084 counts,
1085 );
1086 }
1087
1088 // Pose constraints
1089
1090 /// Applies a pose constraint to a waypoint, and adds an initial guess with
1091 /// the same pose.
1092 ///
1093 /// * `index` - The waypoint's index.
1094 /// * `pose` - The pose.
1095 pub fn pose_wpt(&mut self, index: usize, pose: Pose2d) {
1096 crate::ffi::DifferentialTrajectoryGenerator::pose_wpt(
1097 self.generator.pin_mut(),
1098 index,
1099 pose,
1100 );
1101 }
1102
1103 /// Applies a translation constraint to a waypoint, and adds an initial
1104 /// guess point with the same translation.
1105 ///
1106 /// * `index` - The waypoint's index.
1107 /// * `translation` - The translation.
1108 /// * `heading_guess` - The heading initial guess.
1109 pub fn translation_wpt(
1110 &mut self,
1111 index: usize,
1112 translation: Translation2d,
1113 heading_guess: f64,
1114 ) {
1115 crate::ffi::DifferentialTrajectoryGenerator::translation_wpt(
1116 self.generator.pin_mut(),
1117 index,
1118 translation,
1119 heading_guess,
1120 );
1121 }
1122
1123 /// Sets a waypoint's pose initial guess.
1124 ///
1125 /// * `index` - The waypoint's index.
1126 /// * `pose_guess` - The pose initial guess.
1127 pub fn empty_wpt(&mut self, index: usize, pose_guess: Pose2d) {
1128 crate::ffi::DifferentialTrajectoryGenerator::empty_wpt(
1129 self.generator.pin_mut(),
1130 index,
1131 pose_guess,
1132 );
1133 }
1134
1135 // Segment initial guess points setter
1136
1137 /// Adds a sequence of initial guess points between two waypoints.
1138 ///
1139 /// The points are inserted between the waypoints at fromIndex and
1140 /// fromIndex + 1. Linear interpolation between the waypoint initial guess
1141 /// points and these segment initial guess points is used as the initial
1142 /// guess of the robot's pose over the trajectory.
1143 ///
1144 /// * `from_index` - The index of the waypoint the initial guess point comes
1145 /// after.
1146 /// * `guess_points` - The sequence of initial guess points.
1147 pub fn sgmt_initial_guess_points(
1148 &mut self,
1149 from_index: usize,
1150 guess_points: &Vec<crate::ffi::Pose2d>,
1151 ) {
1152 crate::ffi::DifferentialTrajectoryGenerator::sgmt_initial_guess_points(
1153 self.generator.pin_mut(),
1154 from_index,
1155 guess_points,
1156 );
1157 }
1158
1159 // Constraints with waypoint scope
1160
1161 /// Applies a linear velocity direction constraint to a waypoint.
1162 ///
1163 /// * `index` - The waypoint's index.
1164 /// * `angle` - The angle in radians.
1165 pub fn wpt_linear_velocity_direction(&mut self, index: usize, angle: f64) {
1166 crate::ffi::DifferentialTrajectoryGenerator::wpt_linear_velocity_direction(
1167 self.generator.pin_mut(),
1168 index,
1169 angle,
1170 );
1171 }
1172
1173 /// Applies a linear velocity max magnitude constraint to a waypoint.
1174 ///
1175 /// * `index` - The waypoint's index.
1176 /// * `magnitude` - The magnitude.
1177 pub fn wpt_linear_velocity_max_magnitude(&mut self, index: usize, magnitude: f64) {
1178 crate::ffi::DifferentialTrajectoryGenerator::wpt_linear_velocity_max_magnitude(
1179 self.generator.pin_mut(),
1180 index,
1181 magnitude,
1182 );
1183 }
1184
1185 /// Applies an angular velocity max magnitude constraint to a waypoint.
1186 ///
1187 /// * `index` - The waypoint's index.
1188 /// * `magnitude` - The magnitude.
1189 pub fn wpt_angular_velocity_max_magnitude(&mut self, index: usize, magnitude: f64) {
1190 crate::ffi::DifferentialTrajectoryGenerator::wpt_angular_velocity_max_magnitude(
1191 self.generator.pin_mut(),
1192 index,
1193 magnitude,
1194 );
1195 }
1196
1197 /// Applies a linear acceleration max magnitude constraint to a waypoint.
1198 ///
1199 /// * `index` - The waypoint's index.
1200 /// * `magnitude` - The magnitude.
1201 pub fn wpt_linear_acceleration_max_magnitude(&mut self, index: usize, magnitude: f64) {
1202 crate::ffi::DifferentialTrajectoryGenerator::wpt_linear_acceleration_max_magnitude(
1203 self.generator.pin_mut(),
1204 index,
1205 magnitude,
1206 );
1207 }
1208
1209 /// Applies a point-at constraint to a waypoint.
1210 ///
1211 /// * `index` - The waypoint's index.
1212 /// * `field_point` - The field point to point at.
1213 /// * `heading_tolerance` - The heading tolerance.
1214 /// * `flip` - Whether to point away from the field point.
1215 pub fn wpt_point_at(
1216 &mut self,
1217 index: usize,
1218 field_point: Translation2d,
1219 heading_tolerance: f64,
1220 flip: bool,
1221 ) {
1222 crate::ffi::DifferentialTrajectoryGenerator::wpt_point_at(
1223 self.generator.pin_mut(),
1224 index,
1225 field_point,
1226 heading_tolerance,
1227 flip,
1228 )
1229 }
1230
1231 /// Applies a keep-in circle constraint to a waypoint.
1232 ///
1233 /// Applies to the robot bumpers.
1234 ///
1235 /// * `index` - The waypoint's index.
1236 /// * `center` - The circle's center.
1237 /// * `radius` - The circle's radius.
1238 pub fn wpt_keep_in_circle(&mut self, index: usize, center: Translation2d, radius: f64) {
1239 crate::ffi::DifferentialTrajectoryGenerator::wpt_keep_in_circle(
1240 self.generator.pin_mut(),
1241 index,
1242 center,
1243 radius,
1244 )
1245 }
1246
1247 /// Applies a keep-in polygon constraint to a waypoint.
1248 ///
1249 /// Applies to the robot bumpers.
1250 ///
1251 /// * `index` - The waypoint's index.
1252 /// * `field_points` - The points defining the keep-in polygon (must wind
1253 /// counterclockwise).
1254 pub fn wpt_keep_in_polygon(&mut self, index: usize, field_points: Vec<Translation2d>) {
1255 crate::ffi::DifferentialTrajectoryGenerator::wpt_keep_in_polygon(
1256 self.generator.pin_mut(),
1257 index,
1258 field_points,
1259 );
1260 }
1261
1262 /// Applies a keep-in lane constraint to a waypoint.
1263 ///
1264 /// Applies to the robot center.
1265 ///
1266 /// * `index` - The waypoint's index.
1267 /// * `center_line_start` - The center line's start point.
1268 /// * `center_line_end` - The center line's end point.
1269 /// * `tolerance` - The distance from the center line to each lane edge.
1270 pub fn wpt_keep_in_lane(
1271 &mut self,
1272 index: usize,
1273 center_line_start: Translation2d,
1274 center_line_end: Translation2d,
1275 tolerance: f64,
1276 ) {
1277 crate::ffi::DifferentialTrajectoryGenerator::wpt_keep_in_lane(
1278 self.generator.pin_mut(),
1279 index,
1280 center_line_start,
1281 center_line_end,
1282 tolerance,
1283 );
1284 }
1285
1286 /// Applies a keep-out circle constraint to a waypoint.
1287 ///
1288 /// Applies to the robot bumpers.
1289 ///
1290 /// * `index` - The waypoint's index.
1291 /// * `center` - The circle's center.
1292 /// * `radius` - The circle's radius.
1293 pub fn wpt_keep_out_circle(&mut self, index: usize, center: Translation2d, radius: f64) {
1294 crate::ffi::DifferentialTrajectoryGenerator::wpt_keep_out_circle(
1295 self.generator.pin_mut(),
1296 index,
1297 center,
1298 radius,
1299 )
1300 }
1301
1302 // Constraints with segment scope
1303
1304 /// Applies a linear velocity direction constraint between two waypoints.
1305 ///
1306 /// * `from_index` - The first waypoint's index.
1307 /// * `to_index` - The second waypoint's index.
1308 /// * `angle` - The angle in radians.
1309 pub fn sgmt_linear_velocity_direction(
1310 &mut self,
1311 from_index: usize,
1312 to_index: usize,
1313 angle: f64,
1314 ) {
1315 crate::ffi::DifferentialTrajectoryGenerator::sgmt_linear_velocity_direction(
1316 self.generator.pin_mut(),
1317 from_index,
1318 to_index,
1319 angle,
1320 );
1321 }
1322
1323 /// Applies a linear velocity max magnitude constraint between two
1324 /// waypoints.
1325 ///
1326 /// * `from_index` - The first waypoint's index.
1327 /// * `to_index` - The second waypoint's index.
1328 /// * `magnitude` - The magnitude.
1329 pub fn sgmt_linear_velocity_max_magnitude(
1330 &mut self,
1331 from_index: usize,
1332 to_index: usize,
1333 magnitude: f64,
1334 ) {
1335 crate::ffi::DifferentialTrajectoryGenerator::sgmt_linear_velocity_max_magnitude(
1336 self.generator.pin_mut(),
1337 from_index,
1338 to_index,
1339 magnitude,
1340 );
1341 }
1342
1343 /// Applies an angular velocity max magnitude constraint between two
1344 /// waypoints.
1345 ///
1346 /// * `from_index` - The first waypoint's index.
1347 /// * `to_index` - The second waypoint's index.
1348 /// * `magnitude` - The magnitude.
1349 pub fn sgmt_angular_velocity_max_magnitude(
1350 &mut self,
1351 from_index: usize,
1352 to_index: usize,
1353 magnitude: f64,
1354 ) {
1355 crate::ffi::DifferentialTrajectoryGenerator::sgmt_angular_velocity_max_magnitude(
1356 self.generator.pin_mut(),
1357 from_index,
1358 to_index,
1359 magnitude,
1360 );
1361 }
1362
1363 /// Applies a linear acceleration max magnitude constraint between two
1364 /// waypoints.
1365 ///
1366 /// * `from_index` - The first waypoint's index.
1367 /// * `to_index` - The second waypoint's index.
1368 /// * `magnitude` - The magnitude.
1369 pub fn sgmt_linear_acceleration_max_magnitude(
1370 &mut self,
1371 from_index: usize,
1372 to_index: usize,
1373 magnitude: f64,
1374 ) {
1375 crate::ffi::DifferentialTrajectoryGenerator::sgmt_linear_acceleration_max_magnitude(
1376 self.generator.pin_mut(),
1377 from_index,
1378 to_index,
1379 magnitude,
1380 );
1381 }
1382
1383 /// Applies a keep-in circle constraint between two waypoints.
1384 ///
1385 /// Applies to the robot bumpers.
1386 ///
1387 /// * `from_index` - The first waypoint's index.
1388 /// * `to_index` - The second waypoint's index.
1389 /// * `center` - The circle's center.
1390 /// * `radius` - The circle's radius.
1391 pub fn sgmt_keep_in_circle(
1392 &mut self,
1393 from_index: usize,
1394 to_index: usize,
1395 center: Translation2d,
1396 radius: f64,
1397 ) {
1398 crate::ffi::DifferentialTrajectoryGenerator::sgmt_keep_in_circle(
1399 self.generator.pin_mut(),
1400 from_index,
1401 to_index,
1402 center,
1403 radius,
1404 )
1405 }
1406
1407 /// Applies a keep-in polygon constraint between two waypoints.
1408 ///
1409 /// Applies to the robot bumpers.
1410 ///
1411 /// * `from_index` - The first waypoint's index.
1412 /// * `to_index` - The second waypoint's index.
1413 /// * `field_points` - The points defining the keep-in polygon (must wind
1414 /// counterclockwise).
1415 pub fn sgmt_keep_in_polygon(
1416 &mut self,
1417 from_index: usize,
1418 to_index: usize,
1419 field_points: Vec<Translation2d>,
1420 ) {
1421 crate::ffi::DifferentialTrajectoryGenerator::sgmt_keep_in_polygon(
1422 self.generator.pin_mut(),
1423 from_index,
1424 to_index,
1425 field_points,
1426 );
1427 }
1428
1429 /// Applies a keep-in lane constraint between two waypoints.
1430 ///
1431 /// Applies to the robot center.
1432 ///
1433 /// * `from_index` - The first waypoint's index.
1434 /// * `to_index` - The second waypoint's index.
1435 /// * `center_line_start` - The center line's start point.
1436 /// * `center_line_end` - The center line's end point.
1437 /// * `tolerance` - The distance from the center line to each lane edge.
1438 #[allow(clippy::too_many_arguments)]
1439 pub fn sgmt_keep_in_lane(
1440 &mut self,
1441 from_index: usize,
1442 to_index: usize,
1443 center_line_start: Translation2d,
1444 center_line_end: Translation2d,
1445 tolerance: f64,
1446 ) {
1447 crate::ffi::DifferentialTrajectoryGenerator::sgmt_keep_in_lane(
1448 self.generator.pin_mut(),
1449 from_index,
1450 to_index,
1451 center_line_start,
1452 center_line_end,
1453 tolerance,
1454 )
1455 }
1456
1457 /// Applies a keep-out circle constraint between two waypoints.
1458 ///
1459 /// Applies to the robot bumpers.
1460 ///
1461 /// * `from_index` - The first waypoint's index.
1462 /// * `to_index` - The second waypoint's index.
1463 /// * `center` - The circle's center.
1464 /// * `radius` - The circle's radius.
1465 pub fn sgmt_keep_out_circle(
1466 &mut self,
1467 from_index: usize,
1468 to_index: usize,
1469 center: Translation2d,
1470 radius: f64,
1471 ) {
1472 crate::ffi::DifferentialTrajectoryGenerator::sgmt_keep_out_circle(
1473 self.generator.pin_mut(),
1474 from_index,
1475 to_index,
1476 center,
1477 radius,
1478 );
1479 }
1480
1481 /// Adds a callback that will be called on each iteration of the solver.
1482 ///
1483 /// * `callback` - a `fn` (not a closure) to be executed. The callback's
1484 /// first parameter will be a `trajopt::DifferentialTrajectory`, and the
1485 /// second parameter will be an `i64` equal to the handle passed in
1486 /// `generate()`
1487 ///
1488 /// This function can be called multiple times to add multiple callbacks.
1489 pub fn add_callback(&mut self, callback: fn(DifferentialTrajectory, i64)) {
1490 crate::ffi::DifferentialTrajectoryGenerator::add_callback(
1491 self.generator.pin_mut(),
1492 callback,
1493 );
1494 }
1495
1496 /// Generates the trajectory.
1497 ///
1498 /// * `diagnostics` - If true, prints per-iteration details of the solver to
1499 /// stdout.
1500 /// * `handle` - A number used to identify results from this generation in
1501 /// the `add_callback` callback. If `add_callback` has not been called,
1502 /// this value has no significance.
1503 ///
1504 /// Returns a result with either the final
1505 /// `trajopt::DifferentialTrajectory`, or TrajoptError
1506 /// generation failed.
1507 pub fn generate(
1508 &self,
1509 diagnostics: bool,
1510 handle: i64,
1511 ) -> Result<DifferentialTrajectory, TrajoptError> {
1512 match self.generator.generate(diagnostics, handle) {
1513 Ok(trajectory) => Ok(trajectory),
1514 Err(msg) => {
1515 let what = msg.what();
1516 Err(TrajoptError::from(
1517 what.parse::<i8>()
1518 .map_err(|_| TrajoptError::Unparsable(Box::from(what)))?,
1519 ))
1520 }
1521 }
1522 }
1523}
1524
1525/// Cancels all running generations.
1526pub fn cancel_all() {
1527 crate::ffi::cancel_all();
1528}
1529
1530use error::TrajoptError;
1531pub use ffi::DifferentialDrivetrain;
1532pub use ffi::DifferentialTrajectory;
1533pub use ffi::DifferentialTrajectorySample;
1534pub use ffi::Pose2d;
1535pub use ffi::SwerveDrivetrain;
1536pub use ffi::SwerveTrajectory;
1537pub use ffi::SwerveTrajectorySample;
1538pub use ffi::Translation2d;
1539
1540/// Solver error types.
1541pub mod error;