From 3ed6baa43659ce39fe139cf6a237e407d98955b6 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 24 Jun 2024 22:59:18 -0700 Subject: [PATCH] Migrate examples and compile them --- examples/Examples.md | 147 ------------------------------ examples/Swerve/src/Main.cpp | 167 ++++++++++++++++++++++++++++++++--- examples/swerve.rs | 6 +- 3 files changed, 156 insertions(+), 164 deletions(-) delete mode 100644 examples/Examples.md diff --git a/examples/Examples.md b/examples/Examples.md deleted file mode 100644 index b20f7db9..00000000 --- a/examples/Examples.md +++ /dev/null @@ -1,147 +0,0 @@ -# Examples - -`SwervePathBuilderBuilder` is the class used to build paths that are optimized into full trajectories. -`Wpt` is the abbreviation for waypoint, an instantaneous moment in the path where certain constrains on the -robot's state are applied, and `Sgmt` is the abbreviation for segments, the continuum of state between waypoints where -constraints can also be applied. - -## Swerve Drivetrain model - -```cpp -SwerveDrivetrain swerveDrivetrain{.mass = 45, .moi = 6, - {{+0.6, +0.6, 0.04, 70, 2}, - {+0.6, -0.6, 0.04, 70, 2}, - {-0.6, +0.6, 0.04, 70, 2}, - {-0.6, -0.6, 0.04, 70, 2}}}; -``` - -### Example 1: Swerve, one meter forward motion profile - -```cpp -SwervePathBuilderBuilder path; -path.SetDrivetrain(swerveDrivetrain); -path.PoseWpt(0, 0.0, 0.0, 0.0); -path.PoseWpt(1, 1.0, 0.0, 0.0); -path.WptZeroVelocity(0); -path.WptZeroVelocity(1); -path.ControlIntervalCounts({40}); -auto solution = OptimalTrajectoryGenerator::Generate(path); -``` - -### Example 2: Swerve, basic curve - -```cpp -SwervePathBuilderBuilder path; -path.SetDrivetrain(swerveDrivetrain); -path.PoseWpt(0, 1.0, 1.0, -PI/2); -path.PoseWpt(1, 2.0, 0.0, 0.0); -path.WptZeroVelocity(0); -path.WptZeroVelocity(1); -path.ControlIntervalCounts({40}); -auto solution = OptimalTrajectoryGenerator::Generate(path); -``` - -### Example 3: Swerve, three waypoints - -```cpp -SwervePathBuilder path; -path.SetDrivetrain(swerveDrivetrain); -path.PoseWpt(0, 0.0, 0.0, PI/2); -path.PoseWpt(1, 1.0, 1.0, 0.0); -path.PoseWpt(2, 2.0, 0.0, PI/2); -path.WptZeroVelocity(0); -path.WptZeroVelocity(2); -path.ControlIntervalCounts({40, 40}); -auto solution = OptimalTrajectoryGenerator::Generate(path); -``` - -### Example 4: Swerve, ending velocity - -```cpp -SwervePathBuilder path; -path.SetDrivetrain(swerveDrivetrain); -path.PoseWpt(0, 0.0, 0.0, 0.0); -path.PoseWpt(1, 0.0, 1.0, 0.0); -path.WptZeroVelocity(0); -path.WptVelocityPolar(1, 0.2, 0.0); -path.ControlIntervalCounts({40}); -auto solution = OptimalTrajectoryGenerator::Generate(path); -``` - -### Example 5: Swerve, circle obstacle - -```cpp -SwervePathBuilderBuilder path; -path.SetDrivetrain(swerveDrivetrain); -path.PoseWpt(0, 0.0 0_m, 0_deg); -Obstacle obstacle{ - .safetyDistance = 0.1, // radius of 0.1 - .points = {{0.5, 0.5}} -}; -path.SgmtObstacle(0, 1, obstacle); -path.PoseWpt(1, 1.0, 0.0, 0.0); -path.WptZeroVelocity(0); -path.WptZeroVelocity(1); -path.ControlIntervalCounts({40}); -auto solution = OptimalTrajectoryGenerator::Generate(path); -``` - -### Example 6: Approach a pick up station at a certain direction - -```cpp -SwervePathBuilder path; -path.SetDrivetrain(swerveDrivetrain); - -// starting position -path.PoseWpt(0, 0.0, 0.0, 0.0); - -// align towards the station one meter behind -path.PoseWpt(1, 1.0, 1.0, PI/2); -path.WptZeroAngularVelocity(1); -path.WptVelocityDirection(1, PI/2); - -// go up to the station -// In practice, the optimizer will still end up aligning the heading without -// the pose constraint since it's most optimal -path.TranslationWpt(2, 1.0, 2.0); - -// realign behind the station -path.PoseWpt(3, 1.0, 1.0, PI/2); -path.WptZeroAngularVelocity(3); -path.WptVelocityDirection(3, PI/2); - -// ending position -path.PoseWpt(4, 2.0, 0.0, PI); - -path.WptZeroVelocity(0); -path.WptZeroVelocity(4); -path.ControlIntervalCounts({40, 30, 30, 40}); -auto solution = OptimalTrajectoryGenerator::Generate(path); -``` - -### Example 7: Circular path with a point-point constraint - -Note that forcing a circular path is not a common problem in FRC, this example -is only to demonstrate how various constraints work. - -```cpp -SwervePathBuilder path; -path.SetDrivetrain(swerveDrivetrain); - -path.PoseWpt(0, 0.0, 0.0, 0.0); -path.SgmtConstraint(0, 1 - PointPointConstraint{ - 0.0, 0.0, // robot point--center of bot - 1.0, 0.0, // field point to orbit around - 1.0}); // stay a distance of 1_m away to force circular motion - -// tell optimizer to go in +y direction rather than -y -path.AddInitialGuess(0, 0.0, 0.0, 0.0); - -path.PoseWpt(1, 2.0, 0.0, 0.0); - -path.WptZeroVelocity(0); -path.WptZeroVelocity(1); -path.ControlIntervalCounts({30}); -auto solution = OptimalTrajectoryGenerator::Generate(path); -``` diff --git a/examples/Swerve/src/Main.cpp b/examples/Swerve/src/Main.cpp index fbee745b..4ac8a03e 100644 --- a/examples/Swerve/src/Main.cpp +++ b/examples/Swerve/src/Main.cpp @@ -1,7 +1,18 @@ // Copyright (c) TrajoptLib contributors +#include + #include +// SwervePathBuilder is used to build paths that are optimized into full +// trajectories. +// +// "Wpt" stands for waypoint, an instantaneous moment in the path where certain +// constrains on the robot's state are applied. +// +// "Sgmt" is the abbreviation for segments, the continuum of state between +// waypoints where constraints can also be applied. + int main() { trajopt::SwerveDrivetrain swerveDrivetrain{ .mass = 45, @@ -11,21 +22,149 @@ int main() { {{-0.6, +0.6}, 0.04, 70, 2}, {{-0.6, -0.6}, 0.04, 70, 2}}}; - trajopt::Obstacle bumpers{ - 0, {{+0.5, +0.5}, {-0.5, +0.5}, {-0.5, -0.5}, {+0.5, -0.5}}}; + trajopt::LinearVelocityMaxMagnitudeConstraint zeroLinearVelocity{0.0}; + trajopt::AngularVelocityMaxMagnitudeConstraint zeroAngularVelocity{0.0}; + + // Example 1: Swerve, one meter forward motion profile + { + trajopt::SwervePathBuilder path; + path.SetDrivetrain(swerveDrivetrain); + path.PoseWpt(0, 0.0, 0.0, 0.0); + path.PoseWpt(1, 1.0, 0.0, 0.0); + path.WptConstraint(0, zeroLinearVelocity); + path.WptConstraint(1, zeroLinearVelocity); + path.ControlIntervalCounts({40}); + + trajopt::SwerveTrajectoryGenerator generator{path}; + [[maybe_unused]] + auto solution = generator.Generate(true); + } + + // Example 2: Swerve, basic curve + { + trajopt::SwervePathBuilder path; + path.SetDrivetrain(swerveDrivetrain); + path.PoseWpt(0, 1.0, 1.0, -std::numbers::pi / 2); + path.PoseWpt(1, 2.0, 0.0, 0.0); + path.WptConstraint(0, zeroLinearVelocity); + path.WptConstraint(1, zeroLinearVelocity); + path.ControlIntervalCounts({40}); + + trajopt::SwerveTrajectoryGenerator generator{path}; + [[maybe_unused]] + auto solution = generator.Generate(true); + } + + // Example 3: Swerve, three waypoints + { + trajopt::SwervePathBuilder path; + path.SetDrivetrain(swerveDrivetrain); + path.PoseWpt(0, 0.0, 0.0, std::numbers::pi / 2); + path.PoseWpt(1, 1.0, 1.0, 0.0); + path.PoseWpt(2, 2.0, 0.0, std::numbers::pi / 2); + path.WptConstraint(0, zeroLinearVelocity); + path.WptConstraint(1, zeroLinearVelocity); + path.ControlIntervalCounts({40, 40}); + + trajopt::SwerveTrajectoryGenerator generator{path}; + [[maybe_unused]] + auto solution = generator.Generate(true); + } + + // Example 4: Swerve, ending velocity + { + trajopt::SwervePathBuilder path; + path.SetDrivetrain(swerveDrivetrain); + path.PoseWpt(0, 0.0, 0.0, 0.0); + path.PoseWpt(1, 0.0, 1.0, 0.0); + path.WptConstraint(0, zeroLinearVelocity); + path.ControlIntervalCounts({40}); + + trajopt::SwerveTrajectoryGenerator generator{path}; + [[maybe_unused]] + auto solution = generator.Generate(true); + } + + // Example 5: Swerve, circle obstacle + { + trajopt::SwervePathBuilder path; + path.SetDrivetrain(swerveDrivetrain); + path.PoseWpt(0, 0.0, 0.0, 0.0); + trajopt::Obstacle obstacle{.safetyDistance = 0.1, // radius of 0.1 + .points = {{0.5, 0.5}}}; + path.SgmtObstacle(0, 1, obstacle); + path.PoseWpt(1, 1.0, 0.0, 0.0); + path.WptConstraint(0, zeroLinearVelocity); + path.WptConstraint(1, zeroLinearVelocity); + path.ControlIntervalCounts({40}); + + trajopt::SwerveTrajectoryGenerator generator{path}; + [[maybe_unused]] + auto solution = generator.Generate(true); + } + + // Example 6: Approach a pick up station at a certain direction + { + trajopt::SwervePathBuilder path; + path.SetDrivetrain(swerveDrivetrain); + + // Starting position + path.PoseWpt(0, 0.0, 0.0, 0.0); + + // Align towards the station one meter behind + path.PoseWpt(1, 1.0, 1.0, std::numbers::pi / 2); + path.WptConstraint(1, zeroAngularVelocity); + path.WptConstraint( + 1, trajopt::LinearVelocityDirectionConstraint{std::numbers::pi / 2}); + + // Go up to the station. In practice, the optimizer will still end up + // aligning the heading without the pose constraint since it's most optimal. + path.TranslationWpt(2, 1.0, 2.0); + + // Realign behind the station + path.PoseWpt(3, 1.0, 1.0, std::numbers::pi / 2); + path.WptConstraint(3, zeroAngularVelocity); + path.WptConstraint( + 3, trajopt::LinearVelocityDirectionConstraint{std::numbers::pi / 2}); + + // Ending position + path.PoseWpt(4, 2.0, 0.0, std::numbers::pi); + + path.WptConstraint(0, zeroLinearVelocity); + path.WptConstraint(4, zeroLinearVelocity); + path.ControlIntervalCounts({40, 30, 30, 40}); + + trajopt::SwerveTrajectoryGenerator generator{path}; + [[maybe_unused]] + auto solution = generator.Generate(true); + } + + // Example 7: Circular path with a point-point constraint + { + // Note that forcing a circular path is not a common problem in FRC. This + // example is only here to demonstrate how various constraints work. + trajopt::SwervePathBuilder path; + path.SetDrivetrain(swerveDrivetrain); + + path.PoseWpt(0, 0.0, 0.0, 0.0); + path.SgmtConstraint(0, 1, + trajopt::PointPointConstraint{ + {0.0, 0.0}, // Robot point -- center of robot + {1.0, 0.0}, // Field point around which to orbit + 1.0 // Stay 1 m away to force circular motion + }); + + // Tell optimizer to go in +y direction rather than -y + path.WptInitialGuessPoint(0, {0.0, 0.0, 0.0}); - // One Meter Forward - trajopt::SwervePathBuilder path; - path.SetDrivetrain(swerveDrivetrain); - path.PoseWpt(0, 0.0, 0.0, 0.0); - path.PoseWpt(1, 5.0, 0.0, 0.0); - path.WptConstraint(0, trajopt::LinearVelocityMaxMagnitudeConstraint{0.0}); - path.WptConstraint(1, trajopt::LinearVelocityMaxMagnitudeConstraint{0.0}); - path.ControlIntervalCounts({4}); + path.PoseWpt(1, 2.0, 0.0, 0.0); - trajopt::SwerveTrajectoryGenerator generator{path}; + path.WptConstraint(0, zeroLinearVelocity); + path.WptConstraint(1, zeroLinearVelocity); + path.ControlIntervalCounts({30}); - // SOLVE - [[maybe_unused]] - auto solution = generator.Generate(true); + trajopt::SwerveTrajectoryGenerator generator{path}; + [[maybe_unused]] + auto solution = generator.Generate(true); + } } diff --git a/examples/swerve.rs b/examples/swerve.rs index 71bc8eac..b2ce0f1d 100644 --- a/examples/swerve.rs +++ b/examples/swerve.rs @@ -42,9 +42,9 @@ fn main() { path.set_bumpers(1.3, 1.3); path.pose_wpt(0, 0.0, 0.0, 0.0); path.pose_wpt(1, 1.0, 0.0, 0.0); - // path.wpt_angular_velocity(0, 0.0); - // path.wpt_angular_velocity(1, 0.0); - // path.sgmt_circle_obstacle(0, 1, 0.5, 0.1, 0.2); + path.wpt_angular_velocity(0, 0.0); + path.wpt_angular_velocity(1, 0.0); + path.sgmt_circle_obstacle(0, 1, 0.5, 0.1, 0.2); path.set_control_interval_counts(vec![40]); println!("setup complete"); println!("{:?}", path.generate(true, 0));