Overhaul tests in urdf
This commit is contained in:
Jackie Kay
parent
2ac4821bca
commit
b95dbf2295
|
|
@ -168,15 +168,19 @@ bool treeFromXml(TiXmlDocument *xml_doc, Tree& tree)
|
|||
bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree)
|
||||
{
|
||||
tree = Tree(robot_model.getRoot()->name);
|
||||
ROS_INFO("constructed tree");
|
||||
|
||||
// warn if root link has inertia. KDL does not support this
|
||||
if (robot_model.getRoot()->inertial)
|
||||
ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.", robot_model.getRoot()->name.c_str());
|
||||
|
||||
// add all children
|
||||
for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
|
||||
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
|
||||
for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++) {
|
||||
ROS_INFO("Adding link to KDL tree");
|
||||
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -63,8 +63,6 @@ protected:
|
|||
};
|
||||
|
||||
|
||||
|
||||
|
||||
TEST_F(TestParser, test)
|
||||
{
|
||||
for (int i=1; i<g_argc-2; i++){
|
||||
|
|
@ -73,9 +71,9 @@ TEST_F(TestParser, test)
|
|||
}
|
||||
|
||||
ASSERT_TRUE(treeFromFile(g_argv[g_argc-1], my_tree));
|
||||
ASSERT_EQ(my_tree.getNrOfJoints(), (unsigned int)44);
|
||||
ASSERT_EQ(my_tree.getNrOfSegments(), (unsigned int)81);
|
||||
ASSERT_TRUE(my_tree.getSegment("base_footprint") == my_tree.getRootSegment());
|
||||
ASSERT_EQ(my_tree.getNrOfJoints(), 3);
|
||||
ASSERT_EQ(my_tree.getNrOfSegments(), 4);
|
||||
ASSERT_TRUE(my_tree.getSegment("link1") == my_tree.getRootSegment());
|
||||
ASSERT_EQ(my_tree.getRootSegment()->second.children.size(), (unsigned int)1);
|
||||
ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment());
|
||||
ASSERT_EQ(my_tree.getSegment("base_link")->second.segment.getInertia().getMass(), 116.0);
|
||||
|
|
|
|||
|
|
@ -1,6 +1,3 @@
|
|||
<launch>
|
||||
<test test-name="test_kdl_parser" pkg="kdl_parser" type="test_kdl_parser" args="$(find kdl_parser)/test/pr2_desc_vector.xml \
|
||||
$(find kdl_parser)/test/pr2_desc_bracket.xml \
|
||||
$(find kdl_parser)/test/pr2_desc_bracket2.xml \
|
||||
$(find kdl_parser)/test/pr2_desc.xml" />
|
||||
<test test-name="test_kdl_parser" pkg="kdl_parser" type="test_kdl_parser" args="$(find kdl_parser)/test/test_robot.urdf"/>
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,425 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot name="r2d2">
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="0 0 .8 1"/>
|
||||
</material>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
|
||||
<link name="dummy_link"/>
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="dummy_to_base" type="fixed">
|
||||
<parent link="dummy_link"/>
|
||||
<child link="base_link"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .1 .2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.6 .1 .2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0 -0.22 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.4 .1 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.4 .1 .1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_base_joint" type="fixed">
|
||||
<parent link="right_leg"/>
|
||||
<child link="right_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_front_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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 name="right_front_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_front_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_back_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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 name="right_back_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_back_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .1 .2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.6 .1 .2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="0 0.22 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.4 .1 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.4 .1 .1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_base_joint" type="fixed">
|
||||
<parent link="left_leg"/>
|
||||
<child link="left_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_front_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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 name="left_front_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_front_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_back_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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 name="left_back_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_back_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<joint name="gripper_extension" type="prismatic">
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.19 0 .2"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="left_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_gripper">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_tip_joint" type="fixed">
|
||||
<parent link="left_gripper"/>
|
||||
<child link="left_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_tip">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_gripper">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_tip_joint" type="fixed">
|
||||
<parent link="right_gripper"/>
|
||||
<child link="right_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_tip">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="2"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="head_swivel" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin xyz="0 0 0.3"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
<origin xyz="-0.04 0 0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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 name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0.1814 0 0.1414"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
|
@ -183,8 +183,7 @@ bool Model::initString(const std::string& xml_string)
|
|||
this->root_link_ = model->root_link_;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
return false;
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
<?xml verison="1.0"?>
|
||||
<robot name="no_visual">
|
||||
<robot name="one_link">
|
||||
<link name="link1">
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,425 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot name="r2d2">
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="0 0 .8 1"/>
|
||||
</material>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
|
||||
<link name="dummy_link"/>
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="dummy_to_base" type="fixed">
|
||||
<parent link="dummy_link"/>
|
||||
<child link="base_link"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .1 .2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.6 .1 .2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0 -0.22 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.4 .1 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.4 .1 .1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_base_joint" type="fixed">
|
||||
<parent link="right_leg"/>
|
||||
<child link="right_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_front_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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 name="right_front_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_front_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_back_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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 name="right_back_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_back_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .1 .2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.6 .1 .2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="0 0.22 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.4 .1 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.4 .1 .1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_base_joint" type="fixed">
|
||||
<parent link="left_leg"/>
|
||||
<child link="left_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_front_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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 name="left_front_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_front_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_back_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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 name="left_back_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_back_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<joint name="gripper_extension" type="prismatic">
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.19 0 .2"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="left_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_gripper">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_tip_joint" type="fixed">
|
||||
<parent link="left_gripper"/>
|
||||
<child link="left_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_tip">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_gripper">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_tip_joint" type="fixed">
|
||||
<parent link="right_gripper"/>
|
||||
<child link="right_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_tip">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="2"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="head_swivel" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin xyz="0 0 0.3"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
<origin xyz="-0.04 0 0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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 name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0.1814 0 0.1414"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
|
@ -50,26 +50,26 @@ char** g_argv;
|
|||
class TestParser : public testing::Test
|
||||
{
|
||||
public:
|
||||
Model robot;
|
||||
|
||||
bool checkModel()
|
||||
bool checkModel(urdf::Model & robot)
|
||||
{
|
||||
// get root link
|
||||
boost::shared_ptr<const Link> root_link=this->robot.getRoot();
|
||||
boost::shared_ptr<const Link> root_link = robot.getRoot();
|
||||
if (!root_link)
|
||||
{
|
||||
ROS_ERROR("no root link %s",this->robot.getName().c_str());
|
||||
ROS_ERROR("no root link %s", robot.getName().c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
// go through entire tree
|
||||
return this->traverse_tree(root_link);
|
||||
return this->traverse_tree(root_link);
|
||||
|
||||
};
|
||||
|
||||
protected:
|
||||
/// constructor
|
||||
TestParser()
|
||||
// num_links starts at 1 because traverse_tree doesn't count the root node
|
||||
TestParser() : num_joints(0), num_links(1)
|
||||
{
|
||||
}
|
||||
|
||||
|
|
@ -81,11 +81,15 @@ protected:
|
|||
|
||||
bool traverse_tree(boost::shared_ptr<const Link> link,int level = 0)
|
||||
{
|
||||
ROS_INFO("Traversing tree at level %d, link size %lu", level, link->child_links.size());
|
||||
level+=2;
|
||||
bool retval = true;
|
||||
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
|
||||
{
|
||||
if (*child)
|
||||
++num_links;
|
||||
if (*child && (*child)->parent_joint)
|
||||
{
|
||||
++num_joints;
|
||||
// check rpy
|
||||
double roll,pitch,yaw;
|
||||
(*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll,pitch,yaw);
|
||||
|
|
@ -96,7 +100,7 @@ protected:
|
|||
return false;
|
||||
}
|
||||
// recurse down the tree
|
||||
return this->traverse_tree(*child,level);
|
||||
retval &= this->traverse_tree(*child,level);
|
||||
}
|
||||
else
|
||||
{
|
||||
|
|
@ -104,9 +108,12 @@ protected:
|
|||
return false;
|
||||
}
|
||||
}
|
||||
// no children
|
||||
return true;
|
||||
// no more children
|
||||
return retval;
|
||||
};
|
||||
|
||||
size_t num_joints;
|
||||
size_t num_links;
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -114,21 +121,37 @@ protected:
|
|||
|
||||
TEST_F(TestParser, test)
|
||||
{
|
||||
ASSERT_GE(g_argc, 3);
|
||||
std::string folder = std::string(g_argv[1]) + "/test/";
|
||||
ROS_INFO("Folder %s",folder.c_str());
|
||||
for (int i=2; i<g_argc; i++){
|
||||
std::string file = g_argv[i];
|
||||
bool expect_success = (file.substr(0,5) != "fail_");
|
||||
ROS_INFO("Parsing file %d/%d called %s, expecting %d",(i-1), g_argc-1, (folder + file).c_str(), expect_success);
|
||||
if (!expect_success)
|
||||
ASSERT_FALSE(robot.initFile(folder + file));
|
||||
else
|
||||
{
|
||||
ASSERT_TRUE(robot.initFile(folder + file));
|
||||
ASSERT_TRUE(checkModel());
|
||||
}
|
||||
std::string file = std::string(g_argv[2]);
|
||||
bool expect_success = (file.substr(0,5) != "fail_");
|
||||
urdf::Model robot;
|
||||
ROS_INFO("Parsing file %s, expecting %d",(folder + file).c_str(), expect_success);
|
||||
if (!expect_success) {
|
||||
ASSERT_FALSE(robot.initFile(folder + file));
|
||||
return;
|
||||
}
|
||||
|
||||
ASSERT_EQ(g_argc, 7);
|
||||
std::string robot_name = std::string(g_argv[3]);
|
||||
std::string root_name = std::string(g_argv[4]);
|
||||
size_t expected_num_joints = atoi(g_argv[5]);
|
||||
size_t expected_num_links = atoi(g_argv[6]);
|
||||
|
||||
ASSERT_TRUE(robot.initFile(folder + file));
|
||||
|
||||
EXPECT_EQ(robot.getName(), robot_name);
|
||||
boost::shared_ptr<const urdf::Link> root = robot.getRoot();
|
||||
ASSERT_TRUE(root);
|
||||
EXPECT_EQ(root->name, root_name);
|
||||
|
||||
ASSERT_TRUE(checkModel(robot));
|
||||
EXPECT_EQ(num_joints, expected_num_joints);
|
||||
EXPECT_EQ(num_links, expected_num_links);
|
||||
EXPECT_EQ(robot.joints_.size(), expected_num_joints);
|
||||
EXPECT_EQ(robot.links_.size(), expected_num_links);
|
||||
|
||||
// test reading from parameter server
|
||||
ASSERT_TRUE(robot.initParam("robot_description"));
|
||||
ASSERT_FALSE(robot.initParam("robot_description_wim"));
|
||||
|
|
|
|||
|
|
@ -1,5 +1,19 @@
|
|||
<launch>
|
||||
<param name="robot_description" textfile="$(find urdf)/test/pr2_desc.urdf" />
|
||||
<param name="robot_description" textfile="$(find urdf)/test/test_robot.urdf" />
|
||||
|
||||
<test test-name="robot_model_parser_test" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_pr2_desc_bracket.urdf fail_pr2_desc_double_joint.urdf fail_pr2_desc_double.urdf fail_pr2_desc_loop.urdf fail_pr2_desc_no_filename_in_mesh.urdf fail_pr2_desc_no_joint2.urdf fail_pr2_desc_parent_itself.urdf fail_pr2_desc_two_trees.urdf fail_three_links_one_joint.urdf no_visual.urdf one_link.urdf pr2_desc_no_joint.urdf pr2_desc.urdf two_links_one_joint.urdf singularity.urdf" />
|
||||
<!-- args: path, urdf file, robot name, root name, #joints, #links-->
|
||||
<test test-name="robot_model_parser_test" pkg="urdf" type="test_urdf_parser" args="$(find urdf) test_robot.urdf r2d2 dummy_link 16 17" />
|
||||
|
||||
<test test-name="robot_model_parser_test_no_visual" pkg="urdf" type="test_urdf_parser" args="$(find urdf) no_visual.urdf no_visual link1 0 1" />
|
||||
<test test-name="robot_model_parser_test_one_link" pkg="urdf" type="test_urdf_parser" args="$(find urdf) one_link.urdf one_link link1 0 1" />
|
||||
<test test-name="robot_model_parser_test_two_links" pkg="urdf" type="test_urdf_parser" args="$(find urdf) two_links_one_joint.urdf two_links_one_joint link1 1 2" />
|
||||
|
||||
<!-- Cases expected not to parse correctly only get filename information (path, urdf file) -->
|
||||
<test test-name="robot_model_parser_test_fail_bracket" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_pr2_desc_bracket.urdf" />
|
||||
<test test-name="robot_model_parser_test_fail_joint_mismatch" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_three_links_one_joint.urdf" />
|
||||
<test test-name="robot_model_parser_test_fail_double_joint" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_pr2_desc_double_joint.urdf" />
|
||||
<test test-name="robot_model_parser_test_fail_loop" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_pr2_desc_loop.urdf" />
|
||||
<test test-name="robot_model_parser_test_fail_no_filename" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_pr2_desc_no_filename_in_mesh.urdf" />
|
||||
<test test-name="robot_model_parser_test_fail_no_joint" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_pr2_desc_no_joint2.urdf" />
|
||||
<test test-name="robot_model_parser_test_fail_two_trees" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_pr2_desc_two_trees.urdf" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
<?xml verison="1.0"?>
|
||||
<robot name="no_visual">
|
||||
<robot name="two_links_one_joint">
|
||||
<link name="link1">
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
|
|
|
|||
Loading…
Reference in New Issue