Use clamp-then-shrink for trust-box construction at variable bounds

rjoomen · claude · rjoomen · commit 4f2d9ac8339b · 2026-07-23T15:07:48.000+02:00
Near a variable bound the trust box was built by sliding the full-width
box inside [lb, ub] (the "slide always" policy). That kept the 2*box_size
width but let the QP step exceed box_size on the interior-facing side
whenever a bound was active, breaking the trust-region invariant
||p||_inf &lt;= box_size that the SQP merit-improve machinery relies on --
even for iterates strictly inside their bounds.

Switch to clamp-then-shrink: clamp the iterate into [lb, ub], then
strict-shrink the box against the bounds. For in-bounds iterates this
restores ||p||_inf &lt;= box_size; when the iterate has drifted outside its
bounds (failed step, bad warm-start) the box still stays well-formed
(non-empty, non-inverted), so the QP solver does not error out. Applied
to both QP problem variants (IfoptQPProblem, TrajOptQPProblem) and the
trajopt_sco optimizer. Verified by trust_box_floor_unit.

Co-Authored-By: Claude Opus 4.7 (1M context) &lt;noreply@anthropic.com&gt;
diff --git a/trajopt_ifopt/README.md b/trajopt_ifopt/README.md
@@ -47,6 +47,31 @@ While you can sometimes rewrite formulations so “scale constraints” and “s
 - **Use coefficients/weights** on the **slack penalty** to represent priority/importance of the soft constraint.
 
 
+## Trust-Box Construction Near Variable Bounds
+
+The trust region in `TrajOptQPProblem` is an $L_\infty$ box on the NLP step, $|p_i| \le \Delta_i$. The QP solver sees it as variable bounds on $x_i + p_i$, intersected with the user-provided bounds $[l_i, u_i]$. Two awkward situations can arise at that intersection:
+
+1. The iterate $x_i$ sits at or near a bound, so the naive box $[x_i - \Delta_i, x_i + \Delta_i]$ extends past the bound on one side.
+2. The iterate has drifted outside $[l_i, u_i]$ — after a step rejection, a bad warm-start, or in the recovery regime of a failed solve.
+
+`TrajOptQPProblem::Implementation::updateNLPVariableBounds` handles both with one rule: **clamp $x_i$ into $[l_i, u_i]$, then strict-shrink the trust box against the bounds.**
+
+$$x_i^{\text{eff}} \;=\; \mathrm{clamp}(x_i,\, l_i,\, u_i), \qquad \text{box}_i \;=\; [\,\max(x_i^{\text{eff}} - \Delta_i,\, l_i),\; \min(x_i^{\text{eff}} + \Delta_i,\, u_i)\,].$$
+
+For $x_i$ strictly inside $[l_i, u_i]$ the clamp is a no-op and the box is the standard trust region — $\|p\|_\infty \le \Delta$ is preserved, which is the contract the rest of the SQP machinery relies on (the merit-improve ratio $\rho$ is only meaningful for steps within the radius the model was expanded for). As $x$ approaches a bound the box width shrinks monotonically:
+
+| $x$ position relative to upper bound $u$ | Box width |
+|---|---|
+| $x \le u - \Delta$ | $2\Delta$ (centered) |
+| $u - \Delta < x < u$ | $\Delta + (u - x)$ (linear ramp from $2\Delta$ down to $\Delta$) |
+| $x = u$ | $\Delta$ |
+| $x > u$ | $\Delta$ (constant; $x^{\text{eff}} = u$) |
+
+Past the bound the trust-region invariant is unavoidably violated — any step pulling the iterate back through the bound has magnitude $\ge |x - u|$ — but the QP box itself stays well-formed (non-empty, non-inverted), so the QP solver does not error out.
+
+The earlier "slide always" policy instead kept the full $2\Delta$ width by sliding the box inside $[l, u]$. That preserved the bug-fix property but let the QP step exceed $\Delta$ on the side facing the interior whenever a bound was active, even with the iterate strictly inside its bounds (within a trust-radius of the bound) — breaking $\|p\|_\infty \le \Delta$ in the regime where the SQP merit-improve machinery needs it most. The clamp-then-shrink design keeps the bug-fix property and restores the trust-region invariant for in-bounds iterates.
+
+
 ## Currently Supported Constraints
 * Joint Position
 * Joint Velocity
diff --git a/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp
@@ -467,11 +467,13 @@ void IfoptQPProblem::updateNLPVariableBounds()
     var_bounds_upper[i] = bounds.getUpper();
   }
 
