TOPPRA Optionally Relax Constraints to Handle Numerically-Challenging Trajectories

bindings/generated_docstrings/multibody_optimization.h

@@ -1062,6 +1062,25 @@ Parameter ``gridpoints``:
     ensure the reachable set calculated in the backward pass is always
     bounded.)""";
         } ctor;
+        // Symbol: drake::multibody::Toppra::constraint_relaxation
+        struct /* constraint_relaxation */ {
+          // Source: drake/multibody/optimization/toppra.h
+          const char* doc =
+R"""(Returns the problem relaxation tolerance used in the forward pass.)""";
+        } constraint_relaxation;
+        // Symbol: drake::multibody::Toppra::set_constraint_relaxation
+        struct /* set_constraint_relaxation */ {
+          // Source: drake/multibody/optimization/toppra.h
+          const char* doc =
+R"""(Sets the tolerance used to slightly relax the forward pass linear
+constraints. This is a problem tolerance, not a solver tolerance,
+meaning it slightly relaxes the physical problem bounds. This is
+useful for numerically difficult trajectories where the backward pass
+solver finds a solution on the boundary of the feasible set, causing
+the forward pass to fail due to solver precision limits. A typical
+value is ``1e-4`` or ``1e-5``. If 0.0, no relaxation is applied. The
+default is 0.0 (no relaxation).)""";
+        } set_constraint_relaxation;
       } Toppra;
       // Symbol: drake::multibody::ToppraDiscretization
       struct /* ToppraDiscretization */ {

bindings/pydrake/multibody/optimization_py.cc

@@ -226,7 +226,11 @@ PYDRAKE_MODULE(optimization, m) {
             py::arg("constraint_frame"), py::arg("lower_limit"),
             py::arg("upper_limit"),
             py::arg("discretization") = ToppraDiscretization::kInterpolation,
-            cls_doc.AddFrameAccelerationLimit.doc_trajectory);
+            cls_doc.AddFrameAccelerationLimit.doc_trajectory)
+        .def("set_constraint_relaxation", &Class::set_constraint_relaxation,
+            py::arg("relaxation"), cls_doc.set_constraint_relaxation.doc)
+        .def("constraint_relaxation", &Class::constraint_relaxation,
+            cls_doc.constraint_relaxation.doc);
   }
 }
 }  // namespace

bindings/pydrake/multibody/test/optimization_test.py

@@ -397,6 +397,10 @@ def test_all_constraints(self):
         gridpoints = Toppra.CalcGridPoints(path, CalcGridPointsOptions())
         toppra = Toppra(path=path, plant=plant, gridpoints=gridpoints)
 
+        # Test basic constraint relaxation API.
+        toppra.set_constraint_relaxation(relaxation=1e-3)
+        self.assertEqual(toppra.constraint_relaxation(), 1e-3)
+
         # Joint constraints
         upper_limit = np.arange(7)
         lower_limit = -upper_limit

multibody/optimization/test/toppra_test.cc

@@ -816,6 +816,43 @@ TEST_F(PrismaticToppraTest, InfiniteTimeStepError) {
   EXPECT_FALSE(result);
 }
 
+TEST_F(IiwaToppraTest, ConstraintRelaxationTest) {
+  // We construct a trajectory with a tiny velocity but massive acceleration,
+  // causing the linear constraints on path acceleration `u` to have very tight
+  // bounds. This replicates the scenario where the forward pass fails due to
+  // primal feasibility tolerance gaps inherited from the backward pass.
+  std::vector<Eigen::Matrix<Polynomial<double>, Eigen::Dynamic, Eigen::Dynamic>>
+      segments(1);
+  Eigen::Matrix<Polynomial<double>, 7, 1> poly_mat;
+  poly_mat(0, 0) = Polynomial<double>(Eigen::Vector3d(0, -999.99, 500.0));
+  poly_mat(1, 0) = Polynomial<double>(Eigen::Vector3d(0, -1000.01, 500.0));
+  for (int i = 2; i < 7; ++i) {
+    poly_mat(i, 0) = Polynomial<double>(Eigen::Vector3d::Zero());
+  }
+  segments[0] = poly_mat;
+  std::vector<double> breaks = {0.0, 2.0};
+  PiecewisePolynomial<double> path(segments, breaks);
+
+  Eigen::VectorXd gridpoints(3);
+  gridpoints << 0.0, 1.0, 2.0;
+
+  Toppra toppra(path, *iiwa_plant_, gridpoints);
+  toppra.AddJointAccelerationLimit(Eigen::VectorXd::Constant(7, -10),
+                                   Eigen::VectorXd::Constant(7, 10));
+
+  // The forward pass LP should fail due to numerical overlap if the bounds are
+  // artificially tightened (simulating the numerical noise from complex or
+  // numerically-challenging trajectories).
+  toppra.set_constraint_relaxation(-1e-8);
+  auto result = toppra.SolvePathParameterization();
+  EXPECT_FALSE(result.has_value());
+
+  // But with a sufficiently large positive tolerance, it should succeed!
+  toppra.set_constraint_relaxation(1e-2);
+  result = toppra.SolvePathParameterization();
+  EXPECT_TRUE(result.has_value());
+}
+
 }  // namespace
 }  // namespace multibody
 }  // namespace drake

multibody/optimization/toppra.cc

@@ -579,18 +579,30 @@ Toppra::ComputeForwardPass(double s_dot_0,
         Vector1d(2 * delta), Vector1d(K(0, knot + 1) - xstar(knot)),
         Vector1d(K(1, knot + 1) - xstar(knot)));
     for (auto& [constraint, coefficients] : forward_lin_constraint_) {
-      constraint.evaluator()->UpdateCoefficients(
-          coefficients.coeffs.col(2 * knot + 1),
-          coefficients.lb.col(knot) -
-              xstar(knot) * coefficients.coeffs.col(2 * knot),
-          coefficients.ub.col(knot) -
-              xstar(knot) * coefficients.coeffs.col(2 * knot));
+      auto lb_val = coefficients.lb.col(knot) -
+                    xstar(knot) * coefficients.coeffs.col(2 * knot);
+      auto ub_val = coefficients.ub.col(knot) -
+                    xstar(knot) * coefficients.coeffs.col(2 * knot);
+      if (constraint_relaxation_ == 0.0) {
+        constraint.evaluator()->UpdateCoefficients(
+            coefficients.coeffs.col(2 * knot + 1), lb_val, ub_val);
+      } else {
+        constraint.evaluator()->UpdateCoefficients(
+            coefficients.coeffs.col(2 * knot + 1),
+            lb_val - Eigen::VectorXd::Constant(lb_val.rows(),
+                                               constraint_relaxation_),
+            ub_val + Eigen::VectorXd::Constant(ub_val.rows(),
+                                               constraint_relaxation_));
+      }
     }
     solvers::MathematicalProgramResult result;
     solver.Solve(*forward_prog_, {}, {}, &result);
     if (!result.is_success()) {
       drake::log()->error(
-          "Toppra failed to find the maximum path acceleration at knot {}/{}.",
+          "Toppra failed to find the maximum path acceleration at knot {}/{}. "
+          "This failure may be caused by a numerically-challenging trajectory. "
+          "Consider slightly relaxing the constraints with "
+          "`Toppra::set_constraint_relaxation()`.",
           knot, N);
       return std::nullopt;
     } else {

multibody/optimization/toppra.h

@@ -158,7 +158,8 @@ class Toppra {
    * @param discretization The discretization scheme to use for this linear
    *                       constraint. See ToppraDiscretization for details.
    * @return A pair containing the linear constraints that will enforce the
-   *         torque limit on the backward pass and forward pass respectively.
+   *         torque limit on the backward pass and forward pass
+   *         respectively.
    */
   std::pair<Binding<LinearConstraint>, Binding<LinearConstraint>>
   AddJointTorqueLimit(const Eigen::Ref<const Eigen::VectorXd>& lower_limit,
@@ -267,6 +268,24 @@ class Toppra {
                             ToppraDiscretization discretization =
                                 ToppraDiscretization::kInterpolation);
 
+  /**
+   * Sets the tolerance used to slightly relax the forward pass linear
+   * constraints. This is a problem tolerance, not a solver tolerance, meaning
+   * it slightly relaxes the physical problem bounds. This is useful for
+   * numerically difficult trajectories where the backward pass solver finds a
+   * solution on the boundary of the feasible set, causing the forward pass to
+   * fail due to solver precision limits. A typical value is `1e-4` or `1e-5`.
+   * If 0.0, no relaxation is applied. The default is 0.0 (no relaxation).
+   */
+  void set_constraint_relaxation(double relaxation) {
+    constraint_relaxation_ = relaxation;
+  }
+
+  /**
+   * Returns the problem relaxation tolerance used in the forward pass.
+   */
+  double constraint_relaxation() const { return constraint_relaxation_; }
+
  private:
   /*
    * Performs the backward pass step of TOPPRA, returning the controllable set,
@@ -368,6 +387,9 @@ class Toppra {
   // backward_lin_constraint_.
   std::unordered_map<Binding<LinearConstraint>, ToppraLinearConstraint>
       forward_lin_constraint_;
+
+  // Problem tolerance to relax forward pass linear constraints.
+  double constraint_relaxation_{0.0};
 };
 }  // namespace multibody
 }  // namespace drake