Skip to content

Commit

Permalink
[parsing] Add support for URDF mimic element. (#18728)
Browse files Browse the repository at this point in the history
  • Loading branch information
@RussTedrake
RussTedrake authored Feb 3, 2023
1 parent 60ea3d0 commit 1397c24
Show file tree
Hide file tree
Showing 4 changed files with 238 additions and 23 deletions.
108 changes: 89 additions & 19 deletions multibody/parsing/detail_urdf_parser.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -533,30 +532,31 @@ void UrdfParser::ParseJoint(
};

auto plant = w_.plant;
std::optional<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<RevoluteJoint>(
index = plant->AddJoint<RevoluteJoint>(
name, *parent_body, X_PJ,
*child_body, std::nullopt, axis, lower, upper, damping).index();
Joint<double>& joint = plant->get_mutable_joint(index);
Joint<double>& joint = plant->get_mutable_joint(*index);
joint.set_velocity_limits(Vector1d(-velocity), Vector1d(velocity));
joint.set_acceleration_limits(
Vector1d(-acceleration), Vector1d(acceleration));
} else if (type.compare("fixed") == 0) {
throw_on_custom_joint(false);
plant->AddJoint<WeldJoint>(name, *parent_body, X_PJ,
*child_body, std::nullopt,
RigidTransformd::Identity());
index = plant->AddJoint<WeldJoint>(
name, *parent_body, X_PJ, *child_body, std::nullopt,
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<PrismaticJoint>(
name, *parent_body, X_PJ,
*child_body, std::nullopt, axis, lower, upper, damping).index();
Joint<double>& joint = plant->get_mutable_joint(index);
index = plant->AddJoint<PrismaticJoint>(
name, *parent_body, X_PJ, *child_body, std::nullopt, axis, lower,
upper, damping).index();
Joint<double>& joint = plant->get_mutable_joint(*index);
joint.set_velocity_limits(Vector1d(-velocity), Vector1d(velocity));
joint.set_acceleration_limits(
Vector1d(-acceleration), Vector1d(acceleration));
Expand All @@ -568,8 +568,8 @@ void UrdfParser::ParseJoint(
} else if (type.compare("ball") == 0) {
throw_on_custom_joint(true);
ParseJointDynamics(node, &damping);
plant->AddJoint<BallRpyJoint>(name, *parent_body, X_PJ,
*child_body, std::nullopt, damping);
index = plant->AddJoint<BallRpyJoint>(
name, *parent_body, X_PJ, *child_body, std::nullopt, damping).index();
} else if (type.compare("planar") == 0) {
// Permit both the standard 'joint' and custom 'drake:joint' spellings
// here. The standard spelling was actually always correct, but Drake only
Expand All @@ -580,28 +580,98 @@ void UrdfParser::ParseJoint(
if (dynamics_node) {
ParseVectorAttribute(dynamics_node, "damping", &damping_vec);
}
plant->AddJoint<PlanarJoint>(name, *parent_body, X_PJ,
*child_body, std::nullopt, damping_vec);
index = plant->AddJoint<PlanarJoint>(
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<ScrewJoint>(name, *parent_body, X_PJ, *child_body,
std::nullopt, axis, screw_thread_pitch,
damping);
index = plant->AddJoint<ScrewJoint>(
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<UniversalJoint>(name, *parent_body, X_PJ,
*child_body, std::nullopt, damping);
index = plant->AddJoint<UniversalJoint>(
name, *parent_body, X_PJ, *child_body, std::nullopt, damping).index();
} else {
Error(*node, fmt::format("Joint '{}' has unrecognized type: '{}'",
name, type));
return;
}

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);

if (!index) {
// This can currently happen if we have a "floating" joint, which does
// not produce the actual QuaternionFloatingJoint above.
Warning(*mimic_node,
fmt::format("Drake only supports the mimic element for "
"single-dof joints. The mimic element in joint "
"'{}' will be ignored.",
name));
} else {
const Joint<double>& joint0 = plant->get_joint(*index);
const Joint<double>& 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("Drake only supports the mimic element for "
"single-dof joints. The joint '{}' (with {} "
"dofs) is attempting to mimic joint '{}' (with "
"{} dofs). The mimic element will be ignored.",
name, joint0.num_velocities(), joint_to_mimic,
joint1.num_velocities()));
} else {
plant->AddCouplerConstraint(joint0, joint1, gear_ratio, offset);
}
}
}
}
}

