Skip to content

Commit

Permalink
Update individual entry in A/b of some costs (RobotLocomotion#22260)
Browse files Browse the repository at this point in the history 
Including LInfNormCost, L1NormCost, PerspectiveQuadraticCost.
  • Loading branch information
@hongkai-dai
hongkai-dai authored Dec 11, 2024
1 parent 9e6a49e commit cb73411
Show file tree
Hide file tree
Showing 5 changed files with 116 additions and 3 deletions.
20 changes: 17 additions & 3 deletions bindings/pydrake/solvers/solvers_py_evaluators.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -691,7 +691,11 @@ void BindEvaluatorsAndBindings(py::module m) {
self.UpdateCoefficients(new_A, new_b);
},
py::arg("new_A"), py::arg("new_b") = 0,
doc.L1NormCost.UpdateCoefficients.doc);
doc.L1NormCost.UpdateCoefficients.doc)
.def("update_A_entry", &L1NormCost::update_A_entry, py::arg("i"),
py::arg("j"), py::arg("val"), doc.L1NormCost.update_A_entry.doc)
.def("update_b_entry", &L1NormCost::update_b_entry, py::arg("i"),
py::arg("val"), doc.L1NormCost.update_b_entry.doc);

{
py::class_<L2NormCost, Cost, std::shared_ptr<L2NormCost>> cls(
Expand Down Expand Up @@ -742,7 +746,11 @@ void BindEvaluatorsAndBindings(py::module m) {
self.UpdateCoefficients(new_A, new_b);
},
py::arg("new_A"), py::arg("new_b") = 0,
doc.LInfNormCost.UpdateCoefficients.doc);
doc.LInfNormCost.UpdateCoefficients.doc)
.def("update_A_entry", &LInfNormCost::update_A_entry, py::arg("i"),
py::arg("j"), py::arg("val"), doc.LInfNormCost.update_A_entry.doc)
.def("update_b_entry", &LInfNormCost::update_b_entry, py::arg("i"),
py::arg("val"), doc.LInfNormCost.update_b_entry.doc);

py::class_<PerspectiveQuadraticCost, Cost,
std::shared_ptr<PerspectiveQuadraticCost>>(
Expand All @@ -762,7 +770,13 @@ void BindEvaluatorsAndBindings(py::module m) {
self.UpdateCoefficients(new_A, new_b);
},
py::arg("new_A"), py::arg("new_b"),
doc.PerspectiveQuadraticCost.UpdateCoefficients.doc);
doc.PerspectiveQuadraticCost.UpdateCoefficients.doc)
.def("update_A_entry", &PerspectiveQuadraticCost::update_A_entry,
py::arg("i"), py::arg("j"), py::arg("val"),
doc.PerspectiveQuadraticCost.update_A_entry.doc)
.def("update_b_entry", &PerspectiveQuadraticCost::update_b_entry,
py::arg("i"), py::arg("val"),
doc.PerspectiveQuadraticCost.update_b_entry.doc);

py::class_<ExpressionCost, Cost, std::shared_ptr<ExpressionCost>>(
m, "ExpressionCost", doc.ExpressionCost.doc)
Expand Down
12 changes: 12 additions & 0 deletions bindings/pydrake/solvers/test/evaluators_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,10 @@ def test_l1norm_cost(self):
cost.UpdateCoefficients(new_A=2*A, new_b=2*b)
np.testing.assert_allclose(cost.A(), 2*A)
np.testing.assert_allclose(cost.b(), 2*b)
cost.update_A_entry(i=0, j=1, val=0.5)
np.testing.assert_allclose(cost.A(), np.array([[2, 0.5], [-0.8, 1.4]]))
cost.update_b_entry(i=0, val=1)
np.testing.assert_allclose(cost.b(), np.array([1, -0.8]))

