src/parsers/urdf/geometry.cpp

Go to the documentation of this file.

//
// Copyright (c) 2015-2020 CNRS INRIA
//
 
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/urdf/types.hpp"
#include "pinocchio/parsers/urdf/utils.hpp"
#include "pinocchio/parsers/utils.hpp"
 
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
 
#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
 
#ifdef PINOCCHIO_WITH_HPP_FCL
 
  #include <hpp/fcl/mesh_loader/loader.h>
  #include <hpp/fcl/mesh_loader/assimp.h>
 
#endif // PINOCCHIO_WITH_HPP_FCL
 
namespace pinocchio
{
  namespace urdf
  {
    namespace details
    {
      struct UrdfTree
      {
        typedef boost::property_tree::ptree ptree;
        typedef std::map<std::string, const ptree &> LinkMap_t;
 
        void parse(const std::string & xmlStr)
        {
          urdf_ = ::urdf::parseURDF(xmlStr);
          if (!urdf_)
          {
            throw std::invalid_argument("Unable to parse URDF");
          }
 
          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<std::string>("<xmlattr>.name");
              links_.insert(std::pair<std::string, const ptree &>(name, link.second));
            }
          } // BOOST_FOREACH
        }
 
        bool isCapsule(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")
            {
#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
              std::cerr << "Warning: support for tag link/collision_checking/capsule"
                           " is not available for URDFDOM < 0.3.0"
                        << std::endl;
#else
              std::string name = cc.second.get<std::string>("<xmlattr>.name");
              if (geomName == name)
                return true;
#endif
            }
          } // BOOST_FOREACH
 
          return false;
        }
 
        bool isMeshConvex(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 == "convex")
            {
#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
              std::cerr << "Warning: support for tag link/collision_checking/convex"
                           " is not available for URDFDOM < 0.3.0"
                        << std::endl;
#else
              std::string name = cc.second.get<std::string>("<xmlattr>.name");
              if (geomName == name)
                return true;
#endif
            }
          } // 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_;
      };
 
      template<typename Vector3>
      static void retrieveMeshScale(
        const ::urdf::MeshSharedPtr & mesh, const Eigen::MatrixBase<Vector3> & scale)
      {
        Vector3 & scale_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3, scale);
        scale_ << mesh->scale.x, mesh->scale.y, mesh->scale.z;
      }
 
