From 459f02a5c8264bde62bf1d02969ecdcdca2e5cfe Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Sun, 24 Nov 2024 07:10:25 +0800 Subject: [PATCH] [trajoptlib] Build and publish rust docs TODO - [ ] document constraints Resolves https://github.com/SleipnirGroup/Choreo/issues/942 Signed-off-by: Jade Turner --- make-docs.sh | 2 + .../DifferentialTrajectoryGenerator.hpp | 22 +-- .../trajopt/SwerveTrajectoryGenerator.hpp | 10 +- .../include/trajopt/path/PathBuilder.hpp | 2 +- trajoptlib/src/error.rs | 12 +- trajoptlib/src/lib.rs | 141 ++++++++++++++---- 6 files changed, 144 insertions(+), 45 deletions(-) diff --git a/make-docs.sh b/make-docs.sh index 569c947c8..ff803ca92 100755 --- a/make-docs.sh +++ b/make-docs.sh @@ -13,11 +13,13 @@ popd pushd trajoptlib mkdir -p build/docs doxygen docs/Doxyfile +cargo doc popd mkdir -p site/api/{choreolib,trajoptlib} cp -r choreolib/build/docs/javadoc site/api/choreolib/java cp -r choreolib/build/docs/cpp/html site/api/choreolib/cpp cp -r trajoptlib/build/docs/html site/api/trajoptlib/cpp +cp -r target/docs/trajoptlib site/api/trajoptlib/rs mkdocs build --dirty diff --git a/trajoptlib/include/trajopt/DifferentialTrajectoryGenerator.hpp b/trajoptlib/include/trajopt/DifferentialTrajectoryGenerator.hpp index e7677c6df..d2c9d33da 100644 --- a/trajoptlib/include/trajopt/DifferentialTrajectoryGenerator.hpp +++ b/trajoptlib/include/trajopt/DifferentialTrajectoryGenerator.hpp @@ -29,13 +29,13 @@ struct TRAJOPT_DLLEXPORT DifferentialDrivetrain { /// The moment of inertia of the robot about the origin (kg−m²). double moi; - /// Radius of wheel (m). + /// Radius of the wheels (m). double wheelRadius; - /// Maximum angular velocity of wheel (rad/s). + /// Maximum angular velocity of the wheels (rad/s). double wheelMaxAngularVelocity; - /// Maximum torque applied to wheel (N−m). + /// Maximum torque applied to the wheels (N−m). double wheelMaxTorque; /// The Coefficient of Friction (CoF) of the wheels. @@ -97,22 +97,22 @@ class TRAJOPT_DLLEXPORT DifferentialTrajectorySample { /// The heading. double heading = 0.0; - /// The left wheel velocity. + /// The left wheels velocity. double velocityL = 0.0; - /// The right wheel velocity. + /// The right wheels velocity. double velocityR = 0.0; - /// The left wheel acceleration. + /// The left wheels acceleration. double accelerationL = 0.0; - /// The right wheel acceleration. + /// The right wheels acceleration. double accelerationR = 0.0; - /// The left wheel force. + /// The left wheels force. double forceL = 0.0; - /// The right wheel force. + /// The right wheels force. double forceR = 0.0; DifferentialTrajectorySample() = default; @@ -152,7 +152,7 @@ class TRAJOPT_DLLEXPORT DifferentialTrajectorySample { */ class TRAJOPT_DLLEXPORT DifferentialTrajectory { public: - /// Trajectory samples. + /// The samples that make up the trajectory. std::vector samples; DifferentialTrajectory() = default; @@ -160,7 +160,7 @@ class TRAJOPT_DLLEXPORT DifferentialTrajectory { /** * Construct a DifferentialTrajectory from samples. * - * @param samples The samples. + * @param samples The samples that make up the trajectory. */ explicit DifferentialTrajectory( std::vector samples) diff --git a/trajoptlib/include/trajopt/SwerveTrajectoryGenerator.hpp b/trajoptlib/include/trajopt/SwerveTrajectoryGenerator.hpp index 79e87942c..a5915e645 100644 --- a/trajoptlib/include/trajopt/SwerveTrajectoryGenerator.hpp +++ b/trajoptlib/include/trajopt/SwerveTrajectoryGenerator.hpp @@ -27,13 +27,13 @@ struct TRAJOPT_DLLEXPORT SwerveDrivetrain { /// The moment of inertia of the robot about the origin (kg−m²). double moi; - /// Radius of wheel (m). + /// Radius of the wheels (m). double wheelRadius; - /// Maximum angular velocity of wheel (rad/s). + /// Maximum angular velocity of each wheel (rad/s). double wheelMaxAngularVelocity; - /// Maximum torque applied to wheel (N−m). + /// Maximum torque applied to each wheel (N−m). double wheelMaxTorque; /// The Coefficient of Friction (CoF) of the wheels. @@ -173,7 +173,7 @@ class TRAJOPT_DLLEXPORT SwerveTrajectorySample { */ class TRAJOPT_DLLEXPORT SwerveTrajectory { public: - /// Trajectory samples. + /// The samples that make up the trajectory. std::vector samples; SwerveTrajectory() = default; @@ -181,7 +181,7 @@ class TRAJOPT_DLLEXPORT SwerveTrajectory { /** * Construct a SwerveTrajectory from samples. * - * @param samples The samples. + * @param samples The samples that make up the trajectory. */ explicit SwerveTrajectory(std::vector samples) : samples{std::move(samples)} {} diff --git a/trajoptlib/include/trajopt/path/PathBuilder.hpp b/trajoptlib/include/trajopt/path/PathBuilder.hpp index 9b6b85138..06bb04f67 100644 --- a/trajoptlib/include/trajopt/path/PathBuilder.hpp +++ b/trajoptlib/include/trajopt/path/PathBuilder.hpp @@ -42,7 +42,7 @@ template class TRAJOPT_DLLEXPORT PathBuilder { public: /** - * Set the Drivetrain object + * Set the Drivetrain object. * * @param drivetrain the new drivetrain */ diff --git a/trajoptlib/src/error.rs b/trajoptlib/src/error.rs index aa9bac6bd..f673018c6 100644 --- a/trajoptlib/src/error.rs +++ b/trajoptlib/src/error.rs @@ -1,27 +1,37 @@ use thiserror::Error; #[derive(Debug, Error)] -// messages taken from https://github.com/SleipnirGroup/Sleipnir/blob/main/include/sleipnir/optimization/SolverExitCondition.hpp#L47-L78 +/// Represents an error returned by Trajoptlib. Primarily exposes the Sleipnir [Solver Exit +/// Condition](https://github.com/SleipnirGroup/Sleipnir/blob/main/include/sleipnir/optimization/SolverExitCondition.hpp#L47-L78) pub enum TrajoptError { #[error("The solver determined the problem to be overconstrained and gave up")] + /// The solver determined the problem to be overconstrained and gave up TooFewDOF, #[error("The solver determined the problem to be locally infeasible and gave up")] + /// The solver determined the problem to be locally infeasible and gave up LocallyInfeasible, #[error("The solver failed to reach the desired tolerance, and feasibility restoration failed to converge")] + /// The solver failed to reach the desired tolerance, and feasibility restoration failed to converge FeasibilityRestorationFailed, #[error("The solver encountered nonfinite initial cost or constraints and gave up")] + /// The solver encountered nonfinite initial cost or constraints and gave up NonfiniteInitialCostOrConstraints, #[error("The solver encountered diverging primal iterates xₖ and/or sₖ and gave up")] + /// The solver encountered diverging primal iterates xₖ and/or sₖ and gave up DivergingIterates, #[error( "The solver returned its solution so far after exceeding the maximum number of iterations" )] + /// The solver returned its solution so far after exceeding the maximum number of iterations MaxIterationsExceeded, #[error("The solver returned its solution so far after exceeding the maximum elapsed wall clock time")] + /// The solver returned its solution so far after exceeding the maximum elapsed wall clock time Timeout, #[error("The solver returned an unparsable error code: {0}")] + /// The solver returned an unparsable error code Unparsable(Box), #[error("Unknown error: {0:?}")] + /// Unknown error type Unknown(i8), } diff --git a/trajoptlib/src/lib.rs b/trajoptlib/src/lib.rs index 04359da1b..fe5dbae3d 100644 --- a/trajoptlib/src/lib.rs +++ b/trajoptlib/src/lib.rs @@ -1,77 +1,133 @@ +#![deny(missing_docs)] +#![doc = include_str!("../README.md")] + #[cxx::bridge(namespace = "trajopt::rsffi")] mod ffi { #[derive(Debug, Deserialize, Serialize, Clone)] + /// Represents a translation in 2D space. struct Translation2d { + /// The x component of the translation. x: f64, + /// The y component of the translation. y: f64, } #[derive(Debug, Deserialize, Serialize, Clone)] - struct SwerveDrivetrain { - mass: f64, - moi: f64, - wheel_radius: f64, - wheel_max_angular_velocity: f64, - wheel_max_torque: f64, - wheel_cof: f64, - modules: Vec, + /// Represents a 2D pose with translation and rotation. + struct Pose2d { + /// The x component of the translational component of the pose. + x: f64, + /// The y component of the translational component of the pose. + y: f64, + /// The rotational component of the pose. + heading: f64, } #[derive(Debug, Deserialize, Serialize, Clone)] - struct DifferentialDrivetrain { + /// A swerve drivetrain physical model. + struct SwerveDrivetrain { + /// The mass of the robot (kg). mass: f64, + /// The moment of inertia of the robot about the origin (kg−m²). moi: f64, + /// Radius of the wheels (m). wheel_radius: f64, + /// Maximum angular velocity of each wheel (rad/s). wheel_max_angular_velocity: f64, + /// Maximum torque applied to each wheel (N−m). wheel_max_torque: f64, + /// The Coefficient of Friction (CoF) of the wheels. wheel_cof: f64, - trackwidth: f64, - } - - #[derive(Debug, Deserialize, Serialize, Clone)] - struct Pose2d { - x: f64, - y: f64, - heading: f64, + /// Translation of each swerve module from the origin of the robot coordinate + /// system to the center of the module (m). There's usually one in each + /// corner. + modules: Vec, } #[derive(Debug, Deserialize, Serialize, Clone)] + /// Swerve trajectory sample. struct SwerveTrajectorySample { + /// The timestamp. timestamp: f64, + /// The x coordinate. x: f64, + /// The y coordinate. y: f64, + /// The heading. heading: f64, + /// The velocity's x component. velocity_x: f64, + /// The velocity's y component. velocity_y: f64, + /// The angular velocity. angular_velocity: f64, + /// The acceleration's x component. acceleration_x: f64, + /// The acceleration's y component. acceleration_y: f64, + /// The angular acceleration. angular_acceleration: f64, + /// The force on each module in the X direction. module_forces_x: Vec, + /// The force on each module in the Y direction. module_forces_y: Vec, } #[derive(Debug, Deserialize, Serialize, Clone)] + /// Swerve trajectory. struct SwerveTrajectory { + /// The samples that make up the trajectory. samples: Vec, } #[derive(Debug, Deserialize, Serialize, Clone)] + /// A differential drivetrain physical model. + struct DifferentialDrivetrain { + /// The mass of the robot (kg). + mass: f64, + /// The moment of inertia of the robot about the origin (kg−m²). + moi: f64, + /// Radius of the wheels (m). + wheel_radius: f64, + /// Maximum angular velocity of the wheels (rad/s). + wheel_max_angular_velocity: f64, + /// Maximum torque applied to the wheels (N−m). + wheel_max_torque: f64, + /// The Coefficient of Friction (CoF) of the wheels. + wheel_cof: f64, + /// Distance between the two driverails (m). + trackwidth: f64, + } + + #[derive(Debug, Deserialize, Serialize, Clone)] + /// Differential trajectory sample. struct DifferentialTrajectorySample { + /// The timestamp. timestamp: f64, + /// The x coordinate. x: f64, + /// The y coordinate. y: f64, + /// The heading. heading: f64, + /// The left wheels velocity. velocity_l: f64, + /// The right wheels velocity. velocity_r: f64, + /// The left wheels acceleration. acceleration_l: f64, + /// The right wheels acceleration. acceleration_r: f64, + /// The left wheels force. force_l: f64, + /// The right wheels force. force_r: f64, } #[derive(Debug, Deserialize, Serialize, Clone)] + /// Differential trajectory. struct DifferentialTrajectory { + /// The samples that make up the trajectory. samples: Vec, } @@ -501,6 +557,9 @@ mod ffi { } } + +/// This trajectory generator class contains functions to generate +/// time-optimal trajectories for several drivetrain types. pub struct SwerveTrajectoryGenerator { generator: cxx::UniquePtr, } @@ -512,16 +571,25 @@ impl Default for SwerveTrajectoryGenerator { } impl SwerveTrajectoryGenerator { + /// Construct a new swerve trajectory optimization problem. pub fn new() -> SwerveTrajectoryGenerator { SwerveTrajectoryGenerator { generator: crate::ffi::swerve_trajectory_generator_new(), } } + /// Set the Drivetrain object. pub fn set_drivetrain(&mut self, drivetrain: &crate::ffi::SwerveDrivetrain) { crate::ffi::SwerveTrajectoryGenerator::set_drivetrain(self.generator.pin_mut(), drivetrain); } + /// Add a rectangular bumper to a list used when applying + /// keep-out constraints. + /// + /// * `front` - Distance in meters from center to front bumper edge + /// * `left` - Distance in meters from center to left bumper edge + /// * `right` - Distance in meters from center to right bumper edge + /// * `back` - Distance in meters from center to back bumper edge pub fn set_bumpers(&mut self, front: f64, left: f64, right: f64, back: f64) { crate::ffi::SwerveTrajectoryGenerator::set_bumpers( self.generator.pin_mut(), @@ -532,6 +600,11 @@ impl SwerveTrajectoryGenerator { ); } + /// If using a discrete algorithm, specify the number of discrete + /// samples for every segment of the trajectory + /// + /// * `counts` - the sequence of control interval counts per segment, length + /// is number of waypoints - 1 pub fn set_control_interval_counts(&mut self, counts: Vec) { crate::ffi::SwerveTrajectoryGenerator::set_control_interval_counts( self.generator.pin_mut(), @@ -856,10 +929,9 @@ impl SwerveTrajectoryGenerator { ) } - /// /// Add a callback that will be called on each iteration of the solver. /// - /// * callback: a `fn` (not a closure) to be executed. The callback's first + /// * `callback` - a `fn` (not a closure) to be executed. The callback's first /// parameter will be a `trajopt::SwerveTrajectory`, and the second /// parameter will be an `i64` equal to the handle passed in `generate()` /// @@ -868,12 +940,11 @@ impl SwerveTrajectoryGenerator { crate::ffi::SwerveTrajectoryGenerator::add_callback(self.generator.pin_mut(), callback); } - /// /// Generate the trajectory; /// - /// * diagnostics: If true, prints per-iteration details of the solver to + /// * `diagnostics` - If true, prints per-iteration details of the solver to /// stdout. - /// * handle: A number used to identify results from this generation in the + /// * `handle` - A number used to identify results from this generation in the /// `add_callback` callback. If `add_callback` has not been called, this /// value has no significance. /// @@ -897,6 +968,8 @@ impl SwerveTrajectoryGenerator { } } +/// This trajectory generator class contains functions to generate +/// time-optimal trajectories for differential drivetrain types. pub struct DifferentialTrajectoryGenerator { generator: cxx::UniquePtr, } @@ -908,12 +981,14 @@ impl Default for DifferentialTrajectoryGenerator { } impl DifferentialTrajectoryGenerator { + /// Construct a new differential trajectory optimization problem. pub fn new() -> DifferentialTrajectoryGenerator { DifferentialTrajectoryGenerator { generator: crate::ffi::differential_trajectory_generator_new(), } } + /// Set the Drivetrain object. pub fn set_drivetrain(&mut self, drivetrain: &crate::ffi::DifferentialDrivetrain) { crate::ffi::DifferentialTrajectoryGenerator::set_drivetrain( self.generator.pin_mut(), @@ -921,6 +996,13 @@ impl DifferentialTrajectoryGenerator { ); } + /// Add a rectangular bumper to a list used when applying + /// keep-out constraints. + /// + /// * `front` - Distance in meters from center to front bumper edge + /// * `left` - Distance in meters from center to left bumper edge + /// * `right` - Distance in meters from center to right bumper edge + /// * `back` - Distance in meters from center to back bumper edge pub fn set_bumpers(&mut self, front: f64, left: f64, right: f64, back: f64) { crate::ffi::DifferentialTrajectoryGenerator::set_bumpers( self.generator.pin_mut(), @@ -931,6 +1013,11 @@ impl DifferentialTrajectoryGenerator { ); } + /// If using a discrete algorithm, specify the number of discrete + /// samples for every segment of the trajectory + /// + /// * `counts` - the sequence of control interval counts per segment, length + /// is number of waypoints - 1 pub fn set_control_interval_counts(&mut self, counts: Vec) { crate::ffi::DifferentialTrajectoryGenerator::set_control_interval_counts( self.generator.pin_mut(), @@ -1235,10 +1322,9 @@ impl DifferentialTrajectoryGenerator { ); } - /// /// Add a callback that will be called on each iteration of the solver. /// - /// * callback: a `fn` (not a closure) to be executed. The callback's first + /// * `callback` - a `fn` (not a closure) to be executed. The callback's first /// parameter will be a `trajopt::DifferentialTrajectory`, and the second /// parameter will be an `i64` equal to the handle passed in `generate()` /// @@ -1250,12 +1336,11 @@ impl DifferentialTrajectoryGenerator { ); } - /// /// Generate the trajectory; /// - /// * diagnostics: If true, prints per-iteration details of the solver to + /// * `diagnostics` - If true, prints per-iteration details of the solver to /// stdout. - /// * handle: A number used to identify results from this generation in the + /// * `handle` A number used to identify results from this generation in the /// `add_callback` callback. If `add_callback` has not been called, this /// value has no significance. /// @@ -1280,6 +1365,7 @@ impl DifferentialTrajectoryGenerator { } } +/// Cancels all running generations. pub fn cancel_all() { crate::ffi::cancel_all(); } @@ -1294,4 +1380,5 @@ pub use ffi::SwerveTrajectory; pub use ffi::SwerveTrajectorySample; pub use ffi::Translation2d; +/// Solver error types. pub mod error;