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__",