Function to partition convex sets into pieces that satisfy the convexity radius property.

[cohnt]

Towards #20449.

This function splits a convex set (or list of convex sets) into pieces that can be used in GcsTrajectoryOptimization when continuous revolute joints are part of the system.

---

This change is 

[cohnt]

Right now, the only thing that is causing this to fail is a frustrating bug with pybind. So I'm going to go ahead and request a feature review from +@sadraddini, since the pybind bug shouldn't have any impact on that.

[sadraddini]

Sorry I was slow in the past couple of days. Will get to this today.

[sadraddini]

Nice methods. See the comments.

I suggest adding a static bool CheckIfSatisfiesConvexityRadius(const ConvexSet& convex_set, std::vector<int> continous_joint_indices, const double epsilon) method. It is useful for users and also comes handy in unit tests.

Reviewable status: 15 unresolved discussions, LGTM missing from assignee sadraddini, needs platform reviewer assigned, needs at least two assigned reviewers

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 554 at r2 (raw file):

};

/* Given a convex set, and a list of indices corresponding to continuous

btw I would make the first sentence of the docs shorter and more concise. Something like: "Partitions a convex set into a list of convex sets that respect convexity radius inequality as in CheckIfSatisfiesConvexityRadius." And then use "@return the vector of convex sets that respect convexity radius." I leave up to you on the final version of the wording here once everything is in.

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 556 at r2 (raw file):

/* Given a convex set, and a list of indices corresponding to continuous
revolute joints, return a list of convex sets that respect the convexity
radius, and whose union is the given convex set. Respecting the convexity radius

btw can you provide a reference for "convexity radius" ?

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 564 at r2 (raw file):

purposes.
@throws std::exception if ϵ <= 0.
@throws std::exception if the input convex set is unbounded.

nit you also need to throw an error if any index is out of range. You also need to make sure the indices are not repetitive. Maybe use std::unordered_set<int> instead?

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 566 at r2 (raw file):

@throws std::exception if the input convex set is unbounded.
*/
geometry::optimization::ConvexSets PartitionConvexSet(

nit I believe this method can be, and better be, static

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 575 at r2 (raw file):

ambient dimension.
@throws std::exception if ϵ <= 0. */
geometry::optimization::ConvexSets PartitionConvexSet(

nit I believe this method also can be, and better be, static

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1072 at r2 (raw file):

  const double convexity_radius_step = M_PI - 2 * epsilon;
  const int dim = convex_set.ambient_dimension();
  const auto bbox_maybe =

nit there is a bit of computational waste in using AABB, which solves the bounds in every direction via optimization, here. You are better off solving the optimization problem only for the designated indices. You may decide to postpone this to another PR, as the API will remain intact.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1075 at r2 (raw file):

      Hyperrectangle::MaybeCalcAxisAlignedBoundingBox(convex_set);
  DRAKE_THROW_UNLESS(bbox_maybe.has_value());
  const Hyperrectangle bbox = bbox_maybe.value();

nit copy reference Hyperrectangle&

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1081 at r2 (raw file):

  // original set.
  for (const int& i : continuous_revolute_joints) {
    const double min_value = bbox.lb()[i];

nit you need to check if i<=0<dim and throw an error otherwise, and capture the error message in a unit test.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1083 at r2 (raw file):

    const double min_value = bbox.lb()[i];
    const double max_value = bbox.ub()[i];
    if (max_value - min_value >= M_PI) {

nit apparently if max_value - min_value is between M_PI -epsilon and M_PI then you will wrongly skip and return the convex set that does not satisfy convexity-radius with epsilon tolerance.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1100 at r2 (raw file):

        }
      }
      sets.erase(sets.begin(), sets.begin() + j_max);

I am not able to follow the logic here as it is hard to see what is erased. Can you write the code without erasing the elements in a loop? Comments are encouraged.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1115 at r2 (raw file):

  for (int i = 1; i < ssize(convex_sets); ++i) {
    DRAKE_THROW_UNLESS(convex_sets[i] != nullptr);
    DRAKE_THROW_UNLESS(convex_sets[i]->ambient_dimension() ==

nit I think it is better to throw a message here as it is not immediately clear to a sloppy user why this failed.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1125 at r2 (raw file):

    sets.insert(sets.end(), new_sets.begin(), new_sets.end());
  }
  return sets;

btw why do you care about convex sets match dimensions. It is not important unless the user adds them to the GCS regions. See comment on making this method static

---

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 1327 at r2 (raw file):

}

bool PointInASet(ConvexSets sets, Eigen::VectorXd point, double kTol) {

nit pass by ref, i.e. const ConvexSets& sets, const VectorXd& point, const double kTol)

---

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 1336 at r2 (raw file):

}

GTEST_TEST(GcsTrajectoryOptimizationTest, PartitionConvexSet) {

nit You need more unit tests. You should definitely have a test that you will not partition of convex set that respects convexity radius. And also a test that you are partitioning according to the correct indices. The unit test here has all indices.

---

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 1397 at r2 (raw file):

  // List of convex sets must have matching ambient dimension.
  VPolytope v2(Eigen::MatrixXd::Zero(3, 1));
  EXPECT_THROW(

nit use DRAKE_EXPECT_THROWS_MESSAGE to capture the message to the sloppy user

[cohnt]

Added such a method as a nonmember function.

Reviewable status: 10 unresolved discussions, LGTM missing from assignee sadraddini, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 554 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

btw I would make the first sentence of the docs shorter and more concise. Something like: "Partitions a convex set into a list of convex sets that respect convexity radius inequality as in CheckIfSatisfiesConvexityRadius." And then use "@return the vector of convex sets that respect convexity radius." I leave up to you on the final version of the wording here once everything is in.

Done.

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 556 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

btw can you provide a reference for "convexity radius" ?

Done.

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 564 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit you also need to throw an error if any index is out of range. You also need to make sure the indices are not repetitive. Maybe use std::unordered_set<int> instead?

Documented that it throws for this reason (and wrote a helper function to check this, since it needs to be checked elsewhere as well). Since other parts of the code also use std::vector<int>, I'd prefer to keep it that way for now.

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 566 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit I believe this method can be, and better be, static

Right now, it's a nonmember function, so it should be okay.

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 575 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit I believe this method also can be, and better be, static

(Same as above)

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1072 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit there is a bit of computational waste in using AABB, which solves the bounds in every direction via optimization, here. You are better off solving the optimization problem only for the designated indices. You may decide to postpone this to another PR, as the API will remain intact.

Made a note to fix this later.

Long term, I'd actually like to store the AABB for all sets, and use them to speed up the construction of the graph -- we can check if the AABBs overlap before checking if the convex sets overlap, and this might lead to a speedup.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1075 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit copy reference Hyperrectangle&

Done.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1081 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit you need to check if i<=0<dim and throw an error otherwise, and capture the error message in a unit test.

Now handled within ThrowsForInvalidContinuousJointsList.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1083 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit apparently if max_value - min_value is between M_PI -epsilon and M_PI then you will wrongly skip and return the convex set that does not satisfy convexity-radius with epsilon tolerance.

I went ahead and cleaned up the logic here -- it solves the problem you've identified, and is also a lot cleaner.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1100 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

I am not able to follow the logic here as it is hard to see what is erased. Can you write the code without erasing the elements in a loop? Comments are encouraged.

Added documentation describing which sets are erased.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1115 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit I think it is better to throw a message here as it is not immediately clear to a sloppy user why this failed.

I documented in the function header that all convex sets must have the same ambient dimension. I think that should be sufficient, since the error message will show the condition text convex_sets[i]->ambient_dimension() == ambient_dimension, which should clearly point to the failure.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1125 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

btw why do you care about convex sets match dimensions. It is not important unless the user adds them to the GCS regions. See comment on making this method static

The fact that we're specifying something about the configuration space (in this case, the dimensions which are circle-valued), implies that there has to be some consistency as to what that configuration space is. In this case, that is evident by the ambient dimension having to be consistent across all sets. Lower dimensional sets can still be added to GcsTrajectoryOptimization, bu they do have to be in the same ambient space.

---

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 1327 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit pass by ref, i.e. const ConvexSets& sets, const VectorXd& point, const double kTol)

Done.

---

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 1336 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit You need more unit tests. You should definitely have a test that you will not partition of convex set that respects convexity radius. And also a test that you are partitioning according to the correct indices. The unit test here has all indices.

Added a bunch more tests to consider these cases.

---

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 1397 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit use DRAKE_EXPECT_THROWS_MESSAGE to capture the message to the sloppy user

Done.

[sadraddini]

Great! This is getting close. I am leaving reviewing the python bindings for the platform reviewer.

I left few more comments / questions.

Reviewed all commit messages.
Reviewable status: 10 unresolved discussions, LGTM missing from assignee sadraddini, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 576 at r2 (raw file):

@throws std::exception if ϵ <= 0. */
geometry::optimization::ConvexSets PartitionConvexSet(
    geometry::optimization::ConvexSets convex_sets,

nit const geometry::optimization::ConvexSets&

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 557 at r5 (raw file):

of stricty less than π along each dimension corresponding to a continuous
revolute joint. Each entry in continuous_revolute_joints must be non-negative,
less than num_positions, and unique. */

nit replace num_positions with ambient_dimension as the function calls a convex set, not a plant. And add in the docs that the function throws if the indices do not make sense (repetitive, invalid range).

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 560 at r5 (raw file):

bool CheckIfSatisfiesConvexityRadius(
    const geometry::optimization::ConvexSet& convex_set,
    std::vector<int> continuous_revolute_joints);

nit pass by ref const std::vector<int>&

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1100 at r2 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

Added documentation describing which sets are erased.

I now understand how the code works. Some suggestions.

I still don't like std::erase - it is a bit hard to follow. How about creating a local ConvexSets partitioned_sets_current_indexright after calling every continuous revolute joint, and then replace this line with sets = partitioned_sets_current_index.

Also, you know the exact final size of sets after you get the AABB and have epsilon. So you can pre-allocate the required memory with vector::reserve . Also put some asserts that you push that exact number of sets in every for loop and final result.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 75 at r5 (raw file):

dimension for a point constrained to lie within a convex set. */
const std::pair<double, double> GetMinimumAndMaximumValueAlongDimension(
    const ConvexSet& region, int dimension) {

nit const int

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 89 at r5 (raw file):

  VectorXd objective_vector = VectorXd::Zero(region.ambient_dimension());
  objective_vector[dimension] = 1;
  prog.AddLinearCost(objective_vector.dot(y - z));

nit I believe this requires symbolic parsing. It is slightly more efficient if you add this using a vector as in https://drake.mit.edu/doxygen_cxx/classdrake_1_1solvers_1_1_mathematical_program.html#ab5d909ab79bbc45767206daaa8d145c8

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 97 at r5 (raw file):

        "of a convex set!");
  }

btw is it more efficient to solve one program with 2X variables or solve a program twice with two different costs but one decision vector? My 2 cents are on the latter. Linear programs often scale super-linearly.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1130 at r5 (raw file):

      for (int j = 0; j < j_max; ++j) {
        for (double k = min_value; k < max_value;
             k += convexity_radius_step - epsilon) {

nit replace k with a more meaningful name. Let's call it lower_bound because it really is.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1139 at r5 (raw file):

          HPolyhedron chunk_bbox(A, b);
          Intersection candidate_set(*sets[j], chunk_bbox);
          if (!candidate_set.IsEmpty()) {

Following the logic, I can not see any intersection be empty, right?

---

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 1327 at r2 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

Done.

You are still passing point by value and kTol is not constant.

---

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 1460 at r5 (raw file):

  EXPECT_THROW(PartitionConvexSet(v, continuous_revolute_joints, 0.0),
               std::exception);
}

You are not testing if epsilon is working as intended. How about testing if the AABB of the intersection of partitioned sets has length epsilon along-side the specified joints.

[cohnt]

Reviewable status: 4 unresolved discussions, LGTM missing from assignee sadraddini, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 576 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit const geometry::optimization::ConvexSets&

Done.

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 557 at r5 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit replace num_positions with ambient_dimension as the function calls a convex set, not a plant. And add in the docs that the function throws if the indices do not make sense (repetitive, invalid range).

Done.

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 560 at r5 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit pass by ref const std::vector<int>&

Done.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1100 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

I now understand how the code works. Some suggestions.

I still don't like std::erase - it is a bit hard to follow. How about creating a local ConvexSets partitioned_sets_current_indexright after calling every continuous revolute joint, and then replace this line with sets = partitioned_sets_current_index.

Also, you know the exact final size of sets after you get the AABB and have epsilon. So you can pre-allocate the required memory with vector::reserve . Also put some asserts that you push that exact number of sets in every for loop and final result.

I like deleting in the loop once sets have been processed to free up the memory. (There could potentially be a large number of sets being processed, and these sets could potentially require a lot of memory to describe them.) I can switch to using vector::erase if you think that's clearer to people reading the code.

Because it's just a vector of pointers, there should be very little overhead due to the reallocation. Per the above comment, I don't think we can assert the number of sets that will be present at the end of each loop, but this is being handled in the test cases.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 75 at r5 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit const int

Done.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 89 at r5 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit I believe this requires symbolic parsing. It is slightly more efficient if you add this using a vector as in https://drake.mit.edu/doxygen_cxx/classdrake_1_1solvers_1_1_mathematical_program.html#ab5d909ab79bbc45767206daaa8d145c8

Done.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 97 at r5 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

btw is it more efficient to solve one program with 2X variables or solve a program twice with two different costs but one decision vector? My 2 cents are on the latter. Linear programs often scale super-linearly.

Good call! Switched it to solving the program twice.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1130 at r5 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit replace k with a more meaningful name. Let's call it lower_bound because it really is.

Done.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1139 at r5 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

Following the logic, I can not see any intersection be empty, right?

Imagine a really big square, rotated 45 degrees. When we partition along the axes, we'll end up with some sets that are empty.

---

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 1327 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

You are still passing point by value and kTol is not constant.

Fixed.

---

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 1460 at r5 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

You are not testing if epsilon is working as intended. How about testing if the AABB of the intersection of partitioned sets has length epsilon along-side the specified joints.

I've realized that the sets will not always intersect by epsilon, for similar reasons as to the above discussion where the partitioned pieces may not take up the whole of the bounding box they're included in. I've clarified the function documentation to note this behavior, and also added a test case to show that when the partitioned pieces do take up the whole bounding box, it's of size at least epsilon.

[sadraddini]

for math and feature. Except for a comments on the unit tests.

I now understood your point on the fact that partitioning can lead to empty sets. The math looks correct to me now.

Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 557 at r5 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

Done.

Change to @throws std::exception if each entry in continuous_revolute_joints is not non-negative, less than convex_set's ambient dimension, or unique to align with Drake-style docs.

---

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 1349 at r6 (raw file):

}

GTEST_TEST(GcsTrajectoryOptimizationTest, PartitionConvexSet) {

nit You have too many examples with minimal common assets. I suggest breaking this into PartitionConvexSet1, 2, 3 ... tests

---

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc line 1407 at r6 (raw file):

    // epsilon value.
    EXPECT_TRUE(intersection_bbox.ub()[i] - intersection_bbox.lb()[i] >=
                epsilon - (epsilon * 1e5));

what is 1e5 doing here?

[xuchenhan-tri]

Reviewable status: 6 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

bindings/pydrake/geometry/optimization_pybind.h line 42 at r7 (raw file):

      sets_out.push_back(nullptr);
    } else {
      sets_out.push_back(set->Clone());

Also, you just need a move instead of a clone here, so the name of the function is inappropriate in that sense too.

[cohnt]

Will avoid using that tag in the future!

Reviewable status: 5 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 572 at r7 (raw file):

Previously, xuchenhan-tri wrote…

nit, it may help to mention the unit of the epsilon. Does it have unit of meters or is it unitless?

Done. (It's in radians.)

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 114 at r7 (raw file):

Previously, xuchenhan-tri wrote…

nit the comment here seems to be at the wrong place?

I'm not sure how that got there! Replaced it with a comment that actually describes that function.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1116 at r7 (raw file):

Previously, xuchenhan-tri wrote…

BTW, might be a dumb question, does this need to be positive?

Yes. Added a check to assert this, and a test case to verify it behaves as intended.

---

bindings/pydrake/planning/planning_py_trajectory_optimization.cc line 475 at r7 (raw file):

Previously, xuchenhan-tri wrote…

py_rvp::take_ownership the not the right thing to do here.

See https://drakedevelopers.slack.com/archives/C3YB3EV5W/p1704311846258009?thread_ts=1704310737.927239&cid=C3YB3EV5W.

I went ahead and implemented something down the lines of what Jeremy suggested in that Slack thread.

---

bindings/pydrake/geometry/optimization_pybind.h line 34 at r7 (raw file):

Previously, xuchenhan-tri wrote…

nit, please consider a different name for this helper here.

I went ahead and removed this function, since it's no longer needed with the new python bindings.

---

bindings/pydrake/geometry/optimization_pybind.h line 42 at r7 (raw file):

Previously, xuchenhan-tri wrote…

Also, you just need a move instead of a clone here, so the name of the function is inappropriate in that sense too.

(See above)

[xuchenhan-tri]

Reviewed 5 of 6 files at r8, 1 of 1 files at r9, all commit messages.
Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

[cohnt]

Not sure why the last couple tests aren't running. But I think this PR is ready to squash and merge?

[RussTedrake]

Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 559 at r9 (raw file):

@throws std::exception if continuous_revolute_joints has repeated entries, or if
any entry is outside the interval [0, convex_set.ambient_dimension()). */
bool CheckIfSatisfiesConvexityRadius(

sorry I'm late to the party. my only question is: why are these methods in gcs_trajectory_optimization.h? That seems wrong, no? Shouldn't they be e.g. in geometry/optimization?

[wrangelvid]

Reviewed 5 of 6 files at r8, 1 of 1 files at r9, all commit messages.
Reviewable status: 3 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 111 at r9 (raw file):

/* Helper function to assert that a given list of continuous revolute joint
 * indices satisfies the requirements for the constructor to

nit: It seems like your IDE has snuck in some extra stars *

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1118 at r9 (raw file):

  const double convexity_radius_step = M_PI - epsilon;
  const int dim = convex_set.ambient_dimension();
  // TODO(cohnt): Don't calculate bounding box components for joints that aren't

Why not use GetMinimumAndMaximumValueAlongDimension to compute the minimum and maximum value?

[ggould-tri]

Huh, something is jammed up in the CI. Let me see if I can unblock that.

@drake-jenkins-bot linux-jammy-clang-bazel-experimental-everything-release please.
@drake-jenkins-bot linux-jammy-gcc-bazel-experimental-debug please.

[cohnt]

Reviewable status: 3 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

planning/trajectory_optimization/gcs_trajectory_optimization.h line 559 at r9 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

sorry I'm late to the party. my only question is: why are these methods in gcs_trajectory_optimization.h? That seems wrong, no? Shouldn't they be e.g. in geometry/optimization?

We only use notions of geodesic convexity in the context of GcsTrajOpt, so I didn't want to gum up geometry/optimization with code that's only ever used in this spot.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 111 at r9 (raw file):

Previously, wrangelvid (David von Wrangel) wrote…

nit: It seems like your IDE has snuck in some extra stars *

Fixed.

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1118 at r9 (raw file):

Previously, wrangelvid (David von Wrangel) wrote…

Why not use GetMinimumAndMaximumValueAlongDimension to compute the minimum and maximum value?

Switched to use that function. This also allows us to make the function slightly less restrictive -- sets just have to be bounded along dimensions corresponding to continuous revolute joints. I've updated the documentation to reflect this, and added a test to check the behavior.

[sadraddini]

Reviewable status: 3 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

planning/trajectory_optimization/gcs_trajectory_optimization.cc line 1118 at r9 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

Switched to use that function. This also allows us to make the function slightly less restrictive -- sets just have to be bounded along dimensions corresponding to continuous revolute joints. I've updated the documentation to reflect this, and added a test to check the behavior.

The minor disadvantage of calling GetMinimumAndMaximumValueAlongDimension multiple times is that it sets up the mathematical program every time from scratch. That's what MaybeCalcAxisAlignedBoundingBox avoids but many of dimensions checks are not needed. The fastest solution is having a version of GetMinimumAndMaximumValueAlongDimension that takes a list of dimensions and uses one mathematical program.

You can do it later, not necessary for this PR.

[wrangelvid]

Reviewed 3 of 3 files at r10, all commit messages.
Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

[cohnt]

Per a conversation with @RussTedrake, I've moved the new functions from GcsTrajectoryOptimization to a new file geodesic_convexity.h, within geometry/optimization.

Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

[xuchenhan-tri]

Reviewed 12 of 12 files at r11, all commit messages.
Reviewable status: 8 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

geometry/optimization/geodesic_convexity.h line 21 at r11 (raw file):

These functions are primarily used by GcsTrajectoryOptimization to make motion
plans for these types of robots. */

I don't think this doc is rendered the way you intended. The documentation will be rendered as part of the doc for GetMinimumAndMaximumValueAlongDimension below.

Code quote:

/** A robot that has revolute joints without any limits has an inherently
non-Euclidean configuration space, but one can still consider
"geodesically-convex" sets, akin to convex sets in Euclidean space. In
practice, this only requires that the width of the set along each dimension
corresponding to an unbounded revolute joint be strictly less than π.

These functions are primarily used by GcsTrajectoryOptimization to make motion
plans for these types of robots. */

---

geometry/optimization/geodesic_convexity.h line 25 at r11 (raw file):

/** Computes the minimum and maximum values that can be attained along a certain
dimension for a point constrained to lie within a convex set. */
const std::pair<double, double> GetMinimumAndMaximumValueAlongDimension(

nit this const doesn't achieve anything.

See https://drake.mit.edu/styleguide/cppguide.html#Use_of_const

Suggestion:

std::pair<double, double>

---

geometry/optimization/geodesic_convexity.h line 26 at r11 (raw file):

dimension for a point constrained to lie within a convex set. */
const std::pair<double, double> GetMinimumAndMaximumValueAlongDimension(
    const ConvexSet& region, const int dimension);

nit this const doesn't achieve anything.

See https://drake.mit.edu/styleguide/cppguide.html#Use_of_const

Suggestion:

    const ConvexSet& region, int dimension);

---

geometry/optimization/geodesic_convexity.h line 32 at r11 (raw file):

GcsTrajectoryOptimization, as well as any static functions that may take in
such a list. */
void ThrowsForInvalidContinuousJointsList(

Is this function (and the one above) useful for users or are they strictly for internal uses? If the latter, consider moving them into internal namespace.

---

geometry/optimization/geodesic_convexity.h line 36 at r11 (raw file):

/** Given a convex set, and a list of indices corresponding to continuous
revolute joints, check whether or not the set satisfies the convexity radius.

nit

Suggestion:

revolute joints, checks whether or not the set satisfies the convexity radius.

---

geometry/optimization/geodesic_convexity.cc line 27 at r11 (raw file):

  // a member of ConvexSet. Also potentially only compute the value once, and
  // then return the pre-computed value instead of re-computing.
  DRAKE_THROW_UNLESS(dimension >= 0 && dimension < region.ambient_dimension());

nit this throw condition needs to be documented.

---

geometry/optimization/test/geodesic_convexity_test.cc line 31 at r11 (raw file):

  // Test the function overload that takes in multiple convex sets.
  ConvexSets sets5 = PartitionConvexSet(MakeConvexSets(v, v),
                                        continuous_revolute_joints, epsilon);

nit, odd variable name (likely due to test shuffling).

BTW you can reuse variable names across different GTESTs, it may be a good idea to kill all the trailing numbers in your variable names to avoid confusions like this.

Suggestion:

  // Test the function overload that takes in multiple convex sets.
  ConvexSets sets = PartitionConvexSet(MakeConvexSets(v, v),
                                        continuous_revolute_joints, epsilon);

[cohnt]

Reviewable status: 3 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

geometry/optimization/geodesic_convexity.h line 21 at r11 (raw file):

Previously, xuchenhan-tri wrote…

I don't think this doc is rendered the way you intended. The documentation will be rendered as part of the doc for GetMinimumAndMaximumValueAlongDimension below.

I just wanted to include something at the top of the file for a developer who might come across it -- I don't need it to render in the documentation at all. Will this work?

---

geometry/optimization/geodesic_convexity.h line 32 at r11 (raw file):

Previously, xuchenhan-tri wrote…

Is this function (and the one above) useful for users or are they strictly for internal uses? If the latter, consider moving them into internal namespace.

Done.

---

geometry/optimization/geodesic_convexity.cc line 27 at r11 (raw file):

Previously, xuchenhan-tri wrote…

nit this throw condition needs to be documented.

Done.

---

geometry/optimization/test/geodesic_convexity_test.cc line 31 at r11 (raw file):

Previously, xuchenhan-tri wrote…

nit, odd variable name (likely due to test shuffling).

BTW you can reuse variable names across different GTESTs, it may be a good idea to kill all the trailing numbers in your variable names to avoid confusions like this.

Fixed throughout.

[xuchenhan-tri]

Reviewed 5 of 5 files at r12, all commit messages.
Reviewable status: 4 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

geometry/optimization/geodesic_convexity.h line 21 at r11 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

I just wanted to include something at the top of the file for a developer who might come across it -- I don't need it to render in the documentation at all. Will this work?

Yes, that will work. Anything inside internal namespace is not rendered in doxygen.

If you do want it to render as a file level doxygen, you can use a @file. See for example: https://github.com/RobotLocomotion/drake/blob/master/math/autodiff_gradient.h#L1 and the resulting doxygen https://drake.mit.edu/doxygen_cxx/autodiff__gradient_8h.html.

---

geometry/optimization/geodesic_convexity.h line 26 at r12 (raw file):

dimension for a point constrained to lie within a convex set.
@throws std::exception if dimension is outside the interval
[0, region.ambient_dimension()). */

nit, since internal space doc doesn't get rendered in doxygen, we usually use /* */ instead of /** */.

Suggestion:

/* Computes the minimum and maximum values that can be attained along a certain
dimension for a point constrained to lie within a convex set.
@throws std::exception if dimension is outside the interval
[0, region.ambient_dimension()). */

---

geometry/optimization/geodesic_convexity.h line 33 at r12 (raw file):

indices satisfies the requirements for the constructor to
GcsTrajectoryOptimization, as well as any static functions that may take in
such a list. */

nit

Suggestion:

/* Helper function to assert that a given list of continuous revolute joint
indices satisfies the requirements for the constructor to
GcsTrajectoryOptimization, as well as any static functions that may take in
such a list. */

---

bindings/pydrake/geometry/geometry_py_optimization.cc line 1084 at r12 (raw file):

  using drake::geometry::optimization::ConvexSet;
  m.def("CheckIfSatisfiesConvexityRadius", &CheckIfSatisfiesConvexityRadius,

nit, this binding is not tested.

[cohnt]

Reviewable status: 2 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

geometry/optimization/geodesic_convexity.h line 21 at r11 (raw file):

Previously, xuchenhan-tri wrote…

Yes, that will work. Anything inside internal namespace is not rendered in doxygen.

If you do want it to render as a file level doxygen, you can use a @file. See for example: https://github.com/RobotLocomotion/drake/blob/master/math/autodiff_gradient.h#L1 and the resulting doxygen https://drake.mit.edu/doxygen_cxx/autodiff__gradient_8h.html.

I'm actually a fan of using @file. Changed to use that. Thanks!

---

geometry/optimization/geodesic_convexity.h line 26 at r12 (raw file):

Previously, xuchenhan-tri wrote…

nit, since internal space doc doesn't get rendered in doxygen, we usually use /* */ instead of /** */.

Done.

---

geometry/optimization/geodesic_convexity.h line 33 at r12 (raw file):

Previously, xuchenhan-tri wrote…

nit

Done.

---

bindings/pydrake/geometry/geometry_py_optimization.cc line 1084 at r12 (raw file):

Previously, xuchenhan-tri wrote…

nit, this binding is not tested.

Added a test.

[xuchenhan-tri]

Reviewed 2 of 2 files at r13, all commit messages.
Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

[xuchenhan-tri]

Reviewed 1 of 1 files at r14, all commit messages.
Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

[xuchenhan-tri]

+(status: squashing now)