From 329d713ededd2f091487a9f2ea13fcf1dd9c924d Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Tue, 5 Dec 2017 15:14:40 +0100 Subject: [PATCH 1/5] [URDF] Add class UrdfTree to enable reading extra tags in URDF --- src/parsers/urdf.hpp | 4 +- src/parsers/urdf/geometry.cpp | 111 +++++++++++++++++++++++++++++----- 2 files changed, 97 insertions(+), 18 deletions(-) diff --git a/src/parsers/urdf.hpp b/src/parsers/urdf.hpp index 015195957e..16312e5cf0 100644 --- a/src/parsers/urdf.hpp +++ b/src/parsers/urdf.hpp @@ -123,7 +123,7 @@ namespace se3 * * @param[in] model The model of the robot, built with * urdf::buildModel - * @param[in] urdfTree The URDF model + * @param[in] xmlStream Stream containing the URDF model * @param[in] packageDirs A vector containing the different directories * where to search for models and meshes, typically * obtained from calling se3::rosPaths() @@ -137,7 +137,7 @@ namespace se3 * */ GeometryModel& buildGeom(const Model & model, - const ::urdf::ModelInterfaceSharedPtr & urdfTree, + const std::istream& xmlStream, const GeometryType type, GeometryModel & geomModel, const std::vector & packageDirs = std::vector ()) diff --git a/src/parsers/urdf/geometry.cpp b/src/parsers/urdf/geometry.cpp index c1115493ce..318b9c8592 100644 --- a/src/parsers/urdf/geometry.cpp +++ b/src/parsers/urdf/geometry.cpp @@ -20,6 +20,9 @@ #include "pinocchio/parsers/urdf/utils.hpp" #include "pinocchio/parsers/utils.hpp" +#include +#include + #include #include @@ -39,6 +42,53 @@ namespace se3 { namespace details { + struct UrdfTree + { + typedef boost::property_tree::ptree ptree; + typedef std::map LinkMap_t; + + void parse (const std::string & xmlStr) + { + urdf_ = ::urdf::parseURDF(xmlStr); + + std::istringstream iss(xmlStr); + using namespace boost::property_tree; + read_xml(iss, tree_, xml_parser::no_comments); + + BOOST_FOREACH(const ptree::value_type & link, tree_.get_child("robot")) { + if (link.first == "link") { + std::string name = link.second.get(".name"); + links_.insert(std::pair(name, link.second)); + } + } // BOOST_FOREACH + } + + bool replaceCylinderByCapsule (const std::string& linkName, + const std::string& geomName) const + { + LinkMap_t::const_iterator _link = links_.find(linkName); + assert (_link != links_.end()); + const ptree& link = _link->second; + if (link.count ("collision_checking") == 0) + return false; + BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) { + if (cc.first == "capsule") { + std::string name = cc.second.get(".name"); + if (geomName == name) return true; + } + } // BOOST_FOREACH + + return false; + } + + // For standard URDF tags + ::urdf::ModelInterfaceSharedPtr urdf_; + // For other tags + ptree tree_; + // A mapping from link name to corresponding child of tree_ + LinkMap_t links_; + }; + #ifdef WITH_HPP_FCL typedef fcl::BVHModel< fcl::OBBRSS > PolyhedronType; typedef boost::shared_ptr PolyhedronPtrType; @@ -54,10 +104,14 @@ namespace se3 * * @return A shared pointer on the he geometry converted as a fcl::CollisionGeometry */ - boost::shared_ptr retrieveCollisionGeometry(const ::urdf::GeometrySharedPtr urdf_geometry, - const std::vector & package_dirs, - std::string & meshPath, - Eigen::Vector3d & meshScale) + boost::shared_ptr retrieveCollisionGeometry( + const UrdfTree& tree, + const std::string& linkName, + const std::string& geomName, + const ::urdf::GeometrySharedPtr urdf_geometry, + const std::vector & package_dirs, + std::string & meshPath, + Eigen::Vector3d & meshScale) { boost::shared_ptr geometry; @@ -94,7 +148,7 @@ namespace se3 // Use FCL capsules for cylinders else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER) { - meshPath = "CYLINDER"; + bool capsule = tree.replaceCylinderByCapsule(linkName, geomName); meshScale << 1,1,1; const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry); @@ -102,7 +156,13 @@ namespace se3 double length = collisionGeometry->length; // Create fcl capsule geometry. - geometry = boost::shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length)); + if (capsule) { + meshPath = "CAPSULE"; + geometry = boost::shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length)); + } else { + meshPath = "CYLINDER"; + geometry = boost::shared_ptr < fcl::CollisionGeometry >(new fcl::Cylinder (radius, length)); + } } // Handle the case where collision geometry is a box. else if (urdf_geometry->type == ::urdf::Geometry::BOX) @@ -243,7 +303,8 @@ namespace se3 * @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION) */ template - inline void addLinkGeometryToGeomModel(::urdf::LinkConstSharedPtr link, + inline void addLinkGeometryToGeomModel(const UrdfTree& tree, + ::urdf::LinkConstSharedPtr link, const Model & model, GeometryModel & geomModel, const std::vector & package_dirs) throw (std::invalid_argument) @@ -270,7 +331,9 @@ namespace se3 { meshPath.clear(); #ifdef WITH_HPP_FCL - const boost::shared_ptr geometry = retrieveCollisionGeometry((*i)->geometry, package_dirs, meshPath, meshScale); + const boost::shared_ptr geometry = + retrieveCollisionGeometry(tree, link_name, (*i)->name, + (*i)->geometry, package_dirs, meshPath, meshScale); #else ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry); if (urdf_mesh) meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs); @@ -307,7 +370,8 @@ namespace se3 * @param[in] package_dirs A vector containing the different directories where to search for packages * @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION) */ - void parseTreeForGeom(::urdf::LinkConstSharedPtr link, + void parseTreeForGeom(const UrdfTree& tree, + ::urdf::LinkConstSharedPtr link, const Model & model, GeometryModel & geomModel, const std::vector & package_dirs, @@ -317,10 +381,10 @@ namespace se3 switch(type) { case COLLISION: - addLinkGeometryToGeomModel< ::urdf::Collision >(link, model, geomModel, package_dirs); + addLinkGeometryToGeomModel< ::urdf::Collision >(tree, link, model, geomModel, package_dirs); break; case VISUAL: - addLinkGeometryToGeomModel< ::urdf::Visual >(link, model, geomModel, package_dirs); + addLinkGeometryToGeomModel< ::urdf::Visual >(tree, link, model, geomModel, package_dirs); break; default: break; @@ -328,7 +392,7 @@ namespace se3 BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links) { - parseTreeForGeom(child, model, geomModel, package_dirs,type); + parseTreeForGeom(tree, child, model, geomModel, package_dirs,type); } } @@ -344,17 +408,32 @@ namespace se3 const std::vector & package_dirs) throw(std::invalid_argument) { - ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename); - return buildGeom (model, urdfTree, type, geomModel, package_dirs); + std::ifstream xmlStream(filename.c_str()); + if (! xmlStream.is_open()) + { + const std::string exception_message (filename + " does not seem to be a valid file."); + throw std::invalid_argument(exception_message); + } + return buildGeom (model, xmlStream, type, geomModel, package_dirs); } GeometryModel& buildGeom(const Model & model, - const ::urdf::ModelInterfaceSharedPtr & urdfTree, + const std::istream& xmlStream, const GeometryType type, GeometryModel & geomModel, const std::vector & package_dirs) throw(std::invalid_argument) { + std::string xmlStr; + { + std::ostringstream os; + os << xmlStream.rdbuf(); + xmlStr = os.str(); + } + + details::UrdfTree tree; + tree.parse (xmlStr); + std::vector hint_directories(package_dirs); // Append the ROS_PACKAGE_PATH @@ -366,7 +445,7 @@ namespace se3 throw std::runtime_error("You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash"); } - details::parseTreeForGeom(urdfTree->getRoot(), model, geomModel, hint_directories,type); + details::parseTreeForGeom(tree, tree.urdf_->getRoot(), model, geomModel, hint_directories,type); return geomModel; } From b4b2288a6e1e3bd03b19e3c9151174ac6d11a917 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Tue, 5 Dec 2017 15:15:55 +0100 Subject: [PATCH 2/5] [unittest] URDF unittest reads collision geometry --- unittest/urdf.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/unittest/urdf.cpp b/unittest/urdf.cpp index 882d8a992e..7eb7b19582 100644 --- a/unittest/urdf.cpp +++ b/unittest/urdf.cpp @@ -34,6 +34,8 @@ BOOST_AUTO_TEST_CASE ( buildModel ) #endif se3::Model model; se3::urdf::buildModel(filename, model); + se3::GeometryModel geomModel; + se3::urdf::buildGeom(model, filename, se3::COLLISION, geomModel); std::cout << "Robot's name:" << model.name << std::endl; } From ebae412ce48cbb31478c9ce389e36520baba26a8 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Tue, 5 Dec 2017 15:16:17 +0100 Subject: [PATCH 3/5] [models] Add a geometry in simple_humanoid.urdf --- models/simple_humanoid.urdf | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/models/simple_humanoid.urdf b/models/simple_humanoid.urdf index d997cb81ba..4a1c6621a0 100644 --- a/models/simple_humanoid.urdf +++ b/models/simple_humanoid.urdf @@ -12,6 +12,16 @@ + + + + + + + + + From 7b10def77cba9934975403335b98d61e21158acf Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Tue, 5 Dec 2017 15:45:51 +0100 Subject: [PATCH 4/5] [URDF] Throw exception when URDF could not be read. --- src/parsers/urdf/geometry.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/parsers/urdf/geometry.cpp b/src/parsers/urdf/geometry.cpp index 318b9c8592..68e664bbde 100644 --- a/src/parsers/urdf/geometry.cpp +++ b/src/parsers/urdf/geometry.cpp @@ -50,6 +50,9 @@ namespace se3 void parse (const std::string & xmlStr) { urdf_ = ::urdf::parseURDF(xmlStr); + if (!urdf_) { + throw std::invalid_argument ("Enable to parse URDF"); + } std::istringstream iss(xmlStr); using namespace boost::property_tree; From 8d3a807d6ede4a44207596c80e32cb99a1c9b150 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Tue, 5 Dec 2017 15:46:18 +0100 Subject: [PATCH 5/5] [URDF] Do not throw when the list of package directory is empty. --- src/parsers/urdf/geometry.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/parsers/urdf/geometry.cpp b/src/parsers/urdf/geometry.cpp index 68e664bbde..bda85b2b27 100644 --- a/src/parsers/urdf/geometry.cpp +++ b/src/parsers/urdf/geometry.cpp @@ -443,11 +443,6 @@ namespace se3 std::vector ros_pkg_paths = rosPaths(); hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end()); - if(hint_directories.empty()) - { - throw std::runtime_error("You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash"); - } - details::parseTreeForGeom(tree, tree.urdf_->getRoot(), model, geomModel, hint_directories,type); return geomModel; }