def test_l2norm_cost(self):
A = np.array([[1., 2.], [-.4, .7]])
Expand All @@ -95,6 +99,10 @@ def test_linfnorm_cost(self):
cost.UpdateCoefficients(new_A=2*A, new_b=2*b)
np.testing.assert_allclose(cost.A(), 2*A)
np.testing.assert_allclose(cost.b(), 2*b)
cost.update_A_entry(i=0, j=1, val=0.5)
np.testing.assert_allclose(cost.A(), np.array([[2, 0.5], [-0.8, 1.4]]))
cost.update_b_entry(i=0, val=1)
np.testing.assert_allclose(cost.b(), np.array([1, -0.8]))

def test_perspective_quadratic_cost(self):
A = np.array([[1., 2.], [-.4, .7]])
Expand All @@ -105,6 +113,10 @@ def test_perspective_quadratic_cost(self):
cost.UpdateCoefficients(new_A=2*A, new_b=2*b)
np.testing.assert_allclose(cost.A(), 2*A)
np.testing.assert_allclose(cost.b(), 2*b)
cost.update_A_entry(i=0, j=1, val=0.5)
np.testing.assert_allclose(cost.A(), np.array([[2, 0.5], [-0.8, 1.4]]))
cost.update_b_entry(i=0, val=1)
np.testing.assert_allclose(cost.b(), np.array([1, -0.8]))

def test_expression_cost(self):
x = sym.Variable("x")
Expand Down
33 changes: 33 additions & 0 deletions solvers/cost.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -216,6 +216,17 @@ void L1NormCost::UpdateCoefficients(
b_ = new_b;
}

void L1NormCost::update_A_entry(int i, int j, double val) {
DRAKE_DEMAND(i >= 0 && i < A_.rows());
DRAKE_DEMAND(j >= 0 && j < A_.cols());
A_(i, j) = val;
}

void L1NormCost::update_b_entry(int i, double val) {
DRAKE_DEMAND(i >= 0 && i < b_.rows());
b_(i) = val;
}