-  // Calculate box constraints, while limiting to variable bounds and maintaining the trust region size
-  const Eigen::VectorXd var_bounds_lower_final =
-      (x_initial.cwiseMin(var_bounds_upper - box_size_) - box_size_).cwiseMax(var_bounds_lower);
-  const Eigen::VectorXd var_bounds_upper_final =
-      (x_initial.cwiseMax(var_bounds_lower + box_size_) + box_size_).cwiseMin(var_bounds_upper);
+  // Calculate box constraints, clamped to variable bounds. The iterate is first clamped into
+  // [lb, ub] so the box stays non-empty when x has drifted outside its bounds (failed step, bad
+  // warm-start); for x inside [lb, ub] this is a no-op and the box is the standard strict-shrink
+  // trust region.
+  const Eigen::VectorXd x_clamped = x_initial.cwiseMin(var_bounds_upper).cwiseMax(var_bounds_lower);
+  const Eigen::VectorXd var_bounds_lower_final = (x_clamped - box_size_).cwiseMax(var_bounds_lower);
+  const Eigen::VectorXd var_bounds_upper_final = (x_clamped + box_size_).cwiseMin(var_bounds_upper);
   bounds_lower_.block(num_nlp_cnts_, 0, var_bounds_lower_final.size(), 1) = var_bounds_lower_final;
   bounds_upper_.block(num_nlp_cnts_, 0, var_bounds_upper_final.size(), 1) = var_bounds_upper_final;
 }
diff --git a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp
@@ -6,6 +6,7 @@
 #include <trajopt_sqp/expressions.h>
 #include <trajopt_sqp/types.h>
 
+#include <algorithm>
 #include <utility>
 #include <iostream>
 #include <cassert>
@@ -1098,16 +1099,19 @@ void TrajOptQPProblem::Implementation::updateNLPVariableBounds(const Eigen::Ref<
   auto lower = cvp.bounds_lower.segment(idx, cvp.n_nlp_vars);
   auto upper = cvp.bounds_upper.segment(idx, cvp.n_nlp_vars);
 
-  // Assumes x, var_bounds_lower/upper are size n (or you use matching segments)
+  // Calculate box constraints, clamped to variable bounds. The iterate is first
+  // clamped into [lb, ub] so the box stays non-empty when x has drifted outside
+  // its bounds (failed step, bad warm-start); for x inside [lb, ub] this is a
+  // no-op and the box is the standard strict-shrink trust region.
   for (Eigen::Index i = 0; i < cvp.n_nlp_vars; ++i)
   {
-    const double xi = nlp_values[i];
     const double bi = box_size[i];
     const double lb = var_bounds_lower[i];
     const double ub = var_bounds_upper[i];
+    const double xi = std::clamp(nlp_values[i], lb, ub);
 
-    lower[i] = std::max(lb, std::min(xi, ub - bi) - bi);
-    upper[i] = std::min(ub, std::max(xi, lb + bi) + bi);
+    lower[i] = std::max(xi - bi, lb);
+    upper[i] = std::min(xi + bi, ub);
   }
 }
 
diff --git a/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt b/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt
@@ -37,3 +37,4 @@ add_gtest(${PROJECT_NAME}_planning_unit planning_unit.cpp)
 add_gtest(${PROJECT_NAME}_simple_collision_unit simple_collision_unit.cpp)
 add_gtest(${PROJECT_NAME}_cart_position_optimization_unit cart_position_optimization_unit.cpp)
 add_gtest(${PROJECT_NAME}_hessian_gradient_unit hessian_gradient_unit.cpp)