void UrdfParser::ParseMechanicalReduction(const XMLElement& node) {
Expand Down
1 change: 0 additions & 1 deletion multibody/parsing/parsing_doxygen.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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`
Expand Down
140 changes: 137 additions & 3 deletions multibody/parsing/test/detail_urdf_parser_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,9 @@ class UrdfParserTest : public test::DiagnosticPolicyTestBase {

protected:
PackageMap package_map_;
MultibodyPlant<double> plant_{0.0};
// Note: We currently use a discrete plant here to be able to test
// Sap-specific features like the joint 'mimic' element.
MultibodyPlant<double> plant_{0.1};
SceneGraph<double> scene_graph_;
};

Expand Down Expand Up @@ -328,6 +330,130 @@ TEST_F(UrdfParserTest, JointTypeUnknown) {
".*Joint 'joint' has unrecognized type: 'who'"));
}

// TODO(rpoyner-tri): Add MimicContinuousTime (which should throw the same
// warning as MimicNoSap).

TEST_F(UrdfParserTest, MimicNoSap) {
plant_.set_discrete_contact_solver(DiscreteContactSolver::kTamsi);
EXPECT_NE(AddModelFromUrdfString(R"""(
<robot name='a'>
<link name='parent'/>
<link name='child'/>
<joint name='joint' type='revolute'>
<parent link='parent'/>
<child link='child'/>
<mimic/>
</joint>
</robot>)""", ""), 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"""(
<robot name='a'>
<link name='parent'/>
<link name='child'/>
<joint name='joint' type='revolute'>
<parent link='parent'/>
<child link='child'/>
<mimic/>
</joint>
</robot>)""", ""), 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"""(
<robot name='a'>
<link name='parent'/>
<link name='child'/>
<joint name='joint' type='revolute'>
<parent link='parent'/>
<child link='child'/>
<mimic joint='nonexistent'/>
</joint>
</robot>)""", ""), 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"""(
<robot name='a'>
<link name='parent'/>
<link name='child0'/>
<link name='child1'/>
<joint name='joint0' type='fixed'>
<parent link='parent'/>
<child link='child0'/>
</joint>
<joint name='joint1' type='revolute'>
<parent link='parent'/>
<child link='child1'/>
<mimic joint='joint0'/>
</joint>
</robot>)""", ""), std::nullopt);
EXPECT_THAT(TakeError(),
MatchesRegex(".*Joint 'joint1' which has 1 DOF cannot mimic "
"joint 'joint0' which has 0 DOF."));
}

TEST_F(UrdfParserTest, MimicOnlyOneDOFJoint) {
plant_.set_discrete_contact_solver(DiscreteContactSolver::kSap);
EXPECT_NE(AddModelFromUrdfString(R"""(
<robot name='a'>
<link name='parent'/>
<link name='child0'/>
<link name='child1'/>
<joint name='joint0' type='fixed'>
<parent link='parent'/>
<child link='child0'/>
</joint>
<joint name='joint1' type='fixed'>
<parent link='parent'/>
<child link='child1'/>
<mimic joint='joint0'/>
</joint>
</robot>)""", ""), std::nullopt);
EXPECT_THAT(TakeWarning(),
MatchesRegex(".*Drake only supports the mimic element for "
"single-dof joints.*"));
}

TEST_F(UrdfParserTest, MimicFloatingJoint) {
plant_.set_discrete_contact_solver(DiscreteContactSolver::kSap);
EXPECT_NE(AddModelFromUrdfString(R"""(
<robot name='a'>
<link name='parent'/>
<link name='child0'/>
<link name='child1'/>
<joint name='joint0' type='fixed'>
<parent link='parent'/>
<child link='child0'/>
</joint>
<joint name='joint1' type='floating'>
<parent link='parent'/>
<child link='child1'/>
<mimic joint='joint0'/>
</joint>
</robot>)""", ""), std::nullopt);
TakeWarning(); // The first warning is about not supporting floating joints.
// See issue #13691.
EXPECT_THAT(TakeWarning(),
MatchesRegex(".*Drake only supports the mimic element for "
"single-dof joints.*"));
}

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.
Expand Down Expand Up @@ -592,6 +718,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");
Expand Down Expand Up @@ -766,6 +894,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);
}

// Custom planar joints were not necessary, but long supported. See #18730.
Expand Down Expand Up @@ -1486,8 +1620,8 @@ TEST_F(UrdfParserTest, UnsupportedLinkTypeIgnored) {
}

TEST_F(UrdfParserTest, UnsupportedJointStuffIgnored) {
const std::array<std::string, 3> tags{
"calibration", "mimic", "safety_controller"};
const std::array<std::string, 2> tags{
"calibration", "safety_controller"};
for (const auto& tag : tags) {
EXPECT_NE(AddModelFromUrdfString(fmt::format(R"""(
<robot>
Expand Down
12 changes: 12 additions & 0 deletions multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,12 @@ Defines a URDF model with various types of joints.
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
</link>
<link name="link10">
<inertial>
<mass value="1"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
</link>
<joint name="fixed_joint" type="fixed">
<parent link="world"/>
<child link="link1"/>
Expand Down Expand Up @@ -115,6 +121,12 @@ Defines a URDF model with various types of joints.
<drake:screw_thread_pitch value="0.04"/>
<dynamics damping="0.1"/>
</drake:joint>
<joint name="revolute_joint_with_mimic" type="revolute">
<axis xyz="0 0 1"/>
<parent link="link9"/>
<child link="link10"/>
<mimic joint="revolute_joint" multiplier="1.23" offset="4.56"/>
</joint>
<!-- Normal transmission/joint, should be created with appropriate
effort limit. -->
<transmission>
Expand Down

0 comments on commit 1397c24

Please sign in to comment.