#ifdef PINOCCHIO_WITH_HPP_FCL
 
      std::shared_ptr<fcl::CollisionGeometry> inline retrieveCollisionGeometry(
        const UrdfTree & tree,
        fcl::MeshLoaderPtr & meshLoader,
        const std::string & linkName,
        const std::string & geomName,
        const ::urdf::GeometrySharedPtr urdf_geometry,
        const std::vector<std::string> & package_dirs,
        std::string & meshPath,
        Eigen::Vector3d & meshScale)
      {
        std::shared_ptr<fcl::CollisionGeometry> geometry;
 
        // Handle the case where collision geometry is a mesh
        if (urdf_geometry->type == ::urdf::Geometry::MESH)
        {
          const ::urdf::MeshSharedPtr urdf_mesh =
            ::urdf::dynamic_pointer_cast<::urdf::Mesh>(urdf_geometry);
          std::string collisionFilename = urdf_mesh->filename;
 
          meshPath = retrieveResourcePath(collisionFilename, package_dirs);
          if (meshPath == "")
          {
            std::stringstream ss;
            ss << "Mesh " << collisionFilename << " could not be found.";
            throw std::invalid_argument(ss.str());
          }
 
          fcl::Vec3f scale = fcl::Vec3f(urdf_mesh->scale.x, urdf_mesh->scale.y, urdf_mesh->scale.z);
 
          retrieveMeshScale(urdf_mesh, meshScale);
 
          // Create FCL mesh by parsing Collada file.
          hpp::fcl::BVHModelPtr_t bvh = meshLoader->load(meshPath, scale);
          bool convex = tree.isMeshConvex(linkName, geomName);
          if (convex)
          {
            bvh->buildConvexRepresentation(false);
            geometry = std::shared_ptr<fcl::CollisionGeometry>(
              bvh->convex.get(), [bvh](...) mutable { bvh->convex.reset(); });
          }
          else
          {
            geometry = std::shared_ptr<fcl::CollisionGeometry>(
              bvh.get(), [bvh](...) mutable { bvh.reset(); });
          }
        }
 
        // Handle the case where collision geometry is a cylinder
        // Use FCL capsules for cylinders
        else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
        {
          const bool is_capsule = tree.isCapsule(linkName, geomName);
          meshScale << 1, 1, 1;
          const ::urdf::CylinderSharedPtr collisionGeometry =
            ::urdf::dynamic_pointer_cast<::urdf::Cylinder>(urdf_geometry);
 
          double radius = collisionGeometry->radius;
          double length = collisionGeometry->length;
 
          // Create fcl capsule geometry.
          if (is_capsule)
          {
            meshPath = "CAPSULE";
            geometry = std::shared_ptr<fcl::CollisionGeometry>(new fcl::Capsule(radius, length));
          }
          else
          {
            meshPath = "CYLINDER";
            geometry = std::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)
        {
          meshPath = "BOX";
          meshScale << 1, 1, 1;
          const ::urdf::BoxSharedPtr collisionGeometry =
            ::urdf::dynamic_pointer_cast<::urdf::Box>(urdf_geometry);
 
          double x = collisionGeometry->dim.x;
          double y = collisionGeometry->dim.y;
          double z = collisionGeometry->dim.z;
 
          geometry = std::shared_ptr<fcl::CollisionGeometry>(new fcl::Box(x, y, z));
        }
        // Handle the case where collision geometry is a sphere.
        else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
        {
          meshPath = "SPHERE";
          meshScale << 1, 1, 1;
          const ::urdf::SphereSharedPtr collisionGeometry =
            ::urdf::dynamic_pointer_cast<::urdf::Sphere>(urdf_geometry);
 
          double radius = collisionGeometry->radius;
 
          geometry = std::shared_ptr<fcl::CollisionGeometry>(new fcl::Sphere(radius));
        }
        else
          throw std::invalid_argument("Unknown geometry type :");
 
        if (!geometry)
        {
          throw std::invalid_argument("The polyhedron retrieved is empty");
        }
 
        return geometry;
      }
#endif // PINOCCHIO_WITH_HPP_FCL
 
      template<typename T>
      inline PINOCCHIO_URDF_SHARED_PTR(const T)
        getLinkGeometry(const ::urdf::LinkConstSharedPtr link);
 
      template<>
      inline ::urdf::CollisionConstSharedPtr
      getLinkGeometry<::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
      {
        return link->collision;
      }
 
      template<>
      inline ::urdf::VisualConstSharedPtr
      getLinkGeometry<::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
      {
        return link->visual;
      }
 
      template<typename urdfObject>
      inline bool getVisualMaterial(
        const PINOCCHIO_URDF_SHARED_PTR(urdfObject) urdf_object,
        std::string & meshTexturePath,
        Eigen::Vector4d & meshColor,
        const std::vector<std::string> & package_dirs);
 
      template<>
      inline bool getVisualMaterial<::urdf::Collision>(
        const ::urdf::CollisionSharedPtr,
        std::string & meshTexturePath,
        Eigen::Vector4d & meshColor,
        const std::vector<std::string> &)
      {
        meshColor << 0.9, 0.9, 0.9, 1.;
        meshTexturePath = "";
        return false;
      }
 
      template<>
      inline bool getVisualMaterial<::urdf::Visual>(
        const ::urdf::VisualSharedPtr urdf_visual,
        std::string & meshTexturePath,
        Eigen::Vector4d & meshColor,
        const std::vector<std::string> & package_dirs)
      {
        meshColor << 0.9, 0.9, 0.9, 1.;
        meshTexturePath = "";
        bool overrideMaterial = false;
        if (urdf_visual->material)
        {
          overrideMaterial = true;
          meshColor << urdf_visual->material->color.r, urdf_visual->material->color.g,
            urdf_visual->material->color.b, urdf_visual->material->color.a;
          if (urdf_visual->material->texture_filename != "")
            meshTexturePath =
              retrieveResourcePath((urdf_visual)->material->texture_filename, package_dirs);
        }
        return overrideMaterial;
      }
 
      template<typename T>
      inline const std::vector<PINOCCHIO_URDF_SHARED_PTR(T)> &
      getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link);
 
      template<>
      inline const std::vector<::urdf::CollisionSharedPtr> &
      getLinkGeometryArray<::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
      {
        return link->collision_array;
      }
 
      template<>
      inline const std::vector<::urdf::VisualSharedPtr> &
      getLinkGeometryArray<::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
      {
        return link->visual_array;
      }
 
      template<typename GeometryType>
      static void addLinkGeometryToGeomModel(
        const UrdfTree & tree,
        ::hpp::fcl::MeshLoaderPtr & meshLoader,
        ::urdf::LinkConstSharedPtr link,
        UrdfGeomVisitorBase & visitor,
        GeometryModel & geomModel,
        const std::vector<std::string> & package_dirs)
      {
#ifndef PINOCCHIO_WITH_HPP_FCL
        PINOCCHIO_UNUSED_VARIABLE(tree);
        PINOCCHIO_UNUSED_VARIABLE(meshLoader);
#endif // PINOCCHIO_WITH_HPP_FCL
 
        typedef std::vector<PINOCCHIO_URDF_SHARED_PTR(GeometryType)> VectorSharedT;
        typedef GeometryModel::SE3 SE3;
 
        if (getLinkGeometry<GeometryType>(link))
        {
          std::string meshPath = "";
 
          Eigen::Vector3d meshScale(Eigen::Vector3d::Ones());
 
          const std::string & link_name = link->name;
 
          VectorSharedT geometries_array = getLinkGeometryArray<GeometryType>(link);
 
          FrameIndex frame_id;
          UrdfGeomVisitorBase::Frame frame = visitor.getBodyFrame(link_name, frame_id);
          const SE3 & body_placement = frame.placement.cast<SE3::Scalar>();
 
          std::size_t objectId = 0;
          for (typename VectorSharedT::const_iterator i = geometries_array.begin();
               i != geometries_array.end(); ++i)
          {
            meshPath.clear();
#ifdef PINOCCHIO_WITH_HPP_FCL
 
  #ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
            const std::string & geom_name = (*i)->group_name;
  #else
            const std::string & geom_name = (*i)->name;
  #endif // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
            const GeometryObject::CollisionGeometryPtr geometry = retrieveCollisionGeometry(
              tree, meshLoader, link_name, geom_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);
              retrieveMeshScale(urdf_mesh, meshScale);
            }
 
            const std::shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry());
#endif // PINOCCHIO_WITH_HPP_FCL
 
            Eigen::Vector4d meshColor;
            std::string meshTexturePath;
            bool overrideMaterial =
              getVisualMaterial<GeometryType>((*i), meshTexturePath, meshColor, package_dirs);
 
            const SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin);
            std::ostringstream geometry_object_suffix;
            geometry_object_suffix << "_" << objectId;
            const std::string & geometry_object_name =
              std::string(link_name + geometry_object_suffix.str());
            GeometryObject geometry_object(
              geometry_object_name, frame.parentJoint, frame_id, geomPlacement, geometry, meshPath,
              meshScale, overrideMaterial, meshColor, meshTexturePath);
            geomModel.addGeometryObject(geometry_object);
            ++objectId;
          }
        }
      }
 
      void recursiveParseTreeForGeom(
        const UrdfTree & tree,
        ::hpp::fcl::MeshLoaderPtr & meshLoader,
        ::urdf::LinkConstSharedPtr link,
        UrdfGeomVisitorBase & visitor,
        GeometryModel & geomModel,
        const std::vector<std::string> & package_dirs,
        const GeometryType type)
      {
 
        switch (type)
        {
        case COLLISION:
          addLinkGeometryToGeomModel<::urdf::Collision>(
            tree, meshLoader, link, visitor, geomModel, package_dirs);
          break;
        case VISUAL:
          addLinkGeometryToGeomModel<::urdf::Visual>(
            tree, meshLoader, link, visitor, geomModel, package_dirs);
          break;
        default:
          break;
        }
 
        BOOST_FOREACH (::urdf::LinkConstSharedPtr child, link->child_links)
        {
          recursiveParseTreeForGeom(
            tree, meshLoader, child, visitor, geomModel, package_dirs, type);
        }
      }
 
      void parseTreeForGeom(
        UrdfGeomVisitorBase & visitor,
        const std::istream & xmlStream,
        const GeometryType type,
        GeometryModel & geomModel,
        const std::vector<std::string> & package_dirs,
        ::hpp::fcl::MeshLoaderPtr meshLoader)
      {
        std::string xmlStr;
        {
          std::ostringstream os;
          os << xmlStream.rdbuf();
          xmlStr = os.str();
        }
 
        details::UrdfTree tree;
        tree.parse(xmlStr);
 
        std::vector<std::string> hint_directories(package_dirs);
 
        // Append the ROS_PACKAGE_PATH
        std::vector<std::string> ros_pkg_paths = rosPaths();
        hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());
 
#ifdef PINOCCHIO_WITH_HPP_FCL
        if (!meshLoader)
          meshLoader = fcl::MeshLoaderPtr(new fcl::MeshLoader);
#endif // ifdef PINOCCHIO_WITH_HPP_FCL
 
        recursiveParseTreeForGeom(
          tree, meshLoader, tree.urdf_->getRoot(), visitor, geomModel, hint_directories, type);
      }
    } // namespace details
  } // namespace urdf
} // namespace pinocchio