Parse joint effort limits from URDF (RobotLocomotion#12863)

Fixes RobotLocomotion#12859

multibody/benchmarks/acrobot/acrobot.urdf

@@ -69,7 +69,6 @@
     <child link="Link2"/>
     <axis xyz="0 1 0"/>
     <origin xyz="0 0.15 -1"/>
-    <limit effort="0"/>
     <dynamics damping="0.1"/>
   </joint>
 

multibody/parsing/detail_urdf_parser.cc

@@ -283,16 +283,18 @@ void ParseJointKeyParams(XMLElement* node,
 }
 
 void ParseJointLimits(XMLElement* node, double* lower, double* upper,
-                      double* velocity) {
+                      double* velocity, double* effort) {
   *lower = -std::numeric_limits<double>::infinity();
   *upper = std::numeric_limits<double>::infinity();
   *velocity = std::numeric_limits<double>::infinity();
+  *effort = std::numeric_limits<double>::infinity();
 
   XMLElement* limit_node = node->FirstChildElement("limit");
   if (limit_node) {
     ParseScalarAttribute(limit_node, "lower", lower);
     ParseScalarAttribute(limit_node, "upper", upper);
     ParseScalarAttribute(limit_node, "velocity", velocity);
+    ParseScalarAttribute(limit_node, "effort", effort);
   }
 }
 
@@ -338,6 +340,7 @@ const Body<double>& GetBodyForElement(
 }
 
 void ParseJoint(ModelInstanceIndex model_instance,
+                std::map<std::string, double>* joint_effort_limits,
                 XMLElement* node,
                 MultibodyPlant<double>* plant) {
   std::string drake_ignore;
@@ -373,14 +376,25 @@ void ParseJoint(ModelInstanceIndex model_instance,
     axis.normalize();
   }
 
-  // These are only used by some joint types.
-  double upper = 0;
-  double lower = 0;
+  // Joint properties -- these are only used by some joint types.
+
+  // Dynamic properties
   double damping = 0;
-  double velocity = 0;
+
+  // Limits
+  double upper = std::numeric_limits<double>::infinity();
+  double lower = -std::numeric_limits<double>::infinity();
+  double velocity = std::numeric_limits<double>::infinity();
+
+  // In MultibodyPlant, the effort limit is a property of the actuator, which
+  // isn't created until the transmission element is parsed.  Stash a value
+  // for all joints when parsing the joint element so that we can look it up
+  // later if/when an actuator is created.
+  double effort = std::numeric_limits<double>::infinity();
+
 
   if (type.compare("revolute") == 0 || type.compare("continuous") == 0) {
-    ParseJointLimits(node, &lower, &upper, &velocity);
+    ParseJointLimits(node, &lower, &upper, &velocity, &effort);
     ParseJointDynamics(name, node, &damping);
     const JointIndex index = plant->AddJoint<RevoluteJoint>(
         name, parent_body, X_PJ,
@@ -392,7 +406,7 @@ void ParseJoint(ModelInstanceIndex model_instance,
                                child_body, std::nullopt,
                                RigidTransformd::Identity());
   } else if (type.compare("prismatic") == 0) {
-    ParseJointLimits(node, &lower, &upper, &velocity);
+    ParseJointLimits(node, &lower, &upper, &velocity, &effort);
     ParseJointDynamics(name, node, &damping);
     const JointIndex index = plant->AddJoint<PrismaticJoint>(
         name, parent_body, X_PJ,
@@ -412,11 +426,15 @@ void ParseJoint(ModelInstanceIndex model_instance,
     throw std::runtime_error(
         "ERROR: Joint " + name + " has unrecognized type: " + type);
   }
+
+  joint_effort_limits->emplace(name, effort);
 }
 
-void ParseTransmission(ModelInstanceIndex model_instance,
-                       XMLElement* node,
-                       MultibodyPlant<double>* plant) {
+void ParseTransmission(
+    ModelInstanceIndex model_instance,
+    const std::map<std::string, double>& joint_effort_limits,
+    XMLElement* node,
+    MultibodyPlant<double>* plant) {
   // Determines the transmission type.
   std::string type;
   XMLElement* type_node = node->FirstChildElement("type");
@@ -484,7 +502,23 @@ void ParseTransmission(ModelInstanceIndex model_instance,
     return;
   }
 
-  plant->AddJointActuator(actuator_name, joint);
+  const auto effort_iter = joint_effort_limits.find(joint_name);
+  DRAKE_DEMAND(effort_iter != joint_effort_limits.end());
+  if (effort_iter->second < 0) {
+    throw std::runtime_error(
+        "ERROR: Transmission specifies joint " + joint_name +
+        " which has a negative effort limit.");
+  }
+
+  if (effort_iter->second <= 0) {
+    drake::log()->warn(
+        "WARNING: Skipping transmission since it's attached to "
+        "joint \"" + joint_name + "\" which has a zero "
+        "effort limit {}.", effort_iter->second);
+    return;
+  }
+
+  plant->AddJointActuator(actuator_name, joint, effort_iter->second);
 }
 
 void ParseFrame(ModelInstanceIndex model_instance,
@@ -555,18 +589,23 @@ ModelInstanceIndex ParseUrdf(
     ParseCollisionFilterGroup(node, plant);
   }
 
+  // Joint effort limits are stored with joints, but used when creating the
+  // actuator (which is done when parsing the transmission).
+  std::map<std::string, double> joint_effort_limits;
+
   // Parses the model's joint elements.
   for (XMLElement* joint_node = node->FirstChildElement("joint"); joint_node;
        joint_node = joint_node->NextSiblingElement("joint")) {
-    ParseJoint(model_instance, joint_node, plant);
+    ParseJoint(model_instance, &joint_effort_limits, joint_node, plant);
   }
 
   // Parses the model's transmission elements.
   for (XMLElement* transmission_node = node->FirstChildElement("transmission");
        transmission_node;
        transmission_node =
            transmission_node->NextSiblingElement("transmission")) {
-    ParseTransmission(model_instance, transmission_node, plant);
+    ParseTransmission(model_instance, joint_effort_limits,
+                      transmission_node, plant);
   }
 
   if (node->FirstChildElement("loop_joint")) {

multibody/parsing/test/acrobot_parser_test.cc

@@ -142,6 +142,11 @@ TEST_P(AcrobotModelTests, ModelBasics) {
   EXPECT_EQ(elbow_joint.parent_body().name(), parameters_.link1_name());
   EXPECT_EQ(elbow_joint.child_body().name(), parameters_.link2_name());
 
+  // Get actuators by name.
+  const JointActuator<double>& elbow_actuator =
+      plant_->GetJointActuatorByName("ElbowJoint");
+  EXPECT_TRUE(std::isinf(elbow_actuator.effort_limit()));
+
   // Get frames by name.
   const Frame<double>& link1_frame = plant_->GetFrameByName("Link1");
   EXPECT_EQ(link1_frame.name(), "Link1");

multibody/parsing/test/detail_urdf_parser_test.cc

@@ -98,6 +98,12 @@ GTEST_TEST(MultibodyPlantUrdfParserTest, TestAtlasMinimalContact) {
 
   EXPECT_EQ(plant.num_positions(), 37);
   EXPECT_EQ(plant.num_velocities(), 36);
+
+  // Verify that joint actuator limits are set correctly.
+  ASSERT_TRUE(plant.HasJointActuatorNamed("back_bkz_motor"));
+  const JointActuator<double>& actuator =
+      plant.GetJointActuatorByName("back_bkz_motor");
+  EXPECT_EQ(actuator.effort_limit(), 106);
 }
 
 GTEST_TEST(MultibodyPlantUrdfParserTest, TestAddWithQuaternionFloatingDof) {
@@ -165,6 +171,10 @@ GTEST_TEST(MultibodyPlantUrdfParserTest, JointParsingTest) {
   EXPECT_TRUE(
       CompareMatrices(revolute_joint.velocity_upper_limits(), Vector1d(100)));
 
+  const JointActuator<double>& revolute_actuator =
+      plant.GetJointActuatorByName("revolute_actuator");
+  EXPECT_EQ(revolute_actuator.effort_limit(), 100);
+
   const Joint<double>& prismatic_joint =
       plant.GetJointByName("prismatic_joint");
   EXPECT_TRUE(
@@ -175,6 +185,7 @@ GTEST_TEST(MultibodyPlantUrdfParserTest, JointParsingTest) {
       CompareMatrices(prismatic_joint.velocity_lower_limits(), Vector1d(-5)));
   EXPECT_TRUE(
       CompareMatrices(prismatic_joint.velocity_upper_limits(), Vector1d(5)));
+  EXPECT_FALSE(plant.HasJointActuatorNamed("prismatic_actuator"));
 
   const Joint<double>& no_limit_joint =
       plant.GetJointByName("revolute_joint_no_limits");
@@ -185,6 +196,10 @@ GTEST_TEST(MultibodyPlantUrdfParserTest, JointParsingTest) {
   EXPECT_TRUE(CompareMatrices(no_limit_joint.position_upper_limits(), inf));
   EXPECT_TRUE(CompareMatrices(no_limit_joint.velocity_lower_limits(), neg_inf));
   EXPECT_TRUE(CompareMatrices(no_limit_joint.velocity_upper_limits(), inf));
+
+  const JointActuator<double>& revolute_actuator_no_limits =
+      plant.GetJointActuatorByName("revolute_actuator_no_limits");
+  EXPECT_EQ(revolute_actuator_no_limits.effort_limit(), inf(0));
 }
 
 GTEST_TEST(MultibodyPlantUrdfParserTest, CollisionFilterGroupParsingTest) {

multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf

@@ -46,7 +46,7 @@ Defines a URDF model with various types of joints.
     <parent link="link2"/>
     <child link="link3"/>
     <origin rpy="0 0 0" xyz="0 0 0"/>
-    <limit effort="100" lower="-2" upper="1" velocity="5"/>
+    <limit effort="0" lower="-2" upper="1" velocity="5"/>
   </joint>
 
   <joint name="revolute_joint_no_limits" type="revolute">
@@ -55,4 +55,42 @@ Defines a URDF model with various types of joints.
     <child link="link4"/>
     <origin rpy="0 0 0" xyz="0 0 0"/>
   </joint>
+
+  <!-- Normal transmission/joint, should be created with appropriate
+  effort limit. -->
+  <transmission>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="revolute_joint">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="revolute_actuator">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint defined with zero effort, actuator should be skipped. -->
+ <transmission>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="prismatic_joint">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="prismatic_actuator">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint defined with no effort limit, actuator should be created
+       with default limit (infinity). -->
+  <transmission>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="revolute_joint_no_limits">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="revolute_actuator_no_limits">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
 </robot>