diff --git a/examples/manipulation_station/BUILD.bazel b/examples/manipulation_station/BUILD.bazel index 75ea32ac8ab9..7747149e19a8 100644 --- a/examples/manipulation_station/BUILD.bazel +++ b/examples/manipulation_station/BUILD.bazel @@ -17,6 +17,17 @@ load("//tools/skylark:test_tags.bzl", "vtk_test_tags") models_filegroup( name = "models", + extra_srcs = [ + "@drake_models//:manipulation_station/bin.mtl", + "@drake_models//:manipulation_station/bin.obj", + "@drake_models//:manipulation_station/bin.png", + "@drake_models//:manipulation_station/table_wide.mtl", + "@drake_models//:manipulation_station/table_wide.obj", + "@drake_models//:manipulation_station/table_wide.png", + "@drake_models//:manipulation_station/shelves.mtl", + "@drake_models//:manipulation_station/shelves.obj", + "@drake_models//:manipulation_station/shelves.png", + ], visibility = ["//visibility:public"], ) diff --git a/examples/manipulation_station/models/bin2.sdf b/examples/manipulation_station/models/bin2.sdf new file mode 100644 index 000000000000..1c504b4ec429 --- /dev/null +++ b/examples/manipulation_station/models/bin2.sdf @@ -0,0 +1,77 @@ + + + + + + + 18.70 + + 0.79 + 0 + 0 + 0.53 + 0 + 1.2 + + + + 0 0 0 0 0 1.570796 + + + package://drake_models/manipulation_station/bin.obj + + + + + 0.2025 0 0.105 0 0 0 + + + 0.015 0.56 0.21 + + + + + -0.2025 0 0.105 0 0 0 + + + 0.015 0.56 0.21 + + + + + 0 0.2725 0.105 0 0 0 + + + 0.42 0.015 0.21 + + + + + 0 -0.2725 0.105 0 0 0 + + + 0.42 0.015 0.21 + + + + + 0.0 0.0 0.0075 0 0 0 + + + 0.42 0.56 0.015 + + + + + + 0.22 0 0.21 0 0 0 + + + diff --git a/examples/manipulation_station/models/shelves.sdf b/examples/manipulation_station/models/shelves.sdf new file mode 100644 index 000000000000..a6d1eba9d698 --- /dev/null +++ b/examples/manipulation_station/models/shelves.sdf @@ -0,0 +1,71 @@ + + + + + + 0 0 0 0 0 3.14159 + + + package://drake_models/manipulation_station/shelves.obj + + + + + 0 0.292 0 0 0 0 + + + 0.3 0.016 0.783 + + + + + 0 -0.292 0 0 0 0 + + + 0.3 0.016 0.783 + + + + + 0 0 0.3995 0 0 0 + + + 0.3 0.6 0.016 + + + + + 0 0 -0.13115 0 0 0 + + + 0.3 0.6 0.016 + + + + + 0 0 0.13115 0 0 0 + + + 0.3 0.6 0.016 + + + + + 0 0 -0.3995 0 0 0 + + + 0.3 0.6 0.016 + + + + + -0.142 0 0 0 0 0 + + + 0.016 0.6 0.783 + + + + + + diff --git a/examples/manipulation_station/models/table_wide.sdf b/examples/manipulation_station/models/table_wide.sdf new file mode 100644 index 000000000000..714f570c1bcd --- /dev/null +++ b/examples/manipulation_station/models/table_wide.sdf @@ -0,0 +1,22 @@ + + + + + + + + package://drake_models/manipulation_station/table_wide.obj + + + + + 0 0 -0.05 0 0 0 + + + 1.5 2.1875 0.1 + + + + + + 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..480a83aa0f4a 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/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..6be2d497bfcf --- /dev/null +++ b/geometry/benchmarking/iris_in_configuration_space_benchmarks.cc @@ -0,0 +1,236 @@ +#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" + +/* These scenarios should capture the most important/representative use cases +for IrisInConfigurationSpace. They will be used to guide performance +optimizations of the core algorithm. */ + +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 solvers::SnoptSolver; +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); } + + 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; + 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/bin2.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/bin2.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 ]} +# Add shelves +- add_model: + name: shelves + file: package://drake/examples/manipulation_station/models/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://drake/examples/manipulation_station/models/table_wide.sdf + +- add_weld: + parent: world + child: table::table_body + X_PC: + translation: [0.4, 0.0, 0.0] +)"""; + + 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_.clear(); + 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. + iris_options_.configuration_obstacles.clear(); + Context& plant_context = + plant_->GetMyMutableContextFromRoot(diagram_context_.get()); + 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)); + } + } + + 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 diff --git a/tools/workspace/drake_models/repository.bzl b/tools/workspace/drake_models/repository.bzl index d9590728877e..48afce4027e0 100644 --- a/tools/workspace/drake_models/repository.bzl +++ b/tools/workspace/drake_models/repository.bzl @@ -6,8 +6,8 @@ def drake_models_repository( github_archive( name = name, repository = "RobotLocomotion/models", - commit = "611246c443152946e9dcc901b4f956d89a439a61", - sha256 = "66ad3b0f85b2aaa823d2e69cdaa980af9c0414cdb829b8224fbaa8a2dc083a7c", # noqa + commit = "90397c33cab9b7234d94c1018f2755bb9989c5a8", + sha256 = "823e8d3461da43473582c1b99c740cba5c305e576fb0324df8fa9e45047e0f98", # noqa build_file = ":package.BUILD.bazel", mirrors = mirrors, )