From 0b5abd1dc0b883e82375d2e7f7b1e1ae360f1a73 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sun, 5 Jan 2025 22:34:19 -0800
Subject: [PATCH] [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.
---
 trajoptlib/src/DifferentialTrajectoryGenerator.cpp | 2 ++
 trajoptlib/src/SwerveTrajectoryGenerator.cpp       | 1 +
 2 files changed, 3 insertions(+)

diff --git a/trajoptlib/src/DifferentialTrajectoryGenerator.cpp b/trajoptlib/src/DifferentialTrajectoryGenerator.cpp
index ecafdfad6..56b4bf554 100644
--- 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
index 7dc016d33..f99bc19ba 100644
--- 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);