Skip to content

Commit 0f4b194

Browse files
calcmogulcalcmogul
committed
[trajoptlib] Upgrade Sleipnir for faster solves
1 parent ef95b1c commit 0f4b194

26 files changed

+316
-325
lines changed

‎trajoptlib/CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -103,8 +103,8 @@ set(BUILD_EXAMPLES OFF)
103103
fetchcontent_declare(
104104
Sleipnir
105105
GIT_REPOSITORY https://github.com/SleipnirGroup/Sleipnir
106-
# main on 2025-01-14
107-
GIT_TAG 742092895f076733f5f9c584e4aae71d86ec5ea1
106+
# main on 2025-04-16
107+
GIT_TAG 294722035a920ae8c44bb8f562959e892d7861b7
108108
)
109109
fetchcontent_makeavailable(Sleipnir)
110110

‎trajoptlib/cmake/modules/CompilerFlags.cmake‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,5 +25,7 @@ macro(compiler_flags target)
2525
target_compile_features(${target} PUBLIC cxx_std_23)
2626
if(MSVC)
2727
target_compile_options(${target} PUBLIC /MP /Zf /utf-8 /bigobj)
28+
elseif(APPLE)
29+
target_compile_options(${target} PUBLIC -Wno-pre-c++2b-compat)
2830
endif()
2931
endmacro()

‎trajoptlib/include/trajopt/DifferentialTrajectoryGenerator.hpp‎

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -9,9 +9,9 @@
99
#include <utility>
1010
#include <vector>
1111

12-
#include <sleipnir/autodiff/Variable.hpp>
13-
#include <sleipnir/optimization/OptimizationProblem.hpp>
14-
#include <sleipnir/optimization/SolverExitCondition.hpp>
12+
#include <sleipnir/autodiff/variable.hpp>
13+
#include <sleipnir/optimization/problem.hpp>
14+
#include <sleipnir/optimization/solver/exit_status.hpp>
1515

