From a462cb485728b592fb8619c17da358f28f65e8b2 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Tue, 4 Jul 2023 21:21:36 -0400 Subject: [PATCH] Adds a benchmark for IrisInConfigurationSpace This benchmark plants a flag before we start optimizing the IrisInConfigurationSpace runtimes. --- .../optimization/benchmarking/BUILD.bazel | 37 +++ geometry/optimization/benchmarking/README.md | 17 ++ .../iris_in_configuration_space_benchmarks.cc | 258 ++++++++++++++++++ tools/performance/BUILD.bazel | 1 + 4 files changed, 313 insertions(+) create mode 100644 geometry/optimization/benchmarking/BUILD.bazel create mode 100644 geometry/optimization/benchmarking/README.md create mode 100644 geometry/optimization/benchmarking/iris_in_configuration_space_benchmarks.cc diff --git a/geometry/optimization/benchmarking/BUILD.bazel b/geometry/optimization/benchmarking/BUILD.bazel new file mode 100644 index 000000000000..ed03809b10b7 --- /dev/null +++ b/geometry/optimization/benchmarking/BUILD.bazel @@ -0,0 +1,37 @@ +load( + "@drake//tools/performance:defs.bzl", + "drake_cc_googlebench_binary", + "drake_py_experiment_binary", +) +load("//tools/lint:lint.bzl", "add_lint_tests") + +package(default_visibility = ["//visibility:public"]) + +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", +) + +add_lint_tests() diff --git a/geometry/optimization/benchmarking/README.md b/geometry/optimization/benchmarking/README.md new file mode 100644 index 000000000000..82a2b920089f --- /dev/null +++ b/geometry/optimization/benchmarking/README.md @@ -0,0 +1,17 @@ +Runtime Performance Benchmarks for Geometry Operations +------------------------------------------------------ + +# Supported experiments + +## 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: +https://github.com/google/benchmark#command-line diff --git a/geometry/optimization/benchmarking/iris_in_configuration_space_benchmarks.cc b/geometry/optimization/benchmarking/iris_in_configuration_space_benchmarks.cc new file mode 100644 index 000000000000..d195542eeee0 --- /dev/null +++ b/geometry/optimization/benchmarking/iris_in_configuration_space_benchmarks.cc @@ -0,0 +1,258 @@ +#include +#include + +#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/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) { + 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__",