Skip to content

Commit

Permalink
Fix ConvexSet move operations
Browse files Browse the repository at this point in the history 
Add now-required virtual DoClone to the base class; deprecate the
ConvexSetCloner function and the ConvexSet constructor that accepted it.

Fix some DEMAND checks on user input to be THROW instead.
Invalid arguments should result in exceptions, not aborts.

Fix documentation and implementations to be precise and correct in the
case of zero-dimensional sets.
  • Loading branch information
@jwnimmer-tri
jwnimmer-tri committed Apr 30, 2023
1 parent 41ae074 commit fa098f0
Show file tree
Hide file tree
Showing 27 changed files with 469 additions and 137 deletions.
7 changes: 7 additions & 0 deletions bindings/pydrake/geometry/geometry_py_optimization.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,7 @@ void DefineGeometryOptimization(py::module m) {
{
const auto& cls_doc = doc.CartesianProduct;
py::class_<CartesianProduct, ConvexSet>(m, "CartesianProduct", cls_doc.doc)
.def(py::init<>(), cls_doc.ctor.doc_0args)
.def(py::init<const ConvexSets&>(), py::arg("sets"),
cls_doc.ctor.doc_1args_sets)
.def(py::init<const ConvexSet&, const ConvexSet&>(), py::arg("setA"),
Expand All @@ -114,6 +115,7 @@ void DefineGeometryOptimization(py::module m) {
{
const auto& cls_doc = doc.HPolyhedron;
py::class_<HPolyhedron, ConvexSet>(m, "HPolyhedron", cls_doc.doc)
.def(py::init<>(), cls_doc.ctor.doc_0args)
.def(py::init<const Eigen::Ref<const Eigen::MatrixXd>&,
const Eigen::Ref<const Eigen::VectorXd>&>(),
py::arg("A"), py::arg("b"), cls_doc.ctor.doc_2args)
Expand Down Expand Up @@ -176,6 +178,7 @@ void DefineGeometryOptimization(py::module m) {
{
const auto& cls_doc = doc.Hyperellipsoid;
py::class_<Hyperellipsoid, ConvexSet>(m, "Hyperellipsoid", cls_doc.doc)
.def(py::init<>(), cls_doc.ctor.doc_0args)
.def(py::init<const Eigen::Ref<const Eigen::MatrixXd>&,
const Eigen::Ref<const Eigen::VectorXd>&>(),
py::arg("A"), py::arg("center"), cls_doc.ctor.doc_2args)
Expand Down Expand Up @@ -210,6 +213,7 @@ void DefineGeometryOptimization(py::module m) {
{
const auto& cls_doc = doc.Intersection;
py::class_<Intersection, ConvexSet>(m, "Intersection", cls_doc.doc)
.def(py::init<>(), cls_doc.ctor.doc_0args)
.def(py::init<const ConvexSets&>(), py::arg("sets"),
cls_doc.ctor.doc_1args)
.def(py::init<const ConvexSet&, const ConvexSet&>(), py::arg("setA"),
Expand All @@ -225,6 +229,7 @@ void DefineGeometryOptimization(py::module m) {
{
const auto& cls_doc = doc.MinkowskiSum;
py::class_<MinkowskiSum, ConvexSet>(m, "MinkowskiSum", cls_doc.doc)
.def(py::init<>(), cls_doc.ctor.doc_0args)
.def(py::init<const ConvexSets&>(), py::arg("sets"),
cls_doc.ctor.doc_1args)
.def(py::init<const ConvexSet&, const ConvexSet&>(), py::arg("setA"),
Expand All @@ -243,6 +248,7 @@ void DefineGeometryOptimization(py::module m) {
{
const auto& cls_doc = doc.Point;
py::class_<Point, ConvexSet>(m, "Point", cls_doc.doc)
.def(py::init<>(), cls_doc.ctor.doc_0args)
.def(py::init<const Eigen::Ref<const Eigen::VectorXd>&>(), py::arg("x"),
cls_doc.ctor.doc_1args)
.def(py::init<const QueryObject<double>&, GeometryId,
Expand All @@ -261,6 +267,7 @@ void DefineGeometryOptimization(py::module m) {
{
const auto& cls_doc = doc.VPolytope;
py::class_<VPolytope, ConvexSet>(m, "VPolytope", cls_doc.doc)
.def(py::init<>(), cls_doc.ctor.doc)
.def(py::init<const Eigen::Ref<const Eigen::MatrixXd>&>(),
py::arg("vertices"), cls_doc.ctor.doc_vertices)
.def(py::init<const HPolyhedron&>(), py::arg("H"),
Expand Down
7 changes: 7 additions & 0 deletions bindings/pydrake/geometry/test/optimization_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@ def __init__(self, *args, **kwargs):
self.z = self.prog.NewContinuousVariables(2, "z")

def test_point_convex_set(self):
mut.Point()
p = np.array([11.1, 12.2, 13.3])
point = mut.Point(p)
self.assertEqual(point.ambient_dimension(), 3)
Expand All @@ -53,6 +54,7 @@ def test_point_convex_set(self):
# builds from shape.

def test_h_polyhedron(self):
mut.HPolyhedron()
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), self.A)
Expand Down Expand Up @@ -158,6 +160,7 @@ def test_h_polyhedron(self):
self.assertEqual(hpoly.A().shape, (4, 3))

def test_hyper_ellipsoid(self):
mut.Hyperellipsoid()
ellipsoid = mut.Hyperellipsoid(A=self.A, center=self.b)
self.assertEqual(ellipsoid.ambient_dimension(), 3)
np.testing.assert_array_equal(ellipsoid.A(), self.A)
Expand Down Expand Up @@ -196,6 +199,7 @@ def test_hyper_ellipsoid(self):
np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0])

def test_minkowski_sum(self):
mut.MinkowskiSum()
point = mut.Point(np.array([11.1, 12.2, 13.3]))
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
sum = mut.MinkowskiSum(setA=point, setB=hpoly)
Expand All @@ -207,6 +211,7 @@ def test_minkowski_sum(self):
self.assertIsInstance(sum2.term(0), mut.Point)

def test_v_polytope(self):
mut.VPolytope()
vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]])
vpoly = mut.VPolytope(vertices=vertices)
self.assertEqual(vpoly.ambient_dimension(), 2)
Expand Down Expand Up @@ -280,6 +285,7 @@ def _calculate_path_length(self, vertices):
return length

def test_cartesian_product(self):
mut.CartesianProduct()
point = mut.Point(np.array([11.1, 12.2, 13.3]))
h_box = mut.HPolyhedron.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
Expand All @@ -297,6 +303,7 @@ def test_cartesian_product(self):
self.assertIsInstance(sum2.factor(1), mut.HPolyhedron)

def test_intersection(self):
mut.Intersection()
point = mut.Point(np.array([0.1, 0.2, 0.3]))
h_box = mut.HPolyhedron.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
Expand Down
1 change: 1 addition & 0 deletions geometry/optimization/BUILD.bazel
View file
Original file line number Diff line number Diff line change
Expand Up @@ -159,6 +159,7 @@ drake_cc_googletest(
name = "convex_set_test",
deps = [
":convex_set",
"//common/test_utilities:expect_throws_message",
"//common/test_utilities:limit_malloc",
],
)
Expand Down
28 changes: 14 additions & 14 deletions geometry/optimization/cartesian_product.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,34 +37,30 @@ int sum_ambient_dimensions(const ConvexSets& sets) {

} // namespace

CartesianProduct::CartesianProduct() : ConvexSet(0) {}

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 +85,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
6 changes: 6 additions & 0 deletions geometry/optimization/cartesian_product.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,12 +28,16 @@ class CartesianProduct final : public ConvexSet {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(CartesianProduct)

/** Constructs a default (zero-dimensional) set. */
CartesianProduct();

/** Constructs the product from a vector of convex sets. */
explicit CartesianProduct(const ConvexSets& sets);

/** Constructs the product from a pair of convex sets. */
CartesianProduct(const ConvexSet& setA, const ConvexSet& setB);


/** Constructs the product of convex sets in the transformed coordinates:
{x | y = Ax + b, y ∈ Y₁ × Y₂ × ⋯ × Yₙ}. `A` must be full column rank. */
CartesianProduct(const ConvexSets& sets,
Expand Down Expand Up @@ -71,6 +75,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
35 changes: 21 additions & 14 deletions geometry/optimization/convex_set.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,21 +11,18 @@ 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());
if (ambient_dimension() == 0) {
return false;
}
solvers::MathematicalProgram prog{};
const auto& x = prog.NewContinuousVariables(this->ambient_dimension(), "x");
this->AddPointInSetConstraints(&prog, x);
Expand All @@ -34,12 +31,21 @@ bool ConvexSet::IntersectsWith(const ConvexSet& other) const {
return result.is_success();
}

void ConvexSet::AddPointInSetConstraints(
solvers::MathematicalProgram* prog,
const Eigen::Ref<const solvers::VectorXDecisionVariable>& vars) const {
DRAKE_THROW_UNLESS(vars.size() == ambient_dimension());
DRAKE_THROW_UNLESS(ambient_dimension() > 0);
return DoAddPointInSetConstraints(prog, vars);
}

std::vector<solvers::Binding<solvers::Constraint>>
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());
DRAKE_THROW_UNLESS(ambient_dimension() > 0);
std::vector<solvers::Binding<solvers::Constraint>> constraints =
DoAddPointInNonnegativeScalingConstraints(prog, x, t);
constraints.emplace_back(prog->AddBoundingBoxConstraint(
Expand All @@ -55,10 +61,11 @@ 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());
DRAKE_THROW_UNLESS(ambient_dimension() > 0);
std::vector<solvers::Binding<solvers::Constraint>> constraints =
DoAddPointInNonnegativeScalingConstraints(prog, A, b, c, d, x, t);
constraints.emplace_back(prog->AddLinearConstraint(
Expand Down

0 comments on commit fa098f0

Please sign in to comment.