geometry:optimization Implements IsBounded and Hyperellipsoid Volumes

[RussTedrake]

Also includes a ConvexSets (plural) container as my proposed solution to memory management for workflows like this:

ConvexSets MakeIrisObstacles(
    const QueryObject<double>& query_object,
    std::optional<FrameId> reference_frame = std::nullopt);
HPolyhedron Iris(const ConvexSets& obstacles,
                 const Eigen::Ref<const Eigen::VectorXd>& sample,
                 const HPolyhedron& domain,
                 const IrisOptions& options = IrisOptions());

Also includes a number of small fix-ups and style updates:

• HyperEllipsoid => Hyperellipsoid (per discussion with Sean)
• AddPointInSetConstraint => AddPointInSetConstraints
• expressed_in => reference_frame
  This are API breaking changes, but we marked the classes as Experimental so that we could do this in the early stages.

This is not chopped up quite as finely as I've been trying to do so far. But it's the last batch of changes, which had been piling up, before I land the IRIS algorithm in the next PR. I hope it's ok(better?) this way for my reviewers!

---

This change is 

[RussTedrake]

+@hongkai-dai for feature review, please.
You guys are awesome. Almost there! Thanks for your help!
cc @TobiaMarcucci .

Reviewable status: LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @hongkai-dai)

[hongkai-dai]

Reviewed 20 of 20 files at r1.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @RussTedrake)

---

geometry/optimization/convex_set.h, line 202 at r1 (raw file):

  const ConvexSet& emplace_back(const ConvexSet& set);
  const ConvexSet& emplace_back(std::unique_ptr<ConvexSet> set);

Does this transfer the ownership of set? Since ConvexSets stores shared pointers, I think it is worthwhile to document the ownership of set after calling this function.

---

geometry/optimization/convex_set.cc, line 35 at r1 (raw file):

}

std::vector<const ConvexSet*> ConvexSetsFromOwned(

This function is not used?

---

geometry/optimization/hyperellipsoid.h, line 64 at r1 (raw file):

  @returns the minimal scaling and the witness point, x, on other.
  */
  std::pair<double, Eigen::VectorXd> MinimumUniformScalingToTouch(

BTW, a special case is that center already belongs to other, in which case the smallest scaling is zero. Not sure if you want to exclude this special case (as that is not what IRIS wants to solve), or you would like to mention it in the documentation.

---

geometry/optimization/hyperellipsoid.cc, line 99 at r1 (raw file):

        "Solver {} failed to solve the `minimum uniform scaling to touch' "
        "problem; it terminated with SolutionResult {}). Make sure that your "
        "ellipsoid is not empty.",

I can't see how the ellipsoid can be empty. Our parameterization of the ellipsoid guarantees that center_ is always inside the ellipsoid, so it cannot be an empty set.

---

geometry/optimization/test/hpolyhedron_test.cc, line 260 at r1 (raw file):

}

GTEST_TEST(HPolyhedronTest, IsBounded) {

I think we haven't tested the case that A.row() >= A.col() but A has a non-empty kernel yet.

---

geometry/optimization/test/hyperellipsoid_test.cc, line 283 at r1 (raw file):

  EXPECT_TRUE(CompareMatrices(E.A(), MatrixXd::Identity(4, 4) / kRadius));
  EXPECT_TRUE(CompareMatrices(E.center(), center));
  const Vector4d xvec = Eigen::MatrixXd::Identity(1, 4);

This should be Identity(4, 1)?

[RussTedrake]

Reviewable status: 5 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @hongkai-dai)

---

geometry/optimization/convex_set.h, line 202 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Does this transfer the ownership of set? Since ConvexSets stores shared pointers, I think it is worthwhile to document the ownership of set after calling this function.

Done. Good call. I missed these docstrings.

---

geometry/optimization/convex_set.cc, line 35 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

This function is not used?

Done. Good catch (that was an old attempt!)

---

geometry/optimization/hyperellipsoid.h, line 64 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

BTW, a special case is that center already belongs to other, in which case the smallest scaling is zero. Not sure if you want to exclude this special case (as that is not what IRIS wants to solve), or you would like to mention it in the documentation.

I like that. I've also added this as a unit test.

---

geometry/optimization/hyperellipsoid.h, line 64 at r1 (raw file):

  @returns the minimal scaling and the witness point, x, on other.
  */
  std::pair<double, Eigen::VectorXd> MinimumUniformScalingToTouch(

BTW -- I was torn between having the witness as an optional Eigen::Ptr argument vs this was with the std::pair. Which do you like?

---

geometry/optimization/hyperellipsoid.cc, line 99 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

I can't see how the ellipsoid can be empty. Our parameterization of the ellipsoid guarantees that center_ is always inside the ellipsoid, so it cannot be an empty set.

The only case I was worried about was if some coefficients of A are infinite. Perhaps I could check isFinite for A in the constructor?

---

geometry/optimization/test/hpolyhedron_test.cc, line 260 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

I think we haven't tested the case that A.row() >= A.col() but A has a non-empty kernel yet.

Done. Not sure how I missed it!

---

geometry/optimization/test/hyperellipsoid_test.cc, line 283 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

This should be Identity(4, 1)?

Done. Thanks

[hongkai-dai]

+@soonho-tri for platform review please, thanks!

Reviewed 5 of 5 files at r2.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee soonho-tri(platform) (waiting on @RussTedrake and @soonho-tri)

---

geometry/optimization/hyperellipsoid.h, line 64 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

BTW -- I was torn between having the witness as an optional Eigen::Ptr argument vs this was with the std::pair. Which do you like?

I think I like std::pair version better. If the user doesn't want to know the witness point, they can do

std::tie(scaling, std::ignore) = MinimumUniformScalingToTouch(...);

---

geometry/optimization/hyperellipsoid.cc, line 99 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

The only case I was worried about was if some coefficients of A are infinite. Perhaps I could check isFinite for A in the constructor?

Ah I see, I think it makes sense to check isFinite for A in the constructor.

[hongkai-dai]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee soonho-tri(platform) (waiting on @RussTedrake and @soonho-tri)

---

bindings/pydrake/geometry_py.cc, line 1360 at r2 (raw file):

            py::arg("reference_frame") = std::nullopt, cls_doc.ctor.doc_3args)
        .def("A", &Hyperellipsoid::A, cls_doc.A.doc)
        .def("center", &Hyperellipsoid::center, cls_doc.center.doc);

BTW, would you also add the python binding for the new function MinimumUniformScalingToTouch (and its python test)?

[RussTedrake]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee soonho-tri(platform) (waiting on @hongkai-dai and @soonho-tri)

---

bindings/pydrake/geometry_py.cc, line 1360 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

BTW, would you also add the python binding for the new function MinimumUniformScalingToTouch (and its python test)?

yes. I'm going to send one PR with all of the new python bindings at the end of the train.

---

geometry/optimization/hyperellipsoid.cc, line 99 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Ah I see, I think it makes sense to check isFinite for A in the constructor.

Done.

---

geometry/optimization/hyperellipsoid.cc, line 96 at r3 (raw file):

  // If we have only linear constraints, then add a quadratic cost and solve the
  // QP.  Otherwise add a slack variable and solve the SOCP.

@hongkai-dai I've added this to address #15316 (and because it's probably the right thing to do more generally). PTAL?

