From 6ae2c34d6fc6df4e0fcef0f005b0ec63253c599a Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Tue, 2 May 2023 07:10:54 -0700 Subject: [PATCH] WIP --- geometry/optimization/spectrahedron.cc | 75 +++++++++++++------------- geometry/optimization/spectrahedron.h | 11 ++-- 2 files changed, 41 insertions(+), 45 deletions(-) diff --git a/geometry/optimization/spectrahedron.cc b/geometry/optimization/spectrahedron.cc index 0d71f4dced49..58b1aba2b858 100644 --- a/geometry/optimization/spectrahedron.cc +++ b/geometry/optimization/spectrahedron.cc @@ -28,7 +28,7 @@ VectorXDecisionVariable GetVariablesByIndex( const Eigen::Ref& vars, std::vector indices) { VectorXDecisionVariable new_vars(indices.size()); - for (int i = 0; i < static_cast(indices.size()); ++i) { + for (int i = 0; i < ssize(indices); ++i) { new_vars[i] = vars[indices[i]]; } return new_vars; @@ -36,23 +36,18 @@ VectorXDecisionVariable GetVariablesByIndex( } // namespace -const solvers::ProgramAttributes Spectrahedron::supported_attributes_ = { - ProgramAttribute::kLinearCost, ProgramAttribute::kLinearConstraint, - ProgramAttribute::kLinearEqualityConstraint, - ProgramAttribute::kPositiveSemidefiniteConstraint}; - Spectrahedron::Spectrahedron() : ConvexSet(&ConvexSetCloner, 0) {} Spectrahedron::Spectrahedron(const MathematicalProgram& prog) : ConvexSet(&ConvexSetCloner, prog.num_vars()) { - for (const auto& attr : prog.required_capabilities()) { + for (const ProgramAttribute& attr : prog.required_capabilities()) { if (supported_attributes().count(attr) < 1) { - throw std::runtime_error( - fmt::format("Spectrahedron does not support MathematicalPrograms " - "that require ProgramAttribute {}. If that attribute is " - "convex, it might be possible to add that support.", - attr)); + throw std::runtime_error(fmt::format( + "Spectrahedron does not support MathematicalPrograms that require " + "ProgramAttribute {}. If that attribute is convex, it might be " + "possible to add that support.", + attr)); } } sdp_ = prog.Clone(); @@ -64,13 +59,22 @@ Spectrahedron::Spectrahedron(const MathematicalProgram& prog) Spectrahedron::~Spectrahedron() = default; +const ProgramAttributes& Spectrahedron::supported_attributes() { + static const never_destroyed kSupportedAttributes{ + ProgramAttributes{ProgramAttribute::kLinearCost, + ProgramAttribute::kLinearConstraint, + ProgramAttribute::kLinearEqualityConstraint, + ProgramAttribute::kPositiveSemidefiniteConstraint}}; + return kSupportedAttributes.access(); +} + bool Spectrahedron::DoIsBounded() const { throw std::runtime_error( "Spectrahedron::IsBounded() is not implemented yet."); } bool Spectrahedron::DoPointInSet(const Eigen::Ref& x, - double tol) const { + double tol) const { return sdp_->CheckSatisfied(sdp_->GetAllConstraints(), x, tol); } @@ -95,16 +99,22 @@ Spectrahedron::DoAddPointInNonnegativeScalingConstraints( std::vector> constraints; const double kInf = std::numeric_limits::infinity(); + // Helper function that given a binding.variables() returns the corresponding + // subset of variables from `x` with `t` tacked on the end. + auto stack_xt = [&x, &t, this](const VectorXDecisionVariable& bind_vars) { + VectorXDecisionVariable xt(bind_vars.size() + 1); + xt << GetVariablesByIndex(x, sdp_->FindDecisionVariableIndices(bind_vars)), + t; + return xt; + }; + // TODO(russt): Support SparseMatrix constraints. for (const auto& binding : sdp_->bounding_box_constraints()) { // t*lb ≤ x ≤ t*ub, implemented as // [I,-lb]*[x;t] ≥ 0, [I,-ub]*[x;t] ≤ 0. - VectorXDecisionVariable vars(binding.evaluator()->num_vars()+1); - vars << GetVariablesByIndex( - x, sdp_->FindDecisionVariableIndices(binding.variables())), - t; - MatrixXd Ab = MatrixXd::Identity(binding.evaluator()->num_constraints(), - binding.evaluator()->num_vars() + 1); + VectorXDecisionVariable vars = stack_xt(binding.variables()); + MatrixXd Ab = + MatrixXd::Identity(binding.evaluator()->num_constraints(), vars.size()); // TODO(russt): Handle individual elements that are infinite. if (binding.evaluator()->lower_bound().array().isFinite().any()) { Ab.rightCols<1>() = -binding.evaluator()->lower_bound(); @@ -118,12 +128,8 @@ Spectrahedron::DoAddPointInNonnegativeScalingConstraints( for (const auto& binding : sdp_->linear_equality_constraints()) { // Ax = t*b, implemented as // [A,-lb]*[x;t] == 0. - VectorXDecisionVariable vars(binding.evaluator()->num_vars()+1); - vars << GetVariablesByIndex( - x, sdp_->FindDecisionVariableIndices(binding.variables())), - t; - MatrixXd Ab(binding.evaluator()->num_constraints(), - binding.evaluator()->num_vars() + 1); + VectorXDecisionVariable vars = stack_xt(binding.variables()); + MatrixXd Ab(binding.evaluator()->num_constraints(), vars.size()); Ab.leftCols(binding.evaluator()->num_vars()) = binding.evaluator()->GetDenseA(); Ab.rightCols<1>() = -binding.evaluator()->lower_bound(); @@ -132,12 +138,8 @@ Spectrahedron::DoAddPointInNonnegativeScalingConstraints( for (const auto& binding : sdp_->linear_constraints()) { // t*lb <= Ax = t*ub, implemented as // [A,-lb]*[x;t] ≥ 0, [A,-ub]*[x;t] ≤ 0. - VectorXDecisionVariable vars(binding.evaluator()->num_vars()+1); - vars << GetVariablesByIndex( - x, sdp_->FindDecisionVariableIndices(binding.variables())), - t; - MatrixXd Ab(binding.evaluator()->num_constraints(), - binding.evaluator()->num_vars() + 1); + VectorXDecisionVariable vars = stack_xt(binding.variables()); + MatrixXd Ab(binding.evaluator()->num_constraints(), vars.size()); Ab.leftCols(binding.evaluator()->num_vars()) = binding.evaluator()->GetDenseA(); if (binding.evaluator()->lower_bound().array().isFinite().any()) { @@ -152,14 +154,13 @@ Spectrahedron::DoAddPointInNonnegativeScalingConstraints( for (const auto& binding : sdp_->positive_semidefinite_constraints()) { // These constraints get added without modification -- a non-negative // scaling of the PSD cone is just the PSD cone. + VectorXDecisionVariable vars = GetVariablesByIndex( + x, sdp_->FindDecisionVariableIndices(binding.variables())); constraints.emplace_back(prog->AddConstraint( binding.evaluator(), - Eigen::Map>( - GetVariablesByIndex( - x, sdp_->FindDecisionVariableIndices(binding.variables())) - .data(), - binding.evaluator()->matrix_rows(), - binding.evaluator()->matrix_rows()))); + Eigen::Map>(vars.data(), + binding.evaluator()->matrix_rows(), + binding.evaluator()->matrix_rows()))); } return constraints; } diff --git a/geometry/optimization/spectrahedron.h b/geometry/optimization/spectrahedron.h index 99888fe3aabe..575762d65e98 100644 --- a/geometry/optimization/spectrahedron.h +++ b/geometry/optimization/spectrahedron.h @@ -16,8 +16,7 @@ namespace optimization { The ambient dimension of the set is N(N+1)/2; the number of variables required to describe the N-by-N semidefinite matrix. -@ingroup geometry_optimization -*/ +@ingroup geometry_optimization */ class Spectrahedron final : public ConvexSet { public: DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Spectrahedron) @@ -26,16 +25,14 @@ class Spectrahedron final : public ConvexSet { Spectrahedron(); /** Constructs the spectrahedron from a MathematicalProgram. - @throws std::exception if @p prog.required_capabilities() is not a subset of + @throws std::exception if `prog.required_capabilities()` is not a subset of supported_attributes(). */ explicit Spectrahedron(const solvers::MathematicalProgram& prog); ~Spectrahedron() final; /** Returns the list of solvers::ProgramAttributes supported by this class. */ - static const solvers::ProgramAttributes& supported_attributes() { - return supported_attributes_; - } + static const solvers::ProgramAttributes& supported_attributes(); // TODO(russt): Add PointInSet(MatrixXd X, double tol) overload, which will // only work in the case where the ambient_dimension is ONLY symmetric @@ -71,8 +68,6 @@ class Spectrahedron final : public ConvexSet { const final; copyable_unique_ptr sdp_{}; - - static const solvers::ProgramAttributes supported_attributes_; }; } // namespace optimization