void L1NormCost::DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
Eigen::VectorXd* y) const {
y->resize(1);
Expand Down Expand Up @@ -340,6 +351,17 @@ void LInfNormCost::UpdateCoefficients(
b_ = new_b;
}

void LInfNormCost::update_A_entry(int i, int j, double val) {
DRAKE_DEMAND(i >= 0 && i < A_.rows());
DRAKE_DEMAND(j >= 0 && j < A_.cols());
A_(i, j) = val;
}

void LInfNormCost::update_b_entry(int i, double val) {
DRAKE_DEMAND(i >= 0 && i < b_.rows());
b_(i) = val;
}

void LInfNormCost::DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
Eigen::VectorXd* y) const {
y->resize(1);
Expand Down Expand Up @@ -395,6 +417,17 @@ void PerspectiveQuadraticCost::UpdateCoefficients(
b_ = new_b;
}

void PerspectiveQuadraticCost::update_A_entry(int i, int j, double val) {
DRAKE_DEMAND(i >= 0 && i < A_.rows());
DRAKE_DEMAND(j >= 0 && j < A_.cols());
A_(i, j) = val;
}

void PerspectiveQuadraticCost::update_b_entry(int i, double val) {
DRAKE_DEMAND(i >= 0 && i < b_.rows());
b_(i) = val;
}

template <typename DerivedX, typename U>
void PerspectiveQuadraticCost::DoEvalGeneric(
const Eigen::MatrixBase<DerivedX>& x, VectorX<U>* y) const {
Expand Down
30 changes: 30 additions & 0 deletions solvers/cost.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -304,6 +304,16 @@ class L1NormCost : public Cost {
void UpdateCoefficients(const Eigen::Ref<const Eigen::MatrixXd>& new_A,
const Eigen::Ref<const Eigen::VectorXd>& new_b);

/** Updates A(i, j) = val.
* @throws if i or j are invalid indices.
*/
void update_A_entry(int i, int j, double val);

/** Updates b(i) = val.
* @throws if i is an invalid index.
*/
void update_b_entry(int i, double val);

protected:
void DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
Eigen::VectorXd* y) const override;
Expand Down Expand Up @@ -433,6 +443,16 @@ class LInfNormCost : public Cost {
void UpdateCoefficients(const Eigen::Ref<const Eigen::MatrixXd>& new_A,
const Eigen::Ref<const Eigen::VectorXd>& new_b);

/** Updates A(i, j) = val.
* @throws if i or j are invalid indices.
*/
void update_A_entry(int i, int j, double val);

/** Updates b(i) = val.
* @throws if i is an invalid index.
*/
void update_b_entry(int i, double val);

protected:
void DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
Eigen::VectorXd* y) const override;
Expand Down Expand Up @@ -493,6 +513,16 @@ class PerspectiveQuadraticCost : public Cost {
void UpdateCoefficients(const Eigen::Ref<const Eigen::MatrixXd>& new_A,
const Eigen::Ref<const Eigen::VectorXd>& new_b);

/** Updates A(i, j) = val.
* @throws if i or j are invalid indices.
*/
void update_A_entry(int i, int j, double val);

/** Updates b(i) = val.
* @throws if i is an invalid index.
*/
void update_b_entry(int i, double val);

protected:
void DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
Eigen::VectorXd* y) const override;
Expand Down
24 changes: 24 additions & 0 deletions solvers/test/cost_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -466,6 +466,14 @@ GTEST_TEST(TestL1NormCost, UpdateCoefficients) {
// A and b must have the same number of rows.
EXPECT_THROW(cost.UpdateCoefficients(Matrix3d::Identity(), Vector4d::Zero()),
std::exception);

cost.update_A_entry(0, 1, 0.5);
EXPECT_TRUE(CompareMatrices(
cost.A(),
(Eigen::Matrix<double, 4, 2>() << 1, 0.5, 0, 1, 0, 0, 0, 0).finished()));

cost.update_b_entry(0, 1.5);
EXPECT_TRUE(CompareMatrices(cost.b(), Eigen::Vector4d(1.5, 0, 0, 0)));
}

GTEST_TEST(TestL1NormCost, Display) {
Expand Down Expand Up @@ -625,6 +633,14 @@ GTEST_TEST(TestLInfNormCost, UpdateCoefficients) {
// A and b must have the same number of rows.
EXPECT_THROW(cost.UpdateCoefficients(Matrix3d::Identity(), Vector4d::Zero()),
std::exception);

cost.update_A_entry(0, 1, 0.5);
EXPECT_TRUE(CompareMatrices(
cost.A(),
(Eigen::Matrix<double, 4, 2>() << 1, 0.5, 0, 1, 0, 0, 0, 0).finished()));

cost.update_b_entry(0, 1.5);
EXPECT_TRUE(CompareMatrices(cost.b(), Eigen::Vector4d(1.5, 0, 0, 0)));
}

GTEST_TEST(TestLInfNormCost, Display) {
Expand Down Expand Up @@ -694,6 +710,14 @@ GTEST_TEST(TestPerspectiveQuadraticCost, UpdateCoefficients) {
// A and b must have the same number of rows.
EXPECT_THROW(cost.UpdateCoefficients(Matrix3d::Identity(), Vector4d::Zero()),
std::exception);

cost.update_A_entry(0, 1, 0.5);
EXPECT_TRUE(CompareMatrices(
cost.A(),
(Eigen::Matrix<double, 4, 2>() << 1, 0.5, 0, 1, 0, 0, 0, 0).finished()));

cost.update_b_entry(0, 1.5);
EXPECT_TRUE(CompareMatrices(cost.b(), Eigen::Vector4d(1.5, 0, 0, 0)));
}

GTEST_TEST(TestPerspectiveQuadraticCost, Display) {
Expand Down

0 comments on commit cb73411

Please sign in to comment.