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 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 343af98 commit bf2bf23
Show file tree
Hide file tree
Showing 27 changed files with 423 additions and 86 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
34 changes: 19 additions & 15 deletions geometry/optimization/cartesian_product.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,44 +27,44 @@ using symbolic::Variable;

namespace {

int sum_ambient_dimensions(const ConvexSets& sets) {
int SumAmbientDimensions(const ConvexSets& sets) {
int dim = 0;
for (const auto& s : sets) {
dim += s->ambient_dimension();
for (const copyable_unique_ptr<ConvexSet>& set : sets) {
DRAKE_THROW_UNLESS(set != nullptr);
dim += set->ambient_dimension();
}
return dim;
}

} // namespace

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

CartesianProduct::CartesianProduct(const ConvexSets& sets)
: ConvexSet(&ConvexSetCloner<CartesianProduct>,
sum_ambient_dimensions(sets)),
sets_{sets} {}
: ConvexSet(SumAmbientDimensions(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 MatrixXd>& A,
const Eigen::Ref<const VectorXd>& b)
: ConvexSet(&ConvexSetCloner<CartesianProduct>, A.cols()),
sets_{sets},
A_{A},
b_{b} {
DRAKE_THROW_UNLESS(A_->rows() == b_->rows());
DRAKE_THROW_UNLESS(A_->rows() == sum_ambient_dimensions(sets));
: ConvexSet(A.cols()), sets_{sets}, A_{A}, b_{b} {
const int x_ambient_dimension = SumAmbientDimensions(sets);
const int y_ambient_dimension = ambient_dimension();
DRAKE_THROW_UNLESS(A_->rows() == x_ambient_dimension);
DRAKE_THROW_UNLESS(b_->rows() == x_ambient_dimension);
DRAKE_THROW_UNLESS(A_->cols() == y_ambient_dimension);
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 Down Expand Up @@ -93,6 +93,10 @@ const ConvexSet& CartesianProduct::factor(int index) const {
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
5 changes: 5 additions & 0 deletions geometry/optimization/cartesian_product.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,9 @@ 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);

Expand Down Expand Up @@ -69,6 +72,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
23 changes: 15 additions & 8 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) {
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,11 +31,20 @@ 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_THROW_UNLESS(ambient_dimension() > 0);
DRAKE_THROW_UNLESS(x.size() == ambient_dimension());
std::vector<solvers::Binding<solvers::Constraint>> constraints =
DoAddPointInNonnegativeScalingConstraints(prog, x, t);
Expand All @@ -55,6 +61,7 @@ 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_THROW_UNLESS(ambient_dimension() > 0);
DRAKE_THROW_UNLESS(A.rows() == ambient_dimension());
DRAKE_THROW_UNLESS(A.rows() == b.rows());
DRAKE_THROW_UNLESS(A.cols() == x.size());
Expand Down

0 comments on commit bf2bf23

Please sign in to comment.