diff --git a/CMakeLists.txt b/CMakeLists.txt index 58ce34da3c5e5e3e9cfe9e893b2f6b2e941a7959..ecb12a9dfc50d6216c5c11ea37733afc05c47ebb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -92,13 +92,24 @@ IF(EIGEN3_FOUND) ENDIF(${EIGEN3_VERSION} VERSION_GREATER "3.2.10") ENDIF(EIGEN3_FOUND) -# Special care of urdfdom less than 0.3.0 +# Special care of urdfdom version IF(URDFDOM_FOUND) IF(${URDFDOM_VERSION} VERSION_LESS "0.3.0") ADD_DEFINITIONS(-DURDFDOM_COLLISION_WITH_GROUP_NAME) - SET(URDFDOM_COLLISION_WITH_GROUP_NAME TRUE) PKG_CONFIG_APPEND_CFLAGS("-DURDFDOM_COLLISION_WITH_GROUP_NAME") ENDIF(${URDFDOM_VERSION} VERSION_LESS "0.3.0") + + # defines types from version 0.4.0 + IF(NOT ${URDFDOM_VERSION} VERSION_LESS "0.4.0") + ADD_DEFINITIONS(-DURDFDOM_TYPEDEF_SHARED_PTR) + PKG_CONFIG_APPEND_CFLAGS("-DURDFDOM_TYPEDEF_SHARED_PTR") + ENDIF(NOT ${URDFDOM_VERSION} VERSION_LESS "0.4.0") + + # std::shared_ptr appears from version 1.0.0 + IF(${URDFDOM_VERSION} VERSION_GREATER "0.4.2") + ADD_DEFINITIONS(-DURDFDOM_USE_STD_SHARED_PTR) + PKG_CONFIG_APPEND_CFLAGS("-DURDFDOM_USE_STD_SHARED_PTR") + ENDIF(${URDFDOM_VERSION} VERSION_GREATER "0.4.2") ENDIF(URDFDOM_FOUND) # Special care of lua which can be of versions 5.1 or 5.2 diff --git a/bindings/python/multibody/fcl/distance-result.hpp b/bindings/python/multibody/fcl/distance-result.hpp new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/src/parsers/urdf.hpp b/src/parsers/urdf.hpp index 01955134ebd97098a9db84e3a034d2672a3bac9e..f5b70303606319de952ce988e949166880cb7523 100644 --- a/src/parsers/urdf.hpp +++ b/src/parsers/urdf.hpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2016 CNRS +// Copyright (c) 2015-2017 CNRS // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // // This file is part of Pinocchio @@ -27,17 +27,73 @@ #include <string> #include <exception> -#include <boost/shared_ptr.hpp> +#ifdef URDFDOM_USE_STD_SHARED_PTR + #include <memory> + #define URDF_SHARED_PTR(type) std::shared_ptr<type> + #define URDF_WEAK_PTR(type) std::weak_ptr<type> +#else + #include <boost/shared_ptr.hpp> + #define URDF_SHARED_PTR(type) boost::shared_ptr<type> + #define URDF_WEAK_PTR(type) boost::weak_ptr<type> +#endif + +#ifndef URDFDOM_TYPEDEF_SHARED_PTR + +#define URDF_TYPEDEF_CLASS_POINTER(Class) \ +typedef URDF_SHARED_PTR(Class) Class##SharedPtr; \ +typedef URDF_SHARED_PTR(const Class) Class##ConstSharedPtr; \ +typedef URDF_WEAK_PTR(Class) Class##WeakPtr namespace urdf { - typedef boost::shared_ptr<ModelInterface> ModelInterfacePtr; - typedef boost::shared_ptr<const Joint> JointConstPtr; - typedef boost::shared_ptr<const Link> LinkConstPtr; - typedef boost::shared_ptr<Link> LinkPtr; - typedef boost::shared_ptr<const Inertial> InertialConstPtr; + URDF_TYPEDEF_CLASS_POINTER(Box); + URDF_TYPEDEF_CLASS_POINTER(Collision); + URDF_TYPEDEF_CLASS_POINTER(Cylinder); + URDF_TYPEDEF_CLASS_POINTER(Geometry); + URDF_TYPEDEF_CLASS_POINTER(Inertial); + URDF_TYPEDEF_CLASS_POINTER(Joint); + URDF_TYPEDEF_CLASS_POINTER(Link); + URDF_TYPEDEF_CLASS_POINTER(Material); + URDF_TYPEDEF_CLASS_POINTER(Mesh); + URDF_TYPEDEF_CLASS_POINTER(ModelInterface); + URDF_TYPEDEF_CLASS_POINTER(Sphere); + URDF_TYPEDEF_CLASS_POINTER(Visual); + + template<class T, class U> + URDF_SHARED_PTR(T) const_pointer_cast(URDF_SHARED_PTR(U) const & r) + { +#ifdef URDFDOM_USE_STD_SHARED_PTR + return std::const_pointer_cast<T>(r); +#else + return boost::const_pointer_cast<T>(r); +#endif + } + + template<class T, class U> + URDF_SHARED_PTR(T) dynamic_pointer_cast(URDF_SHARED_PTR(U) const & r) + { +#ifdef URDFDOM_USE_STD_SHARED_PTR + return std::dynamic_pointer_cast<T>(r); +#else + return boost::dynamic_pointer_cast<T>(r); +#endif + } + + template<class T, class U> + URDF_SHARED_PTR(T) static_pointer_cast(URDF_SHARED_PTR(U) const & r) + { +#ifdef URDFDOM_USE_STD_SHARED_PTR + return std:static_pointer_cast<T>(r); +#else + return boost::static_pointer_cast<T>(r); +#endif + } } +#undef URDF_TYPEDEF_CLASS_POINTER + +#endif + namespace se3 { namespace urdf diff --git a/src/parsers/urdf/geometry.cpp b/src/parsers/urdf/geometry.cpp index 49e28707bb140308fb291c14d5b5e72a2fca86dd..236ccdc29f2c0b987b54b1fa55386c6eba03dae4 100644 --- a/src/parsers/urdf/geometry.cpp +++ b/src/parsers/urdf/geometry.cpp @@ -27,6 +27,7 @@ #include <sstream> #include <iomanip> #include <boost/foreach.hpp> +#include <boost/shared_ptr.hpp> #ifdef WITH_HPP_FCL #include <hpp/fcl/mesh_loader/assimp.h> @@ -53,7 +54,7 @@ namespace se3 * * @return A shared pointer on the he geometry converted as a fcl::CollisionGeometry */ - boost::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry(const boost::shared_ptr< ::urdf::Geometry> urdf_geometry, + boost::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry(const ::urdf::GeometrySharedPtr urdf_geometry, const std::vector<std::string> & package_dirs, std::string & meshPath, Eigen::Vector3d & meshScale) @@ -63,7 +64,7 @@ namespace se3 // Handle the case where collision geometry is a mesh if (urdf_geometry->type == ::urdf::Geometry::MESH) { - boost::shared_ptr < ::urdf::Mesh> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry); + const ::urdf::MeshSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry); std::string collisionFilename = collisionGeometry->filename; meshPath = retrieveResourcePath(collisionFilename, package_dirs); @@ -95,7 +96,7 @@ namespace se3 { meshPath = "CYLINDER"; meshScale << 1,1,1; - boost::shared_ptr < ::urdf::Cylinder> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry); + const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry); double radius = collisionGeometry->radius; double length = collisionGeometry->length; @@ -108,7 +109,7 @@ namespace se3 { meshPath = "BOX"; meshScale << 1,1,1; - boost::shared_ptr < ::urdf::Box> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry); + const ::urdf::BoxSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry); double x = collisionGeometry->dim.x; double y = collisionGeometry->dim.y; @@ -121,7 +122,7 @@ namespace se3 { meshPath = "SPHERE"; meshScale << 1,1,1; - boost::shared_ptr < ::urdf::Sphere> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry); + const ::urdf::SphereSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry); double radius = collisionGeometry->radius; @@ -146,16 +147,19 @@ namespace se3 * @return Either the first collision or visual */ template<typename T> - inline boost::shared_ptr<T> getLinkGeometry(::urdf::LinkConstPtr link); + inline const URDF_SHARED_PTR(T) + getLinkGeometry(const ::urdf::LinkConstSharedPtr link); template<> - inline boost::shared_ptr< ::urdf::Collision> getLinkGeometry< ::urdf::Collision>(::urdf::LinkConstPtr link) + inline const ::urdf::CollisionSharedPtr + getLinkGeometry< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link) { return link->collision; } template<> - inline boost::shared_ptr< ::urdf::Visual> getLinkGeometry< ::urdf::Visual>(::urdf::LinkConstPtr link) + inline const ::urdf::VisualSharedPtr + getLinkGeometry< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link) { return link->visual; } @@ -171,11 +175,11 @@ namespace se3 * */ template<typename urdfObject> - inline bool getVisualMaterial(const boost::shared_ptr< urdfObject > urdf_object,std::string& meshTexturePath, + inline bool getVisualMaterial(const 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 boost::shared_ptr< ::urdf::Collision >, std::string& meshTexturePath, + inline bool getVisualMaterial< ::urdf::Collision>(const ::urdf::CollisionSharedPtr, std::string& meshTexturePath, Eigen::Vector4d & meshColor, const std::vector<std::string> &) { meshColor.setZero(); @@ -184,7 +188,7 @@ namespace se3 } template<> - inline bool getVisualMaterial< ::urdf::Visual>(const boost::shared_ptr< ::urdf::Visual > urdf_visual, std::string& meshTexturePath, + inline bool getVisualMaterial< ::urdf::Visual>(const ::urdf::VisualSharedPtr urdf_visual, std::string& meshTexturePath, Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs) { meshColor.setZero(); @@ -211,16 +215,19 @@ namespace se3 * @return the array of either collisions or visuals */ template<typename T> - inline std::vector<boost::shared_ptr<T> > getLinkGeometryArray(::urdf::LinkConstPtr link); + inline const std::vector< URDF_SHARED_PTR(T) > & + getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link); template<> - inline std::vector< boost::shared_ptr< ::urdf::Collision> > getLinkGeometryArray< ::urdf::Collision>(::urdf::LinkConstPtr link) + inline const std::vector< ::urdf::CollisionSharedPtr> & + getLinkGeometryArray< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link) { return link->collision_array; } template<> - inline std::vector< boost::shared_ptr< ::urdf::Visual> > getLinkGeometryArray< ::urdf::Visual>(::urdf::LinkConstPtr link) + inline const std::vector< ::urdf::VisualSharedPtr> & + getLinkGeometryArray< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link) { return link->visual_array; } @@ -236,20 +243,21 @@ namespace se3 * @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION) */ template<typename T> - inline void addLinkGeometryToGeomModel(::urdf::LinkConstPtr link, + inline void addLinkGeometryToGeomModel(::urdf::LinkConstSharedPtr link, const Model & model, GeometryModel & geomModel, const std::vector<std::string> & package_dirs) throw (std::invalid_argument) { + typedef std::vector< URDF_SHARED_PTR(T) > VectorSharedT; if(getLinkGeometry<T>(link)) { std::string meshPath = ""; Eigen::Vector3d meshScale; - std::string link_name = link->name; + const std::string & link_name = link->name; - std::vector< boost::shared_ptr< T > > geometries_array = getLinkGeometryArray<T>(link); + VectorSharedT geometries_array = getLinkGeometryArray<T>(link); if (!model.existFrame(link_name, BODY)) throw std::invalid_argument("No link " + link_name + " in model"); @@ -258,7 +266,7 @@ namespace se3 assert(model.frames[frame_id].type == BODY); std::size_t objectId = 0; - for (typename std::vector< boost::shared_ptr< T > >::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i) + for (typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i) { meshPath.clear(); #ifdef WITH_HPP_FCL @@ -299,7 +307,7 @@ 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::LinkConstPtr link, + void parseTreeForGeom(::urdf::LinkConstSharedPtr link, const Model & model, GeometryModel & geomModel, const std::vector<std::string> & package_dirs, @@ -318,7 +326,7 @@ namespace se3 break; } - BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links) + BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links) { parseTreeForGeom(child, model, geomModel, package_dirs,type); } @@ -347,7 +355,7 @@ namespace se3 throw std::runtime_error("You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash"); } - ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename); + ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename); details::parseTreeForGeom(urdfTree->getRoot(), model, geomModel, hint_directories,type); return geomModel; } diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp index e57e85c89a8ee5ccc3dd5678c387f65d42a5146e..a8822fac116708511353cec8f3a0ef409c3ba936 100644 --- a/src/parsers/urdf/model.cpp +++ b/src/parsers/urdf/model.cpp @@ -37,7 +37,7 @@ namespace se3 const FrameType JOINT_OR_FIXED_JOINT = (FrameType) (JOINT | FIXED_JOINT); const double infty = std::numeric_limits<double>::infinity(); - FrameIndex getParentJointFrame(::urdf::LinkConstPtr link, Model & model) + FrameIndex getParentJointFrame(const ::urdf::LinkConstSharedPtr link, const Model & model) { assert(link && link->getParent()); @@ -55,23 +55,23 @@ namespace se3 + link->getParent()->parent_joint->name); } - Frame& f = model.frames[id]; + const Frame & f = model.frames[id]; if (f.type == JOINT || f.type == FIXED_JOINT) return id; throw std::invalid_argument ("Parent frame is not a JOINT neither a FIXED_JOINT"); } - void appendBodyToJoint(Model& model, const FrameIndex fid, - const boost::shared_ptr< ::urdf::Inertial> Y, + void appendBodyToJoint(Model & model, const FrameIndex fid, + const ::urdf::InertialConstSharedPtr Y_ptr, const SE3 & placement, const std::string & body_name) { const Frame& frame = model.frames[fid]; const SE3& p = frame.placement * placement; if (frame.parent > 0 - && Y - && Y->mass > Eigen::NumTraits<double>::epsilon()) { - model.appendBodyToJoint(frame.parent, convertFromUrdf(*Y), p); + && Y_ptr + && Y_ptr->mass > Eigen::NumTraits<double>::epsilon()) { + model.appendBodyToJoint(frame.parent, convertFromUrdf(*Y_ptr), p); } model.addBodyFrame(body_name, frame.parent, p, (int)fid); // Reference to model.frames[fid] can has changed because the vector @@ -86,9 +86,10 @@ namespace se3 /// \brief Shortcut for adding a joint and directly append a body to it. /// template<typename JointModel> - void addJointAndBody(Model & model, const JointModelBase<JointModel> & jmodel, const FrameIndex& parentFrameId, + void addJointAndBody(Model & model, const JointModelBase<JointModel> & jmodel, + const FrameIndex & parentFrameId, const SE3 & joint_placement, const std::string & joint_name, - const boost::shared_ptr< ::urdf::Inertial> Y, + const ::urdf::InertialConstSharedPtr Y, const std::string & body_name, const typename JointModel::TangentVector_t & max_effort = JointModel::TangentVector_t::Constant( infty), const typename JointModel::TangentVector_t & max_velocity = JointModel::TangentVector_t::Constant( infty), @@ -96,7 +97,7 @@ namespace se3 const typename JointModel::ConfigVector_t & max_config = JointModel::ConfigVector_t ::Constant( infty)) { Model::JointIndex idx; - Frame& frame = model.frames[parentFrameId]; + const Frame & frame = model.frames[parentFrameId]; idx = model.addJoint(frame.parent,jmodel, frame.placement * joint_placement, @@ -112,10 +113,10 @@ namespace se3 /// void addFixedJointAndBody(Model & model, const FrameIndex& parentFrameId, const SE3 & joint_placement, const std::string & joint_name, - const boost::shared_ptr< ::urdf::Inertial> Y, + const ::urdf::InertialConstSharedPtr Y, const std::string & body_name) { - Frame& frame = model.frames[parentFrameId]; + const Frame & frame = model.frames[parentFrameId]; int fid = model.addFrame( Frame (joint_name, frame.parent, parentFrameId, @@ -129,9 +130,12 @@ namespace se3 /// /// \brief Handle the case of JointModelComposite which is dynamic. /// - void addJointAndBody(Model & model, const JointModelBase< JointModelComposite > & jmodel, const Model::JointIndex parent_id, + void addJointAndBody(Model & model, + const JointModelBase< JointModelComposite > & jmodel, + const Model::JointIndex parent_id, const SE3 & joint_placement, const std::string & joint_name, - const boost::shared_ptr< ::urdf::Inertial> Y, const std::string & body_name) + const ::urdf::InertialConstSharedPtr Y, + const std::string & body_name) { Model::JointIndex idx; @@ -148,11 +152,12 @@ namespace se3 /// \param[in] link The current URDF link. /// \param[in] model The model where the link must be added. /// - void parseTree(::urdf::LinkConstPtr link, Model & model, bool verbose) throw (std::invalid_argument) + void parseTree(::urdf::LinkConstSharedPtr link, Model & model, bool verbose) throw (std::invalid_argument) { // Parent joint of the current body - ::urdf::JointConstPtr joint = link->parent_joint; + const ::urdf::JointConstSharedPtr joint = + ::urdf::const_pointer_cast< ::urdf::Joint>(link->parent_joint); if(joint) // if the link is not the root of the tree { @@ -168,7 +173,8 @@ namespace se3 // Transformation from the parent link to the joint origin const SE3 jointPlacement = convertFromUrdf(joint->parent_to_joint_origin_transform); - const boost::shared_ptr< ::urdf::Inertial> Y = link->inertial; + const ::urdf::InertialSharedPtr Y = + ::urdf::const_pointer_cast< ::urdf::Inertial>(link->inertial); switch(joint->type) { @@ -477,7 +483,7 @@ namespace se3 } - BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links) + BOOST_FOREACH(::urdf::LinkConstSharedPtr child, link->child_links) { parseTree(child, model, verbose); } @@ -491,12 +497,12 @@ namespace se3 /// \param[in] model The model where the link must be added. /// \param[in] verbose Print parsing info. /// - void parseRootTree (::urdf::LinkConstPtr root_link, Model & model, const bool verbose) throw (std::invalid_argument) + void parseRootTree (::urdf::LinkConstSharedPtr root_link, Model & model, const bool verbose) throw (std::invalid_argument) { addFixedJointAndBody(model, 0, SE3::Identity(), "root_joint", root_link->inertial, root_link->name); - BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links) + BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links) { parseTree(child, model, verbose); } @@ -517,13 +523,13 @@ namespace se3 /// \param[in] verbose Print parsing info. /// template <typename D> - void parseRootTree (::urdf::LinkConstPtr root_link, Model & model, const JointModelBase<D> & root_joint, const bool verbose) throw (std::invalid_argument) + void parseRootTree (::urdf::LinkConstSharedPtr root_link, Model & model, const JointModelBase<D> & root_joint, const bool verbose) throw (std::invalid_argument) { addJointAndBody(model,root_joint, 0,SE3::Identity(),"root_joint", root_link->inertial,root_link->name); - BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links) + BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links) { parseTree(child, model, verbose); } @@ -537,11 +543,11 @@ namespace se3 /// struct ParseRootTreeVisitor : public boost::static_visitor<> { - ::urdf::LinkConstPtr m_root_link; + ::urdf::LinkConstSharedPtr m_root_link; Model & m_model; const bool m_verbose; - ParseRootTreeVisitor(::urdf::LinkConstPtr root_link, Model & model, const bool verbose) + ParseRootTreeVisitor(::urdf::LinkConstSharedPtr root_link, Model & model, const bool verbose) : m_root_link(root_link) , m_model(model) , m_verbose(verbose) @@ -553,7 +559,7 @@ namespace se3 details::parseRootTree(m_root_link,m_model,root_joint,m_verbose); } - static void run(::urdf::LinkConstPtr root_link, Model & model, const JointModelVariant & root_joint, const bool verbose) + static void run(::urdf::LinkConstSharedPtr root_link, Model & model, const JointModelVariant & root_joint, const bool verbose) { boost::apply_visitor(ParseRootTreeVisitor(root_link,model,verbose),root_joint); } @@ -565,7 +571,7 @@ namespace se3 const bool verbose) throw (std::invalid_argument) { - ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename); + ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename); if (urdfTree) ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose); else @@ -579,7 +585,7 @@ namespace se3 Model& buildModel(const std::string & filename, Model& model,const bool verbose) throw (std::invalid_argument) { - ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename); + ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename); if (urdfTree) details::parseRootTree(urdfTree->getRoot(),model,verbose); else