+add_gtest(${PROJECT_NAME}_trust_box_floor_unit trust_box_floor_unit.cpp)
diff --git a/trajopt_optimizers/trajopt_sqp/test/trust_box_floor_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/trust_box_floor_unit.cpp
@@ -0,0 +1,142 @@
+/**
+ * @file trust_box_floor_unit.cpp
+ * @brief Unit tests for TrajOptQPProblem trust-box construction near and past variable bounds.
+ *
+ * Policy: clamp the iterate into its variable bounds [lb, ub], then strict-shrink the trust box
+ * [x-Δ, x+Δ] against [lb, ub]. For x inside [lb, ub] this is a no-op and yields the standard
+ * trust-region box with |p|_∞ ≤ Δ. For x drifted outside [lb, ub], the clamp keeps the box
+ * non-empty (width Δ, flush against the violated bound) so the next QP step pulls the iterate
+ * back through the bound.
+ *
+ * Replaces the earlier PR #472 "slide always" behavior, which kept full 2·Δ width even when the
+ * iterate sat on a bound — at the cost of letting the QP step exceed Δ on the open side.
+ */
+#include <trajopt_common/macros.h>
+TRAJOPT_IGNORE_WARNINGS_PUSH
+#include <gtest/gtest.h>
+#include <Eigen/Core>
+TRAJOPT_IGNORE_WARNINGS_POP
+
+#include <trajopt_sqp/trajopt_qp_problem.h>
+#include <trajopt_ifopt/variable_sets/nodes_variables.h>
+#include <trajopt_ifopt/variable_sets/node.h>
+
+namespace
+{
+constexpr double kLb = -2.5;
+constexpr double kUb = 2.5;
+constexpr double kBi = 0.0015;  // trust half-width Δ
+
+std::shared_ptr<trajopt_sqp::TrajOptQPProblem> makeProblem(double x_init, double lb, double ub)
+{
+  auto node = std::make_unique<trajopt_ifopt::Node>("Joints");
+  const std::vector<std::string> names{ "j0" };
+  const Eigen::VectorXd values = (Eigen::VectorXd(1) << x_init).finished();
+  const std::vector<trajopt_ifopt::Bounds> bounds{ trajopt_ifopt::Bounds(lb, ub) };
+  node->addVar("position", names, values, bounds);
+
+  std::vector<std::unique_ptr<trajopt_ifopt::Node>> nodes;
+  nodes.push_back(std::move(node));
+  auto variables = std::make_shared<trajopt_ifopt::NodesVariables>("trajectory", std::move(nodes));
+
+  auto qp = std::make_shared<trajopt_sqp::TrajOptQPProblem>(variables);
+  qp->setup();
+  return qp;
+}
+
+// Read the variable trust-box segment. With no constraints, it occupies bounds_lower.head(n_nlp_vars).
+struct TrustBox
+{
+  double lower;
+  double upper;
+  double width() const { return upper - lower; }
+};
+TrustBox readBox(const trajopt_sqp::TrajOptQPProblem& qp)
+{
+  const Eigen::Index n = qp.getNumNLPVars();
+  return { qp.getBoundsLower().head(n)[0], qp.getBoundsUpper().head(n)[0] };
+}
+}  // namespace
+
+// Iterate well inside [lb, ub]: standard centered box [x-Δ, x+Δ], width 2·Δ.
+TEST(TrajOptQPTrustBox, InteriorIterateGetsCenteredBox)  // NOLINT
+{
+  auto qp = makeProblem(0.0, kLb, kUb);
+  qp->setBoxSize(Eigen::VectorXd::Constant(1, kBi));
+  const auto box = readBox(*qp);
+
+  EXPECT_NEAR(box.lower, 0.0 - kBi, 1e-12);
+  EXPECT_NEAR(box.upper, 0.0 + kBi, 1e-12);
+  EXPECT_NEAR(box.width(), 2.0 * kBi, 1e-12);
+}
+
+// Iterate within Δ of the upper bound: box clamped at ub, lower side asymmetric. Width transitions
+// from 2·Δ down to Δ as x rises from (ub-Δ) to ub. Verifying the midpoint case x = ub - Δ/2.
+TEST(TrajOptQPTrustBox, IterateNearUpperBoundShrinksAsymmetrically)  // NOLINT
+{
+  const double x_val = kUb - (kBi / 2.0);
+  auto qp = makeProblem(x_val, kLb, kUb);
+  qp->setBoxSize(Eigen::VectorXd::Constant(1, kBi));
+  const auto box = readBox(*qp);
+
+  EXPECT_NEAR(box.lower, x_val - kBi, 1e-12);
+  EXPECT_NEAR(box.upper, kUb, 1e-12);
+  EXPECT_NEAR(box.width(), 1.5 * kBi, 1e-12);
+  // TR invariant: max(|lower - x|, |upper - x|) = max(Δ, Δ/2) = Δ. Held.
+  EXPECT_LE(std::max(std::abs(box.lower - x_val), std::abs(box.upper - x_val)), kBi + 1e-12);
+}
+
+// Iterate at the upper bound exactly: box [ub-Δ, ub], width Δ, fully TR-compliant.
+TEST(TrajOptQPTrustBox, IterateAtUpperBoundGetsBoxOfWidthBi)  // NOLINT
+{
+  auto qp = makeProblem(kUb, kLb, kUb);
+  qp->setBoxSize(Eigen::VectorXd::Constant(1, kBi));
+  const auto box = readBox(*qp);
+
+  EXPECT_NEAR(box.lower, kUb - kBi, 1e-12);
+  EXPECT_NEAR(box.upper, kUb, 1e-12);
+  EXPECT_NEAR(box.width(), kBi, 1e-12);
+}
+
+// PR #472 bug case: iterate past the upper bound by >> Δ. The clamp keeps the box flush against
+// the violated bound with width Δ — no inversion, no OSQP error. The QP step will pull x back
+// through the bound (step magnitude ≈ |x - ub|, unavoidable for recovery).
+TEST(TrajOptQPTrustBox, IterateFarPastUpperBoundStaysAtWidthBi)  // NOLINT
+{
+  const double x_val = 2.6;  // ub = 2.5, violation = 0.1, Δ = 0.0015
+  auto qp = makeProblem(x_val, kLb, kUb);
+  qp->setBoxSize(Eigen::VectorXd::Constant(1, kBi));
+  const auto box = readBox(*qp);
+
+  EXPECT_NEAR(box.lower, kUb - kBi, 1e-12);
+  EXPECT_NEAR(box.upper, kUb, 1e-12);
+  EXPECT_NEAR(box.width(), kBi, 1e-12);
+  EXPECT_GT(box.width(), 0.0);  // explicit non-inversion guard
+}
+
+// Mirror: iterate past the lower bound. Box flush at lb with width Δ.
+TEST(TrajOptQPTrustBox, IterateFarPastLowerBoundStaysAtWidthBi)  // NOLINT
+{
+  auto qp = makeProblem(-2.6, kLb, kUb);
+  qp->setBoxSize(Eigen::VectorXd::Constant(1, kBi));
+  const auto box = readBox(*qp);
+
+  EXPECT_NEAR(box.lower, kLb, 1e-12);
+  EXPECT_NEAR(box.upper, kLb + kBi, 1e-12);
+  EXPECT_NEAR(box.width(), kBi, 1e-12);
+}
+
+// Variable range narrower than the trust radius: box is clamped to the full natural range
+// [lb, ub], which is the best we can do.
+TEST(TrajOptQPTrustBox, BoundsNarrowerThanBiClampToRange)  // NOLINT
+{
+  const double narrow_lb = 0.0;
+  const double narrow_ub = 0.1;
+  const double bi = 0.5;  // Δ wider than the variable's allowed range
+  auto qp = makeProblem(0.05, narrow_lb, narrow_ub);
+  qp->setBoxSize(Eigen::VectorXd::Constant(1, bi));
+  const auto box = readBox(*qp);
+
+  EXPECT_NEAR(box.lower, narrow_lb, 1e-12);
+  EXPECT_NEAR(box.upper, narrow_ub, 1e-12);
+}
diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp
@@ -1,6 +1,7 @@
 #include <trajopt_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <boost/format.hpp>
