Skip to content

Commit

Permalink
Test fixed joint reduction of SDF joint names
Browse files Browse the repository at this point in the history 
This adds a test case for a URDF with a chain of links
with fixed joint reduction and an SDFormat joint embedded
in a <gazebo> block whose parent link is one of the
reduced links. This confirms that the parent link name
is updated properly during fixed joint reduction.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
  • Loading branch information
@scpeters
scpeters committed Jul 22, 2022
1 parent 3dd02cc commit 968fed4
Show file tree
Hide file tree
Showing 2 changed files with 162 additions and 0 deletions.
29 changes: 29 additions & 0 deletions test/integration/fixed_joint_reduction.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,8 @@ const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT[] =
"fixed_joint_reduction_collision_visual_empty_root.urdf";
const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF[] =
"fixed_joint_reduction_collision_visual_empty_root.sdf";
const char SDF_TEST_FILE_JOINT_FRAME_EXTENSION[] =
"fixed_joint_reduction_joint_frame_extension.urdf";
const char SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION[] =
"fixed_joint_reduction_plugin_frame_extension.urdf";

Expand Down Expand Up @@ -737,6 +739,33 @@ TEST(SDFParser, FixedJointReductionSimple)
}
}

/////////////////////////////////////////////////
// This test uses a urdf that has an SDFormat ball joint embedded in a
// <gazebo> tag, and the parent link part of a chain of fixed joints
// that reduces to the base_link.
// Test to make sure that the parent link name changes appropriately.
TEST(SDFParser, FixedJointReductionJointFrameExtensionTest)
{
sdf::Root root;
auto errors =
root.Load(GetFullTestFilePath(SDF_TEST_FILE_JOINT_FRAME_EXTENSION));
EXPECT_TRUE(errors.empty()) << errors[0].Message();

// Get the first model
const sdf::Model *model = root.Model();
ASSERT_NE(nullptr, model);
EXPECT_EQ("chained_fixed_joint_links", model->Name());

EXPECT_EQ(3u, model->JointCount());
const std::string ball_joint_name = "sdf_ball_joint";
EXPECT_TRUE(model->JointNameExists(ball_joint_name));
const sdf::Joint *joint = model->JointByName(ball_joint_name);
ASSERT_NE(nullptr, joint);

EXPECT_EQ("link3", joint->ChildLinkName());
EXPECT_EQ("base_link", joint->ParentLinkName());
}

/////////////////////////////////////////////////
// This test uses a urdf that has chained fixed joints with plugin that
// contains bodyName, xyzOffset and rpyOffset.
Expand Down
133 changes: 133 additions & 0 deletions test/integration/fixed_joint_reduction_joint_frame_extension.urdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,133 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="chained_fixed_joint_links">
<gazebo>
<joint name="sdf_ball_joint" type="ball">
<parent>link1</parent>
<child>link3</child>
</joint>
</gazebo>

<!-- Base Link -->
<link name="base_link">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="1.0 1.0 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<geometry>
<box size="0.1 0.1 2"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 1" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0"/>
</inertial>
</link>

<!-- Link 1 -->
<link name="link1">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="1.0 1.0 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 1" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0"/>
</inertial>
</link>

<!-- Link 2 -->
<link name="link2">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="1.0 1.0 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 1" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0"/>
</inertial>
</link>

<!-- Link 3 -->
<link name="link3">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="1.0 1.0 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 1" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0"/>
</inertial>
</link>

<!-- Joint 1 -->
<joint name="joint1" type="fixed">
<parent link="base_link"/>
<child link="link1"/>
<origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
<dynamics damping="0.7"/>
</joint>

<!-- Joint 2 -->
<joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link2"/>
<origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>

<!-- Joint 3 -->
<joint name="joint3" type="continuous">
<parent link="link2"/>
<child link="link3"/>
<origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>

</robot>

0 comments on commit 968fed4

Please sign in to comment.