From 1885e28bdd0ee8b31263d16e729d070d480c036b Mon Sep 17 00:00:00 2001 From: Joe Masterjohn Date: Wed, 11 Nov 2020 14:44:26 -0500 Subject: [PATCH] Add parsing for reflected inertia parameters --- multibody/parsing/detail_sdf_parser.cc | 19 +++- multibody/parsing/detail_urdf_parser.cc | 31 +++++- .../parsing/test/detail_sdf_parser_test.cc | 83 ++++++++++++++ .../parsing/test/detail_urdf_parser_test.cc | 103 ++++++++++++++++++ 4 files changed, 234 insertions(+), 2 deletions(-) diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc index 214d27ec15e5..f5d8b6168db3 100644 --- a/multibody/parsing/detail_sdf_parser.cc +++ b/multibody/parsing/detail_sdf_parser.cc @@ -236,7 +236,24 @@ void AddJointActuatorFromSpecification( // as a way to specify un-actuated joints. Thus, the user would say // 0 for un-actuated joints. if (effort_limit != 0) { - plant->AddJointActuator(joint_spec.Name(), joint, effort_limit); + const JointActuator& 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("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("drake:gear_ratio")); + } + + drake::log()->trace("sdf_parser: Add JointActuator parameters."); } } 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& 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..78c7d17f8289 100644 --- a/multibody/parsing/test/detail_sdf_parser_test.cc +++ b/multibody/parsing/test/detail_sdf_parser_test.cc @@ -1139,6 +1139,89 @@ GTEST_TEST(SdfParser, BushingParsing) { " child tag."); } +GTEST_TEST(SdfParser, ReflectedInertiaParametersParsing) { + // Test successful parsing of both parameters + { + auto [plant, scene_graph] = ParseTestString(R"( + + + + + A + B + + 0 0 1 + + -1 + + + 1.5 + 300.0 + + )"); + + const JointActuator& 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(R"( + + + + + A + B + + 0 0 1 + + -1 + + + 1.5 + + )"); + + const JointActuator& 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(R"( + + + + + A + B + + 0 0 1 + + -1 + + + 300.0 + + )"); + + const JointActuator& 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..01a8648e71e8 100644 --- a/multibody/parsing/test/detail_urdf_parser_test.cc +++ b/multibody/parsing/test/detail_urdf_parser_test.cc @@ -733,6 +733,109 @@ GTEST_TEST(MultibodyPlantUrdfParserTest, BushingParsing) { " tag on line [0-9]+"); } +GTEST_TEST(MultibodyPlantUrdfParserTest, ReflectedInertiaParametersParsing) { + // Test successful parsing of both parameters + { + auto [plant, scene_graph] = ParseTestString(R"( + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + + + + + )"); + + const JointActuator& 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(R"( + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + + + + + )"); + + const JointActuator& 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(R"( + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + + + + + )"); + + const JointActuator& 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