1616
#include "trajopt/path/PathBuilder.hpp"
1717
#include "trajopt/util/SymbolExports.hpp"
@@ -229,33 +229,33 @@ class TRAJOPT_DLLEXPORT DifferentialTrajectoryGenerator {
229229
* @return Returns a holonomic trajectory on success, or a string containing a
230230
* failure reason.
231231
*/
232-
std::expected<DifferentialSolution, sleipnir::SolverExitCondition> Generate(
232+
std::expected<DifferentialSolution, slp::ExitStatus> Generate(
233233
bool diagnostics = false);
234234

235235
private:
236236
/// Differential path
237237
DifferentialPath path;
238238

239239
/// State Variables
240-
std::vector<sleipnir::Variable> x;
241-
std::vector<sleipnir::Variable> y;
242-
std::vector<sleipnir::Variable> θ;
243-
std::vector<sleipnir::Variable> vl;
244-
std::vector<sleipnir::Variable> vr;
245-
std::vector<sleipnir::Variable> al;
246-
std::vector<sleipnir::Variable> ar;
240+
std::vector<slp::Variable> x;
241+
std::vector<slp::Variable> y;
242+
std::vector<slp::Variable> θ;
243+
std::vector<slp::Variable> vl;
244+
std::vector<slp::Variable> vr;
245+
std::vector<slp::Variable> al;
246+
std::vector<slp::Variable> ar;
247247

248248
/// Input Variables
249-
std::vector<sleipnir::Variable> Fl;
250-
std::vector<sleipnir::Variable> Fr;
249+
std::vector<slp::Variable> Fl;
250+
std::vector<slp::Variable> Fr;
251251

252252
/// Time Variables
253-
std::vector<sleipnir::Variable> dts;
253+
std::vector<slp::Variable> dts;
254254

255255
/// Discretization Constants
256256
std::vector<size_t> Ns;
257257

258-
sleipnir::OptimizationProblem problem;
258+
slp::Problem problem;
259259

260260
void ApplyInitialGuess(const DifferentialSolution& solution);
261261

‎trajoptlib/include/trajopt/SwerveTrajectoryGenerator.hpp‎

Lines changed: 18 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -6,9 +6,9 @@
66
#include <utility>
77
#include <vector>
88

9-
#include <sleipnir/autodiff/Variable.hpp>
10-
#include <sleipnir/optimization/OptimizationProblem.hpp>
11-
#include <sleipnir/optimization/SolverExitCondition.hpp>
9+
#include <sleipnir/autodiff/variable.hpp>
10+
#include <sleipnir/optimization/problem.hpp>
11+
#include <sleipnir/optimization/solver/exit_status.hpp>
1212

1313
#include "trajopt/geometry/Translation2.hpp"
1414
#include "trajopt/path/PathBuilder.hpp"
@@ -240,36 +240,36 @@ class TRAJOPT_DLLEXPORT SwerveTrajectoryGenerator {
240240
* @return Returns a holonomic trajectory on success, or a string containing a
241241
* failure reason.
242242
*/
243-
std::expected<SwerveSolution, sleipnir::SolverExitCondition> Generate(
243+
std::expected<SwerveSolution, slp::ExitStatus> Generate(
244244
bool diagnostics = false);
245245

246246
private:
247247
/// Swerve path
248248
SwervePath path;
249249

250250
/// State Variables
251-
std::vector<sleipnir::Variable> x;
252-
std::vector<sleipnir::Variable> y;
253-
std::vector<sleipnir::Variable> cosθ;
254-
std::vector<sleipnir::Variable> sinθ;
255-
std::vector<sleipnir::Variable> vx;
256-
std::vector<sleipnir::Variable> vy;
257-
std::vector<sleipnir::Variable> ω;
258-
std::vector<sleipnir::Variable> ax;
259-
std::vector<sleipnir::Variable> ay;
260-
std::vector<sleipnir::Variable> α;
251+
std::vector<slp::Variable> x;
252+
std::vector<slp::Variable> y;
253+
std::vector<slp::Variable> cosθ;
254+
std::vector<slp::Variable> sinθ;
255+
std::vector<slp::Variable> vx;
256+
std::vector<slp::Variable> vy;
257+
std::vector<slp::Variable> ω;
258+
std::vector<slp::Variable> ax;
259+
std::vector<slp::Variable> ay;
260+
std::vector<slp::Variable> α;
261261

262262
/// Input Variables
263-
std::vector<std::vector<sleipnir::Variable>> Fx;
264-
std::vector<std::vector<sleipnir::Variable>> Fy;
263+
std::vector<std::vector<slp::Variable>> Fx;
264+
std::vector<std::vector<slp::Variable>> Fy;
265265

266266
/// Time Variables
267-
std::vector<sleipnir::Variable> dts;
267+
std::vector<slp::Variable> dts;
268268

269269
/// Discretization Constants
270270
std::vector<size_t> Ns;
271271

272-
sleipnir::OptimizationProblem problem;
272+
slp::Problem problem;
273273

274274
void ApplyInitialGuess(const SwerveSolution& solution);
275275

‎trajoptlib/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp‎

Lines changed: 8 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,8 @@
44

55
#include <cassert>
66

7-
#include <sleipnir/autodiff/Variable.hpp>
8-
#include <sleipnir/optimization/OptimizationProblem.hpp>
7+
#include <sleipnir/autodiff/variable.hpp>
8+
#include <sleipnir/optimization/problem.hpp>
99

1010
#include "trajopt/geometry/Pose2.hpp"
1111
#include "trajopt/geometry/Translation2.hpp"
@@ -39,17 +39,16 @@ class TRAJOPT_DLLEXPORT AngularVelocityMaxMagnitudeConstraint {
3939
* @param linearAcceleration The robot's linear acceleration.
4040
* @param angularAcceleration The robot's angular acceleration.
4141
*/
42-
void Apply(sleipnir::OptimizationProblem& problem,
43-
[[maybe_unused]] const Pose2v& pose,
42+
void Apply(slp::Problem& problem, [[maybe_unused]] const Pose2v& pose,
4443
[[maybe_unused]] const Translation2v& linearVelocity,
45-
const sleipnir::Variable& angularVelocity,
44+
const slp::Variable& angularVelocity,
4645
[[maybe_unused]] const Translation2v& linearAcceleration,
47-
[[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
46+
[[maybe_unused]] const slp::Variable& angularAcceleration) {
4847
if (m_maxMagnitude == 0.0) {
49-
problem.SubjectTo(angularVelocity == 0.0);
48+
problem.subject_to(angularVelocity == 0.0);
5049
} else {
51-
problem.SubjectTo(angularVelocity >= -m_maxMagnitude);
52-
problem.SubjectTo(angularVelocity <= m_maxMagnitude);
50+
problem.subject_to(angularVelocity >= -m_maxMagnitude);
51+
problem.subject_to(angularVelocity <= m_maxMagnitude);
5352
}
5453
}
5554

‎trajoptlib/include/trajopt/constraint/Constraint.hpp‎

Lines changed: 12 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,8 @@
55
#include <concepts>
66
#include <variant>
77

8-
#include <sleipnir/autodiff/Variable.hpp>
9-
#include <sleipnir/optimization/OptimizationProblem.hpp>
8+
#include <sleipnir/autodiff/variable.hpp>
9+
#include <sleipnir/optimization/problem.hpp>
1010

1111
#include "trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp"
1212
#include "trajopt/constraint/LaneConstraint.hpp"
@@ -37,17 +37,16 @@ namespace trajopt {
3737
* 3. Add the type to Constraint's std::variant type list
3838
*/
3939
template <typename T>
40-
concept ConstraintType =
41-
requires(T self, sleipnir::OptimizationProblem& problem, const Pose2v& pose,
42-
const Translation2v& linearVelocity,
43-
const sleipnir::Variable& angularVelocity,
44-
const Translation2v& linearAcceleration,
45-
const sleipnir::Variable& angularAcceleration) {
46-
{
47-
self.Apply(problem, pose, linearVelocity, angularVelocity,
48-
linearAcceleration, angularAcceleration)
49-
} -> std::same_as<void>;
50-
};
40+
concept ConstraintType = requires(
41+
T self, slp::Problem& problem, const Pose2v& pose,
42+
const Translation2v& linearVelocity, const slp::Variable& angularVelocity,
43+
const Translation2v& linearAcceleration,
44+
const slp::Variable& angularAcceleration) {
45+
{
46+
self.Apply(problem, pose, linearVelocity, angularVelocity,
47+
linearAcceleration, angularAcceleration)
48+
} -> std::same_as<void>;
49+
};
5150

5251
static_assert(ConstraintType<AngularVelocityMaxMagnitudeConstraint>);
5352
static_assert(ConstraintType<LaneConstraint>);

‎trajoptlib/include/trajopt/constraint/LaneConstraint.hpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -73,11 +73,11 @@ class TRAJOPT_DLLEXPORT LaneConstraint {
7373
* @param linearAcceleration The robot's linear acceleration.
7474
* @param angularAcceleration The robot's angular acceleration.
7575
*/
76-
void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose,
76+
void Apply(slp::Problem& problem, const Pose2v& pose,
7777
const Translation2v& linearVelocity,
78-
const sleipnir::Variable& angularVelocity,
78+
const slp::Variable& angularVelocity,
7979
const Translation2v& linearAcceleration,
80-
const sleipnir::Variable& angularAcceleration) {
80+
const slp::Variable& angularAcceleration) {
8181
m_topLine.Apply(problem, pose, linearVelocity, angularVelocity,
8282
linearAcceleration, angularAcceleration);
8383
if (m_bottomLine.has_value()) {

‎trajoptlib/include/trajopt/constraint/LinePointConstraint.hpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,8 @@
55
#include <cassert>
66
#include <utility>
77

8-
#include <sleipnir/autodiff/Variable.hpp>
9-
#include <sleipnir/optimization/OptimizationProblem.hpp>
8+
#include <sleipnir/autodiff/variable.hpp>
9+
#include <sleipnir/optimization/problem.hpp>
1010

1111
#include "trajopt/constraint/detail/LinePointSquaredDistance.hpp"
1212
#include "trajopt/geometry/Pose2.hpp"
@@ -52,18 +52,18 @@ class TRAJOPT_DLLEXPORT LinePointConstraint {
5252
* @param linearAcceleration The robot's linear acceleration.
5353
* @param angularAcceleration The robot's angular acceleration.
5454
*/
55-
void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose,
55+
void Apply(slp::Problem& problem, const Pose2v& pose,
5656
[[maybe_unused]] const Translation2v& linearVelocity,
57-
[[maybe_unused]] const sleipnir::Variable& angularVelocity,
57+
[[maybe_unused]] const slp::Variable& angularVelocity,
5858
[[maybe_unused]] const Translation2v& linearAcceleration,
59-
[[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
59+
[[maybe_unused]] const slp::Variable& angularAcceleration) {
6060
auto lineStart =
6161
pose.Translation() + m_robotLineStart.RotateBy(pose.Rotation());
6262
auto lineEnd =
6363
pose.Translation() + m_robotLineEnd.RotateBy(pose.Rotation());
6464
auto squaredDistance =
6565
detail::LinePointSquaredDistance(lineStart, lineEnd, m_fieldPoint);
66-
problem.SubjectTo(squaredDistance >= m_minDistance * m_minDistance);
66+
problem.subject_to(squaredDistance >= m_minDistance * m_minDistance);
6767
}
6868

6969
private:

‎trajoptlib/include/trajopt/constraint/LinearAccelerationMaxMagnitudeConstraint.hpp‎

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,8 @@
44

55
#include <cassert>
66

7-
#include <sleipnir/autodiff/Variable.hpp>
8-
#include <sleipnir/optimization/OptimizationProblem.hpp>
7+
#include <sleipnir/autodiff/variable.hpp>
8+
#include <sleipnir/optimization/problem.hpp>
99

1010
#include "trajopt/geometry/Pose2.hpp"
1111
#include "trajopt/geometry/Translation2.hpp"
@@ -39,18 +39,17 @@ class TRAJOPT_DLLEXPORT LinearAccelerationMaxMagnitudeConstraint {
3939
* @param linearAcceleration The robot's linear acceleration.
4040
* @param angularAcceleration The robot's angular acceleration.
4141
*/
42-
void Apply(sleipnir::OptimizationProblem& problem,
43-
[[maybe_unused]] const Pose2v& pose,
42+
void Apply(slp::Problem& problem, [[maybe_unused]] const Pose2v& pose,
4443
[[maybe_unused]] const Translation2v& linearVelocity,
45-
[[maybe_unused]] const sleipnir::Variable& angularVelocity,
44+
[[maybe_unused]] const slp::Variable& angularVelocity,
4645
const Translation2v& linearAcceleration,
47-
[[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
46+
[[maybe_unused]] const slp::Variable& angularAcceleration) {
4847
if (m_maxMagnitude == 0.0) {
49-
problem.SubjectTo(linearAcceleration.X() == 0.0);
50-
problem.SubjectTo(linearAcceleration.Y() == 0.0);
48+
problem.subject_to(linearAcceleration.X() == 0.0);
49+
problem.subject_to(linearAcceleration.Y() == 0.0);
5150
} else {
52-
problem.SubjectTo(linearAcceleration.SquaredNorm() <=
53-
m_maxMagnitude * m_maxMagnitude);
51+
problem.subject_to(linearAcceleration.SquaredNorm() <=
52+
m_maxMagnitude * m_maxMagnitude);
5453
}
5554
}
5655

‎trajoptlib/include/trajopt/constraint/LinearVelocityDirectionConstraint.hpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -2,8 +2,8 @@
22

33
#pragma once
44

5-
#include <sleipnir/autodiff/Variable.hpp>
6-
#include <sleipnir/optimization/OptimizationProblem.hpp>
5+
#include <sleipnir/autodiff/variable.hpp>
6+
#include <sleipnir/optimization/problem.hpp>
77

88
#include "trajopt/geometry/Pose2.hpp"
99
#include "trajopt/geometry/Translation2.hpp"
@@ -33,19 +33,19 @@ class TRAJOPT_DLLEXPORT LinearVelocityDirectionConstraint {
3333
* @param linearAcceleration The robot's linear acceleration.
3434
* @param angularAcceleration The robot's angular acceleration.
3535
*/
36-
void Apply([[maybe_unused]] sleipnir::OptimizationProblem& problem,
36+
void Apply([[maybe_unused]] slp::Problem& problem,
3737
[[maybe_unused]] const Pose2v& pose,
3838
const Translation2v& linearVelocity,
39-
[[maybe_unused]] const sleipnir::Variable& angularVelocity,
39+
[[maybe_unused]] const slp::Variable& angularVelocity,
4040
[[maybe_unused]] const Translation2v& linearAcceleration,
41-
[[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
41+
[[maybe_unused]] const slp::Variable& angularAcceleration) {
4242
// <v_x, v_y> and <u_x, u_y> must be parallel
4343
//
4444
// (v ⋅ u)/‖v‖ = 1
4545
// v ⋅ u = ‖v‖
4646
// (v ⋅ u)² = ‖v‖²
4747
auto dot = linearVelocity.Dot(Translation2d{m_angle.Cos(), m_angle.Sin()});
48-
problem.SubjectTo(dot * dot == linearVelocity.SquaredNorm());
48+
problem.subject_to(dot * dot == linearVelocity.SquaredNorm());
4949
}
5050

5151
private:

0 commit comments

Comments
 (0)