Adds a benchmark for IrisInConfigurationSpace

[RussTedrake]

This benchmark plants a flag before we start optimizing the IrisInConfigurationSpace runtimes.

+@wrangelvid for feature review (especially confirmation of the iris workflow).

---

This change is 

[wrangelvid]

Reviewed 4 of 4 files at r1, all commit messages.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee wrangelvid, needs platform reviewer assigned, needs at least two assigned reviewers

---

geometry/optimization/benchmarking/iris_in_configuration_space_benchmarks.cc line 209 at r1 (raw file):

    seeds_.emplace_back("Middle Rack", MyInverseKinematics(RigidTransformd(
                                           RollPitchYawd(0, -M_PI, -M_PI / 2),
                                           Vector3d(0.75, 0, 0.41))));

We changed the names of the seeds in the soon to be merged prm_comparison branch to stay consistent with the paper.

---

geometry/optimization/benchmarking/README.md line 8 at r1 (raw file):

## IRIS in Configuration Space

  $ bazel run //geometry/optimization/benchmarking:iris_in_configuration_space_experiment -- --output_dir=foo

nit: Can we wrap this in a bash code block?

Code quote:

  $ bazel run //geometry/optimization/benchmarking:iris_in_configuration_space_experiment -- --output_dir=foo

---

geometry/optimization/benchmarking/README.md line 12 at r1 (raw file):

This benchmark is provided to help understand the implications of changes the
will impact the performance of the IrisInConfigurationSpace algorithm. It
should grow to include a number of our most important/relevant examples.

I accidentally tried to run this locally without snopt enabled and got stuck with IPOPT for over half an hour. It would help to mention a preferred solver/configurations with some estimated time. Printing a warning to the console could be beneficial.

[wrangelvid]

The workflow makes sense. I like that the previous regions get added as c-space obstacles. We haven't done it in the gcs repository, but it makes a lot of sense here.

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

[RussTedrake]

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

---

geometry/optimization/benchmarking/iris_in_configuration_space_benchmarks.cc line 209 at r1 (raw file):

Previously, wrangelvid (David von Wrangel) wrote…

We changed the names of the seeds in the soon to be merged prm_comparison branch to stay consistent with the paper.

Oh. I'd rather leave them in long form, rather than the two-letter acronyms.

---

geometry/optimization/benchmarking/README.md line 8 at r1 (raw file):

Previously, wrangelvid (David von Wrangel) wrote…

nit: Can we wrap this in a bash code block?

Done. I decided to actually exit when SNOPT is not present, because the tests even timeout on CI with IPOPT.

---

geometry/optimization/benchmarking/README.md line 12 at r1 (raw file):

Previously, wrangelvid (David von Wrangel) wrote…

I accidentally tried to run this locally without snopt enabled and got stuck with IPOPT for over half an hour. It would help to mention a preferred solver/configurations with some estimated time. Printing a warning to the console could be beneficial.

Done.

[wrangelvid]

Reviewed 2 of 2 files at r2, all commit messages.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers

---

geometry/optimization/benchmarking/README.md line 8 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

Done. I decided to actually exit when SNOPT is not present, because the tests even timeout on CI with IPOPT.

Even better!

[jwnimmer-tri]

Reviewed all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform)

a discussion (no related file):
By convention, benchmarking is always a second-level directory, not third level:

drake/tools/performance/README.md

Lines 6 to 10 in 0fe0901

	Fully worked examples that integrate this tooling are available at:
	- drake/geometry/benchmarking
	- drake/multibody/benchmarking
	- drake/solvers/benchmarking
	- drake/systems/benchmarking

Please move the new program into geometry/benchmarking.

---

[RussTedrake]

+@jwnimmer-tri for platform review, please.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform)

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

By convention, benchmarking is always a second-level directory, not third level:

drake/tools/performance/README.md

Lines 6 to 10 in 0fe0901

	Fully worked examples that integrate this tooling are available at:
	- drake/geometry/benchmarking
	- drake/multibody/benchmarking
	- drake/solvers/benchmarking
	- drake/systems/benchmarking

Please move the new program into geometry/benchmarking.

Done.

---

[jwnimmer-tri]

