Skip to content

Commit

Permalink
Add parsing for reflected inertia parameters
Browse files Browse the repository at this point in the history
  • Loading branch information
@joemasterjohn
joemasterjohn committed Nov 12, 2020
1 parent f2cdfd2 commit 1885e28
Show file tree
Hide file tree
Showing 4 changed files with 234 additions and 2 deletions.
19 changes: 18 additions & 1 deletion multibody/parsing/detail_sdf_parser.cc
View file
Expand Up @@ -236,7 +236,24 @@ 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"));
}

drake::log()->trace("sdf_parser: Add JointActuator parameters.");
}
}

Expand Down
31 changes: 30 additions & 1 deletion multibody/parsing/detail_urdf_parser.cc
View file
Expand Up @@ -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,
Expand Down
83 changes: 83 additions & 0 deletions multibody/parsing/test/detail_sdf_parser_test.cc
View file
Expand Up @@ -1139,6 +1139,89 @@ GTEST_TEST(SdfParser, BushingParsing) {
"<drake:bushing_torque_damping> child tag.");
}

GTEST_TEST(SdfParser, ReflectedInertiaParametersParsing) {
// Test successful parsing of both parameters
{
auto [plant, scene_graph] = ParseTestString(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>
<drake:rotor_inertia>1.5</drake:rotor_inertia>
<drake:gear_ratio>300.0</drake:gear_ratio>
</joint>
</model>)");

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(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>
<drake:rotor_inertia>1.5</drake:rotor_inertia>
</joint>
</model>)");

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(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>
<drake:gear_ratio>300.0</drake:gear_ratio>
</joint>
</model>)");

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
Expand Down
103 changes: 103 additions & 0 deletions multibody/parsing/test/detail_urdf_parser_test.cc
View file
Expand Up @@ -733,6 +733,109 @@ GTEST_TEST(MultibodyPlantUrdfParserTest, BushingParsing) {
" <drake:bushing_torque_stiffness> tag on line [0-9]+");
}

GTEST_TEST(MultibodyPlantUrdfParserTest, ReflectedInertiaParametersParsing) {
// Test successful parsing of both parameters
{
auto [plant, scene_graph] = ParseTestString(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'>
<drake:rotor_inertia value='1.5' />
<drake:gear_ratio value='300.0' />
</actuator>
</transmission>
</robot>)");

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(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'>
<drake:rotor_inertia value='1.5' />
<!-- missing drake:gear_ratio tag -->
</actuator>
</transmission>
</robot>)");

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(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'>
<!-- missing drake:rotor_inertia tag -->
<drake:gear_ratio value='300.0' />
</actuator>
</transmission>
</robot>)");

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
Expand Down

0 comments on commit 1885e28

Please sign in to comment.