Skip to content
This repository has been archived by the owner on Jul 8, 2024. It is now read-only.

Commit

Permalink
Migrate examples and compile them (#214)
Browse files Browse the repository at this point in the history
  • Loading branch information
@calcmogul
calcmogul authored Jun 25, 2024
1 parent 80b6b4b commit f95414a
Show file tree
Hide file tree
Showing 3 changed files with 159 additions and 164 deletions.
147 changes: 0 additions & 147 deletions examples/Examples.md
View file

This file was deleted.

170 changes: 156 additions & 14 deletions examples/Swerve/src/Main.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,18 @@
// Copyright (c) TrajoptLib contributors

#include <numbers>

#include <trajopt/SwerveTrajectoryGenerator.hpp>

// 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,
Expand All @@ -11,21 +22,152 @@ 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{// Radius of 0.1
.safetyDistance = 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{// Robot point -- center of robot
{0.0, 0.0},
// Field point around which to orbit
{1.0, 0.0},
// Stay 1 m away to force circular motion
1.0});

// 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);
}
}
6 changes: 3 additions & 3 deletions examples/swerve.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down

0 comments on commit f95414a

Please sign in to comment.