Update single entry of coefficients in LinearCost and QuadraticCost. (R…

…obotLocomotion#22152)

bindings/pydrake/solvers/solvers_py_evaluators.cc

@@ -636,7 +636,12 @@ void BindEvaluatorsAndBindings(py::module m) {
             self.UpdateCoefficients(new_a, new_b);
           },
           py::arg("new_a"), py::arg("new_b") = 0,
-          doc.LinearCost.UpdateCoefficients.doc);
+          doc.LinearCost.UpdateCoefficients.doc)
+      .def("update_coefficient_entry", &LinearCost::update_coefficient_entry,
+          py::arg("i"), py::arg("val"),
+          doc.LinearCost.update_coefficient_entry.doc)
+      .def("update_constant_term", &LinearCost::update_constant_term,
+          py::arg("new_b"), doc.LinearCost.update_constant_term.doc);
 
   py::class_<QuadraticCost, Cost, std::shared_ptr<QuadraticCost>>(
       m, "QuadraticCost", doc.QuadraticCost.doc)
@@ -660,7 +665,16 @@ void BindEvaluatorsAndBindings(py::module m) {
           },
           py::arg("new_Q"), py::arg("new_b"), py::arg("new_c") = 0,
           py::arg("is_convex") = py::none(),
-          doc.QuadraticCost.UpdateCoefficients.doc);
+          doc.QuadraticCost.UpdateCoefficients.doc)
+      .def("UpdateHessianEntry", &QuadraticCost::UpdateHessianEntry,
+          py::arg("i"), py::arg("j"), py::arg("val"),
+          py::arg("is_hessian_psd") = py::none(),
+          doc.QuadraticCost.UpdateHessianEntry.doc)
+      .def("update_linear_coefficient_entry",
+          &QuadraticCost::update_linear_coefficient_entry, py::arg("i"),
+          py::arg("val"), doc.QuadraticCost.update_linear_coefficient_entry.doc)
+      .def("update_constant_term", &QuadraticCost::update_constant_term,
+          py::arg("new_c"), doc.QuadraticCost.update_constant_term.doc);
 
   py::class_<L1NormCost, Cost, std::shared_ptr<L1NormCost>>(
       m, "L1NormCost", doc.L1NormCost.doc)

bindings/pydrake/solvers/test/evaluators_test.py

@@ -17,6 +17,16 @@ def test_linear_cost(self):
         cost = mp.LinearCost(a, b)
         np.testing.assert_allclose(cost.a(), a)
         self.assertEqual(cost.b(), b)
+        cost.UpdateCoefficients(new_a=[2, 3.], new_b=1)
+        np.testing.assert_allclose(cost.a(), [2, 3.])
+        self.assertEqual(cost.b(), 1)
+
+        cost.update_coefficient_entry(i=0, val=4)
+        np.testing.assert_allclose(cost.a(), [4, 3])
+        self.assertEqual(cost.b(), 1)
+
+        cost.update_constant_term(new_b=2)
+        self.assertEqual(cost.b(), 2)
 
     def test_quadratic_cost(self):
         Q = np.array([[1., 2.], [2., 3.]])
@@ -34,6 +44,28 @@ def test_quadratic_cost(self):
         cost = mp.QuadraticCost(np.array([[1., 2.], [2., 6.]]), b, c)
         self.assertTrue(cost.is_convex())
 
+        cost.UpdateCoefficients(
+            new_Q=np.array([[1, 3], [3, 6.]]),
+            new_b=np.array([2, 4.]),
+            new_c=1,
+            is_convex=False
+        )
+        np.testing.assert_allclose(cost.Q(), np.array([[1, 3], [3, 6]]))
+        np.testing.assert_allclose(cost.b(), np.array([2, 4.]))
+        self.assertEqual(cost.c(), 1)
+
+        cost.UpdateHessianEntry(i=0, j=1, val=1, is_hessian_psd=None)
+        np.testing.assert_allclose(cost.Q(), np.array([[1, 1], [1, 6]]))
+        self.assertTrue(cost.is_convex())
+        cost.update_linear_coefficient_entry(i=1, val=5)
+        np.testing.assert_allclose(cost.b(), np.array([2, 5.]))
+        cost.update_constant_term(new_c=2)
+        self.assertEqual(cost.c(), 2)
+
+        # User-specify is_hessian_psd.
+        cost.UpdateHessianEntry(i=0, j=0, val=20, is_hessian_psd=True)
+        self.assertTrue(cost.is_convex())
+
     def test_l1norm_cost(self):
         A = np.array([[1., 2.], [-.4, .7]])
         b = np.array([0.5, -.4])