[hongkai-dai]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee soonho-tri(platform) (waiting on @hongkai-dai, @RussTedrake, and @soonho-tri)

---

geometry/optimization/hyperellipsoid.cc, line 96 at r3 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

@hongkai-dai I've added this to address #15316 (and because it's probably the right thing to do more generally). PTAL?

I am not sure if this resolves #15316. As far as I see, the problem is in IpoptSolver handing Lorentz cone constraint. So if other.AddPointInSetConstraints(&prog, x); adds lorentz cone constraint, it should still cause seg fault.

[soonho-tri]

FWIW, I wonder if your workflow includes calling DoIsBounded / MinimumUniformScalingToTouch methods multiple times on the same instance. If so, we can cache the first computation and re-use it in the following calls.

Reviewed 14 of 20 files at r1, 4 of 5 files at r2, 2 of 2 files at r3.
Reviewable status: 1 unresolved discussion (waiting on @RussTedrake)

[jwnimmer-tri]

Implementing three new ideas in one PR makes it easy for reviewers to miss things. "Also" is a four-letter word in commit messages.

Reviewable status: 3 unresolved discussions (waiting on @RussTedrake)

---

geometry/optimization/convex_set.h, line 214 at r3 (raw file):

 private:
  std::vector<std::shared_ptr<ConvexSet>> sets_{};

Neither the lack of const on the ConvexSet nor the use of shared_ptr (vs unique_ptr or copyable_unique_ptr) seems to be justified within GSG.

---

geometry/optimization/convex_set.cc, line 45 at r3 (raw file):

