diff --git a/BUILD.bazel b/BUILD.bazel index dde52a7e7b3c..723954a6f8c6 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -103,7 +103,6 @@ install( "//examples:install", "//geometry:install", "//lcmtypes:install", - "//manipulation/models:install_data", "//multibody/parsing:install", "//setup:install", "//tools/install/libdrake:install", diff --git a/bindings/pydrake/visualization/model_visualizer.py b/bindings/pydrake/visualization/model_visualizer.py index 541c991e1979..23ebae29802a 100644 --- a/bindings/pydrake/visualization/model_visualizer.py +++ b/bindings/pydrake/visualization/model_visualizer.py @@ -28,7 +28,7 @@ An example of viewing an iiwa model file:: python3 -m pydrake.visualization.model_visualizer --open-window \ - package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf + package://drake_models/iiwa_description/sdf/iiwa7_with_box_collision.sdf This program respects the ``ROS_PACKAGE_PATH``; if your model uses external resources then you will need to set that environment variable. diff --git a/examples/manipulation_station/BUILD.bazel b/examples/manipulation_station/BUILD.bazel index b56200a67479..2d5944d8b500 100644 --- a/examples/manipulation_station/BUILD.bazel +++ b/examples/manipulation_station/BUILD.bazel @@ -54,9 +54,12 @@ drake_cc_library( data = [ ":models", "//examples/kuka_iiwa_arm:models", + "@drake_models", + # TODO(jwnimmer-tri) These are not actually used by this library + # anymore, but removing them breaks too many other things. We'll + # revisit this later when we finally delete these paths entirely. "//manipulation/models/iiwa_description:models", "//manipulation/models/wsg_50_description:models", - "@drake_models", ], visibility = ["//visibility:public"], deps = [ @@ -85,8 +88,7 @@ drake_cc_library( "manipulation_station_hardware_interface.h", ], data = [ - "//manipulation/models/iiwa_description:models", - "//manipulation/models/wsg_50_description:models", + "@drake_models", ], visibility = ["//visibility:public"], deps = [ diff --git a/examples/manipulation_station/manipulation_station.cc b/examples/manipulation_station/manipulation_station.cc index 9ca2ab052d2f..28b991f68cd9 100644 --- a/examples/manipulation_station/manipulation_station.cc +++ b/examples/manipulation_station/manipulation_station.cc @@ -49,6 +49,7 @@ using multibody::ExternallyAppliedSpatialForce; using multibody::Joint; using multibody::MultibodyForces; using multibody::MultibodyPlant; +using multibody::PackageMap; using multibody::PrismaticJoint; using multibody::RevoluteJoint; using multibody::RigidBody; @@ -455,15 +456,14 @@ void ManipulationStation::SetupPlanarIiwaStation( // Add planar iiwa model. { - std::string sdf_path = - "drake/manipulation/models/iiwa_description/urdf/" + const std::string sdf_url = + "package://drake_models/iiwa_description/urdf/" "planar_iiwa14_spheres_dense_elbow_collision.urdf"; - std::string sdf_url = "package://" + sdf_path; const auto X_WI = RigidTransform::Identity(); auto iiwa_instance = internal::AddAndWeldModelFrom( sdf_url, "iiwa", plant_->world_frame(), "iiwa_link_0", X_WI, plant_); RegisterIiwaControllerModel( - FindResourceOrThrow(sdf_path), iiwa_instance, plant_->world_frame(), + PackageMap{}.ResolveUrl(sdf_url), iiwa_instance, plant_->world_frame(), plant_->GetFrameByName("iiwa_link_0", iiwa_instance), X_WI); } @@ -1115,25 +1115,24 @@ ManipulationStation::GetStaticCameraPosesInWorld() const { template void ManipulationStation::AddDefaultIiwa( const IiwaCollisionModel collision_model) { - std::string sdf_path; + std::string sdf_url; switch (collision_model) { case IiwaCollisionModel::kNoCollision: - sdf_path = - "drake/manipulation/models/iiwa_description/iiwa7/" + sdf_url = + "package://drake_models/iiwa_description/sdf/" "iiwa7_no_collision.sdf"; break; case IiwaCollisionModel::kBoxCollision: - sdf_path = - "drake/manipulation/models/iiwa_description/iiwa7/" + sdf_url = + "package://drake_models/iiwa_description/sdf/" "iiwa7_with_box_collision.sdf"; break; } - std::string sdf_url = "package://" + sdf_path; const auto X_WI = RigidTransform::Identity(); auto iiwa_instance = internal::AddAndWeldModelFrom( sdf_url, "iiwa", plant_->world_frame(), "iiwa_link_0", X_WI, plant_); RegisterIiwaControllerModel( - FindResourceOrThrow(sdf_path), iiwa_instance, plant_->world_frame(), + PackageMap{}.ResolveUrl(sdf_url), iiwa_instance, plant_->world_frame(), plant_->GetFrameByName("iiwa_link_0", iiwa_instance), X_WI); } @@ -1141,29 +1140,28 @@ void ManipulationStation::AddDefaultIiwa( template void ManipulationStation::AddDefaultWsg( const SchunkCollisionModel schunk_model) { - std::string sdf_path; + std::string sdf_url; switch (schunk_model) { case SchunkCollisionModel::kBox: - sdf_path = - "drake/manipulation/models/wsg_50_description/sdf" - "/schunk_wsg_50_no_tip.sdf"; + sdf_url = + "package://drake_models/wsg_50_description/sdf/" + "schunk_wsg_50_no_tip.sdf"; break; case SchunkCollisionModel::kBoxPlusFingertipSpheres: - sdf_path = - "drake/manipulation/models/wsg_50_description/sdf" - "/schunk_wsg_50_with_tip.sdf"; + sdf_url = + "package://drake_models/wsg_50_description/sdf/" + "schunk_wsg_50_with_tip.sdf"; break; } - std::string sdf_url = "package://" + sdf_path; const multibody::Frame& link7 = plant_->GetFrameByName("iiwa_link_7", iiwa_model_.model_instance); const RigidTransform X_7G(RollPitchYaw(M_PI_2, 0, M_PI_2), Vector3d(0, 0, 0.114)); auto wsg_instance = internal::AddAndWeldModelFrom(sdf_url, "gripper", link7, "body", X_7G, plant_); - RegisterWsgControllerModel(FindResourceOrThrow(sdf_path), wsg_instance, link7, - plant_->GetFrameByName("body", wsg_instance), - X_7G); + RegisterWsgControllerModel( + PackageMap{}.ResolveUrl(sdf_url), wsg_instance, link7, + plant_->GetFrameByName("body", wsg_instance), X_7G); } } // namespace manipulation_station diff --git a/examples/manipulation_station/manipulation_station_hardware_interface.cc b/examples/manipulation_station/manipulation_station_hardware_interface.cc index 258f26147da5..28cf20783037 100644 --- a/examples/manipulation_station/manipulation_station_hardware_interface.cc +++ b/examples/manipulation_station/manipulation_station_hardware_interface.cc @@ -129,8 +129,7 @@ ManipulationStationHardwareInterface::ManipulationStationHardwareInterface( // Build the controller's version of the plant, which only contains the // IIWA and the equivalent inertia of the gripper. const std::string iiwa_sdf_url = - "package://drake/manipulation/models/iiwa_description/sdf/" - "iiwa14_no_collision.sdf"; + "package://drake_models/iiwa_description/sdf/iiwa14_no_collision.sdf"; Parser parser(owned_controller_plant_.get()); iiwa_model_instance_ = parser.AddModelsFromUrl(iiwa_sdf_url).at(0); diff --git a/examples/manipulation_station/test/manipulation_station_test.cc b/examples/manipulation_station/test/manipulation_station_test.cc index d6309b00a832..c95745214fdc 100644 --- a/examples/manipulation_station/test/manipulation_station_test.cc +++ b/examples/manipulation_station/test/manipulation_station_test.cc @@ -44,8 +44,7 @@ multibody::SpatialInertia MakeCompositeGripperInertia() { multibody::MultibodyPlant plant(1.0); multibody::Parser parser(&plant); parser.AddModelsFromUrl( - "package://drake/manipulation/models/wsg_50_description/sdf/" - "schunk_wsg_50_no_tip.sdf"); + "package://drake_models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf"); plant.Finalize(); const std::string gripper_body_frame_name = "body"; const auto& frame = plant.GetFrameByName(gripper_body_frame_name); diff --git a/manipulation/models/BUILD.bazel b/manipulation/models/BUILD.bazel index cc8f340ad638..67914ea7e0a0 100644 --- a/manipulation/models/BUILD.bazel +++ b/manipulation/models/BUILD.bazel @@ -1,16 +1,3 @@ -load("//tools/install:install.bzl", "install") load("//tools/lint:lint.bzl", "add_lint_tests") -install( - name = "install_data", - visibility = ["//visibility:public"], - deps = [ - "//manipulation/models/franka_description:install_data", - "//manipulation/models/iiwa_description:install_data", - "//manipulation/models/jaco_description:install_data", - "//manipulation/models/ur3e:install_data", - "//manipulation/models/wsg_50_description:install_data", - ], -) - add_lint_tests() diff --git a/manipulation/models/README.md b/manipulation/models/README.md new file mode 100644 index 000000000000..717700fc1e00 --- /dev/null +++ b/manipulation/models/README.md @@ -0,0 +1,11 @@ + +# Drake Users + +All files in drake/manipulation/models are in the process of being moved to the +https://github.com/RobotLocomotion/models/ repository, instead. Do not use any +of these (package://drake) models in new code. Instead, use the models from +their new home (package://drake_models). + +# Drake Developers + +Do not many any further changes to these model files. diff --git a/manipulation/models/franka_description/BUILD.bazel b/manipulation/models/franka_description/BUILD.bazel index 3a8a58f878be..b3a92fa8eea8 100644 --- a/manipulation/models/franka_description/BUILD.bazel +++ b/manipulation/models/franka_description/BUILD.bazel @@ -1,9 +1,4 @@ -load( - "//tools/skylark:drake_cc.bzl", - "drake_cc_googletest", -) load("//tools/skylark:drake_data.bzl", "models_filegroup") -load("//tools/install:install_data.bzl", "install_data") load("//tools/lint:lint.bzl", "add_lint_tests") package(default_visibility = ["//visibility:public"]) @@ -17,11 +12,6 @@ models_filegroup( visibility = ["//visibility:private"], ) -install_data( - name = "install_data", - data = [":glob_models"], -) - filegroup( name = "models", srcs = [ @@ -49,18 +39,4 @@ filegroup( ], ) -# === test/ === - -drake_cc_googletest( - name = "franka_arm_test", - srcs = ["urdf/test/franka_arm_test.cc"], - data = [":models"], - deps = [ - "//common:find_resource", - "//common/test_utilities:eigen_matrix_compare", - "//multibody/parsing", - "//multibody/plant", - ], -) - add_lint_tests() diff --git a/manipulation/models/franka_description/VESTIGIAL.md b/manipulation/models/franka_description/VESTIGIAL.md new file mode 120000 index 000000000000..32d46ee883b5 --- /dev/null +++ b/manipulation/models/franka_description/VESTIGIAL.md @@ -0,0 +1 @@ +../README.md \ No newline at end of file diff --git a/manipulation/models/franka_description/urdf/test/franka_arm_test.cc b/manipulation/models/franka_description/urdf/test/franka_arm_test.cc deleted file mode 100644 index 462d0f223dac..000000000000 --- a/manipulation/models/franka_description/urdf/test/franka_arm_test.cc +++ /dev/null @@ -1,117 +0,0 @@ -#include - -#include - -#include "drake/common/find_resource.h" -#include "drake/common/test_utilities/eigen_matrix_compare.h" -#include "drake/multibody/parsing/parser.h" -#include "drake/multibody/plant/multibody_plant.h" - -namespace drake { -namespace manipulation { -namespace { - -// Read the canonical plant from main URDF model -multibody::ModelInstanceIndex LoadFrankaCanonicalModel( - multibody::MultibodyPlant* plant) { - const std::string canonical_model_file( - FindResourceOrThrow("drake/manipulation/models/franka_description/urdf/" - "panda_arm.urdf")); - multibody::Parser parser(plant); - return parser.AddModels(canonical_model_file).at(0); -} - -// Tests that a model can be loaded into a MultibodyPlant. This unit test also -// verifies that the URDF can be parsed by the URDF parser (similarly for the -// remaining tests in this file). -GTEST_TEST(FrankaArmTest, TestLoadArm) { - const std::string kPath( - FindResourceOrThrow("drake/manipulation/models/franka_description/urdf/" - "panda_arm.urdf")); - - multibody::MultibodyPlant plant(0.0); - multibody::Parser parser(&plant); - parser.AddModels(kPath); - - // There should be actuators for all 7 degrees of freedom. - EXPECT_EQ(plant.num_actuators(), 7); - EXPECT_EQ(plant.num_bodies(), 10); -} - -GTEST_TEST(FrankaArmTest, TestLoadHand) { - const std::string kPath( - FindResourceOrThrow("drake/manipulation/models/franka_description/urdf/" - "hand.urdf")); - - multibody::MultibodyPlant plant(0.0); - multibody::Parser parser(&plant); - parser.AddModels(kPath); - - EXPECT_EQ(plant.num_actuators(), 2); - EXPECT_EQ(plant.num_bodies(), 4); -} - -GTEST_TEST(FrankaArmTest, TestLoadCombined) { - const std::string kPath( - FindResourceOrThrow("drake/manipulation/models/franka_description/urdf/" - "panda_arm_hand.urdf")); - - multibody::MultibodyPlant plant(0.0); - multibody::Parser parser(&plant); - parser.AddModels(kPath); - - EXPECT_EQ(plant.num_actuators(), 9); - EXPECT_EQ(plant.num_bodies(), 13); -} - -// Compares velocity, acceleration, effort and position limits of two given -// actuators. -void CompareActuatorLimits(const multibody::JointActuator& joint_a, - const multibody::JointActuator& joint_b) { - EXPECT_NE(&joint_a, &joint_b); // Different instance. - EXPECT_TRUE(CompareMatrices(joint_a.joint().velocity_lower_limits(), - joint_b.joint().velocity_lower_limits())); - EXPECT_TRUE(CompareMatrices(joint_a.joint().velocity_upper_limits(), - joint_b.joint().velocity_upper_limits())); - EXPECT_TRUE(CompareMatrices(joint_a.joint().position_lower_limits(), - joint_b.joint().position_lower_limits())); - EXPECT_TRUE(CompareMatrices(joint_a.joint().position_upper_limits(), - joint_b.joint().position_upper_limits())); - EXPECT_EQ(joint_a.effort_limit(), joint_b.effort_limit()); - EXPECT_TRUE(CompareMatrices(joint_a.joint().acceleration_lower_limits(), - joint_b.joint().acceleration_lower_limits())); - EXPECT_TRUE(CompareMatrices(joint_a.joint().acceleration_upper_limits(), - joint_b.joint().acceleration_upper_limits())); - EXPECT_EQ(joint_a.default_gear_ratio(), joint_b.default_gear_ratio()); - EXPECT_EQ(joint_a.default_rotor_inertia(), joint_b.default_rotor_inertia()); -} - -// Tests that the reflected interia values are consistent between the URDFs -GTEST_TEST(FrankaArmTest, TestReflectedInertia) { - multibody::MultibodyPlant canonical_plant(0.0); - LoadFrankaCanonicalModel(&canonical_plant); - canonical_plant.Finalize(); - - const std::string kPath( - FindResourceOrThrow("drake/manipulation/models/franka_description/urdf/" - "panda_arm_hand.urdf")); - - multibody::MultibodyPlant plant(0.0); - multibody::Parser parser(&plant); - parser.AddModels(kPath); - plant.Finalize(); - - for (int i = 0; i < canonical_plant.num_actuators(); ++i) { - const multibody::JointActuator& canonical_joint_actuator = - canonical_plant.get_joint_actuator( - drake::multibody::JointActuatorIndex(i)); - const multibody::JointActuator& joint_actuator = - plant.get_joint_actuator(drake::multibody::JointActuatorIndex(i)); - - CompareActuatorLimits(canonical_joint_actuator, joint_actuator); - } -} - -} // namespace -} // namespace manipulation -} // namespace drake diff --git a/manipulation/models/iiwa_description/BUILD.bazel b/manipulation/models/iiwa_description/BUILD.bazel index 518c19182e1a..b2312d31fb1e 100644 --- a/manipulation/models/iiwa_description/BUILD.bazel +++ b/manipulation/models/iiwa_description/BUILD.bazel @@ -1,9 +1,4 @@ -load( - "//tools/skylark:drake_cc.bzl", - "drake_cc_googletest", -) load("//tools/skylark:drake_data.bzl", "models_filegroup") -load("//tools/install:install_data.bzl", "install_data") load("//tools/lint:lint.bzl", "add_lint_tests") package(default_visibility = ["//visibility:public"]) @@ -17,11 +12,6 @@ models_filegroup( visibility = ["//visibility:private"], ) -install_data( - name = "install_data", - data = [":glob_models"], -) - filegroup( name = "models", srcs = [ @@ -80,17 +70,4 @@ filegroup( ], ) -# === test/ === - -drake_cc_googletest( - name = "iiwa_variants_parsing_test", - srcs = ["test/iiwa_variants_parsing_test.cc"], - data = [":models"], - deps = [ - "//common:find_resource", - "//common/test_utilities:eigen_matrix_compare", - "//multibody/parsing", - ], -) - add_lint_tests() diff --git a/manipulation/models/iiwa_description/VESTIGIAL.md b/manipulation/models/iiwa_description/VESTIGIAL.md new file mode 120000 index 000000000000..32d46ee883b5 --- /dev/null +++ b/manipulation/models/iiwa_description/VESTIGIAL.md @@ -0,0 +1 @@ +../README.md \ No newline at end of file diff --git a/manipulation/models/iiwa_description/test/iiwa_variants_parsing_test.cc b/manipulation/models/iiwa_description/test/iiwa_variants_parsing_test.cc deleted file mode 100644 index 0e41985d2522..000000000000 --- a/manipulation/models/iiwa_description/test/iiwa_variants_parsing_test.cc +++ /dev/null @@ -1,314 +0,0 @@ -#include - -#include - -#include "drake/common/find_resource.h" -#include "drake/common/test_utilities/eigen_matrix_compare.h" -#include "drake/multibody/parsing/parser.h" -#include "drake/multibody/plant/multibody_plant.h" - -namespace drake { -namespace manipulation { -namespace { - -// Read the canonical plant for IIWA7 from main SDFormat model -multibody::ModelInstanceIndex LoadIiwa7CanonicalModel( - multibody::MultibodyPlant* plant) { - const std::string canonical_model_file( - FindResourceOrThrow("drake/manipulation/models/iiwa_description/iiwa7/" - "iiwa7_no_collision.sdf")); - multibody::Parser parser(plant); - return parser.AddModels(canonical_model_file).at(0); -} -// Read the canonical plant for IIWA14 from main SDFormat model -multibody::ModelInstanceIndex LoadIiwa14CanonicalModel( - multibody::MultibodyPlant* plant) { - const std::string canonical_model_file( - FindResourceOrThrow("drake/manipulation/models/iiwa_description/sdf/" - "iiwa14_no_collision.sdf")); - multibody::Parser parser(plant); - return parser.AddModels(canonical_model_file).at(0); -} - -// Read the common robot model files for IIWA14 -const std::vector GetCommonIiwa14ModelFiles() { - const std::vector model_files = { - "drake/manipulation/models/iiwa_description/sdf/" - "iiwa14_polytope_collision.sdf", - "drake/manipulation/models/iiwa_description/urdf/" - "iiwa14_no_collision.urdf", - "drake/manipulation/models/iiwa_description/urdf/" - "iiwa14_polytope_collision.urdf", - "drake/manipulation/models/iiwa_description/urdf/" - "iiwa14_primitive_collision.urdf", - "drake/manipulation/models/iiwa_description/urdf/" - "iiwa14_spheres_collision.urdf", - "drake/manipulation/models/iiwa_description/urdf/" - "iiwa14_spheres_dense_collision.urdf", - "drake/manipulation/models/iiwa_description/urdf/" - "iiwa14_spheres_dense_elbow_collision.urdf"}; - return model_files; -} - -// Read the common robot model files for IIWA7 -const std::vector GetCommonIiwa7ModelFiles() { - const std::vector model_files = { - "drake/manipulation/models/iiwa_description/iiwa7/" - "iiwa7_no_collision.sdf", - "drake/manipulation/models/iiwa_description/iiwa7/" - "iiwa7_with_box_collision.sdf"}; - return model_files; -} - -// Read the dual IIWA model file -const std::string GetDualIiwaModelFile() { - return "drake/manipulation/models/iiwa_description/urdf/" - "dual_iiwa14_polytope_collision.urdf"; -} - -// Read the planar IIWA model file -const std::string GetPlanarIiwaModelFile() { - return "drake/manipulation/models/iiwa_description/urdf/" - "planar_iiwa14_spheres_dense_elbow_collision.urdf"; -} - -// Compares velocity, acceleration, effort and position limits of two given -// actuators. -void CompareActuatorLimits(const multibody::JointActuator& joint_a, - const multibody::JointActuator& joint_b) { - EXPECT_NE(&joint_a, &joint_b); // Different instance. - EXPECT_TRUE(CompareMatrices(joint_a.joint().velocity_lower_limits(), - joint_b.joint().velocity_lower_limits())); - EXPECT_TRUE(CompareMatrices(joint_a.joint().velocity_upper_limits(), - joint_b.joint().velocity_upper_limits())); - EXPECT_TRUE(CompareMatrices(joint_a.joint().position_lower_limits(), - joint_b.joint().position_lower_limits())); - EXPECT_TRUE(CompareMatrices(joint_a.joint().position_upper_limits(), - joint_b.joint().position_upper_limits())); - EXPECT_EQ(joint_a.effort_limit(), joint_b.effort_limit()); - EXPECT_TRUE(CompareMatrices(joint_a.joint().acceleration_lower_limits(), - joint_b.joint().acceleration_lower_limits())); - EXPECT_TRUE(CompareMatrices(joint_a.joint().acceleration_upper_limits(), - joint_b.joint().acceleration_upper_limits())); - EXPECT_EQ(joint_a.default_gear_ratio(), joint_b.default_gear_ratio()); - EXPECT_EQ(joint_a.default_rotor_inertia(), joint_b.default_rotor_inertia()); -} - -// Compare rotational inertias i.e moments and products of inertia -void CompareRotationalInertias( - const multibody::RigidBody& canonical_body, - const multibody::RigidBody& robot_body) { - EXPECT_TRUE( - CompareMatrices(canonical_body.default_rotational_inertia().get_moments(), - robot_body.default_rotational_inertia().get_moments())); - EXPECT_TRUE(CompareMatrices( - canonical_body.default_rotational_inertia().get_products(), - robot_body.default_rotational_inertia().get_products())); -} - -// Tests that KUKA LBR iiwa7 models have consistent joint limits. -// It takes iiwa7_no_collision.sdf as the canonical model. -// It assumes all joints are declared in the same order. -GTEST_TEST(JointLimitsIiwa7, TestEffortVelocityPositionValues) { - multibody::MultibodyPlant canonical_plant(0.0); - multibody::ModelInstanceIndex canonical_model_instance = - LoadIiwa7CanonicalModel(&canonical_plant); - canonical_plant.Finalize(); - - const std::vector joint_canonical_indices = - canonical_plant.GetJointIndices(canonical_model_instance); - - std::vector model_files = GetCommonIiwa7ModelFiles(); - - for (auto& model_file : model_files) { - SCOPED_TRACE(fmt::format("model file: {}", model_file)); - multibody::MultibodyPlant plant(0.0); - multibody::Parser parser(&plant); - parser.AddModels(FindResourceOrThrow(model_file)); - plant.Finalize(); - - for (int i = 0; i < canonical_plant.num_actuators(); ++i) { - const multibody::JointActuator& canonical_joint_actuator = - canonical_plant.get_joint_actuator( - drake::multibody::JointActuatorIndex(i)); - const multibody::JointActuator& joint_actuator = - plant.get_joint_actuator(drake::multibody::JointActuatorIndex(i)); - - CompareActuatorLimits(canonical_joint_actuator, joint_actuator); - } - } -} - -// Tests that KUKA LBR iiwa7 models have consistent inertias. -// It takes iiwa7_no_collision.sdf as the canonical model. -// It assumes all links are declared in the same order. -// It checks values directly on urdf files, generated from xacro. -GTEST_TEST(InertiasIiwa7, TestInertiaValues) { - multibody::MultibodyPlant canonical_plant(0.0); - multibody::ModelInstanceIndex canonical_model_instance = - LoadIiwa7CanonicalModel(&canonical_plant); - canonical_plant.Finalize(); - - const std::vector body_canonical_indices = - canonical_plant.GetBodyIndices(canonical_model_instance); - - std::vector model_files = GetCommonIiwa7ModelFiles(); - - std::vector link_names = { - "iiwa_link_0", "iiwa_link_1", "iiwa_link_2", "iiwa_link_3", - "iiwa_link_4", "iiwa_link_5", "iiwa_link_6", "iiwa_link_7"}; - - for (auto& model_file : model_files) { - SCOPED_TRACE(fmt::format("model file: {}", model_file)); - multibody::MultibodyPlant plant(0.0); - multibody::Parser parser(&plant); - parser.AddModels(FindResourceOrThrow(model_file)); - plant.Finalize(); - std::filesystem::path model_path(model_file); - for (size_t i = 0; i < link_names.size(); ++i) { - SCOPED_TRACE(fmt::format("Link: {}", link_names[i])); - const multibody::RigidBody& canonical_body = - canonical_plant.GetBodyByName(link_names[i]); - const multibody::RigidBody& robot_body = - plant.GetBodyByName(link_names[i]); - CompareRotationalInertias(canonical_body, robot_body); - } - } -} - -// Tests that KUKA LBR iiwa14 models have consistent joint limits. -// It takes iiwa14_no_collisions.sdf as the canonical model. -// It assumes all joints are declared in the same order. -// It checks values directly on urdf files, generated from xacro. -// TODO(marcoag): when xacro support is used as per (#15613) -// check values on files directly generated from xacro, -// rather than ones checked into the source tree with manual edits. -GTEST_TEST(JointLimitsIiwa14, TestEffortVelocityPositionValues) { - multibody::MultibodyPlant canonical_plant(0.0); - multibody::ModelInstanceIndex canonical_model_instance = - LoadIiwa14CanonicalModel(&canonical_plant); - canonical_plant.Finalize(); - - const std::vector joint_canonical_indices = - canonical_plant.GetJointIndices(canonical_model_instance); - - std::vector model_files = GetCommonIiwa14ModelFiles(); - model_files.push_back(GetDualIiwaModelFile()); - - for (auto& model_file : model_files) { - multibody::MultibodyPlant plant(0.0); - multibody::Parser parser(&plant); - parser.AddModels(FindResourceOrThrow(model_file)); - plant.Finalize(); - - for (int i = 0; i < canonical_plant.num_actuators(); ++i) { - const multibody::JointActuator& canonical_joint_actuator = - canonical_plant.get_joint_actuator( - drake::multibody::JointActuatorIndex(i)); - const multibody::JointActuator& joint_actuator = - plant.get_joint_actuator(drake::multibody::JointActuatorIndex(i)); - - CompareActuatorLimits(canonical_joint_actuator, joint_actuator); - - // Test the joints from the second instance of the dual iiwa14 polytope - // collision model. They correspond to joints 7 to 13 of the model. - std::filesystem::path model_path(model_file); - if (model_path.filename() == "dual_iiwa14_polytope_collision.urdf") { - const multibody::JointActuator& second_instance_joint_actuator = - plant.get_joint_actuator( - drake::multibody::JointActuatorIndex(i + 7)); - CompareActuatorLimits(canonical_joint_actuator, - second_instance_joint_actuator); - } - } - } -} - -// Tests planar KUKA LBR iiwa14 model joint limit values. -// It takes iiwa14_no_collisions.sdf as the canonical model. -GTEST_TEST(JointLimitsIiwa14, TestEffortVelocityPositionValuesPlanarModel) { - multibody::MultibodyPlant canonical_plant(0.0); - multibody::ModelInstanceIndex canonical_model_instance = - LoadIiwa14CanonicalModel(&canonical_plant); - canonical_plant.Finalize(); - - const std::vector joint_canonical_indices = - canonical_plant.GetJointIndices(canonical_model_instance); - - multibody::MultibodyPlant plant(0.0); - multibody::Parser parser(&plant); - parser.AddModels(FindResourceOrThrow(GetPlanarIiwaModelFile())); - plant.Finalize(); - - // This model only has three actuators. They correspond to actuators 1, 3 and - // 5 from the canonical version. - CompareActuatorLimits( - canonical_plant.get_joint_actuator( - drake::multibody::JointActuatorIndex(1)), - plant.get_joint_actuator(drake::multibody::JointActuatorIndex(0))); - CompareActuatorLimits( - canonical_plant.get_joint_actuator( - drake::multibody::JointActuatorIndex(3)), - plant.get_joint_actuator(drake::multibody::JointActuatorIndex(1))); - CompareActuatorLimits( - canonical_plant.get_joint_actuator( - drake::multibody::JointActuatorIndex(5)), - plant.get_joint_actuator(drake::multibody::JointActuatorIndex(2))); -} - -// Tests that KUKA LBR iiwa14 models have consistent inertias. -// It takes iiwa14_no_collisions.sdf as the canonical model. -// It assumes all links are declared in the same order. -// It checks values directly on urdf files, generated from xacro. -GTEST_TEST(InertiasIiwa14, TestInertiaValues) { - multibody::MultibodyPlant canonical_plant(0.0); - multibody::ModelInstanceIndex canonical_model_instance = - LoadIiwa14CanonicalModel(&canonical_plant); - canonical_plant.Finalize(); - - const std::vector body_canonical_indices = - canonical_plant.GetBodyIndices(canonical_model_instance); - - std::vector model_files = GetCommonIiwa14ModelFiles(); - model_files.push_back(GetDualIiwaModelFile()); - model_files.push_back(GetPlanarIiwaModelFile()); - - std::vector link_names = { - "iiwa_link_0", "iiwa_link_1", "iiwa_link_2", "iiwa_link_3", - "iiwa_link_4", "iiwa_link_5", "iiwa_link_6", "iiwa_link_7"}; - - for (auto& model_file : model_files) { - SCOPED_TRACE(fmt::format("model file: {}", model_file)); - multibody::MultibodyPlant plant(0.0); - multibody::Parser parser(&plant); - parser.AddModels(FindResourceOrThrow(model_file)); - plant.Finalize(); - std::filesystem::path model_path(model_file); - if (model_path.filename() == "dual_iiwa14_polytope_collision.urdf") { - for (size_t i = 0; i < link_names.size(); ++i) { - SCOPED_TRACE(fmt::format("Link: {}", link_names[i])); - const multibody::RigidBody& canonical_body = - canonical_plant.GetBodyByName(link_names[i]); - const multibody::RigidBody& left_robot_body = - plant.GetBodyByName("left_" + link_names[i]); - const multibody::RigidBody& right_robot_body = - plant.GetBodyByName("right_" + link_names[i]); - CompareRotationalInertias(canonical_body, left_robot_body); - CompareRotationalInertias(canonical_body, right_robot_body); - } - } else { - for (size_t i = 0; i < link_names.size(); ++i) { - SCOPED_TRACE(fmt::format("Link: {}", link_names[i])); - const multibody::RigidBody& canonical_body = - canonical_plant.GetBodyByName(link_names[i]); - const multibody::RigidBody& robot_body = - plant.GetBodyByName(link_names[i]); - CompareRotationalInertias(canonical_body, robot_body); - } - } - } -} - -} // namespace -} // namespace manipulation -} // namespace drake diff --git a/manipulation/models/jaco_description/BUILD.bazel b/manipulation/models/jaco_description/BUILD.bazel index 4bcebba5d181..de408d02fa57 100644 --- a/manipulation/models/jaco_description/BUILD.bazel +++ b/manipulation/models/jaco_description/BUILD.bazel @@ -1,9 +1,4 @@ -load( - "//tools/skylark:drake_cc.bzl", - "drake_cc_googletest", -) load("//tools/skylark:drake_data.bzl", "models_filegroup") -load("//tools/install:install_data.bzl", "install_data") load("//tools/lint:lint.bzl", "add_lint_tests") package(default_visibility = [":__subpackages__"]) @@ -17,12 +12,6 @@ models_filegroup( visibility = ["//visibility:private"], ) -install_data( - name = "install_data", - data = [":glob_models"], - visibility = ["//visibility:public"], -) - filegroup( name = "models", srcs = [ @@ -48,17 +37,4 @@ filegroup( visibility = ["//visibility:public"], ) -# === test/ === - -drake_cc_googletest( - name = "jaco_arm_test", - srcs = ["urdf/test/jaco_arm_test.cc"], - data = [":models"], - deps = [ - "//common:find_resource", - "//multibody/parsing", - "//multibody/plant", - ], -) - add_lint_tests() diff --git a/manipulation/models/jaco_description/VESTIGIAL.md b/manipulation/models/jaco_description/VESTIGIAL.md new file mode 120000 index 000000000000..32d46ee883b5 --- /dev/null +++ b/manipulation/models/jaco_description/VESTIGIAL.md @@ -0,0 +1 @@ +../README.md \ No newline at end of file diff --git a/manipulation/models/jaco_description/urdf/test/jaco_arm_test.cc b/manipulation/models/jaco_description/urdf/test/jaco_arm_test.cc deleted file mode 100644 index 295662b85c40..000000000000 --- a/manipulation/models/jaco_description/urdf/test/jaco_arm_test.cc +++ /dev/null @@ -1,85 +0,0 @@ -#include -#include -#include - -#include - -#include "drake/common/find_resource.h" -#include "drake/multibody/parsing/parser.h" -#include "drake/multibody/plant/multibody_plant.h" - -namespace drake { -namespace manipulation { -namespace { - -struct TestCase { - std::string urdf; - int num_actuators{}; - int num_bodies{}; -}; - -std::ostream& operator<<(std::ostream& os, const TestCase& self) { - os << self.urdf << "," << self.num_actuators << "," << self.num_bodies; - return os; -} - -class JacoArmParamsTest : public ::testing::TestWithParam {}; - -// Tests that a model can be loaded into a MultibodyPlant. This unit -// test also verifies that the URDF can be parsed by the URDF parser. -TEST_P(JacoArmParamsTest, HasExpected) { - const TestCase param = GetParam(); - const std::string urdf_path = - "drake/manipulation/models/jaco_description/urdf/"; - const std::string urdf(FindResourceOrThrow(urdf_path + param.urdf)); - - multibody::MultibodyPlant plant(0.0); - multibody::Parser parser(&plant); - parser.AddModels(urdf); - - EXPECT_EQ(plant.num_actuators(), param.num_actuators); - EXPECT_EQ(plant.num_bodies(), param.num_bodies); -} - -// clang-format off -std::vector GenerateTestCases() { - return std::vector{ - {"j2n6s300.urdf", 9, - 6 /* robot */ + - 1 /* end effector */ + - 3 * 2 /* two per finger */ + - 3 /* world, root, base */ }, - {"j2s7s300.urdf", 10, - 7 /* robot */ + - 1 /* end effector */ + - 3 * 2 /* two per finger */ + - 3 /* world, root, base */ }, - {"j2s7s300_sphere_collision.urdf", 10, - 7 /* robot */ + - 1 /* end effector */ + - 3 * 2 /* two per finger */ + - 3 /* world, root, base */ }, - {"j2s7s300_arm.urdf", 7, - 7 /* robot */ + - 3 /* world, root, base */ }, - {"j2s7s300_arm_sphere_collision.urdf", 7, - 7 /* robot */ + - 3 /* world, root, base */ }, - {"j2s7s300_hand_sphere_collision.urdf", 3, - 1 /* end effector */ + - 3 * 2 /* two per finger */ + - 2 /* world, base */ }, - {"j2s7s300_hand.urdf", 3, - 1 /* end effector */ + - 3 * 2 /* two per finger */ + - 2 /* world, base */ }, - }; -} -// clang-format on - -INSTANTIATE_TEST_SUITE_P(JacoArmTest, JacoArmParamsTest, - testing::ValuesIn(GenerateTestCases())); - -} // namespace -} // namespace manipulation -} // namespace drake diff --git a/manipulation/models/ur3e/BUILD.bazel b/manipulation/models/ur3e/BUILD.bazel index 6d66807a9d0c..daf232e8de2a 100644 --- a/manipulation/models/ur3e/BUILD.bazel +++ b/manipulation/models/ur3e/BUILD.bazel @@ -1,8 +1,3 @@ -load( - "//tools/skylark:drake_cc.bzl", - "drake_cc_googletest", -) -load("//tools/install:install_data.bzl", "install_data") load("//tools/lint:lint.bzl", "add_lint_tests") load("//tools/workspace/ros_xacro_internal:defs.bzl", "xacro_file") @@ -35,11 +30,6 @@ filegroup( visibility = ["//visibility:private"], ) -install_data( - name = "install_data", - data = [":glob_models"], -) - filegroup( name = "models", srcs = [ @@ -64,18 +54,4 @@ filegroup( ], ) -# === test/ === - -drake_cc_googletest( - name = "parse_test", - srcs = ["test/parse_test.cc"], - data = [ - ":models", - ], - deps = [ - "//common:find_resource", - "//multibody/parsing", - ], -) - add_lint_tests() diff --git a/manipulation/models/ur3e/VESTIGIAL.md b/manipulation/models/ur3e/VESTIGIAL.md new file mode 120000 index 000000000000..32d46ee883b5 --- /dev/null +++ b/manipulation/models/ur3e/VESTIGIAL.md @@ -0,0 +1 @@ +../README.md \ No newline at end of file diff --git a/manipulation/models/ur3e/test/parse_test.cc b/manipulation/models/ur3e/test/parse_test.cc deleted file mode 100644 index 452a232820d6..000000000000 --- a/manipulation/models/ur3e/test/parse_test.cc +++ /dev/null @@ -1,41 +0,0 @@ -#include - -#include - -#include "drake/common/find_resource.h" -#include "drake/multibody/parsing/parser.h" -#include "drake/multibody/plant/multibody_plant.h" - -namespace drake { -namespace manipulation { -namespace { - -GTEST_TEST(ParseTest, SpheresCollision) { - multibody::MultibodyPlant plant(0.0); - geometry::SceneGraph scene_graph; - plant.RegisterAsSourceForSceneGraph(&scene_graph); - multibody::Parser parser(&plant); - parser.AddModels(FindResourceOrThrow( - "drake/manipulation/models/ur3e/ur3e_spheres_collision.urdf")); - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ur_base_link")); - plant.Finalize(); - - EXPECT_EQ(plant.num_positions(), 6); -} - -GTEST_TEST(ParseTest, CylindersCollision) { - multibody::MultibodyPlant plant(0.0); - geometry::SceneGraph scene_graph; - plant.RegisterAsSourceForSceneGraph(&scene_graph); - multibody::Parser parser(&plant); - parser.AddModels(FindResourceOrThrow( - "drake/manipulation/models/ur3e/ur3e_cylinders_collision.urdf")); - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ur_base_link")); - plant.Finalize(); - - EXPECT_EQ(plant.num_positions(), 6); -} - -} // namespace -} // namespace manipulation -} // namespace drake diff --git a/manipulation/models/wsg_50_description/BUILD.bazel b/manipulation/models/wsg_50_description/BUILD.bazel index b64dfd3cb5b8..14fbe3463cec 100644 --- a/manipulation/models/wsg_50_description/BUILD.bazel +++ b/manipulation/models/wsg_50_description/BUILD.bazel @@ -1,9 +1,4 @@ -load( - "//tools/skylark:drake_cc.bzl", - "drake_cc_googletest", -) load("//tools/skylark:drake_data.bzl", "models_filegroup") -load("//tools/install:install_data.bzl", "install_data") load("//tools/lint:lint.bzl", "add_lint_tests") package(default_visibility = [":__subpackages__"]) @@ -13,12 +8,6 @@ models_filegroup( visibility = ["//visibility:private"], ) -install_data( - name = "install_data", - data = [":glob_models"], - visibility = ["//visibility:public"], -) - filegroup( name = "models", srcs = [ @@ -30,17 +19,4 @@ filegroup( visibility = ["//visibility:public"], ) -# === test/ === - -drake_cc_googletest( - name = "wsg_50_sdf_test", - srcs = ["sdf/test/wsg_50_sdf_test.cc"], - data = [":models"], - deps = [ - "//common:find_resource", - "//multibody/parsing", - "//multibody/plant", - ], -) - add_lint_tests() diff --git a/manipulation/models/wsg_50_description/VESTIGIAL.md b/manipulation/models/wsg_50_description/VESTIGIAL.md new file mode 120000 index 000000000000..32d46ee883b5 --- /dev/null +++ b/manipulation/models/wsg_50_description/VESTIGIAL.md @@ -0,0 +1 @@ +../README.md \ No newline at end of file diff --git a/manipulation/models/wsg_50_description/sdf/test/wsg_50_sdf_test.cc b/manipulation/models/wsg_50_description/sdf/test/wsg_50_sdf_test.cc deleted file mode 100644 index 01ab2e26a7d5..000000000000 --- a/manipulation/models/wsg_50_description/sdf/test/wsg_50_sdf_test.cc +++ /dev/null @@ -1,54 +0,0 @@ -#include - -#include -#include - -#include "drake/common/find_resource.h" -#include "drake/multibody/parsing/parser.h" -#include "drake/multibody/plant/multibody_plant.h" - -namespace drake { -namespace manipulation { -namespace { - -// Tests that all the Sdf files can be parsed into a plant. -GTEST_TEST(Wsg50DescriptionSdfTest, TestBoxSchunkSdf) { - const std::string prefix("drake/manipulation/models/wsg_50_description/sdf/"); - const std::vector file_names{ - "schunk_wsg_50.sdf", "schunk_wsg_50_ball_contact.sdf", - "schunk_wsg_50_no_tip.sdf", "schunk_wsg_50_with_tip.sdf"}; - - for (size_t i = 0; i < file_names.size(); i++) { - const std::string kPath(FindResourceOrThrow(prefix + file_names[i])); - - multibody::MultibodyPlant plant(0.0); - multibody::Parser parser(&plant); - parser.AddModels(kPath); - plant.Finalize(); - - // Expect 3 model instances. One for the world, one default model instance - // for unspecified modeling elements, and the gripper model instance. - EXPECT_EQ(plant.num_model_instances(), 3); - - // Expect 9 positions, 6 attached to the body, 2 to each joints of the - // gripper, and the first one reserved for multibody plant. - EXPECT_EQ(plant.num_positions(), 9); - - // Expect 8 velocities, 6 attached to the body, 2 to each joints of the - // gripper. - EXPECT_EQ(plant.num_velocities(), 8); - - ASSERT_EQ(plant.num_bodies(), 4); - const std::vector expected_body_names{ - "" /* Ignore the world body name */, "body", "left_finger", - "right_finger"}; - for (multibody::BodyIndex j{1}; j < plant.num_bodies(); ++j) { - EXPECT_THAT(plant.get_body(j).name(), - testing::EndsWith(expected_body_names.at(j))); - } - } -} - -} // namespace -} // namespace manipulation -} // namespace drake diff --git a/tools/wheel/image/build-wheel.sh b/tools/wheel/image/build-wheel.sh index d09161978aa0..71f5873bd2db 100755 --- a/tools/wheel/image/build-wheel.sh +++ b/tools/wheel/image/build-wheel.sh @@ -87,7 +87,6 @@ cp -r -t ${WHEEL_SHARE_DIR}/drake \ /opt/drake/share/drake/package.xml \ /opt/drake/share/drake/examples \ /opt/drake/share/drake/geometry \ - /opt/drake/share/drake/manipulation \ /opt/drake/share/drake/multibody \ /opt/drake/share/drake/tutorials diff --git a/tools/workspace/drake_models/repository.bzl b/tools/workspace/drake_models/repository.bzl index 2f5054fa61f8..09f0d874838d 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 = "cb72f82b5276cff8257ee13ab59f10327bf9f8f7", - sha256 = "554bb278928e103c005ba2a491c78242a2742f717cda6427570e0e9288f5c84e", # noqa + commit = "7ede868f6dd76f1a68c9dc34da991e16ba61dfca", + sha256 = "e32cda48618fefd28dca06044916c4aad1b46a9de0c410835b5ccb46581826d1", # noqa build_file = ":package.BUILD.bazel", mirrors = mirrors, ) diff --git a/tools/workspace/drake_models/test/parse_test.py b/tools/workspace/drake_models/test/parse_test.py index 21cf17539097..eed9a98e5500 100644 --- a/tools/workspace/drake_models/test/parse_test.py +++ b/tools/workspace/drake_models/test/parse_test.py @@ -67,6 +67,8 @@ def test_all_models(self): # wants to fix the warnings, be our guest. "package://drake_models/allegro_hand_description/urdf/allegro_hand_description_left.urdf", # noqa "package://drake_models/allegro_hand_description/urdf/allegro_hand_description_right.urdf", # noqa + # TODO(jwnimmer-tri) Fix these warnings. + "package://drake_models/jaco_description/urdf/j2n6s300_col.urdf", # We don't have any tracking issue for fixing the PR2 models, # because we don't use them for anything we care about. If someone # wants to fix the warnings, be our guest. diff --git a/tutorials/authoring_multibody_simulation.ipynb b/tutorials/authoring_multibody_simulation.ipynb index 8661db21d624..9435aadef950 100644 --- a/tutorials/authoring_multibody_simulation.ipynb +++ b/tutorials/authoring_multibody_simulation.ipynb @@ -96,8 +96,8 @@ "source": [ "# First we'll choose one of Drake's example model files, a KUKA iiwa arm.\n", "iiwa7_model_url = (\n", - " \"package://drake/manipulation/models/\"\n", - " \"iiwa_description/iiwa7/iiwa7_with_box_collision.sdf\")\n", + " \"package://drake_models/iiwa_description/sdf/\"\n", + " \"iiwa7_with_box_collision.sdf\")\n", "\n", "# Create a model visualizer and add the robot arm.\n", "visualizer = ModelVisualizer(meshcat=meshcat)\n", @@ -121,8 +121,8 @@ "source": [ "# Choose another one of Drake's example model files, a Schunk WSG gripper.\n", "schunk_wsg50_model_url = (\n", - " \"package://drake/manipulation/models/\"\n", - " \"wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf\")\n", + " \"package://drake_models/wsg_50_description/sdf/\"\n", + " \"schunk_wsg_50_with_tip.sdf\")\n", "\n", "# Create a NEW model visualizer and add the robot gripper.\n", "visualizer = ModelVisualizer(meshcat=meshcat)\n", diff --git a/tutorials/mathematical_program_multibody_plant.ipynb b/tutorials/mathematical_program_multibody_plant.ipynb index bc02dd3b35be..8ecf1782f056 100644 --- a/tutorials/mathematical_program_multibody_plant.ipynb +++ b/tutorials/mathematical_program_multibody_plant.ipynb @@ -83,8 +83,8 @@ "source": [ "plant_f = MultibodyPlant(0.0)\n", "iiwa_url = (\n", - " \"package://drake/manipulation/models/iiwa_description/sdf/\"\n", - " \"iiwa14_no_collision.sdf\")\n", + " \"package://drake_models/iiwa_description/sdf/iiwa14_no_collision.sdf\"\n", + ")\n", "(iiwa,) = Parser(plant_f).AddModels(url=iiwa_url)\n", "\n", "# Define some short aliases for frames.\n", diff --git a/tutorials/rendering_multibody_plant.ipynb b/tutorials/rendering_multibody_plant.ipynb index 83a1d1a952d9..143a75893115 100644 --- a/tutorials/rendering_multibody_plant.ipynb +++ b/tutorials/rendering_multibody_plant.ipynb @@ -139,8 +139,7 @@ "outputs": [], "source": [ "iiwa_url = (\n", - " \"package://drake/manipulation/models/iiwa_description/sdf/\"\n", - " \"iiwa14_no_collision.sdf\"\n", + " \"package://drake_models/iiwa_description/sdf/iiwa14_no_collision.sdf\"\n", ")" ] },