[trajoptlib] Set dt upper bound

Without it, the solver increases dt a lot, then reports infeasible on most paths. The anti-tunneling constraint implicitly set an upper bound on dt, but setting an explicit upper bound is better.
SleipnirGroup · Jan 6, 2025 · 0b5abd1 · 0b5abd1
1 parent 97962cb
commit 0b5abd1
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 0 deletions.
diff --git a/trajoptlib/src/DifferentialTrajectoryGenerator.cpp b/trajoptlib/src/DifferentialTrajectoryGenerator.cpp
@@ -152,6 +152,8 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
     T_tot += T_sgmt;
 
     problem.SubjectTo(dt >= 0);
+    problem.SubjectTo(dt <= 0.1);
+
     // Use initialGuess and Ns to find the dx, dy, dθ between wpts
     const auto sgmt_start = GetIndex(Ns, sgmtIndex);
     const auto sgmt_end = GetIndex(Ns, sgmtIndex + 1);

diff --git a/trajoptlib/src/SwerveTrajectoryGenerator.cpp b/trajoptlib/src/SwerveTrajectoryGenerator.cpp
@@ -125,6 +125,7 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
     T_tot += T_sgmt;
 
     problem.SubjectTo(dt >= 0);
+    problem.SubjectTo(dt <= 0.1);
 
     // Use initialGuess and Ns to find the dx, dy, dθ between wpts
     const auto sgmt_start = GetIndex(Ns, sgmtIndex);