Skip to content

Commit

Permalink
add finiteness check to linear constraints (RobotLocomotion#21049)
Browse files Browse the repository at this point in the history 
add finiteness check to linear constraints
  • Loading branch information
@AlexandreAmice
AlexandreAmice authored Feb 28, 2024
1 parent 0076e15 commit 1dd7381
Show file tree
Hide file tree
Showing 3 changed files with 99 additions and 11 deletions.
12 changes: 6 additions & 6 deletions solvers/constraint.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -397,16 +397,16 @@ LinearConstraint::LinearConstraint(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::VectorXd>& lb,
const Eigen::Ref<const Eigen::VectorXd>& ub)
: Constraint(A.rows(), A.cols(), lb, ub), A_(A) {
DRAKE_DEMAND(A.rows() == lb.rows());
DRAKE_DEMAND(A.array().isFinite().all());
DRAKE_THROW_UNLESS(A.rows() == lb.rows());
DRAKE_THROW_UNLESS(A.array().allFinite());
}

LinearConstraint::LinearConstraint(const Eigen::SparseMatrix<double>& A,
const Eigen::Ref<const Eigen::VectorXd>& lb,
const Eigen::Ref<const Eigen::VectorXd>& ub)
: Constraint(A.rows(), A.cols(), lb, ub), A_(A) {
DRAKE_DEMAND(A.rows() == lb.rows());
DRAKE_DEMAND(A_.IsFinite());
DRAKE_THROW_UNLESS(A.rows() == lb.rows());
DRAKE_THROW_UNLESS(A_.IsFinite());
}

const Eigen::MatrixXd& LinearConstraint::GetDenseA() const {
Expand All @@ -426,7 +426,7 @@ void LinearConstraint::UpdateCoefficients(
}

A_ = new_A;
DRAKE_DEMAND(A_.IsFinite());
DRAKE_THROW_UNLESS(A_.IsFinite());
set_num_outputs(A_.get_as_sparse().rows());
set_bounds(new_lb, new_ub);
}
Expand All @@ -443,7 +443,7 @@ void LinearConstraint::UpdateCoefficients(
throw std::runtime_error("Can't change the number of decision variables");
}
A_ = new_A;
DRAKE_DEMAND(A_.IsFinite());
DRAKE_THROW_UNLESS(A_.IsFinite());
set_num_outputs(A_.get_as_sparse().rows());
set_bounds(new_lb, new_ub);
}
Expand Down
34 changes: 29 additions & 5 deletions solvers/constraint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -603,6 +603,8 @@ class LinearConstraint : public Constraint {

/**
* Construct the linear constraint lb <= A*x <= ub
*
* Throws if A has any entry which is not finite.
* @pydrake_mkdoc_identifier{dense_A}
*/
LinearConstraint(const Eigen::Ref<const Eigen::MatrixXd>& A,
Expand All @@ -611,6 +613,7 @@ class LinearConstraint : public Constraint {

/**
* Overloads constructor with a sparse A matrix.
* Throws if A has any entry which is not finite.
* @pydrake_mkdoc_identifier{sparse_A}
*/
LinearConstraint(const Eigen::SparseMatrix<double>& A,
Expand All @@ -637,6 +640,10 @@ class LinearConstraint : public Constraint {
* new_lb <= new_A * x <= new_ub
* Note that the size of constraints (number of rows) can change, but the
* number of variables (number of cols) cannot.
*
* Throws if new_A has any entry which is not finite or if new_A, new_lb, and
* new_ub don't all have the same number of rows.
*
* @param new_A new linear term
* @param new_lb new lower bound
* @param new_up new upper bound
Expand All @@ -648,6 +655,10 @@ class LinearConstraint : public Constraint {

/**
* Overloads UpdateCoefficients but with a sparse A matrix.
*
* Throws if new_A has any entry which is not finite or if new_A, new_lb, and
* new_ub don't all have the same number of rows.
*
* @pydrake_mkdoc_identifier{sparse_A}
*/
void UpdateCoefficients(const Eigen::SparseMatrix<double>& new_A,
Expand Down Expand Up @@ -706,12 +717,15 @@ class LinearEqualityConstraint : public LinearConstraint {
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(LinearEqualityConstraint)

/**
* Constructs the linear equality constraint Aeq * x = beq
* Constructs the linear equality constraint Aeq * x = beq.
*
* Throws is any entry in Aeq or beq is not finite.
* @pydrake_mkdoc_identifier{dense_Aeq}
*/
LinearEqualityConstraint(const Eigen::Ref<const Eigen::MatrixXd>& Aeq,
const Eigen::Ref<const Eigen::VectorXd>& beq)
: LinearConstraint(Aeq, beq, beq) {
DRAKE_THROW_UNLESS(beq.allFinite());
}

/**
Expand All @@ -721,6 +735,7 @@ class LinearEqualityConstraint : public LinearConstraint {
LinearEqualityConstraint(const Eigen::SparseMatrix<double>& Aeq,
const Eigen::Ref<const Eigen::VectorXd>& beq)
: LinearConstraint(Aeq, beq, beq) {
DRAKE_THROW_UNLESS(beq.allFinite());
}

/**
Expand All @@ -729,25 +744,34 @@ class LinearEqualityConstraint : public LinearConstraint {
*/
LinearEqualityConstraint(const Eigen::Ref<const Eigen::RowVectorXd>& a,
double beq)
: LinearEqualityConstraint(a, Vector1d(beq)) {}
: LinearEqualityConstraint(a, Vector1d(beq)) {
DRAKE_THROW_UNLESS(this->lower_bound().allFinite());
}

/*
* @brief change the parameters of the constraint (A and b), but not the
*variable associations
* variable associations.
*
* note that A and b can change size in the rows only (representing a
*different number of linear constraints, but on the same decision variables)
* Note that A and b can change size in the rows only (representing a
* different number of linear constraints, but on the same decision
* variables).
*
* Throws if any entry of beq or Aeq is not finite.
*/
void UpdateCoefficients(const Eigen::Ref<const Eigen::MatrixXd>& Aeq,
const Eigen::Ref<const Eigen::VectorXd>& beq) {
DRAKE_THROW_UNLESS(beq.allFinite());
LinearConstraint::UpdateCoefficients(Aeq, beq, beq);
}

/**
* Overloads UpdateCoefficients but with a sparse A matrix.
*
* Throws if any entry of beq or Aeq is not finite.
*/
void UpdateCoefficients(const Eigen::SparseMatrix<double>& Aeq,
const Eigen::Ref<const Eigen::VectorXd>& beq) {
DRAKE_THROW_UNLESS(beq.allFinite());
LinearConstraint::UpdateCoefficients(Aeq, beq, beq);
}

Expand Down
64 changes: 64 additions & 0 deletions solvers/test/constraint_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,23 @@ GTEST_TEST(TestConstraint, LinearConstraintSparse) {
EXPECT_TRUE(CompareMatrices(dut.upper_bound(), ub));
}

GTEST_TEST(TestConstraint, LinearConstraintInfiniteEntries) {
std::vector<Eigen::Triplet<double>> A_triplets;
A_triplets.emplace_back(0, 1, 0.5);
A_triplets.emplace_back(1, 0, 1.5);
A_triplets.emplace_back(2, 0, kInf);
Eigen::SparseMatrix<double> A_sparse_bad(3, 3);
Eigen::Vector3d lb(0, 1, -2);
Eigen::Vector3d ub(1, 2, 3);
A_sparse_bad.setFromTriplets(A_triplets.begin(), A_triplets.end());
Eigen::Vector2d bound(0, 1);
Eigen::Vector3d bound_bad(0, 1, kInf);
DRAKE_EXPECT_THROWS_MESSAGE(LinearConstraint(A_sparse_bad, lb, ub),
".*IsFinite().*");
DRAKE_EXPECT_THROWS_MESSAGE(LinearConstraint(A_sparse_bad.toDense(), lb, ub),
".*allFinite().*");
}

GTEST_TEST(TestConstraint, LinearEqualityConstraintSparse) {
std::vector<Eigen::Triplet<double>> A_triplets;
A_triplets.emplace_back(0, 1, 0.5);
Expand All @@ -115,6 +132,33 @@ GTEST_TEST(TestConstraint, LinearEqualityConstraintSparse) {
EXPECT_TRUE(CompareMatrices(dut.upper_bound(), bound));
}

GTEST_TEST(TestConstraint, LinearEqualityConstraintInfiniteEntries) {
std::vector<Eigen::Triplet<double>> A_triplets;
A_triplets.emplace_back(0, 1, 0.5);
A_triplets.emplace_back(1, 0, 1.5);
Eigen::SparseMatrix<double> A_sparse(2, 3);
A_sparse.setFromTriplets(A_triplets.begin(), A_triplets.end());
Eigen::SparseMatrix<double> A_sparse_bad(3, 3);
A_triplets.emplace_back(2, 0, kInf);
A_sparse_bad.setFromTriplets(A_triplets.begin(), A_triplets.end());
Eigen::Vector2d bound(0, 1);
Eigen::Vector3d bound_bad(0, 1, kInf);
EXPECT_THROW(LinearEqualityConstraint(A_sparse_bad, bound),
std::exception);
EXPECT_THROW(LinearEqualityConstraint(A_sparse, bound_bad),
std::exception);
EXPECT_THROW(LinearEqualityConstraint(A_sparse_bad.toDense(), bound),
std::exception);
EXPECT_THROW(LinearEqualityConstraint(A_sparse.toDense(), bound_bad),
std::exception);
DRAKE_EXPECT_THROWS_MESSAGE(
LinearEqualityConstraint(A_sparse.toDense().row(0), kInf),
".*allFinite().*");
DRAKE_EXPECT_THROWS_MESSAGE(
LinearEqualityConstraint(A_sparse_bad.toDense().row(2), 0),
".*allFinite().*");
}

GTEST_TEST(TestConstraint, testLinearConstraintUpdate) {
// Update the coefficients or the bound of the linear constraint, and check
// the updated constraint.
Expand Down Expand Up @@ -153,6 +197,26 @@ GTEST_TEST(TestConstraint, testLinearConstraintUpdate) {
EXPECT_EQ(constraint.num_constraints(), 3);
}

GTEST_TEST(TestConstraint, testLinearConstraintUpdateErrors) {
// Update the coefficients or the bound of the linear constraint, and check
// the updated constraint.
const Eigen::Matrix2d A = Eigen::Matrix2d::Identity();
Eigen::Matrix2d A_bad = Eigen::Matrix2d::Identity();
A_bad(0, 1) = kInf;
const Eigen::Vector2d b(1, 2);
const Eigen::Vector2d b_bad(0, kInf);
LinearEqualityConstraint constraint(A, b);
EXPECT_TRUE(CompareMatrices(constraint.lower_bound(), b));
EXPECT_TRUE(CompareMatrices(constraint.upper_bound(), b));
EXPECT_TRUE(CompareMatrices(constraint.GetDenseA(), A));
EXPECT_EQ(constraint.num_constraints(), 2);

EXPECT_THROW(constraint.UpdateCoefficients(A_bad, b), std::exception);
EXPECT_THROW(constraint.UpdateCoefficients(A_bad.sparseView(), b),
std::exception);
EXPECT_THROW(constraint.UpdateCoefficients(A, b_bad), std::exception);
}

GTEST_TEST(testConstraint, testRemoveTinyCoefficient) {
Eigen::Matrix<double, 2, 3> A;
const double tol = 1E-8;
Expand Down

0 comments on commit 1dd7381

Please sign in to comment.