Reviewed 1 of 4 files at r1, 5 of 5 files at r3, all commit messages.
Reviewable status: 8 unresolved discussions

a discussion (no related file):
Test case inputs should be workspace externals, not PackageMap::AddRemote calls.

A workspace external provides for appropriate caching and S3 backups in case the resource disappears. The remote PackageMap does neither (when used in a unit test).

---

---

geometry/benchmarking/README.md line 35 at r3 (raw file):

This benchmark is provided to help understand the implications of changes the

typo

Suggestion:

that

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 15 at r3 (raw file):

#include "drake/tools/performance/fixture_common.h"

/* A collection of scenarios to benchmark. */

BTW This overview comment does not seem very useful.

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 103 at r3 (raw file):

      rotation: !Rpy { deg: [90.0, 0.0, 0.0 ]}

# Add Bins

typo

Suggestion:

# Add bins

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 165 at r3 (raw file):

    X_PC:
      translation: [0.4, 0.0, 0.0]
      rotation: !Rpy { deg: [0., 0., 00]}

nit Missing the final 0 for floating point literals (0.0).

(Alternatively, since there is no rotation anyway, the entire line could be deleted.)

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 216 at r3 (raw file):

  void GenerateAllRegions() {
    // Because we use each computed region as an obstacle for the next, we need
    // to run them all in a single benchmark.

In the typical case that googlebenchmark runs the loop

  for (auto _ : state) {
    GenerateAllRegions();
  }

more than once, this code will crash because of the iris_options packrat:

[2023-07-16 18:48:30.843] [console] [info] Computing region for seed: Home Position
[2023-07-16 18:48:30.913] [console] [info] IrisInConfigurationSpace iteration 0
[2023-07-16 18:48:35.612] [console] [info] Computing region for seed: Left Bin
[2023-07-16 18:48:35.689] [console] [info] IrisInConfigurationSpace iteration 0
[2023-07-16 18:48:39.402] [console] [info] Computing region for seed: Home Position
terminate called after throwing an instance of 'std::runtime_error'
  what():  The seed point is in configuration obstacle 0

I think we need to do iris_options_.configuration_obstacles.clear() at the top of this function, yes?

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 228 at r3 (raw file):

          IrisInConfigurationSpace(*plant_, plant_context, iris_options_);
      iris_options_.configuration_obstacles.emplace_back(hpoly.Scale(0.95));
      iris_regions.emplace(name, std::move(hpoly));

It's not clear why we are keeping around an unused collection of iris_regions.

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 255 at r3 (raw file):

}  // namespace drake

