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

Jump to bottom

Add parsing for reflected inertia parameters #14323

Merged
merged 1 commit into from Nov 12, 2020
Merged

Add parsing for reflected inertia parameters #14323

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.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
17 changes: 16 additions & 1 deletion multibody/parsing/detail_sdf_parser.cc
View file
Open in desktop
Expand Up @@ -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"));
}
}
}

Expand Down
31 changes: 30 additions & 1 deletion multibody/parsing/detail_urdf_parser.cc
View file
Open in desktop
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
60 changes: 60 additions & 0 deletions multibody/parsing/test/detail_sdf_parser_test.cc
View file
Open in desktop
Expand Up @@ -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
Expand Down
67 changes: 67 additions & 0 deletions multibody/parsing/test/detail_urdf_parser_test.cc
View file
Open in desktop
Expand Up @@ -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
Expand Down