Skip to content

Commit

Permalink
Fix ConvexSet move operations
Browse files Browse the repository at this point in the history 
Deprecate the ConvexSetCloner function and add now-required virtual
DoClone to the base class.

Fix some DEMAND checks on user input to be THROW instead.
Invalid arguments should result in exceptions, not aborts.
  • Loading branch information
@jwnimmer-tri
jwnimmer-tri committed Apr 29, 2023
1 parent 41ae074 commit 88c8881
Show file tree
Hide file tree
Showing 23 changed files with 257 additions and 111 deletions.
26 changes: 12 additions & 14 deletions geometry/optimization/cartesian_product.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,33 +38,27 @@ int sum_ambient_dimensions(const ConvexSets& sets) {
} // namespace

CartesianProduct::CartesianProduct(const ConvexSets& sets)
: ConvexSet(&ConvexSetCloner<CartesianProduct>,
sum_ambient_dimensions(sets)),
sets_{sets} {}
: ConvexSet(sum_ambient_dimensions(sets)), sets_{sets} {}

CartesianProduct::CartesianProduct(const ConvexSet& setA, const ConvexSet& setB)
: ConvexSet(&ConvexSetCloner<CartesianProduct>,
setA.ambient_dimension() + setB.ambient_dimension()) {
: ConvexSet(setA.ambient_dimension() + setB.ambient_dimension()) {
sets_.emplace_back(setA.Clone());
sets_.emplace_back(setB.Clone());
}

CartesianProduct::CartesianProduct(const ConvexSets& sets,
const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::VectorXd>& b)
: ConvexSet(&ConvexSetCloner<CartesianProduct>, A.cols()),
sets_{sets},
A_{A},
b_{b} {
DRAKE_DEMAND(A_->rows() == b_->rows());
DRAKE_DEMAND(A_->rows() == sum_ambient_dimensions(sets));
DRAKE_DEMAND(A_->colPivHouseholderQr().rank() == A_->cols());
: ConvexSet(A.cols()), sets_{sets}, A_{A}, b_{b} {
DRAKE_THROW_UNLESS(A_->rows() == b_->rows());
DRAKE_THROW_UNLESS(A_->rows() == sum_ambient_dimensions(sets));
DRAKE_THROW_UNLESS(A_->colPivHouseholderQr().rank() == A_->cols());
}

CartesianProduct::CartesianProduct(const QueryObject<double>& query_object,
GeometryId geometry_id,
std::optional<FrameId> reference_frame)
: ConvexSet(&ConvexSetCloner<CartesianProduct>, 3) {
: ConvexSet(3) {
Cylinder cylinder(1., 1.);
query_object.inspector().GetShape(geometry_id).Reify(this, &cylinder);

Expand All @@ -89,10 +83,14 @@ CartesianProduct::CartesianProduct(const QueryObject<double>& query_object,
CartesianProduct::~CartesianProduct() = default;

const ConvexSet& CartesianProduct::factor(int index) const {
DRAKE_DEMAND(0 <= index && index < static_cast<int>(sets_.size()));
DRAKE_THROW_UNLESS(0 <= index && index < ssize(sets_));
return *sets_[index];
}

std::unique_ptr<ConvexSet> CartesianProduct::DoClone() const {
return std::make_unique<CartesianProduct>(*this);
}

bool CartesianProduct::DoIsBounded() const {
// Note: The constructor enforces that A_ is full column rank.
for (const auto& s : sets_) {
Expand Down
2 changes: 2 additions & 0 deletions geometry/optimization/cartesian_product.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,8 @@ class CartesianProduct final : public ConvexSet {
using ConvexSet::PointInSet;

private:
std::unique_ptr<ConvexSet> DoClone() const final;

bool DoIsBounded() const final;

bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
Expand Down
22 changes: 8 additions & 14 deletions geometry/optimization/convex_set.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,19 +11,13 @@ namespace drake {
namespace geometry {
namespace optimization {

ConvexSet::ConvexSet(
std::function<std::unique_ptr<ConvexSet>(const ConvexSet&)> cloner,
int ambient_dimension)
: cloner_(std::move(cloner)), ambient_dimension_(ambient_dimension) {
DRAKE_DEMAND(ambient_dimension >= 0);
ConvexSet::ConvexSet(int ambient_dimension)
: ambient_dimension_(ambient_dimension) {
DRAKE_THROW_UNLESS(ambient_dimension >= 0);
}

ConvexSet::~ConvexSet() = default;

std::unique_ptr<ConvexSet> ConvexSet::Clone() const {
return cloner_(*this);
}

bool ConvexSet::IntersectsWith(const ConvexSet& other) const {
DRAKE_THROW_UNLESS(other.ambient_dimension() == this->ambient_dimension());
solvers::MathematicalProgram prog{};
Expand All @@ -39,7 +33,7 @@ ConvexSet::AddPointInNonnegativeScalingConstraints(
solvers::MathematicalProgram* prog,
const Eigen::Ref<const solvers::VectorXDecisionVariable>& x,
const symbolic::Variable& t) const {
DRAKE_DEMAND(x.size() == ambient_dimension());
DRAKE_THROW_UNLESS(x.size() == ambient_dimension());
std::vector<solvers::Binding<solvers::Constraint>> constraints =
DoAddPointInNonnegativeScalingConstraints(prog, x, t);
constraints.emplace_back(prog->AddBoundingBoxConstraint(
Expand All @@ -55,10 +49,10 @@ ConvexSet::AddPointInNonnegativeScalingConstraints(
const Eigen::Ref<const Eigen::VectorXd>& c, double d,
const Eigen::Ref<const solvers::VectorXDecisionVariable>& x,
const Eigen::Ref<const solvers::VectorXDecisionVariable>& t) const {
DRAKE_DEMAND(A.rows() == ambient_dimension());
DRAKE_DEMAND(A.rows() == b.rows());
DRAKE_DEMAND(A.cols() == x.size());
DRAKE_DEMAND(c.rows() == t.size());
DRAKE_THROW_UNLESS(A.rows() == ambient_dimension());
DRAKE_THROW_UNLESS(A.rows() == b.rows());
DRAKE_THROW_UNLESS(A.cols() == x.size());
DRAKE_THROW_UNLESS(c.rows() == t.size());
std::vector<solvers::Binding<solvers::Constraint>> constraints =
DoAddPointInNonnegativeScalingConstraints(prog, A, b, c, d, x, t);
constraints.emplace_back(prog->AddLinearConstraint(
Expand Down
45 changes: 22 additions & 23 deletions geometry/optimization/convex_set.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@

#include "drake/common/copyable_unique_ptr.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_deprecated.h"
#include "drake/common/reset_after_move.h"
#include "drake/geometry/geometry_ids.h"
#include "drake/geometry/query_object.h"
#include "drake/geometry/shape_specification.h"
Expand Down Expand Up @@ -64,7 +66,9 @@ class ConvexSet : public ShapeReifier {
virtual ~ConvexSet();

/** Creates a unique deep copy of this set. */
std::unique_ptr<ConvexSet> Clone() const;
std::unique_ptr<ConvexSet> Clone() const {
return DoClone();
}

/** Returns the dimension of the vector space in which the elements of this
set are evaluated. Contrast this with the `affine dimension`: the
Expand All @@ -91,7 +95,7 @@ class ConvexSet : public ShapeReifier {
/** Returns true iff the point x is contained in the set. */
bool PointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
double tol = 0) const {
DRAKE_DEMAND(x.size() == ambient_dimension());
DRAKE_THROW_UNLESS(x.size() == ambient_dimension());
return DoPointInSet(x, tol);
}

Expand All @@ -102,7 +106,7 @@ class ConvexSet : public ShapeReifier {
void AddPointInSetConstraints(
solvers::MathematicalProgram* prog,
const Eigen::Ref<const solvers::VectorXDecisionVariable>& vars) const {
DRAKE_DEMAND(vars.size() == ambient_dimension());
DRAKE_THROW_UNLESS(vars.size() == ambient_dimension());
return DoAddPointInSetConstraints(prog, vars);
}

Expand Down Expand Up @@ -160,7 +164,7 @@ class ConvexSet : public ShapeReifier {
a particular set has not been implemented yet. */
std::pair<std::unique_ptr<Shape>, math::RigidTransformd> ToShapeWithPose()
const {
DRAKE_DEMAND(ambient_dimension_ == 3);
DRAKE_THROW_UNLESS(ambient_dimension_ == 3);
return DoToShapeWithPose();
}

Expand All @@ -170,31 +174,26 @@ class ConvexSet : public ShapeReifier {
protected:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ConvexSet)

/** For use by derived classes to construct a %ConvexSet.
@param cloner Function pointer to implement Clone(), typically of the form
`&ConvexSetCloner<Derived>`.
Here is a typical example:
@code
class MyConvexSet final : public ConvexSet {
public:
MyConvexSet() : ConvexSet(&ConvexSetCloner<MyConvexSet>, 3) {}
...
};
@endcode */
ConvexSet(std::function<std::unique_ptr<ConvexSet>(const ConvexSet&)> cloner,
int ambient_dimension);
/** For use by derived classes to construct a %ConvexSet. */
explicit ConvexSet(int ambient_dimension);

// Implements non-virtual base class serialization.
template <typename Archive>
void Serialize(Archive* a) {
a->Visit(DRAKE_NVP(ambient_dimension_));
int ambient_dimension = ambient_dimension_;
// TODO(#19309) The trailing underscore here is wrong.
a->Visit(MakeNameValue("ambient_dimension_", &ambient_dimension));
ambient_dimension_ = ambient_dimension;
}


// Non-virtual interface implementations.

virtual std::unique_ptr<ConvexSet> DoClone() const = 0;

virtual bool DoIsBounded() const = 0;

/** @pre x.size() == ambient_dimension() */
virtual bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
double tol) const = 0;

Expand Down Expand Up @@ -222,16 +221,16 @@ class ConvexSet : public ShapeReifier {
const Eigen::Ref<const solvers::VectorXDecisionVariable>& x,
const Eigen::Ref<const solvers::VectorXDecisionVariable>& t) const = 0;

/** @pre ambient_dimension() == 3 */
virtual std::pair<std::unique_ptr<Shape>, math::RigidTransformd>
DoToShapeWithPose() const = 0;

std::function<std::unique_ptr<ConvexSet>(const ConvexSet&)> cloner_;
int ambient_dimension_{0};
reset_after_move<int> ambient_dimension_;
};

/** (Advanced) Implementation helper for ConvexSet::Clone. Refer to the
ConvexSet::ConvexSet() constructor documentation for an example. */
template <typename Derived>
DRAKE_DEPRECATED("2023-09-01", "This function is no longer necessary.")
std::unique_ptr<ConvexSet> ConvexSetCloner(const ConvexSet& other) {
static_assert(std::is_base_of_v<ConvexSet, Derived>,
"Concrete sets *must* be derived from the ConvexSet class");
Expand Down
34 changes: 18 additions & 16 deletions geometry/optimization/hpolyhedron.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,18 +111,18 @@ bool IsRedundant(const Eigen::Ref<const MatrixXd>& c, double d,

} // namespace

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

HPolyhedron::HPolyhedron(const Eigen::Ref<const MatrixXd>& A,
const Eigen::Ref<const VectorXd>& b)
: ConvexSet(&ConvexSetCloner<HPolyhedron>, A.cols()), A_{A}, b_{b} {
: ConvexSet(A.cols()), A_{A}, b_{b} {
CheckInvariants();
}

HPolyhedron::HPolyhedron(const QueryObject<double>& query_object,
GeometryId geometry_id,
std::optional<FrameId> reference_frame)
: ConvexSet(&ConvexSetCloner<HPolyhedron>, 3) {
: ConvexSet(3) {
std::pair<MatrixXd, VectorXd> Ab_G;
query_object.inspector().GetShape(geometry_id).Reify(this, &Ab_G);

Expand All @@ -137,7 +137,7 @@ HPolyhedron::HPolyhedron(const QueryObject<double>& query_object,
}

HPolyhedron::HPolyhedron(const VPolytope& vpoly)
: ConvexSet(&ConvexSetCloner<HPolyhedron>, vpoly.ambient_dimension()) {
: ConvexSet(vpoly.ambient_dimension()) {
orgQhull::Qhull qhull;
qhull.runQhull("", vpoly.ambient_dimension(), vpoly.vertices().cols(),
vpoly.vertices().data(), "");
Expand Down Expand Up @@ -254,6 +254,7 @@ HPolyhedron HPolyhedron::CartesianPower(int n) const {
HPolyhedron HPolyhedron::Intersection(const HPolyhedron& other,
bool check_for_redundancy,
double tol) const {
DRAKE_THROW_UNLESS(ambient_dimension() == other.ambient_dimension());
if (check_for_redundancy) {
return this->DoIntersectionWithChecks(other, tol);
}
Expand Down Expand Up @@ -307,8 +308,8 @@ VectorXd HPolyhedron::UniformSample(RandomGenerator* generator) const {

HPolyhedron HPolyhedron::MakeBox(const Eigen::Ref<const VectorXd>& lb,
const Eigen::Ref<const VectorXd>& ub) {
DRAKE_DEMAND(lb.size() == ub.size());
DRAKE_DEMAND((lb.array() <= ub.array()).all());
DRAKE_THROW_UNLESS(lb.size() == ub.size());
DRAKE_THROW_UNLESS((lb.array() <= ub.array()).all());
const int N = lb.size();
MatrixXd A(2 * N, N);
A << MatrixXd::Identity(N, N), -MatrixXd::Identity(N, N);
Expand Down Expand Up @@ -363,7 +364,7 @@ bool HPolyhedron::DoIsBounded() const {
}

bool HPolyhedron::ContainedIn(const HPolyhedron& other, double tol) const {
DRAKE_DEMAND(other.A().cols() == A_.cols());
DRAKE_THROW_UNLESS(other.A().cols() == A_.cols());
// `this` defines an empty set and therefore is contained in any `other`
// HPolyhedron.
if (DoIsEmpty(A_, b_)) {
Expand Down Expand Up @@ -395,7 +396,6 @@ bool HPolyhedron::ContainedIn(const HPolyhedron& other, double tol) const {

HPolyhedron HPolyhedron::DoIntersectionNoChecks(
const HPolyhedron& other) const {
DRAKE_DEMAND(ambient_dimension() == other.ambient_dimension());
MatrixXd A_intersect =
MatrixXd::Zero(A_.rows() + other.A().rows(), A_.cols());
A_intersect.topRows(A_.rows()) = A_;
Expand All @@ -407,8 +407,6 @@ HPolyhedron HPolyhedron::DoIntersectionNoChecks(

HPolyhedron HPolyhedron::DoIntersectionWithChecks(const HPolyhedron& other,
double tol) const {
DRAKE_DEMAND(other.A().cols() == A_.cols());

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A(
A_.rows() + other.A().rows(), A_.cols());
VectorXd b(A_.rows() + other.A().rows());
Expand Down Expand Up @@ -528,6 +526,10 @@ bool HPolyhedron::IsEmpty() const {
return DoIsEmpty(A_, b_);
}

std::unique_ptr<ConvexSet> HPolyhedron::DoClone() const {
return std::make_unique<HPolyhedron>(*this);
}

bool HPolyhedron::DoPointInSet(const Eigen::Ref<const VectorXd>& x,
double tol) const {
DRAKE_DEMAND(A_.cols() == x.size());
Expand Down Expand Up @@ -616,9 +618,9 @@ HPolyhedron HPolyhedron::PontryaginDifference(const HPolyhedron& other) const {
* where hᵢ = max aᵢᵀx subject to x ∈ Q
*/

DRAKE_DEMAND(this->ambient_dimension() == other.ambient_dimension());
DRAKE_DEMAND(this->IsBounded());
DRAKE_DEMAND(other.IsBounded());
DRAKE_THROW_UNLESS(this->ambient_dimension() == other.ambient_dimension());
DRAKE_THROW_UNLESS(this->IsBounded());
DRAKE_THROW_UNLESS(other.IsBounded());

VectorXd b_diff(b_.rows());
MathematicalProgram prog;
Expand Down Expand Up @@ -655,12 +657,12 @@ HPolyhedron HPolyhedron::PontryaginDifference(const HPolyhedron& other) const {
}

void HPolyhedron::CheckInvariants() const {
DRAKE_DEMAND(this->ambient_dimension() == A_.cols());
DRAKE_DEMAND(A_.rows() == b_.size());
DRAKE_THROW_UNLESS(this->ambient_dimension() == A_.cols());
DRAKE_THROW_UNLESS(A_.rows() == b_.size());
// Note: If necessary, we could support infinite b, either by removing the
// corresponding rows of A (since the constraint is vacuous), or checking
// this explicitly in all relevant computations (like IsBounded).
DRAKE_DEMAND(b_.array().isFinite().all());
DRAKE_THROW_UNLESS(b_.array().isFinite().all());
}

} // namespace optimization
Expand Down
3 changes: 3 additions & 0 deletions geometry/optimization/hpolyhedron.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -204,6 +204,7 @@ class HPolyhedron final : public ConvexSet {
template <typename Archive>
void Serialize(Archive* a) {
ConvexSet::Serialize(a);
// TODO(#19309) The trailing underscores here are wrong.
a->Visit(DRAKE_NVP(A_));
a->Visit(DRAKE_NVP(b_));
CheckInvariants();
Expand All @@ -216,6 +217,8 @@ class HPolyhedron final : public ConvexSet {
[[nodiscard]] HPolyhedron DoIntersectionWithChecks(const HPolyhedron& other,
double tol) const;

std::unique_ptr<ConvexSet> DoClone() const final;

bool DoIsBounded() const final;

bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
Expand Down

0 comments on commit 88c8881

Please sign in to comment.