Skip to main content

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;