From 364fde89b3e4ee34bf2458c0ea004ad9d9701b68 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Thu, 2 Feb 2023 08:05:09 -0500 Subject: [PATCH] [parsing] Add support for URDF mimic element. Implements parsing support for the `` element in URDF. Towards #17663. Users could have been using existing URDF models in Drake with mimic tags that were ignored. Now those models will be simulated differently if they are being simulated with DiscreteContactSolver::kSap. --- multibody/parsing/detail_urdf_parser.cc | 90 +++++++++++--- multibody/parsing/parsing_doxygen.h | 1 - .../parsing/test/detail_urdf_parser_test.cc | 111 +++++++++++++++++- .../urdf_parser_test/joint_parsing_test.urdf | 12 ++ 4 files changed, 194 insertions(+), 20 deletions(-) diff --git a/multibody/parsing/detail_urdf_parser.cc b/multibody/parsing/detail_urdf_parser.cc index 203167979520..9ba2fbaaad2e 100644 --- a/multibody/parsing/detail_urdf_parser.cc +++ b/multibody/parsing/detail_urdf_parser.cc @@ -463,7 +463,6 @@ void UrdfParser::ParseJoint( return; } WarnUnsupportedElement(*node, "calibration"); - WarnUnsupportedElement(*node, "mimic"); WarnUnsupportedElement(*node, "safety_controller"); // Parses the parent and child link names. @@ -533,11 +532,12 @@ void UrdfParser::ParseJoint( }; auto plant = w_.plant; + JointIndex index{}; if (type.compare("revolute") == 0 || type.compare("continuous") == 0) { throw_on_custom_joint(false); ParseJointLimits(node, &lower, &upper, &velocity, &acceleration, &effort); ParseJointDynamics(node, &damping); - const JointIndex index = plant->AddJoint( + index = plant->AddJoint( name, *parent_body, X_PJ, *child_body, std::nullopt, axis, lower, upper, damping).index(); Joint& joint = plant->get_mutable_joint(index); @@ -546,16 +546,16 @@ void UrdfParser::ParseJoint( Vector1d(-acceleration), Vector1d(acceleration)); } else if (type.compare("fixed") == 0) { throw_on_custom_joint(false); - plant->AddJoint(name, *parent_body, X_PJ, + index = plant->AddJoint(name, *parent_body, X_PJ, *child_body, std::nullopt, - RigidTransformd::Identity()); + RigidTransformd::Identity()).index(); } else if (type.compare("prismatic") == 0) { throw_on_custom_joint(false); ParseJointLimits(node, &lower, &upper, &velocity, &acceleration, &effort); ParseJointDynamics(node, &damping); - const JointIndex index = plant->AddJoint( - name, *parent_body, X_PJ, - *child_body, std::nullopt, axis, lower, upper, damping).index(); + index = plant->AddJoint( + name, *parent_body, X_PJ, *child_body, std::nullopt, axis, lower, + upper, damping).index(); Joint& joint = plant->get_mutable_joint(index); joint.set_velocity_limits(Vector1d(-velocity), Vector1d(velocity)); joint.set_acceleration_limits( @@ -568,8 +568,8 @@ void UrdfParser::ParseJoint( } else if (type.compare("ball") == 0) { throw_on_custom_joint(true); ParseJointDynamics(node, &damping); - plant->AddJoint(name, *parent_body, X_PJ, - *child_body, std::nullopt, damping); + index = plant->AddJoint( + name, *parent_body, X_PJ, *child_body, std::nullopt, damping).index(); } else if (type.compare("planar") == 0) { throw_on_custom_joint(true); Vector3d damping_vec(0, 0, 0); @@ -577,21 +577,21 @@ void UrdfParser::ParseJoint( if (dynamics_node) { ParseVectorAttribute(dynamics_node, "damping", &damping_vec); } - plant->AddJoint(name, *parent_body, X_PJ, - *child_body, std::nullopt, damping_vec); + index = plant->AddJoint( + name, *parent_body, X_PJ, *child_body, std::nullopt, damping_vec).index(); } else if (type.compare("screw") == 0) { throw_on_custom_joint(true); ParseJointDynamics(node, &damping); double screw_thread_pitch; ParseScrewJointThreadPitch(node, &screw_thread_pitch); - plant->AddJoint(name, *parent_body, X_PJ, *child_body, - std::nullopt, axis, screw_thread_pitch, - damping); + index = plant->AddJoint( + name, *parent_body, X_PJ, *child_body, std::nullopt, axis, + screw_thread_pitch, damping).index(); } else if (type.compare("universal") == 0) { throw_on_custom_joint(true); ParseJointDynamics(node, &damping); - plant->AddJoint(name, *parent_body, X_PJ, - *child_body, std::nullopt, damping); + index = plant->AddJoint( + name, *parent_body, X_PJ, *child_body, std::nullopt, damping).index(); } else { Error(*node, fmt::format("Joint '{}' has unrecognized type: '{}'", name, type)); @@ -599,6 +599,64 @@ void UrdfParser::ParseJoint( } joint_effort_limits->emplace(name, effort); + + XMLElement* mimic_node = node->FirstChildElement("mimic"); + if (mimic_node) { + if (!plant->is_discrete() || + plant->get_discrete_contact_solver() != DiscreteContactSolver::kSap) { + Warning( + *mimic_node, + fmt::format("Joint '{}' specifies a mimic element that will be " + "ignored. Mimic elements are currently only supported by " + "MultibodyPlant with a discrete time step and using " + "DiscreteContactSolver::kSap.", + name)); + } else { + std::string joint_to_mimic; + double gear_ratio{1.0}; + double offset{0.0}; + if (!ParseStringAttribute(mimic_node, "joint", &joint_to_mimic)) { + Error(*mimic_node, + fmt::format("Joint '{}' mimic element is missing the " + "required 'joint' attribute.", + name)); + return; + } + if (!plant->HasJointNamed(joint_to_mimic, model_instance_)) { + Error(*mimic_node, + fmt::format("Joint '{}' mimic element specifies joint '{}' which" + " does not exist.", + name, joint_to_mimic)); + return; + } + ParseScalarAttribute(mimic_node, "multiplier", &gear_ratio); + ParseScalarAttribute(mimic_node, "offset", &offset); + + const Joint& joint0 = plant->get_joint(index); + const Joint& joint1 = + plant->GetJointByName(joint_to_mimic, model_instance_); + if (joint1.num_velocities() != joint0.num_velocities()) { + Error(*mimic_node, + fmt::format("Joint '{}' which has {} DOF cannot mimic " + "joint '{}' which has {} DOF.", + name, joint0.num_velocities(), joint_to_mimic, + joint1.num_velocities())); + return; + } + if (joint0.num_velocities() != 1) { + // The URDF documentation is ambiguous as to whether multi-dof joints + // are supported by the mimic tag. So we only raise a warning, not an + // error. + Warning(*mimic_node, + fmt::format( + "Joint '{}' mimic element is only supported for " + "single-dof joints. This mimic element will be ignored.", + name)); + } else { + plant->AddCouplerConstraint(joint0, joint1, gear_ratio, offset); + } + } + } } void UrdfParser::ParseMechanicalReduction(const XMLElement& node) { diff --git a/multibody/parsing/parsing_doxygen.h b/multibody/parsing/parsing_doxygen.h index 8f2c9e24b72c..52d42448711c 100644 --- a/multibody/parsing/parsing_doxygen.h +++ b/multibody/parsing/parsing_doxygen.h @@ -106,7 +106,6 @@ one of several treaments: - `/robot/@version` - `/robot/joint/calibration` -- `/robot/joint/mimic` - `/robot/joint/safety_controller` - `/robot/link/@type` - `/robot/link/collision/verbose` diff --git a/multibody/parsing/test/detail_urdf_parser_test.cc b/multibody/parsing/test/detail_urdf_parser_test.cc index 65da4b401450..5f1953a52c5e 100644 --- a/multibody/parsing/test/detail_urdf_parser_test.cc +++ b/multibody/parsing/test/detail_urdf_parser_test.cc @@ -79,7 +79,7 @@ class UrdfParserTest : public test::DiagnosticPolicyTestBase { protected: PackageMap package_map_; - MultibodyPlant plant_{0.0}; + MultibodyPlant plant_{0.1}; SceneGraph scene_graph_; }; @@ -328,6 +328,103 @@ TEST_F(UrdfParserTest, JointTypeUnknown) { ".*Joint 'joint' has unrecognized type: 'who'")); } +TEST_F(UrdfParserTest, MimicNoSap) { + EXPECT_NE(AddModelFromUrdfString(R"""( + + + + + + + + + )""", ""), std::nullopt); + EXPECT_THAT( + TakeWarning(), + MatchesRegex( + ".*Mimic elements are currently only supported by MultibodyPlant " + "with a discrete time step and using DiscreteContactSolver::kSap.")); +} + +TEST_F(UrdfParserTest, MimicNoJoint) { + plant_.set_discrete_contact_solver(DiscreteContactSolver::kSap); + EXPECT_NE(AddModelFromUrdfString(R"""( + + + + + + + + + )""", ""), std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Joint 'joint' mimic element is missing the " + "required 'joint' attribute.")); +} + +TEST_F(UrdfParserTest, MimicBadJoint) { + plant_.set_discrete_contact_solver(DiscreteContactSolver::kSap); + EXPECT_NE(AddModelFromUrdfString(R"""( + + + + + + + + + )""", ""), std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Joint 'joint' mimic element specifies joint " + "'nonexistent' which does not exist.")); +} + +TEST_F(UrdfParserTest, MimicMismatchedJoint) { + plant_.set_discrete_contact_solver(DiscreteContactSolver::kSap); + EXPECT_NE(AddModelFromUrdfString(R"""( + + + + + + + + + + + + + + )""", ""), std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Joint 'joint1' which has 1 DOF cannot mimic " + "joint 'joint0' which has 0 DOF.")); +} + +TEST_F(UrdfParserTest, MimicMultiDOFJoint) { + plant_.set_discrete_contact_solver(DiscreteContactSolver::kSap); + EXPECT_NE(AddModelFromUrdfString(R"""( + + + + + + + + + + + + + + )""", ""), std::nullopt); + EXPECT_THAT( + TakeWarning(), + MatchesRegex(".*Joint 'joint1' mimic element is only supported for " + "single-dof joints. This mimic element will be ignored.")); +} + TEST_F(UrdfParserTest, Material) { // Material parsing is tested fully elsewhere (see ParseMaterial()). This // test is just proof-of-life that top-level material stanzas are recognized. @@ -592,6 +689,8 @@ TEST_F(UrdfParserTest, TestRegisteredSceneGraph) { } TEST_F(UrdfParserTest, JointParsingTest) { + // We currently need kSap for the mimic element to parse without error. + plant_.set_discrete_contact_solver(DiscreteContactSolver::kSap); const std::string full_name = FindResourceOrThrow( "drake/multibody/parsing/test/urdf_parser_test/" "joint_parsing_test.urdf"); @@ -766,6 +865,12 @@ TEST_F(UrdfParserTest, JointParsingTest) { CompareMatrices(screw_joint.acceleration_lower_limits(), neg_inf)); EXPECT_TRUE( CompareMatrices(screw_joint.acceleration_upper_limits(), inf)); + + // Revolute joint with mimic + DRAKE_EXPECT_NO_THROW(plant_.GetJointByName("revolute_joint_with_mimic")); + // TODO(russt): Test coupler constraint properties once constraint getters are + // provided by MultibodyPlant (currently a TODO in multibody_plant.h). + EXPECT_EQ(plant_.num_constraints(), 1); } TEST_F(UrdfParserTest, JointParsingTagMismatchTest) { @@ -1472,8 +1577,8 @@ TEST_F(UrdfParserTest, UnsupportedLinkTypeIgnored) { } TEST_F(UrdfParserTest, UnsupportedJointStuffIgnored) { - const std::array tags{ - "calibration", "mimic", "safety_controller"}; + const std::array tags{ + "calibration", "safety_controller"}; for (const auto& tag : tags) { EXPECT_NE(AddModelFromUrdfString(fmt::format(R"""( diff --git a/multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf b/multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf index 855753d12cd5..c6556118c26c 100644 --- a/multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf +++ b/multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf @@ -57,6 +57,12 @@ Defines a URDF model with various types of joints. + + + + + + @@ -115,6 +121,12 @@ Defines a URDF model with various types of joints. + + + + + +