solvers/cost.cc

@@ -49,6 +49,25 @@ std::string ToLatexCost(const Cost& cost,
 
 LinearCost::~LinearCost() = default;
 
+void LinearCost::UpdateCoefficients(
+    const Eigen::Ref<const Eigen::VectorXd>& new_a, double new_b) {
+  if (new_a.rows() != a_.rows()) {
+    throw std::runtime_error("Can't change the number of decision variables");
+  }
+
+  a_ = new_a;
+  b_ = new_b;
+}
+
+void LinearCost::update_coefficient_entry(int i, double val) {
+  DRAKE_DEMAND(i >= 0 && i < a_.rows());
+  a_[i] = val;
+}
+
+void LinearCost::update_constant_term(double new_b) {
+  b_ = new_b;
+}
+
 template <typename DerivedX, typename U>
 void LinearCost::DoEvalGeneric(const Eigen::MatrixBase<DerivedX>& x,
                                VectorX<U>* y) const {
@@ -82,6 +101,28 @@ std::string LinearCost::DoToLatex(const VectorX<symbolic::Variable>& vars,
 
 QuadraticCost::~QuadraticCost() = default;
 
+void QuadraticCost::UpdateHessianEntry(int i, int j, double val,
+                                       std::optional<bool> is_hessian_psd) {
+  DRAKE_DEMAND(i >= 0 && i < Q_.rows());
+  DRAKE_DEMAND(j >= 0 && j < Q_.rows());
+  Q_(i, j) = val;
+  Q_(j, i) = val;
+  if (is_hessian_psd.has_value()) {
+    is_convex_ = is_hessian_psd.value();
+  } else {
+    is_convex_ = CheckHessianPsd();
+  }
+}
+
+void QuadraticCost::update_linear_coefficient_entry(int i, double val) {
+  DRAKE_DEMAND(i >= 0 && i < b_.rows());
+  b_(i) = val;
+}
+
+void QuadraticCost::update_constant_term(double new_c) {
+  c_ = new_c;
+}
+
 template <typename DerivedX, typename U>
 void QuadraticCost::DoEvalGeneric(const Eigen::MatrixBase<DerivedX>& x,
                                   VectorX<U>* y) const {

solvers/cost.h

@@ -75,14 +75,20 @@ class LinearCost : public Cost {
    * @param new_b (optional) New constant term.
    */
   void UpdateCoefficients(const Eigen::Ref<const Eigen::VectorXd>& new_a,
-                          double new_b = 0.) {
-    if (new_a.rows() != a_.rows()) {
-      throw std::runtime_error("Can't change the number of decision variables");
-    }
+                          double new_b = 0.);
 
-    a_ = new_a;
-    b_ = new_b;
-  }
+  /**
+   * Updates one entry in the coefficient of the cost.
+   * a[i] = val.
+   * @param i The index of the coefficient to be updated.
+   * @param val The value of that updated entry.
+   */
+  void update_coefficient_entry(int i, double val);
+
+  /**
+   * Updates the constant term in the cost to `new_b`.
+   */
+  void update_constant_term(double new_b);
 
  protected:
   void DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
@@ -195,6 +201,31 @@ class QuadraticCost : public Cost {
     }
   }
 
+  /**
+   * Updates both Q(i, j) and Q(j, i) to val
+   * @param is_hessian_psd If this is `nullopt`, the new Hessian is
+   * checked (possibly expensively) for PSD-ness.  If this is
+   * set true/false, the cost's convexity is updated to that
+   * value without checking (it is the user's responsibility to make sure the
+   * flag is set correctly).
+   * @note If you have multiple entries in the Hessian matrix to update, and you
+   * don't specify is_hessian_psd, then it is much faster to call
+   * UpdateCoefficients(new_A, new_b) where new_A contains all the updated
+   * entries.
+   */
+  void UpdateHessianEntry(int i, int j, double val,
+                          std::optional<bool> is_hessian_psd = std::nullopt);
+
+  /**
+   * Updates b(i)=val.
+   */
+  void update_linear_coefficient_entry(int i, double val);
+
+  /**
+   * Updates the constant term to `new_c`.
+   */
+  void update_constant_term(double new_c);
+
  private:
   template <typename DerivedX, typename U>
   void DoEvalGeneric(const Eigen::MatrixBase<DerivedX>& x, VectorX<U>* y) const;

solvers/test/cost_test.cc

@@ -125,13 +125,23 @@ GTEST_TEST(testCost, testLinearCost) {
   EXPECT_PRED2(ExprEqual, y_sym[0], 1 * x_sym[0] + 2 * x_sym[1]);
 
   // Update with a constant term.
-  const double b = 100;
+  double b = 100;
   cost->UpdateCoefficients(a, b);
   cost->Eval(x0, &y);
   EXPECT_NEAR(y(0), obj_expected + b, tol);
+  b = 200;
+  cost->update_constant_term(b);
+  cost->Eval(x0, &y);
+  EXPECT_NEAR(y(0), obj_expected + b, tol);
+
   EXPECT_THROW(cost->UpdateCoefficients(Eigen::Vector3d::Ones(), b),
                runtime_error);
 
+  // Update one entry in a.
+  cost->update_coefficient_entry(0, 10);
+  cost->Eval(x0, &y);
+  EXPECT_NEAR(y(0), 10 * x0(0) + a(1) * x0(1) + b, tol);
+
   // Reconstruct the same cost with the constant term.
   auto new_cost = make_shared<LinearCost>(a, b);
   new_cost->Eval(x0, &y);
@@ -140,7 +150,7 @@ GTEST_TEST(testCost, testLinearCost) {
   new_cost->set_description("simple linear cost");
   EXPECT_EQ(
       fmt::format("{}", *new_cost),
-      "LinearCost (100 + $(0) + 2 * $(1)) described as 'simple linear cost'");
+      "LinearCost (200 + $(0) + 2 * $(1)) described as 'simple linear cost'");
 
   EXPECT_TRUE(cost->is_thread_safe());
 }
@@ -218,11 +228,36 @@ GTEST_TEST(TestQuadraticCost, NonconvexCost) {
                               MatrixCompareType::absolute));
   EXPECT_FALSE(cost->is_convex());
 
+  // Update the Hessian to make it convex.
+  cost->UpdateHessianEntry(0, 1, 1, /*is_hessian_psd=*/std::nullopt);
+  Eigen::Matrix2d Q_expected = (Eigen::Matrix2d() << 2, 1, 1, 8).finished();
+  EXPECT_TRUE(CompareMatrices(cost->Q(), Q_expected, 1E-10));
+  EXPECT_TRUE(cost->is_convex());
+
+  // Update the Hessian to make it non-convex
+  cost->UpdateHessianEntry(0, 1, 5, /*is_hessian_psd=*/false);
+  Q_expected << 2, 5, 5, 8;
+  EXPECT_TRUE(CompareMatrices(cost->Q(), Q_expected, 1E-10));
+  EXPECT_FALSE(cost->is_convex());
+
+  // Update the diagonal entry of the Hessian.
+  cost->UpdateHessianEntry(0, 0, 1, /*is_hessian_psd=*/std::nullopt);
+  Q_expected << 1, 5, 5, 8;
+  EXPECT_TRUE(CompareMatrices(cost->Q(), Q_expected, 1E-10));
+  EXPECT_FALSE(cost->is_convex());
+
+  // Update an entry in the linear coefficient.
+  cost->update_linear_coefficient_entry(0, 10);
+  cost->update_linear_coefficient_entry(1, 3);
+  EXPECT_TRUE(CompareMatrices(cost->b(), Eigen::Vector2d(10, 3)));
+
   // Update with a constant term.
   const double c = 100;
   cost->UpdateCoefficients(Q, b, c);
   cost->Eval(x0, &y);
   EXPECT_NEAR(y(0), obj_expected + c, tol);
+  cost->update_constant_term(200);
+  EXPECT_EQ(cost->c(), 200);
 
   EXPECT_THROW(cost->UpdateCoefficients(Eigen::Matrix3d::Identity(), b, c),
                runtime_error);