Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Spectrahedron #48

Merged
merged 1 commit into from
May 2, 2023
+41 −45
Merged

Spectrahedron #48

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
75 changes: 38 additions & 37 deletions geometry/optimization/spectrahedron.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,31 +28,26 @@ VectorXDecisionVariable GetVariablesByIndex(
const Eigen::Ref<const VectorXDecisionVariable>& vars,
std::vector<int> indices) {
VectorXDecisionVariable new_vars(indices.size());
for (int i = 0; i < static_cast<int>(indices.size()); ++i) {
for (int i = 0; i < ssize(indices); ++i) {
new_vars[i] = vars[indices[i]];
}
return new_vars;
}

} // namespace

const solvers::ProgramAttributes Spectrahedron::supported_attributes_ = {
ProgramAttribute::kLinearCost, ProgramAttribute::kLinearConstraint,
ProgramAttribute::kLinearEqualityConstraint,
ProgramAttribute::kPositiveSemidefiniteConstraint};

Spectrahedron::Spectrahedron()
: ConvexSet(&ConvexSetCloner<Spectrahedron>, 0) {}

Spectrahedron::Spectrahedron(const MathematicalProgram& prog)
: ConvexSet(&ConvexSetCloner<Spectrahedron>, 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();
Expand All @@ -64,13 +59,22 @@ Spectrahedron::Spectrahedron(const MathematicalProgram& prog)

Spectrahedron::~Spectrahedron() = default;

const ProgramAttributes& Spectrahedron::supported_attributes() {
static const never_destroyed<ProgramAttributes> 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<const VectorXd>& x,
double tol) const {
double tol) const {
return sdp_->CheckSatisfied(sdp_->GetAllConstraints(), x, tol);
}

Expand All @@ -95,16 +99,22 @@ Spectrahedron::DoAddPointInNonnegativeScalingConstraints(
std::vector<Binding<Constraint>> constraints;
const double kInf = std::numeric_limits<double>::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();
Expand All @@ -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();
Expand All @@ -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()) {
Expand All @@ -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<MatrixX<Variable>>(
GetVariablesByIndex(
x, sdp_->FindDecisionVariableIndices(binding.variables()))
.data(),
binding.evaluator()->matrix_rows(),
binding.evaluator()->matrix_rows())));
Eigen::Map<MatrixX<Variable>>(vars.data(),
binding.evaluator()->matrix_rows(),
binding.evaluator()->matrix_rows())));
}
return constraints;
}
Expand Down
11 changes: 3 additions & 8 deletions geometry/optimization/spectrahedron.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
Expand Down Expand Up @@ -71,8 +68,6 @@ class Spectrahedron final : public ConvexSet {
const final;

copyable_unique_ptr<solvers::MathematicalProgram> sdp_{};

static const solvers::ProgramAttributes supported_attributes_;
};

} // namespace optimization
Expand Down