bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

adding tests for srdf

This commit is contained in:
Ioan Sucan 2011-11-09 10:15:47 +00:00
parent 9fac541542
commit 62c64c5567
8 changed files with 3468 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
srdf/CMakeLists.txt
View File

@ -24,3 +24,6 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#common commands for building c++ executables and libraries
rosbuild_add_library(${PROJECT_NAME} src/model.cpp)
target_link_libraries(${PROJECT_NAME} tinyxml)
rosbuild_add_gtest(test_parser test/test_parser.cpp)
target_link_libraries(test_parser ${PROJECT_NAME})

2
srdf/manifest.xml
View File

@ -12,7 +12,7 @@
<rosdep name="tinyxml" />
<depend package="urdf_interface" />
<depend package="urdf" />
<depend package="rosconsole" />
<export>

15
srdf/src/model.cpp
View File

@ -36,6 +36,7 @@
#include "srdf/model.h"
#include <ros/console.h>
#include <algorithm>
#include <fstream>
#include <sstream>
#include <set>
@ -74,10 +75,16 @@ void srdf::Model::loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXm
continue;
}
VirtualJoint vj;
vj.type_ = std::string(type);
std::transform(vj.type_.begin(), vj.type_.end(), vj.type_.begin(), ::tolower);
if (vj.type_ != "planar" && vj.type_ != "floating" && vj.type_ != "fixed")
{
ROS_ERROR("Unknown type of joint: '%s'. Assuming 'fixed' instead. Other known types are 'planar' and 'floating'.", type);
vj.type_ = "fixed";
}
vj.name_ = std::string(jname);
vj.child_link_ = std::string(child);
vj.parent_frame_ = std::string(parent);
vj.type_ = std::string(type);
virtual_joints_.push_back(vj);
}
}
@ -194,6 +201,8 @@ void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElemen
}
g.subgroups_.push_back(std::string(sub));
}
if (g.links_.empty() && g.joints_.empty() && g.chains_.empty() && g.subgroups_.empty())
ROS_WARN("Group '%s' is empty.", gname);
groups_.push_back(g);
}
@ -307,9 +316,9 @@ void srdf::Model::loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlE
}
std::string jval_str = std::string(jval);
std::stringstream ss(jval_str);
while (!ss.eof())
while (ss.good() && !ss.eof())
{
double val; ss >> val;
double val; ss >> val >> std::ws;
gs.joint_values_[jname_str].push_back(val);
}
if (gs.joint_values_.empty())

3
srdf/test/res/pr2_desc.1.srdf Normal file
View File

@ -0,0 +1,3 @@
<?xml version="1.0" ?>
<robot name="pr2">
</robot>

7
srdf/test/res/pr2_desc.2.srdf Normal file
View File

@ -0,0 +1,7 @@
<?xml version="1.0" ?>
<robot name="pr2">
<virtual_joint name="world_joint" type="planar" parent_frame="odom" child_link="base_footprint"/>
<group name="g1">
<joint name="torso_lift_joint"/>
</group>
</robot>

72
srdf/test/res/pr2_desc.3.srdf Normal file
View File

@ -0,0 +1,72 @@
<?xml version="1.0"?>
<robot name="pr2">
<virtual_joint name="world_joint" type="planar" parent_frame="odom" child_link="base_footprint"/>
<group name="right_arm">
<chain base_link="torso_lift_link" tip_link="r_wrist_roll_link"/>
</group>
<group name="left_arm">
<chain base_link="torso_lift_link" tip_link="l_wrist_roll_link"/>
</group>
<group name="arms">
<group name="left_arm"/>
<group name="right_arm"/>
</group>
<group_state name="tuck_arms" group="arms">
<joint name="l_shoulder_pan_joint" value="0.2" />
<!-- ... the rest of the joint values... -->
</group_state>
<group_state name="home" group="base">
<joint name="world_joint" value=" .4 0 -1 " />
<!-- ... the rest of the joint values... -->
</group_state>
<group name="base">
<joint name="world_joint"/>
</group>
<group name="whole_body">
<group name="arms"/>
<group name="base"/>
<joint name="torso_lift_joint"/>
</group>
<group name="l_end_effector">
<link name="l_gripper_palm_link" />
<joint name="l_gripper_palm_joint" />
<joint name="l_gripper_l_finger_joint" />
<joint name="l_gripper_l_finger_tip_joint" />
<joint name="l_gripper_led_joint" />
<joint name="l_gripper_motor_accelerometer_joint" />
<joint name="l_gripper_r_finger_joint" />
<joint name="l_gripper_r_finger_tip_joint" />
<joint name="l_gripper_joint" />
<joint name="l_gripper_tool_joint" />
</group>
<group name="r_end_effector">
<link name="r_gripper_palm_link" />
<joint name="r_gripper_palm_joint" />
<joint name="r_gripper_l_finger_joint" />
<joint name="r_gripper_l_finger_tip_joint" />
<joint name="r_gripper_led_joint" />
<joint name="r_gripper_motor_accelerometer_joint" />
<joint name="r_gripper_r_finger_joint" />
<joint name="r_gripper_r_finger_tip_joint" />
<joint name="r_gripper_joint" />
<joint name="r_gripper_tool_joint" />
</group>
<end_effector name="r_end_effector" parent_link="r_wrist_roll_link" group="r_end_effector"/>
<end_effector name="l_end_effector" parent_link="l_wrist_roll_link" group="l_end_effector"/>
<disable_collisions link1="r_shoulder_pan_link" link2="r_shoulder_lift_link" />
<disable_collisions link1="r_shoulder_pan_link" link2="l_gripper_palm_link" />
<!-- and many more disable_collisions tags -->
</robot>

