From 39741f1de94cb2c3095edc5f9684f257d65b6297 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Tue, 4 Jul 2023 21:21:36 -0400 Subject: [PATCH 1/4] Adds a benchmark for IrisInConfigurationSpace This benchmark plants a flag before we start optimizing the IrisInConfigurationSpace runtimes. --- geometry/benchmarking/BUILD.bazel | 27 ++ geometry/benchmarking/README.md | 18 +- .../iris_in_configuration_space_benchmarks.cc | 268 ++++++++++++++++++ tools/performance/BUILD.bazel | 1 + 4 files changed, 312 insertions(+), 2 deletions(-) create mode 100644 geometry/benchmarking/iris_in_configuration_space_benchmarks.cc diff --git a/geometry/benchmarking/BUILD.bazel b/geometry/benchmarking/BUILD.bazel index 2286b0b4f7de..2098b15b4563 100644 --- a/geometry/benchmarking/BUILD.bazel +++ b/geometry/benchmarking/BUILD.bazel @@ -9,6 +9,33 @@ load("//tools/skylark:test_tags.bzl", "vtk_test_tags") package(default_visibility = ["//visibility:private"]) +drake_cc_googlebench_binary( + name = "iris_in_configuration_space_benchmarks", + srcs = ["iris_in_configuration_space_benchmarks.cc"], + add_test_rule = True, + data = [ + "//examples/manipulation_station:models", + "//manipulation/models/iiwa_description:models", + "//manipulation/models/wsg_50_description:models", + ], + test_args = [ + "--test", + ], + test_timeout = "moderate", + deps = [ + "//geometry/optimization:iris", + "//multibody/inverse_kinematics", + "//multibody/plant", + "//tools/performance:fixture_common", + "//tools/performance:gflags_main", + ], +) + +drake_py_experiment_binary( + name = "iris_in_configuration_space_experiment", + googlebench_binary = ":iris_in_configuration_space_benchmarks", +) + drake_cc_googlebench_binary( name = "mesh_intersection_benchmark", srcs = ["mesh_intersection_benchmark.cc"], diff --git a/geometry/benchmarking/README.md b/geometry/benchmarking/README.md index 514f5a2c9427..7fba1c4bd689 100644 --- a/geometry/benchmarking/README.md +++ b/geometry/benchmarking/README.md @@ -5,7 +5,9 @@ Runtime Performance Benchmarks for Geometry Operations ## render - $ bazel run //geometry/benchmarking:render_experiment -- --output_dir=foo +``` +$ bazel run //geometry/benchmarking:render_experiment -- --output_dir=foo +``` Benchmark program to help characterize the relative costs of different RenderEngine implementations with varying scene complexity and rendering. It is @@ -15,13 +17,25 @@ renderer choice. ## mesh_intersection - $ bazel run //geometry/benchmarking:mesh_intersection_experiment -- --output_dir=foo +``` +$ bazel run //geometry/benchmarking:mesh_intersection_experiment -- --output_dir=foo +``` Benchmark program to evaluate bounding volume hierarchy impact on mesh-mesh intersections across varying mesh attributes and overlaps. It is targeted toward developers during the process of optimizing the performance of hydroelastic contact and may be removed once sufficient work has been done in that effort. +## iris_in_configuration_space + +``` +$ bazel run //geometry/optimization/benchmarking:iris_in_configuration_space_experiment -- --output_dir=foo +``` + +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. + # Additional information Documentation for command line arguments is here: diff --git a/geometry/benchmarking/iris_in_configuration_space_benchmarks.cc b/geometry/benchmarking/iris_in_configuration_space_benchmarks.cc new file mode 100644 index 000000000000..a1c315811d3c --- /dev/null +++ b/geometry/benchmarking/iris_in_configuration_space_benchmarks.cc @@ -0,0 +1,268 @@ +#include +#include + +#include "drake/common/text_logging.h" +#include "drake/geometry/optimization/iris.h" +#include "drake/math/rigid_transform.h" +#include "drake/multibody/inverse_kinematics/inverse_kinematics.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/solvers/snopt_solver.h" +#include "drake/solvers/solve.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/tools/performance/fixture_common.h" + +/* A collection of scenarios to benchmark. */ + +namespace drake { +namespace geometry { +namespace optimization { +namespace { + +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; +using math::RigidTransformd; +using math::RollPitchYawd; +using math::RotationMatrixd; +using multibody::MultibodyPlant; +using systems::Context; + +DEFINE_bool(test, false, "Enable unit test mode."); + +// This IIWA + Shelves + Bins fixture is one of the primary examples from +// "Motion Planning around Obstacles with Convex Optimization" by Tobia +// Marcucci, Mark Petersen, David von Wrangel, Russ Tedrake. +// https://arxiv.org/abs/2205.04422 +class IiwaWithShelvesAndBins : public benchmark::Fixture { + public: + IiwaWithShelvesAndBins() { tools::performance::AddMinMaxStatistics(this); } + + // This apparently futile using statement works around "overloaded virtual" + // errors in g++. All of this is a consequence of the weird deprecation of + // const-ref State versions of SetUp() and TearDown() in benchmark.h. + using benchmark::Fixture::SetUp; + void SetUp(benchmark::State&) override { + // Configure IRIS. + if (FLAGS_test) { + iris_options_.iteration_limit = 1; + iris_options_.num_collision_infeasible_samples = 1; + } else { + iris_options_.iteration_limit = 10; + iris_options_.num_collision_infeasible_samples = 5; + } + iris_options_.require_sample_point_is_contained = true; + iris_options_.relative_termination_threshold = 0.01; + iris_options_.termination_threshold = -1; + + systems::DiagramBuilder builder; + plant_ = &multibody::AddMultibodyPlantSceneGraph(&builder, 0.0).plant; + LoadRobot(); + plant_->Finalize(); + diagram_ = builder.Build(); + diagram_context_ = diagram_->CreateDefaultContext(); + + GenerateSeeds(); + } + + void LoadRobot() { + multibody::Parser parser(plant_); + + std::string model_directives = R"""( +directives: + +# Add iiwa +- add_model: + name: iiwa + file: package://drake/manipulation/models/iiwa_description/urdf/iiwa14_primitive_collision.urdf + default_joint_positions: + iiwa_joint_1: [0] + iiwa_joint_2: [0.3] + iiwa_joint_3: [0] + iiwa_joint_4: [-1.8] + iiwa_joint_5: [0] + iiwa_joint_6: [1] + iiwa_joint_7: [1.57] + +- add_weld: + parent: world + child: iiwa::base + +# Add schunk +- add_model: + name: wsg + file: package://drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_welded_fingers.sdf + +- add_weld: + parent: iiwa::iiwa_link_7 + child: wsg::body + X_PC: + translation: [0, 0, 0.114] + rotation: !Rpy { deg: [90.0, 0.0, 0.0 ]} + +# Add Bins +- add_model: + name: binR + file: package://drake/examples/manipulation_station/models/bin.sdf + +- add_weld: + parent: world + child: binR::bin_base + X_PC: + translation: [0, -0.6, 0] + rotation: !Rpy { deg: [0.0, 0.0, 90.0 ]} + +- add_model: + name: binL + file: package://drake/examples/manipulation_station/models/bin.sdf + +- add_weld: + parent: world + child: binL::bin_base + X_PC: + translation: [0, 0.6, 0] + rotation: !Rpy { deg: [0.0, 0.0, 90.0 ]} +)"""; + + parser.AddModelsFromString(model_directives, ".dmd.yaml"); + + // Don't add remote resources if we're in test mode. + if (!FLAGS_test) { + // We'll use some tables, shelves, and bins from a remote resource. + multibody::PackageMap::RemoteParams params; + params.urls.push_back( + "https://github.com/mpetersen94/gcs/archive/refs/tags/" + "arxiv_paper_version.tar.gz"); + params.sha256 = + "6dd5e841c8228561b6d622f592359c36517cd3c3d5e1d3e04df74b2f5435680c"; + params.strip_prefix = "gcs-arxiv_paper_version"; + parser.package_map().AddRemote("gcs", params); + + model_directives = R"""( +directives: + +# Add shelves +- add_model: + name: shelves + file: package://gcs/models/shelves/shelves.sdf + +- add_weld: + parent: world + child: shelves::shelves_body + X_PC: + translation: [0.85, 0, 0.4] + +# Add table +- add_model: + name: table + file: package://gcs/models/table/table_wide.sdf + +- add_weld: + parent: world + child: table::table_body + X_PC: + translation: [0.4, 0.0, 0.0] + rotation: !Rpy { deg: [0., 0., 00]} +)"""; + + parser.AddModelsFromString(model_directives, ".dmd.yaml"); + } + } + + VectorXd MyInverseKinematics(const RigidTransformd& X_WE) { + const auto& E = plant_->GetBodyByName("body").body_frame(); + multibody::InverseKinematics ik(*plant_); + ik.AddPositionConstraint(E, Vector3d::Zero(), plant_->world_frame(), + X_WE.translation(), X_WE.translation()); + ik.AddOrientationConstraint(E, RotationMatrixd(), plant_->world_frame(), + X_WE.rotation(), 0.001); + + auto* prog = ik.get_mutable_prog(); + const auto& q = ik.q(); + VectorXd q0 = plant_->GetPositions(ik.context()); + prog->AddQuadraticErrorCost(MatrixXd::Identity(q.size(), q.size()), q0, q); + prog->SetInitialGuess(q, q0); + auto result = solvers::Solve(*prog); + DRAKE_DEMAND(result.is_success()); + return result.GetSolution(q); + } + + void GenerateSeeds() { + auto context = plant_->CreateDefaultContext(); + seeds_.emplace_back("Home Position", plant_->GetPositions(*context)); + seeds_.emplace_back("Left Bin", MyInverseKinematics(RigidTransformd( + RollPitchYawd(M_PI / 2, M_PI, 0), + Vector3d(0.0, 0.6, 0.22)))); + if (FLAGS_test) { + // In test mode, only use the first two seeds. + return; + } + seeds_.emplace_back("Right Bin", MyInverseKinematics(RigidTransformd( + RollPitchYawd(M_PI / 2, M_PI, M_PI), + Vector3d(0.0, -0.6, 0.22)))); + seeds_.emplace_back("Above Shelve", MyInverseKinematics(RigidTransformd( + RollPitchYawd(0, -M_PI, -M_PI / 2), + Vector3d(0.75, 0, 0.9)))); + seeds_.emplace_back("Top Rack", MyInverseKinematics(RigidTransformd( + RollPitchYawd(0, -M_PI, -M_PI / 2), + Vector3d(0.75, 0, 0.67)))); + seeds_.emplace_back("Middle Rack", MyInverseKinematics(RigidTransformd( + RollPitchYawd(0, -M_PI, -M_PI / 2), + Vector3d(0.75, 0, 0.41)))); + } + + void GenerateAllRegions() { + // Because we use each computed region as an obstacle for the next, we need + // to run them all in a single benchmark. + std::map iris_regions; + Context& plant_context = + plant_->GetMyMutableContextFromRoot(diagram_context_.get()); + for (auto seed : seeds_) { + const auto& name = seed.first; + const auto& q0 = seed.second; + plant_->SetPositions(&plant_context, q0); + log()->info("Computing region for seed: {}", name); + HPolyhedron hpoly = + IrisInConfigurationSpace(*plant_, plant_context, iris_options_); + iris_options_.configuration_obstacles.emplace_back(hpoly.Scale(0.95)); + iris_regions.emplace(name, std::move(hpoly)); + } + } + + protected: + IrisOptions iris_options_{}; + std::unique_ptr> diagram_; + std::unique_ptr> diagram_context_; + MultibodyPlant* plant_; + std::vector> seeds_; +}; + +BENCHMARK_DEFINE_F(IiwaWithShelvesAndBins, GenerateAllRegions) +// NOLINTNEXTLINE(runtime/references) +(benchmark::State& state) { + for (auto _ : state) { + GenerateAllRegions(); + } +} +BENCHMARK_REGISTER_F(IiwaWithShelvesAndBins, GenerateAllRegions) + ->Unit(benchmark::kSecond); + +} // namespace +} // namespace optimization +} // namespace geometry +} // namespace drake + +int main(int argc, char** argv) { + if (!drake::solvers::SnoptSolver::is_enabled() || + !drake::solvers::SnoptSolver::is_available()) { + drake::log()->error( + "SNOPT is either not enabled or not available. This benchmark should " + "be evaluated with SNOPT."); + return 0; + } + + benchmark::Initialize(&argc, argv); + gflags::ParseCommandLineFlags(&argc, &argv, true); + benchmark::RunSpecifiedBenchmarks(); + return 0; +} diff --git a/tools/performance/BUILD.bazel b/tools/performance/BUILD.bazel index 50efc2f19fd3..825852a5619d 100644 --- a/tools/performance/BUILD.bazel +++ b/tools/performance/BUILD.bazel @@ -13,6 +13,7 @@ package(default_visibility = [ # All benchmarks should be in folders named "benchmarking". "//common/benchmarking:__pkg__", "//geometry/benchmarking:__pkg__", + "//geometry/optimization/benchmarking:__pkg__", "//multibody/benchmarking:__pkg__", "//solvers/benchmarking:__pkg__", "//systems/benchmarking:__pkg__", From 9a6b619c55c607e4b425d4c2341c7ee58574fef4 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Sun, 16 Jul 2023 11:31:21 -0700 Subject: [PATCH 2/4] fixup! Adds a benchmark for IrisInConfigurationSpace Remove dead code. --- tools/performance/BUILD.bazel | 1 - 1 file changed, 1 deletion(-) diff --git a/tools/performance/BUILD.bazel b/tools/performance/BUILD.bazel index 825852a5619d..50efc2f19fd3 100644 --- a/tools/performance/BUILD.bazel +++ b/tools/performance/BUILD.bazel @@ -13,7 +13,6 @@ package(default_visibility = [ # All benchmarks should be in folders named "benchmarking". "//common/benchmarking:__pkg__", "//geometry/benchmarking:__pkg__", - "//geometry/optimization/benchmarking:__pkg__", "//multibody/benchmarking:__pkg__", "//solvers/benchmarking:__pkg__", "//systems/benchmarking:__pkg__", From fb995801b4d92b1442aa53d1021bf35a8374c539 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Sun, 16 Jul 2023 11:28:46 -0700 Subject: [PATCH 3/4] fixup! Adds a benchmark for IrisInConfigurationSpace Remove duplicate main function. Use googlebench error reporting instead of bespoke code. --- .../iris_in_configuration_space_benchmarks.cc | 25 +++++++------------ 1 file changed, 9 insertions(+), 16 deletions(-) diff --git a/geometry/benchmarking/iris_in_configuration_space_benchmarks.cc b/geometry/benchmarking/iris_in_configuration_space_benchmarks.cc index a1c315811d3c..9737536c3c8d 100644 --- a/geometry/benchmarking/iris_in_configuration_space_benchmarks.cc +++ b/geometry/benchmarking/iris_in_configuration_space_benchmarks.cc @@ -26,6 +26,7 @@ using math::RigidTransformd; using math::RollPitchYawd; using math::RotationMatrixd; using multibody::MultibodyPlant; +using solvers::SnoptSolver; using systems::Context; DEFINE_bool(test, false, "Enable unit test mode."); @@ -42,7 +43,14 @@ class IiwaWithShelvesAndBins : public benchmark::Fixture { // errors in g++. All of this is a consequence of the weird deprecation of // const-ref State versions of SetUp() and TearDown() in benchmark.h. using benchmark::Fixture::SetUp; - void SetUp(benchmark::State&) override { + void SetUp(benchmark::State& state) override { + if (!(SnoptSolver::is_enabled() && SnoptSolver::is_available())) { + state.SkipWithError( + "SNOPT is either not enabled or not available. This benchmark should " + "be evaluated with SNOPT."); + return; + } + // Configure IRIS. if (FLAGS_test) { iris_options_.iteration_limit = 1; @@ -251,18 +259,3 @@ BENCHMARK_REGISTER_F(IiwaWithShelvesAndBins, GenerateAllRegions) } // namespace optimization } // namespace geometry } // namespace drake - -int main(int argc, char** argv) { - if (!drake::solvers::SnoptSolver::is_enabled() || - !drake::solvers::SnoptSolver::is_available()) { - drake::log()->error( - "SNOPT is either not enabled or not available. This benchmark should " - "be evaluated with SNOPT."); - return 0; - } - - benchmark::Initialize(&argc, argv); - gflags::ParseCommandLineFlags(&argc, &argv, true); - benchmark::RunSpecifiedBenchmarks(); - return 0; -} From da2615c958790489e3190b5f980eb689fbb2e1b0 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Sun, 16 Jul 2023 11:46:15 -0700 Subject: [PATCH 4/4] fixup! Adds a benchmark for IrisInConfigurationSpace Use structured bindings. --- .../benchmarking/iris_in_configuration_space_benchmarks.cc | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/geometry/benchmarking/iris_in_configuration_space_benchmarks.cc b/geometry/benchmarking/iris_in_configuration_space_benchmarks.cc index 9737536c3c8d..3bd8faf7e460 100644 --- a/geometry/benchmarking/iris_in_configuration_space_benchmarks.cc +++ b/geometry/benchmarking/iris_in_configuration_space_benchmarks.cc @@ -225,11 +225,9 @@ class IiwaWithShelvesAndBins : public benchmark::Fixture { std::map iris_regions; Context& plant_context = plant_->GetMyMutableContextFromRoot(diagram_context_.get()); - for (auto seed : seeds_) { - const auto& name = seed.first; - const auto& q0 = seed.second; - plant_->SetPositions(&plant_context, q0); + for (const auto& [name, q0] : seeds_) { log()->info("Computing region for seed: {}", name); + plant_->SetPositions(&plant_context, q0); HPolyhedron hpoly = IrisInConfigurationSpace(*plant_, plant_context, iris_options_); iris_options_.configuration_obstacles.emplace_back(hpoly.Scale(0.95));