int main(int argc, char** argv) {

We should choose either to use this main function or the //tools/performance:gflags_main main function. (It's too confusing to define two main functions for the same program and let the linker choose which one wins.)

Fixed in RussTedrake#52.

[RussTedrake]

Reviewable status: 8 unresolved discussions

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Test case inputs should be workspace externals, not PackageMap::AddRemote calls.

A workspace external provides for appropriate caching and S3 backups in case the resource disappears. The remote PackageMap does neither (when used in a unit test).

The PackageMap::AddRemote is not use in the unit test (based on the --test argument). Should i still make it a remote? (I'd more likely just pull over the two or three assets I'm using).

---

[jwnimmer-tri]

Reviewable status: 9 unresolved discussions

a discussion (no related file):

The PackageMap::AddRemote is not use in the unit test (based on the --test argument).

Yes, and that's a defect. I'll post a discussion below that explains more.

---

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 134 at r3 (raw file):

      multibody::PackageMap::RemoteParams params;
      params.urls.push_back(
          "https://github.com/mpetersen94/gcs/archive/refs/tags/"

If we decide to move gcs to be an external instead of a PackageMap entry, then the following is moot.

If we keep it as PackageMap, then read on:

---

The purpose of the CI test case is to guarantee that when a developer locally runs the full benchmark suite, it will produce a result (without crashing).

Here, the --test flag causes us to skip the download of a URL that's outside of our control. Of all the things in this code that might bitrot, a third-party url and sha256sum are one of the most likely.

Therefore, we need to change the --test recipe somehow so that these models are actually downloaded in CI. One way to do that would be to hoist the AddRemote call to be unconditional, and then in the --test branch call package_map.GetPath("gcs") to force the download.

[jwnimmer-tri]

Reviewable status: 9 unresolved discussions

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The PackageMap::AddRemote is not use in the unit test (based on the --test argument).

Yes, and that's a defect. I'll post a discussion below that explains more.

The problem is that PackageMap.AddRemote does not use caching (so is slow and wasteful) and more importantly does not provide for any mirroring / backup for when Mark deletes his repository.

We need Drake's benchmarks to be reliably accessible and repeatable for all Drake developers. Having all of the dependencies be mirrored is a necessary condition for that.

---

[jwnimmer-tri]

Reviewable status: 9 unresolved discussions

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The problem is that PackageMap.AddRemote does not use caching (so is slow and wasteful) and more importantly does not provide for any mirroring / backup for when Mark deletes his repository.

We need Drake's benchmarks to be reliably accessible and repeatable for all Drake developers. Having all of the dependencies be mirrored is a necessary condition for that.

A few more tips -- you have latitude for what to call the new external. My best guess would be mpetersen94_gcs_internal but other things would work too.

Another solution would be to copy the models into RobotLocomotion/models/... instead of adding a new external.

---

[jwnimmer-tri]

Reviewable status: 10 unresolved discussions

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 41 at r3 (raw file):

  IiwaWithShelvesAndBins() { tools::performance::AddMinMaxStatistics(this); }

  // This apparently futile using statement works around "overloaded virtual"

nit The other PR has merged, so this "apparently futile" text and using statement should be removed (no longer necessary).

[RussTedrake]

Reviewable status: 3 unresolved discussions

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

A few more tips -- you have latitude for what to call the new external. My best guess would be mpetersen94_gcs_internal but other things would work too.

Another solution would be to copy the models into RobotLocomotion/models/... instead of adding a new external.

Done. I've added the models to RobotLocomotion/models. (they were small modifications from assets that originated in manipulation_station, so I've left them connected to that example).

---

---

geometry/benchmarking/README.md line 35 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

typo

Done.

---

tools/workspace/drake_models/repository.bzl line 10 at r6 (raw file):

        repository = "RussTedrake/models",
        commit = "4b16cd5227dd1d568de467703b6c659e38886ee3",
        sha256 = "c2af53e43825e5a2ea19368e380f6a1295cb9db3e2c093bf095e77d07e09838f",  # noqa

Working temporary SHA1 until the models PR is merged

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 15 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW This overview comment does not seem very useful.

Done.

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 41 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit The other PR has merged, so this "apparently futile" text and using statement should be removed (no longer necessary).

Done.

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 103 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

typo

Done.

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 134 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

If we decide to move gcs to be an external instead of a PackageMap entry, then the following is moot.

If we keep it as PackageMap, then read on:

---

The purpose of the CI test case is to guarantee that when a developer locally runs the full benchmark suite, it will produce a result (without crashing).

Here, the --test flag causes us to skip the download of a URL that's outside of our control. Of all the things in this code that might bitrot, a third-party url and sha256sum are one of the most likely.

Therefore, we need to change the --test recipe somehow so that these models are actually downloaded in CI. One way to do that would be to hoist the AddRemote call to be unconditional, and then in the --test branch call package_map.GetPath("gcs") to force the download.

Done. N/A. (the assets are now in RobotLocomotion/models)

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 165 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Missing the final 0 for floating point literals (0.0).

(Alternatively, since there is no rotation anyway, the entire line could be deleted.)

Done.

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 216 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

In the typical case that googlebenchmark runs the loop

  for (auto _ : state) {
    GenerateAllRegions();
  }

more than once, this code will crash because of the iris_options packrat:

[2023-07-16 18:48:30.843] [console] [info] Computing region for seed: Home Position
[2023-07-16 18:48:30.913] [console] [info] IrisInConfigurationSpace iteration 0
[2023-07-16 18:48:35.612] [console] [info] Computing region for seed: Left Bin
[2023-07-16 18:48:35.689] [console] [info] IrisInConfigurationSpace iteration 0
[2023-07-16 18:48:39.402] [console] [info] Computing region for seed: Home Position
terminate called after throwing an instance of 'std::runtime_error'
  what():  The seed point is in configuration obstacle 0

I think we need to do iris_options_.configuration_obstacles.clear() at the top of this function, yes?

Done. Good catch.

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 228 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

It's not clear why we are keeping around an unused collection of iris_regions.

Done. Good call.

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 255 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

We should choose either to use this main function or the //tools/performance:gflags_main main function. (It's too confusing to define two main functions for the same program and let the linker choose which one wins.)

Fixed in RussTedrake#52.

Done.

[jwnimmer-tri]

I am happy with the benchmark code now. It probably makes sense to recruit someone else to finish the review of the models changes, as those pieces percolate.

Reviewed 7 of 13 files at r6, all commit messages.
Reviewable status: 1 unresolved discussion

[RussTedrake]

Thanks! +@xuchenhan-tri for final review, please.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee xuchenhan-tri(platform)

[xuchenhan-tri]

Reviewed 2 of 5 files at r3, 8 of 13 files at r6, 2 of 3 files at r7, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee xuchenhan-tri(platform)

[RussTedrake]

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 228 at r7 (raw file):

  for (auto _ : state) {
    GenerateAllRegions();
  }

for some reason, this now runs Setup and GenerateAllRegions twice, but says Iterations = 1.
🤔

[xuchenhan-tri]

Platform on the model changes.

Reviewed 3 of 13 files at r6.
Reviewable status: 8 unresolved discussions

---

examples/manipulation_station/models/bin2.sdf line 1 at r8 (raw file):

<?xml version="1.0"?>

BTW, the collision geometry seems to be a tad bigger than visual geometry. Is that intended? Perhaps it's worth a note somewhere.

---

examples/manipulation_station/models/bin2.sdf line 6 at r8 (raw file):

    <!--
    Axes:
      +X - Pointing towards front (slope)

nit consider

Suggestion:

+X - Pointing towards front

---

examples/manipulation_station/models/bin2.sdf line 41 at r8 (raw file):

      </collision>
      <!--
      <collision name="slope">

nit consider removing.

Code quote:

 <collision name="slope">

---

examples/manipulation_station/models/shelves.sdf line 55 at r7 (raw file):

      </collision>
    </link>
    <!-- joint between bottom_top and world -->

nit, this comment seems to contradict the actual semantic of the link.

---

examples/manipulation_station/models/shelves.sdf line 1 at r8 (raw file):

<?xml version="1.0"?>

BTW, the back and the bottom of the shelf doesn't have collision geometry so one can "punch through" the shelf with no resistance. I want to call that out and make sure that's intended.

---

examples/manipulation_station/models/table_wide.sdf line 16 at r8 (raw file):

        <geometry>
          <box>
            <size>2.5 2.5 0.2</size>

BTW, the collision geometry is much wider/longer than the visual. I want to call that out and make sure that's intended.

[RussTedrake]

Thanks for checking so carefully. I migrated the assets from the gcs repo, but should have scrutinized them more carefully!

Reviewable status: complete! all discussions resolved, LGTM from assignees jwnimmer-tri(platform),wrangelvid,xuchenhan-tri(platform)

---

tools/workspace/drake_models/repository.bzl line 10 at r6 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

Working temporary SHA1 until the models PR is merged

Done. I've updated the SHA.

---

examples/manipulation_station/models/bin2.sdf line 1 at r8 (raw file):

Previously, xuchenhan-tri wrote…

BTW, the collision geometry seems to be a tad bigger than visual geometry. Is that intended? Perhaps it's worth a note somewhere.

Done. I've gone ahead and tightened up the collision geometry to match better. I think that should be fine. (SNOPT will find a way).

---

examples/manipulation_station/models/bin2.sdf line 6 at r8 (raw file):

Previously, xuchenhan-tri wrote…

nit consider

Done.

---

examples/manipulation_station/models/bin2.sdf line 41 at r8 (raw file):

Previously, xuchenhan-tri wrote…

nit consider removing.

Done.

---

examples/manipulation_station/models/shelves.sdf line 55 at r7 (raw file):

Previously, xuchenhan-tri wrote…

nit, this comment seems to contradict the actual semantic of the link.

Done.

---

examples/manipulation_station/models/shelves.sdf line 1 at r8 (raw file):

Previously, xuchenhan-tri wrote…

BTW, the back and the bottom of the shelf doesn't have collision geometry so one can "punch through" the shelf with no resistance. I want to call that out and make sure that's intended.

Done. I've gone ahead and added them. You're right that this is better. Turns out the back was sort of in the front, so I've adjusted that, too.

---

examples/manipulation_station/models/table_wide.sdf line 16 at r8 (raw file):

Previously, xuchenhan-tri wrote…

BTW, the collision geometry is much wider/longer than the visual. I want to call that out and make sure that's intended.

Done.

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 228 at r7 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

for some reason, this now runs Setup and GenerateAllRegions twice, but says Iterations = 1.
🤔

I haven't been able to fix this, so I guess I will just let it go for now. It's good motivation to (dramatically) improve the performance.

[RussTedrake]

examples/manipulation_station/models/shelves.sdf line 61 at r9 (raw file):

        </geometry>
      </collision>
      <collision name="back">

Blocking. One of my seed points is now in collision. Checking...

[RussTedrake]

The improved collision geomtery definitely makes iris perform more slowly (requiring more iterations to converge). But I think it's reasonable to capture that.

Reviewable status: complete! all discussions resolved, LGTM from assignees jwnimmer-tri(platform),wrangelvid,xuchenhan-tri(platform)

---

examples/manipulation_station/models/shelves.sdf line 61 at r9 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

Blocking. One of my seed points is now in collision. Checking...

Done. Resolved. (I had tricked myself with a cached model)

[RussTedrake]

Reviewable status: 1 unresolved discussion

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 246 at r10 (raw file):

  benchmark::RunSpecifiedBenchmarks();
  return 0;
}

