Skip to content

Commit

Permalink
WIP
Browse files Browse the repository at this point in the history
  • Loading branch information
@jwnimmer-tri
jwnimmer-tri committed May 2, 2023
1 parent 88dae28 commit 6ae2c34
Show file tree
Hide file tree
Showing 2 changed files with 41 additions and 45 deletions.
75 changes: 38 additions & 37 deletions geometry/optimization/spectrahedron.cc
View file
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
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

0 comments on commit 6ae2c34

Please sign in to comment.