diff --git a/cmake/modules/FetchSleipnir.cmake b/cmake/modules/FetchSleipnir.cmake index 22f7b757..14b27b2d 100644 --- a/cmake/modules/FetchSleipnir.cmake +++ b/cmake/modules/FetchSleipnir.cmake @@ -4,8 +4,8 @@ macro(fetch_sleipnir) fetchcontent_declare( Sleipnir GIT_REPOSITORY https://github.com/SleipnirGroup/Sleipnir.git - # main on 2024-01-07 - GIT_TAG da2bd2bec834ab4154eb5b0010818e7edda37406 + # main on 2024-01-08 + GIT_TAG 02c5b2446975b45b38f35f4a40e54f988010d486 ) fetchcontent_getproperties(Sleipnir) diff --git a/include/trajopt/TrajectoryGenerationException.h b/include/trajopt/TrajectoryGenerationException.h index 59d499a6..6b48d6f8 100644 --- a/include/trajopt/TrajectoryGenerationException.h +++ b/include/trajopt/TrajectoryGenerationException.h @@ -3,7 +3,7 @@ #pragma once #include -#include +#include #include "trajopt/SymbolExports.h" @@ -22,7 +22,8 @@ class TRAJOPT_DLLEXPORT TrajectoryGenerationException * * @param message the string message */ - explicit TrajectoryGenerationException(const std::string& message); + explicit TrajectoryGenerationException(std::string_view message); + /** * @brief Construct a new Trajectory Generation Exception object with a string * message. diff --git a/src/TrajectoryGenerationException.cpp b/src/TrajectoryGenerationException.cpp index bfa97363..e25e7435 100644 --- a/src/TrajectoryGenerationException.cpp +++ b/src/TrajectoryGenerationException.cpp @@ -2,16 +2,15 @@ #include "trajopt/TrajectoryGenerationException.h" -#include #include namespace trajopt { TrajectoryGenerationException::TrajectoryGenerationException( - const std::string& message) - : logic_error(message) {} + std::string_view message) + : std::logic_error{std::string{message}} {} TrajectoryGenerationException::TrajectoryGenerationException( const char* message) - : logic_error(message) {} + : std::logic_error{message} {} } // namespace trajopt diff --git a/src/optimization/SleipnirOpti.cpp b/src/optimization/SleipnirOpti.cpp index f56d15d6..4080dfff 100644 --- a/src/optimization/SleipnirOpti.cpp +++ b/src/optimization/SleipnirOpti.cpp @@ -7,12 +7,14 @@ #include #include +#include #include #include #include #include "DebugOptions.h" #include "optimization/Cancellation.h" +#include "trajopt/TrajectoryGenerationException.h" namespace trajopt { @@ -107,7 +109,13 @@ void SleipnirOpti::Solve() { opti.Callback([](const sleipnir::SolverIterationInfo&) -> bool { return trajopt::GetCancellationFlag(); }); - opti.Solve({.diagnostics = true}); + + auto status = opti.Solve({.diagnostics = true}); + + if (static_cast(status.exitCondition) < 0) { + throw TrajectoryGenerationException{ + sleipnir::ToMessage(status.exitCondition)}; + } } double SleipnirOpti::SolutionValue(const SleipnirExpr& expression) const { diff --git a/src/optimization/algorithms/SwerveDiscreteOptimal.inc b/src/optimization/algorithms/SwerveDiscreteOptimal.inc index 7203009d..b79bac63 100644 --- a/src/optimization/algorithms/SwerveDiscreteOptimal.inc +++ b/src/optimization/algorithms/SwerveDiscreteOptimal.inc @@ -6,6 +6,8 @@ #include #include +#include + #include "DebugOptions.h" #include "optimization/SwerveTrajoptUtil.h" #include "optimization/TrajoptUtil.h" @@ -25,8 +27,8 @@ SwerveSolution SwerveDiscreteOptimal::Generate() { return ConstructSwerveSolution(opti, x, y, theta, vx, vy, omega, ax, ay, alpha, Fx, Fy, dt, N); } catch (const std::exception& e) { - throw TrajectoryGenerationException("Error optimizing trajectory: " + - std::string(e.what())); + throw TrajectoryGenerationException{ + fmt::format("Error optimizing trajectory: {}", e.what())}; } }