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

adding support for reasons in disabled collisions

This commit is contained in:
Ioan Sucan 2012-06-04 16:31:48 -07:00
parent bca3cb4ba9
commit 4f07e9d19a
4 changed files with 48 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
srdf/include/srdf/model.h
View File

@ -168,6 +168,19 @@ public:
std::map<std::string, std::vector<double> > joint_values_;
};
/// The definition of a disabled collision between two links
struct DisabledCollision
{
/// The name of the first link (as in URDF) of the disabled collision
std::string link1_;
/// The name of the second link (as in URDF) of the disabled collision
std::string link2_;
/// The reason why the collision check was disabled
std::string reason_;
};
/// Get the name of this model
const std::string& getName(void) const
{
@ -175,11 +188,15 @@ public:
}
/// Get the list of pairs of links that need not be checked for collisions (because they can never touch given the geometry and kinematics of the robot)
const std::vector<std::pair<std::string, std::string> >& getDisabledCollisions(void) const
const std::vector<DisabledCollision>& getDisabledCollisionPairs(void) const
{
return disabled_collisions_;
}
/// \deprecated{ Use the version returning DisabledCollision }
__attribute__ ((deprecated))
std::vector<std::pair<std::string, std::string> > getDisabledCollisions(void) const;
/// Get the list of groups defined for this model
const std::vector<Group>& getGroups(void) const
{
@ -216,13 +233,13 @@ private:
void loadVisualSensors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
void loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
std::string name_;
std::vector<Group> groups_;
std::vector<GroupState> group_states_;
std::vector<VirtualJoint> virtual_joints_;
std::vector<EndEffector> end_effectors_;
std::vector<VisualSensor> visual_sensors_;
std::vector<std::pair<std::string, std::string> > disabled_collisions_;
std::string name_;
std::vector<Group> groups_;
std::vector<GroupState> group_states_;
std::vector<VirtualJoint> virtual_joints_;
std::vector<EndEffector> end_effectors_;
std::vector<VisualSensor> visual_sensors_;
std::vector<DisabledCollision> disabled_collisions_;
};
}

22
srdf/src/model.cpp
View File

@ -477,19 +477,23 @@ void srdf::Model::loadDisabledCollisions(const urdf::ModelInterface &urdf_model,
ROS_ERROR("A pair of links needs to be specified to disable collisions");
continue;
}
std::string link1_str = boost::trim_copy(std::string(link1));
std::string link2_str = boost::trim_copy(std::string(link2));
if (!urdf_model.getLink(link1_str))
DisabledCollision dc;
dc.link1_ = boost::trim_copy(std::string(link1));
dc.link2_ = boost::trim_copy(std::string(link2));
if (!urdf_model.getLink(dc.link1_))
{
ROS_ERROR("Link '%s' is not known to URDF. Cannot disable collisons.", link1);
continue;
}
if (!urdf_model.getLink(link2_str))
if (!urdf_model.getLink(dc.link2_))
{
ROS_ERROR("Link '%s' is not known to URDF. Cannot disable collisons.", link2);
continue;
}
disabled_collisions_.push_back(std::make_pair(link1_str, link2_str));
const char *reason = c_xml->Attribute("reason");
if (reason)
dc.reason_ = std::string(reason);
disabled_collisions_.push_back(dc);
}
}
@ -576,3 +580,11 @@ void srdf::Model::clear(void)
visual_sensors_.clear();
disabled_collisions_.clear();
}
std::vector<std::pair<std::string, std::string> > srdf::Model::getDisabledCollisions(void) const
{
std::vector<std::pair<std::string, std::string> > result;
for (std::size_t i = 0 ; i < disabled_collisions_.size() ; ++i)
result.push_back(std::make_pair(disabled_collisions_[i].link1_, disabled_collisions_[i].link2_));
return result;
}

2
srdf/test/res/pr2_desc.3.srdf
View File

@ -65,7 +65,7 @@
<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="r_shoulder_lift_link" reason="adjacent" />
<disable_collisions link1="r_shoulder_pan_link" link2="l_gripper_palm_link" />
<!-- and many more disable_collisions tags -->

9
srdf/test/test_parser.cpp
View File

@ -48,21 +48,21 @@ TEST(SRDF, Simple)
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.getDisabledCollisionPairs().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.getDisabledCollisionPairs().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.getDisabledCollisionPairs().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);
}
@ -76,7 +76,8 @@ TEST(SRDF, Complex)
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.getDisabledCollisionPairs().size() == 2);
EXPECT_TRUE(s.getDisabledCollisionPairs()[0].reason_ == "adjacent");
EXPECT_TRUE(s.getEndEffectors().size() == 2);
EXPECT_EQ(s.getVirtualJoints()[0].name_, "world_joint");