btw -- this is the ONLY way I can find to get the test to run only once. And it's so painfully slow to run the entire benchmark that it's imperative that it runs only once.

I've added a TODO to resolve it (when our gbench experts get back from travels).

[xuchenhan-tri]

with one last comment on the benchmark iterations.

Reviewed 4 of 4 files at r9, 1 of 1 files at r10, all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees jwnimmer-tri(platform),wrangelvid,xuchenhan-tri(platform)

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 246 at r10 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

btw -- this is the ONLY way I can find to get the test to run only once. And it's so painfully slow to run the entire benchmark that it's imperative that it runs only once.

I've added a TODO to resolve it (when our gbench experts get back from travels).

BTW It looks like the top voted answer in https://stackoverflow.com/questions/61843343/how-to-special-case-the-number-of-iterations-in-google-benchmark used to work for you. Does it not work anymore? I tried it and it seems to work.

[xuchenhan-tri]

Reviewable status: 1 unresolved discussion

---

geometry/benchmarking/README.md line 32 at r10 (raw file):

$ bazel run //geometry/benchmarking:iris_in_configuration_space_experiment -- --output_dir=foo

BTW, should we mention something about running this with SNOPT?

[RussTedrake]

Reviewable status: complete! all discussions resolved, LGTM from assignees jwnimmer-tri(platform),wrangelvid,xuchenhan-tri(platform)

---

geometry/benchmarking/README.md line 32 at r10 (raw file):

Previously, xuchenhan-tri wrote…

BTW, should we mention something about running this with SNOPT?

Done.

---

geometry/benchmarking/iris_in_configuration_space_benchmarks.cc line 246 at r10 (raw file):

Previously, xuchenhan-tri wrote…

BTW It looks like the top voted answer in https://stackoverflow.com/questions/61843343/how-to-special-case-the-number-of-iterations-in-google-benchmark used to work for you. Does it not work anymore? I tried it and it seems to work.

Thank you for trying! I had of course tried that... (and did it once more now for good measure) but it doesn't work. The iteration report at the end claims to have run it once, but if you look at the console output you can see it runs Home position, Left bin, then Home position, left bin again. (more logging would reveal it runs the setup twice, too.

[jwnimmer-tri]

Reminder for release notes: we also need to mention the RobotLocomotion/models git sha upgrade in the release notes.