3238
srdf/test/res/pr2_desc.urdf Normal file
View File

File diff suppressed because it is too large Load Diff

132
srdf/test/test_parser.cpp Normal file
View File

@ -0,0 +1,132 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Ioan Sucan */
#include <srdf/model.h>
#include <urdf/model.h>
#include <gtest/gtest.h>
TEST(SRDF, Simple)
{
urdf::Model u;
srdf::Model s;
EXPECT_TRUE(u.initFile("test/res/pr2_desc.urdf"));
EXPECT_TRUE(s.initFile(u, "test/res/pr2_desc.1.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 0);
EXPECT_TRUE(s.getGroups().size() == 0);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisions().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);
EXPECT_TRUE(s.initFile(u, "test/res/pr2_desc.2.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 1);
EXPECT_TRUE(s.getGroups().size() == 1);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisions().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);
EXPECT_TRUE(s.initFile(u, "test/res/pr2_desc.1.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 0);
EXPECT_TRUE(s.getGroups().size() == 0);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisions().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);
}
TEST(SRDF, Complex)
{
urdf::Model u;
srdf::Model s;
EXPECT_TRUE(u.initFile("test/res/pr2_desc.urdf"));
EXPECT_TRUE(s.initFile(u, "test/res/pr2_desc.3.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 1);
EXPECT_TRUE(s.getGroups().size() == 7);
EXPECT_TRUE(s.getGroupStates().size() == 2);
EXPECT_TRUE(s.getDisabledCollisions().size() == 2);
EXPECT_TRUE(s.getEndEffectors().size() == 2);
EXPECT_EQ(s.getVirtualJoints()[0].name_, "world_joint");
EXPECT_EQ(s.getVirtualJoints()[0].type_, "planar");
for (std::size_t i = 0 ; i < s.getGroups().size() ; ++i)
{
if (s.getGroups()[i].name_ == "left_arm" || s.getGroups()[i].name_ == "right_arm")
EXPECT_TRUE(s.getGroups()[i].chains_.size() == 1);
if (s.getGroups()[i].name_ == "arms")
EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
if (s.getGroups()[i].name_ == "base")
EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
if (s.getGroups()[i].name_ == "l_end_effector" || s.getGroups()[i].name_ == "r_end_effector")
{
EXPECT_TRUE(s.getGroups()[i].links_.size() == 1);
EXPECT_TRUE(s.getGroups()[i].joints_.size() == 9);
}
if (s.getGroups()[i].name_ == "whole_body")
{
EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
}
}
int index = 0;
if (s.getGroupStates()[0].group_ != "arms")
index = 1;
EXPECT_EQ(s.getGroupStates()[index].group_, "arms");
EXPECT_EQ(s.getGroupStates()[index].name_, "tuck_arms");
EXPECT_EQ(s.getGroupStates()[1-index].group_, "base");
EXPECT_EQ(s.getGroupStates()[1-index].name_, "home");
const std::vector<double> &v = s.getGroupStates()[index].joint_values_.find("l_shoulder_pan_joint")->second;
EXPECT_EQ(v.size(), 1);
EXPECT_EQ(v[0], 0.2);
const std::vector<double> &w = s.getGroupStates()[1-index].joint_values_.find("world_joint")->second;
EXPECT_EQ(w.size(), 3);
EXPECT_EQ(w[0], 0.4);
EXPECT_EQ(w[1], 0);
EXPECT_EQ(w[2], -1);
index = (s.getEndEffectors()[0].name_[0] == 'r') ? 0 : 1;
EXPECT_EQ(s.getEndEffectors()[index].name_, "r_end_effector");
EXPECT_EQ(s.getEndEffectors()[index].component_group_, "r_end_effector");
EXPECT_EQ(s.getEndEffectors()[index].parent_link_, "r_wrist_roll_link");
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}