Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[parsing] Add support for URDF mimic element. #18728

Merged
merged 1 commit into from
Feb 3, 2023
Merged

[parsing] Add support for URDF mimic element. #18728

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
108 changes: 89 additions & 19 deletions multibody/parsing/detail_urdf_parser.cc
View file
Open in desktop
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,37 +568,107 @@ 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) {
throw_on_custom_joint(true);
Vector3d damping_vec(0, 0, 0);
XMLElement* dynamics_node = node->FirstChildElement("dynamics");
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
Open in desktop
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
Open in desktop
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);
}

TEST_F(UrdfParserTest, JointParsingTagMismatchTest) {
Expand Down Expand Up @@ -1472,8 +1606,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
Open in desktop
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