const ConvexSet& ConvexSets::emplace_back(std::unique_ptr<ConvexSet> set) {
  sets_.emplace_back(std::move(set));

The lack of set->Clone() here allows callers to mutate the set after adding it by retaining a borrowed pointer. If that is desired, then promise it as part of the API. If not, then remove this overload.

[RussTedrake]

@soonho-tri -- In my current workflow, I don't plan to call IsBounded repeatedly, but will call MinimumUniformScalingToTouch repeatedly on the same this with different other. I don't see an easy way to avoid duplication in that case?

Reviewable status: 3 unresolved discussions (waiting on @hongkai-dai, @jwnimmer-tri, and @soonho-tri)

---

geometry/optimization/convex_set.h, line 214 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Neither the lack of const on the ConvexSet nor the use of shared_ptr (vs unique_ptr or copyable_unique_ptr) seems to be justified within GSG.

You're right... I was unclear in my thinking about access here. (and had forgotten we have copyable_unique_ptr). I want to get this right.

My first goal is to prevent access to the sets if I pass a const ConvexSets to a worker method. But I do not actually need to prevent access to someone holding a mutable ConvexSets, nor to the original creator of a ConvexSet.

My second goal was to avoid a copy when it is not necessary. In particular, I think some ConvexSet instances might have a lot of data attached (e.g. if they represent a convex mesh). So I want to be able to move without copying the data. That's how I ended up with a shared_ptr. But I can do the same with explicit std::move and copyable_unique_ptr. I've done that now.

---

geometry/optimization/convex_set.cc, line 45 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The lack of set->Clone() here allows callers to mutate the set after adding it by retaining a borrowed pointer. If that is desired, then promise it as part of the API. If not, then remove this overload.

I've attempted to clarify this above. It's an important part of the API. And I've added the non-const access to make my intentions more clear.

---

geometry/optimization/hyperellipsoid.cc, line 96 at r3 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

I am not sure if this resolves #15316. As far as I see, the problem is in IpoptSolver handing Lorentz cone constraint. So if other.AddPointInSetConstraints(&prog, x); adds lorentz cone constraint, it should still cause seg fault.

I'm pretty sure I don't want to end up with a quadratic cost and a conic constraint (ever). So that leaves me two options: 1) always use the slack variable + conic constraint, or 2) what i've done here. The most important case, which I want to be fast, is to scale an ellipse to meet a convex polytope. So I think formulating that as a QP instead of an SOCP is worthwhile.

[jwnimmer-tri]

Reviewable status: 1 unresolved discussion (waiting on @hongkai-dai and @soonho-tri)

---

geometry/optimization/convex_set.h, line 214 at r3 (raw file):

I do not actually need to prevent access to ... the original creator of a ConvexSet.

In the framework, we used to have similar methods that took a unique_ptr that was notionally invariant but allowed callers to borrow an alias and mutate it later. It was a buggy headache and we had to go back later and deprecate it. Passing types with value semantics by-pointer tends to be brittle, in general.

So, while I disapprove of this design, the code and documentation is at least internally consistent now, so I'll mark this satisfied.

My second goal was to avoid a copy when it is not necessary.

In terms of clone efficiency, that should be possible to address within each class's implementation of Clone -- if the member mesh is shared_ptr<const Mesh> mesh_; then it will just increment the refcount while copying. Since the final subclasses are already copyable, implementing the shallow copies within the copy constructor gives the win to call callers, not just ConvexSet.

[soonho-tri]

@RussTedrake , you're right. For MinimumUniformScalingToTouch, we need push / pop operations on MP to re-use decision variables and cost/constraints from this object. We can revisit this if MinimumUniformScalingToTouch turns out to be a bottleneck.

Reviewed 6 of 6 files at r4.
Reviewable status: 1 unresolved discussion (waiting on @hongkai-dai)

[RussTedrake]

---

geometry/optimization/convex_set.h, line 214 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

I do not actually need to prevent access to ... the original creator of a ConvexSet.

In the framework, we used to have similar methods that took a unique_ptr that was notionally invariant but allowed callers to borrow an alias and mutate it later. It was a buggy headache and we had to go back later and deprecate it. Passing types with value semantics by-pointer tends to be brittle, in general.

So, while I disapprove of this design, the code and documentation is at least internally consistent now, so I'll mark this satisfied.

My second goal was to avoid a copy when it is not necessary.

In terms of clone efficiency, that should be possible to address within each class's implementation of Clone -- if the member mesh is shared_ptr<const Mesh> mesh_; then it will just increment the refcount while copying. Since the final subclasses are already copyable, implementing the shallow copies within the copy constructor gives the win to call callers, not just ConvexSet.

Can you please help me understand what design you would approve of? Even if we defer the copy to the class Clone methods, that doesn't provide the ability to pass my container with const-only access to a method that doesn't need to mutate the ConvexSet members?

[hongkai-dai]

Reviewable status: 1 unresolved discussion (waiting on @RussTedrake)

---

geometry/optimization/hyperellipsoid.cc, line 96 at r3 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

I'm pretty sure I don't want to end up with a quadratic cost and a conic constraint (ever). So that leaves me two options: 1) always use the slack variable + conic constraint, or 2) what i've done here. The most important case, which I want to be fast, is to scale an ellipse to meet a convex polytope. So I think formulating that as a QP instead of an SOCP is worthwhile.

Got it, thanks!

[RussTedrake]

I will merge this one so that future discussions/improvements on ConvexSets can be in a smaller more contained PR.

Reviewable status: complete! all discussions resolved, LGTM from assignees soonho-tri(platform),hongkai-dai (waiting on @RussTedrake)

[jwnimmer-tri]

Peeking at the IRIS PR #15315, I think I would have done:

using ConvexSets = std::vector<copyable_unique_ptr<ConvexSet>>;

That would mean that callers need to say *obstacles[i] instead of obstacles[i], but that's par for the course in C++.

[jwnimmer-tri]

The other option would be to make ConvexSet itself directly copyable (and so, no need for Clone or unique_ptr) via the Impl / Cell pattern, similar to symbolic::Expression. Then vector<ConvexSex> works.