Skip to content

Commit

Permalink
[trajoptlib] Run examples in CI
Browse files Browse the repository at this point in the history
  • Loading branch information
@spacey-sooty
spacey-sooty committed Sep 1, 2024
1 parent 946f947 commit 9dd7072
Show file tree
Hide file tree
Showing 3 changed files with 106 additions and 14 deletions.
5 changes: 5 additions & 0 deletions .github/workflows/trajoptlib-cpp.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,11 @@ jobs:
working-directory: trajoptlib
- run: cmake --install build --config RelWithDebInfo --prefix pkg
working-directory: trajoptlib
- run: ./build/Swerve
working-directory: trajoptlib
# FIXME: fails with locally infeasible
# - run: ./build/Differential
# working-directory: trajoptlib

- uses: actions/upload-artifact@v4
with:
Expand Down
59 changes: 52 additions & 7 deletions trajoptlib/examples/Differential/src/Main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,11 @@

#include <numbers>

#include <sleipnir/optimization/SolverExitCondition.hpp>
#include <trajopt/DifferentialTrajectoryGenerator.hpp>

#include "fmt/base.h"

// DifferentialPathBuilder is used to build paths that are optimized into full
// trajectories.
//
Expand Down Expand Up @@ -36,8 +39,14 @@ int main() {
path.ControlIntervalCounts({40});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 1: {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}

// Example 2: Differential, basic curve
Expand All @@ -51,8 +60,14 @@ int main() {
path.ControlIntervalCounts({40});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 2: {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}

// Example 3: Differential, three waypoints
Expand All @@ -67,8 +82,14 @@ int main() {
path.ControlIntervalCounts({50, 50});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 3: {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}

// Example 4: Differential, ending velocity
Expand All @@ -81,8 +102,14 @@ int main() {
path.ControlIntervalCounts({40});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 4: {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}

// Example 5: Differential, circle obstacle
Expand All @@ -100,8 +127,14 @@ int main() {
path.ControlIntervalCounts({40});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 5: {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}

// Example 6: Approach a pick up station at a certain direction
Expand Down Expand Up @@ -136,8 +169,14 @@ int main() {
path.ControlIntervalCounts({40, 30, 30, 40});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 6: {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}

// Example 7: Circular path with a point-point constraint
Expand Down Expand Up @@ -167,7 +206,13 @@ int main() {
path.ControlIntervalCounts({50});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 7: {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}
}
56 changes: 49 additions & 7 deletions trajoptlib/examples/Swerve/src/Main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,14 @@ int main() {
path.ControlIntervalCounts({40});

trajopt::SwerveTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 1: {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}

// Example 2: Swerve, basic curve
Expand All @@ -51,8 +57,14 @@ int main() {
path.ControlIntervalCounts({40});

trajopt::SwerveTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 2 {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}

// Example 3: Swerve, three waypoints
Expand All @@ -67,8 +79,14 @@ int main() {
path.ControlIntervalCounts({40, 40});

trajopt::SwerveTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 3 {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}

// Example 4: Swerve, ending velocity
Expand All @@ -81,8 +99,14 @@ int main() {
path.ControlIntervalCounts({40});

trajopt::SwerveTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 4: {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}

// Example 5: Swerve, circle obstacle
Expand All @@ -100,8 +124,14 @@ int main() {
path.ControlIntervalCounts({40});

trajopt::SwerveTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 5: {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}

// Example 6: Approach a pick up station at a certain direction
Expand Down Expand Up @@ -136,8 +166,14 @@ int main() {
path.ControlIntervalCounts({40, 30, 30, 40});

trajopt::SwerveTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 6: {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}

// Example 7: Circular path with a point-point constraint
Expand Down Expand Up @@ -167,7 +203,13 @@ int main() {
path.ControlIntervalCounts({30});

trajopt::SwerveTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
if (!(solution.error() == sleipnir::SolverExitCondition::kSuccess ||
solution.error() ==
sleipnir::SolverExitCondition::kSolvedToAcceptableTolerance)) {
fmt::println("Error in example 7: {}",
sleipnir::ToMessage(solution.error()));
return static_cast<int>(solution.error());
}
}
}

0 comments on commit 9dd7072

Please sign in to comment.