From c504fffbd2f76492b52aa0b5eab579fbc16407be Mon Sep 17 00:00:00 2001
From: Joe Masterjohn <joemasterjohn@gmail.com>
Date: Wed, 11 Nov 2020 14:44:26 -0500
Subject: [PATCH] Add parsing for reflected inertia parameters

---
 multibody/parsing/detail_sdf_parser.cc        | 17 ++++-
 multibody/parsing/detail_urdf_parser.cc       | 31 ++++++++-
 .../parsing/test/detail_sdf_parser_test.cc    | 60 +++++++++++++++++
 .../parsing/test/detail_urdf_parser_test.cc   | 67 +++++++++++++++++++
 4 files changed, 173 insertions(+), 2 deletions(-)

diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc
index 214d27ec15e5..76accf5fc239 100644
--- a/multibody/parsing/detail_sdf_parser.cc
+++ b/multibody/parsing/detail_sdf_parser.cc
@@ -236,7 +236,22 @@ void AddJointActuatorFromSpecification(
   // as a way to specify un-actuated joints. Thus, the user would say
   // <effort>0</effort> for un-actuated joints.
   if (effort_limit != 0) {
-    plant->AddJointActuator(joint_spec.Name(), joint, effort_limit);
+    const JointActuator<double>& actuator =
+        plant->AddJointActuator(joint_spec.Name(), joint, effort_limit);
+
+    // Parse and add the optional drake:rotor_inertia parameter.
+    if (joint_spec.Element()->HasElement("drake:rotor_inertia")) {
+      plant->get_mutable_joint_actuator(actuator.index())
+          .set_default_rotor_inertia(
+              joint_spec.Element()->Get<double>("drake:rotor_inertia"));
+    }
+
+    // Parse and add the optional drake:gear_ratio parameter.
+    if (joint_spec.Element()->HasElement("drake:gear_ratio")) {
+      plant->get_mutable_joint_actuator(actuator.index())
+          .set_default_gear_ratio(
+              joint_spec.Element()->Get<double>("drake:gear_ratio"));
+    }
   }
 }
 
diff --git a/multibody/parsing/detail_urdf_parser.cc b/multibody/parsing/detail_urdf_parser.cc
index 2f4df69bdfc2..47287e5fd17c 100644
--- a/multibody/parsing/detail_urdf_parser.cc
+++ b/multibody/parsing/detail_urdf_parser.cc
@@ -561,7 +561,36 @@ void ParseTransmission(
     return;
   }
 
-  plant->AddJointActuator(actuator_name, joint, effort_iter->second);
+  const JointActuator<double>& actuator =
+      plant->AddJointActuator(actuator_name, joint, effort_iter->second);
+
+  // Parse and add the optional drake:rotor_inertia parameter.
+  XMLElement* rotor_inertia_node =
+      actuator_node->FirstChildElement("drake:rotor_inertia");
+  if (rotor_inertia_node) {
+    double rotor_inertia;
+    if (!ParseScalarAttribute(rotor_inertia_node, "value", &rotor_inertia)) {
+      throw std::runtime_error(
+          "ERROR: joint actuator " + actuator_name +
+          "'s drake:rotor_inertia does not have a \"value\" attribute!");
+    }
+    plant->get_mutable_joint_actuator(actuator.index())
+        .set_default_rotor_inertia(rotor_inertia);
+  }
+
+  // Parse and add the optional drake:gear_ratio parameter.
+  XMLElement* gear_ratio_node =
+      actuator_node->FirstChildElement("drake:gear_ratio");
+  if (gear_ratio_node) {
+    double gear_ratio;
+    if (!ParseScalarAttribute(gear_ratio_node, "value", &gear_ratio)) {
+      throw std::runtime_error(
+          "ERROR: joint actuator " + actuator_name +
+          "'s drake:gear_ratio does not have a \"value\" attribute!");
+    }
+    plant->get_mutable_joint_actuator(actuator.index())
+        .set_default_gear_ratio(gear_ratio);
+  }
 }
 
 void ParseFrame(ModelInstanceIndex model_instance,
diff --git a/multibody/parsing/test/detail_sdf_parser_test.cc b/multibody/parsing/test/detail_sdf_parser_test.cc
index 82c13008d78d..511769f8b101 100644
--- a/multibody/parsing/test/detail_sdf_parser_test.cc
+++ b/multibody/parsing/test/detail_sdf_parser_test.cc
@@ -1139,6 +1139,66 @@ GTEST_TEST(SdfParser, BushingParsing) {
                               "<drake:bushing_torque_damping> child tag.");
 }
 