+#include <algorithm>
 #include <cmath>
 #include <chrono>
 #include <cstdio>
@@ -155,11 +156,15 @@ void BasicTrustRegionSQP::setTrustBoxConstraints(const DblVec& x)
   const DblVec& ub = prob_->getUpperBounds();
   DblVec lbtrust(x.size());
   DblVec ubtrust(x.size());
+  // Calculate box constraints, clamped to variable bounds. The iterate is first clamped into
+  // [lb, ub] so the box stays non-empty when x has drifted outside its bounds (failed step, bad
+  // warm-start); for x inside [lb, ub] this is a no-op and the box is the standard strict-shrink
+  // trust region.
   for (std::size_t i = 0; i < x.size(); ++i)
   {
-    // Calculate box constraints, while limiting to variable bounds and maintaining the trust region size
-    lbtrust[i] = fmax(fmin(x[i], ub[i] - param_.trust_box_size) - param_.trust_box_size, lb[i]);
-    ubtrust[i] = fmin(fmax(x[i], lb[i] + param_.trust_box_size) + param_.trust_box_size, ub[i]);
+    const double xi = std::clamp(x[i], lb[i], ub[i]);
+    lbtrust[i] = std::max(xi - param_.trust_box_size, lb[i]);
+    ubtrust[i] = std::min(xi + param_.trust_box_size, ub[i]);
   }
   model_->setVarBounds(vars, lbtrust, ubtrust);
 }
