Fix ParseConstraint for cases with infs in rhs or lhs.

solvers/create_constraint.cc

@@ -31,6 +31,8 @@ using symbolic::Polynomial;
 using symbolic::Variable;
 using symbolic::Variables;
 
+const double kInf = numeric_limits<double>::infinity();
+
 Binding<Constraint> ParseConstraint(
     const Eigen::Ref<const VectorX<Expression>>& v,
     const Eigen::Ref<const Eigen::VectorXd>& lb,
@@ -337,16 +339,64 @@ Binding<Constraint> ParseConstraint(
         ub(k) = 0.0;
       } else if (is_less_than_or_equal_to(f)) {
         // f(i) := (lhs <= rhs)
-        //         (-∞ <= lhs - rhs <= 0)
-        v(k) = get_lhs_expression(f) - get_rhs_expression(f);
-        lb(k) = -std::numeric_limits<double>::infinity();
-        ub(k) = 0.0;
+        const Expression& lhs = get_lhs_expression(f);
+        const Expression& rhs = get_rhs_expression(f);
+        if (is_constant(lhs, kInf)) {
+          throw std::runtime_error(
+              fmt::format("ParseConstraint is called with a formula ({}) with "
+                          "a lower bound of +inf.", f));
+        }
+        if (is_constant(rhs, -kInf)) {
+          throw std::runtime_error(
+              fmt::format("ParseConstraint is called with a formula ({}) with "
+                          "an upper bound of -inf.", f));
+        }
+        if (is_constant(lhs, -kInf)) {
+          // The constraint is trivial, but valid.
+          v(k) = rhs;
+          lb(k) = -kInf;
+          ub(k) = kInf;
+        } else if (is_constant(rhs, kInf)) {
+          // The constraint is trivial, but valid.
+          v(k) = lhs;
+          lb(k) = -kInf;
+          ub(k) = kInf;
+        } else {
+          // -∞ <= lhs - rhs <= 0
+          v(k) = lhs - rhs;
+          lb(k) = -kInf;
+          ub(k) = 0.0;
+        }
       } else if (is_greater_than_or_equal_to(f)) {
         // f(i) := (lhs >= rhs)
-        //         (∞ >= lhs - rhs >= 0)
-        v(k) = get_lhs_expression(f) - get_rhs_expression(f);
-        lb(k) = 0.0;
-        ub(k) = std::numeric_limits<double>::infinity();
+        const Expression& lhs = get_lhs_expression(f);
+        const Expression& rhs = get_rhs_expression(f);
+        if (is_constant(rhs, kInf)) {
+          throw std::runtime_error(
+              fmt::format("ParseConstraint is called with a formula ({}) with "
+                          "a lower bound of +inf.", f));
+        }
+        if (is_constant(lhs, -kInf)) {
+          throw std::runtime_error(
+              fmt::format("ParseConstraint is called with a formula ({}) with "
+                          "an upper bound of -inf.", f));
+        }
+        if (is_constant(rhs, -kInf)) {
+          // The constraint is trivial, but valid.
+          v(k) = lhs;
+          lb(k) = -kInf;
+          ub(k) = kInf;
+        } else if (is_constant(lhs, kInf)) {
+          // The constraint is trivial, but valid.
+          v(k) = rhs;
+          lb(k) = -kInf;
+          ub(k) = kInf;
+        } else {
+          // 0 <= lhs - rhs <= ∞
+          v(k) = lhs - rhs;
+          lb(k) = 0.0;
+          ub(k) = kInf;
+        }
       } else {
         std::ostringstream oss;
         oss << "ParseConstraint is called with an "

solvers/test/create_constraint_test.cc

@@ -16,6 +16,7 @@ namespace solvers {
 namespace {
 
 using Eigen::Vector2d;
+using symbolic::Variable;
 
 const double kInf = std::numeric_limits<double>::infinity();
 
@@ -542,6 +543,33 @@ GTEST_TEST(ParseConstraintTest, Quadratic) {
             nullptr);
 }
 
+GTEST_TEST(ParseConstraintTest, FormulaWithInfiniteLowerOrUpperBounds) {
+  Variable x0("x0"), x1("x1");
+  Vector2<Variable> x(x0, x1);
+  Vector2d b(0.12, kInf);
+  Binding<Constraint> binding =
+      internal::ParseConstraint(x <= b);
+  EXPECT_TRUE(CompareMatrices(binding.evaluator()->upper_bound(), b));
+  binding = internal::ParseConstraint(-b <= x);
+  // Note: The constraints are sorted via get_operands(f) which returns a
+  // std::set. This one gets flipped.
+  EXPECT_TRUE(CompareMatrices(binding.evaluator()->lower_bound(),
+                              -Vector2d(b[1], b[0])));
+  binding = internal::ParseConstraint(b >= x);
+  EXPECT_TRUE(CompareMatrices(binding.evaluator()->upper_bound(), b));
+  binding = internal::ParseConstraint(x >= -b);
+  EXPECT_TRUE(CompareMatrices(binding.evaluator()->lower_bound(), -b));
+
+  DRAKE_EXPECT_THROWS_MESSAGE(internal::ParseConstraint(x <= -b),
+                              ".*an upper bound of -inf.*");
+  DRAKE_EXPECT_THROWS_MESSAGE(internal::ParseConstraint(b <= x),
+                              ".*a lower bound of.*");
+  DRAKE_EXPECT_THROWS_MESSAGE(internal::ParseConstraint(-b >= x),
+                              ".*an upper bound of -inf.*");
+  DRAKE_EXPECT_THROWS_MESSAGE(internal::ParseConstraint(x >= b),
+                              ".*a lower bound of.*");
+}
+
 std::shared_ptr<RotatedLorentzConeConstraint>
 CheckParseQuadraticAsRotatedLorentzConeConstraint(
     const Eigen::Ref<const Eigen::MatrixXd>& Q,