+GTEST_TEST(SdfParser, ReflectedInertiaParametersParsing) {
+  // Common SDF string with format options for the two custom tags.
+  const std::string test_string = R"""(
+    <model name='ReflectedInertiaModel'>
+      <link name='A'/>
+      <link name='B'/>
+      <joint name='revolute_AB' type='revolute'>
+        <child>A</child>
+        <parent>B</parent>
+        <axis>
+          <xyz>0 0 1</xyz>
+          <limit>
+            <effort>-1</effort>
+          </limit>
+        </axis>
+        {0}
+        {1}
+      </joint>
+    </model>)""";
+
+  // Test successful parsing of both parameters.
+  {
+    auto [plant, scene_graph] = ParseTestString(fmt::format(test_string,
+        "<drake:rotor_inertia>1.5</drake:rotor_inertia>",
+        "<drake:gear_ratio>300.0</drake:gear_ratio>"));
+
+    const JointActuator<double>& actuator =
+        plant->GetJointActuatorByName("revolute_AB");
+
+    EXPECT_EQ(actuator.default_rotor_inertia(), 1.5);
+    EXPECT_EQ(actuator.default_gear_ratio(), 300.0);
+  }
+
+  // Test successful parsing of rotor_inertia and default value for
+  // gear_ratio.
+  {
+    auto [plant, scene_graph] = ParseTestString(fmt::format(
+        test_string, "<drake:rotor_inertia>1.5</drake:rotor_inertia>", ""));
+
+    const JointActuator<double>& actuator =
+        plant->GetJointActuatorByName("revolute_AB");
+
+    EXPECT_EQ(actuator.default_rotor_inertia(), 1.5);
+    EXPECT_EQ(actuator.default_gear_ratio(), 1.0);
+  }
+
+  // Test successful parsing of gear_ratio and default value for
+  // rotor_inertia.
+  {
+    auto [plant, scene_graph] = ParseTestString(fmt::format(
+        test_string, "", "<drake:gear_ratio>300.0</drake:gear_ratio>"));
+
+    const JointActuator<double>& actuator =
+        plant->GetJointActuatorByName("revolute_AB");
+
+    EXPECT_EQ(actuator.default_rotor_inertia(), 0.0);
+    EXPECT_EQ(actuator.default_gear_ratio(), 300.0);
+  }
+}
+
 }  // namespace
 }  // namespace internal
 }  // namespace multibody
diff --git a/multibody/parsing/test/detail_urdf_parser_test.cc b/multibody/parsing/test/detail_urdf_parser_test.cc
index 502b46072695..2e06817df25f 100644
--- a/multibody/parsing/test/detail_urdf_parser_test.cc
+++ b/multibody/parsing/test/detail_urdf_parser_test.cc
@@ -733,6 +733,73 @@ GTEST_TEST(MultibodyPlantUrdfParserTest, BushingParsing) {
       " <drake:bushing_torque_stiffness> tag on line [0-9]+");
 }
 
+GTEST_TEST(MultibodyPlantUrdfParserTest, ReflectedInertiaParametersParsing) {
+  // Common URDF string with format options for the two custom tags.
+  const std::string test_string = R"""(
+    <robot name='reflected_inertia_test'>
+      <link name='A'/>
+      <link name='B'/>
+      <joint name='revolute_AB' type='revolute'>
+        <axis xyz='0 0 1'/>
+        <parent link='A'/>
+        <child link='B'/>
+        <origin rpy='0 0 0' xyz='0 0 0'/>
+        <limit effort='100' lower='-1' upper='2' velocity='100'/>
+        <dynamics damping='0.1'/>
+      </joint>
+      <transmission>
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name='revolute_AB'>
+          <hardwareInterface>PositionJointInterface</hardwareInterface>
+        </joint>
+        <actuator name='revolute_AB'>
+          {0}
+          {1}
+        </actuator>
+      </transmission>
+    </robot>)""";
+
+  // Test successful parsing of both parameters.
+  {
+    auto [plant, scene_graph] = ParseTestString(
+        fmt::format(test_string, "<drake:rotor_inertia value='1.5' />",
+                    "<drake:gear_ratio value='300.0' />"));
+
+    const JointActuator<double>& actuator =
+        plant->GetJointActuatorByName("revolute_AB");
+
+    EXPECT_EQ(actuator.default_rotor_inertia(), 1.5);
+    EXPECT_EQ(actuator.default_gear_ratio(), 300.0);
+  }
+
+  // Test successful parsing of rotor_inertia and default value for
+  // gear_ratio.
+  {
+    auto [plant, scene_graph] = ParseTestString(fmt::format(test_string,
+        "<drake:rotor_inertia value='1.5' />",
+        ""));
+
+    const JointActuator<double>& actuator =
+        plant->GetJointActuatorByName("revolute_AB");
+
+    EXPECT_EQ(actuator.default_rotor_inertia(), 1.5);
+    EXPECT_EQ(actuator.default_gear_ratio(), 1.0);
+  }
+
+  // Test successful parsing of gear_ratio and default value for
+  // rotor_inertia.
+  {
+    auto [plant, scene_graph] = ParseTestString(
+        fmt::format(test_string, "", "<drake:gear_ratio value='300.0' />"));
+
+    const JointActuator<double>& actuator =
+        plant->GetJointActuatorByName("revolute_AB");
+
+    EXPECT_EQ(actuator.default_rotor_inertia(), 0.0);
+    EXPECT_EQ(actuator.default_gear_ratio(), 300.0);
+  }
+}
+
 }  // namespace
 }  // namespace internal
 }  // namespace multibody