From c258d7b00a91c759147d4f1f33553bda5e73e932 Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Mon, 11 Jan 2016 11:13:24 +0100
Subject: [PATCH] [C++][CMake][Doc] Split geometry parser into two parts.Added
guard to file to avoid multiple redefinition
---
CMakeLists.txt | 2 +
src/multibody/geometry.hpp | 194 +--------------
src/multibody/geometry.hxx | 237 +++++++++++++++++++
src/multibody/parser/from-collada-to-fcl.hpp | 50 +++-
src/multibody/parser/urdf-with-geometry.hpp | 130 ++++------
src/multibody/parser/urdf-with-geometry.hxx | 130 ++++++++++
6 files changed, 468 insertions(+), 275 deletions(-)
create mode 100644 src/multibody/geometry.hxx
create mode 100644 src/multibody/parser/urdf-with-geometry.hxx
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 33ffd5999..71bea9b02 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -178,6 +178,7 @@ IF(URDFDOM_FOUND)
LIST(APPEND ${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
multibody/parser/from-collada-to-fcl.hpp
multibody/parser/urdf-with-geometry.hpp
+ multibody/parser/urdf-with-geometry.hxx
)
ENDIF(HPP_FCL_FOUND)
@@ -190,6 +191,7 @@ IF(HPP_FCL_FOUND)
)
LIST(APPEND ${PROJECT_NAME}_MULTIBODY_HEADERS
multibody/geometry.hpp
+ multibody/geometry.hxx
)
ENDIF(HPP_FCL_FOUND)
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index 0966c5986..9f7851de7 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -189,195 +189,11 @@ namespace se3
};
- inline GeometryModel::Index GeometryModel::addGeomObject( Index parent,const fcl::CollisionObject & co, const SE3 & placement, const std::string & geoName )
- {
-
- Index idx = (Index) (ngeom ++);
-
-
- collision_objects .push_back(co);
- geom_parents .push_back(parent);
- geometryPlacement .push_back(placement);
- geom_names .push_back( (geoName!="")?geoName:random(8));
-
- return idx;
- }
- inline GeometryModel::Index GeometryModel::getGeomId( const std::string & name ) const
- {
- std::vector<std::string>::iterator::difference_type
- res = std::find(geom_names.begin(),geom_names.end(),name) - geom_names.begin();
- assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
- assert( (res>=0)&&(res<(long)collision_objects.size())&&"The joint name you asked do not exist" );
- return Index(res);
- }
- inline bool GeometryModel::existGeomName( const std::string & name ) const
- {
- return (geom_names.end() != std::find(geom_names.begin(),geom_names.end(),name));
- }
- inline const std::string& GeometryModel::getGeomName( Index index ) const
- {
- assert( index < (Index)collision_objects.size() );
- return geom_names[index];
- }
-
- inline void GeometryModel::addInnerObject(Index joint, Index inner_object)
- {
- innerObjects[joint].push_back(inner_object);
- }
-
- inline void GeometryModel::addOutterObject(Index joint, Index outer_object)
- {
- outerObjects[joint].push_back(outer_object);
- }
-
- inline std::ostream& operator<<(std::ostream& os, const GeometryModel& model_geom)
- {
- os << "Nb collision objects = " << model_geom.ngeom << std::endl;
-
- for(GeometryModel::Index i=0;i<(GeometryModel::Index)(model_geom.ngeom);++i)
- {
- os << "Object n " << i << " : " << model_geom.geom_names[i] << ": attached to joint = " << model_geom.geom_parents[i]
- << "\nwith offset \n" << model_geom.geometryPlacement[i] <<std::endl;
- }
-
- return os;
- }
-
- inline std::ostream& operator<<(std::ostream& os, const GeometryData& data_geom)
- {
-
- for(GeometryData::Index i=0;i<(GeometryData::Index)(data_geom.model_geom.ngeom);++i)
- {
- os << "collision object oMi " << data_geom.oMg[i] << std::endl;
- }
-
- return os;
- }
-
- inline void GeometryData::addCollisionPair (Index co1, Index co2)
- {
- assert ( co1 < co2);
- assert ( co2 < model_geom.ngeom);
- CollisionPair_t pair(co1, co2);
-
- addCollisionPair(pair);
- }
-
- inline void GeometryData::addCollisionPair (const CollisionPair_t& pair)
- {
- assert(pair.first < pair.second);
- assert(pair.second < model_geom.ngeom);
- collision_pairs.push_back(pair);
- nCollisionPairs++;
- }
-
- inline void GeometryData::removeCollisionPair (Index co1, Index co2)
- {
- assert(co1 < co2);
- assert(co2 < model_geom.ngeom);
- assert(isCollisionPair(co1,co2));
-
- removeCollisionPair (CollisionPair_t(co1,co2));
- }
-
- inline void GeometryData::removeCollisionPair (const CollisionPair_t& pair)
- {
- assert(pair.first < pair.second);
- assert(pair.second < model_geom.ngeom);
- assert(isCollisionPair(pair));
-
- collision_pairs.erase(std::remove(collision_pairs.begin(), collision_pairs.end(), pair), collision_pairs.end());
- nCollisionPairs--;
- }
-
- inline bool GeometryData::isCollisionPair (Index co1, Index co2) const
- {
- return isCollisionPair(CollisionPair_t(co1,co2));
- }
-
- inline bool GeometryData::isCollisionPair (const CollisionPair_t& pair) const
- {
- return (std::find_if ( collision_pairs.begin(), collision_pairs.end(),
- IsSameCollisionPair(pair)) != collision_pairs.end());
-
- }
-
- inline void GeometryData::fillAllPairsAsCollisions()
- {
- for (Index i = 0; i < model_geom.ngeom; ++i)
- {
- for (Index j = i+1; j < model_geom.ngeom; ++j)
- {
- addCollisionPair(i,j);
- }
- }
- }
-
- // TODO : give a srdf file as argument, read it, and remove corresponding
- // pairs from list collision_pairs
- inline void GeometryData::desactivateCollisionPairs()
- {
-
- }
-
- inline void GeometryData::initializeListOfCollisionPairs()
- {
- fillAllPairsAsCollisions();
- desactivateCollisionPairs();
- assert(nCollisionPairs == collision_pairs.size());
- }
-
- inline bool GeometryData::collide(Index co1, Index co2) const
- {
- fcl::CollisionRequest collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP);
- fcl::CollisionResult collisionResult;
-
- if (fcl::collide (model_geom.collision_objects[co1].collisionGeometry().get(), oMg_fcl[co1],
- model_geom.collision_objects[co2].collisionGeometry().get(), oMg_fcl[co2],
- collisionRequest, collisionResult) != 0)
- {
- return true;
- }
- return false;
- }
-
- inline bool GeometryData::isColliding() const
- {
- for (std::vector<CollisionPair_t>::const_iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
- {
- if (collide(it->first, it->second))
- {
- return true;
- }
- }
- return false;
- }
-
- inline fcl::DistanceResult GeometryData::computeDistance(Index co1, Index co2) const
- {
- fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
- fcl::DistanceResult result;
- fcl::distance ( model_geom.collision_objects[co1].collisionGeometry().get(), oMg_fcl[co1],
- model_geom.collision_objects[co2].collisionGeometry().get(), oMg_fcl[co2],
- distanceRequest, result);
- return result;
- }
-
- inline void GeometryData::resetDistances()
- {
- std::fill(distances.begin(), distances.end(), DistanceResult( fcl::DistanceResult(), 0, 0) );
- }
-
- inline void GeometryData::computeDistances ()
- {
- std::size_t cpt = 0;
- for (std::vector<CollisionPair_t>::iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
- {
- distances[cpt] = DistanceResult(computeDistance(it->first, it->second), it->first, it->second);
- cpt++;
- }
- }
-
} // namespace se3
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+#include "pinocchio/multibody/geometry.hxx"
+
#endif // ifndef __se3_geom_hpp__
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
new file mode 100644
index 000000000..1812367d8
--- /dev/null
+++ b/src/multibody/geometry.hxx
@@ -0,0 +1,237 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_geom_hxx__
+#define __se3_geom_hxx__
+
+
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/spatial/force.hpp"
+#include "pinocchio/spatial/motion.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/joint/joint-variant.hpp"
+#include <iostream>
+
+#include <hpp/fcl/collision_object.h>
+#include <hpp/fcl/collision.h>
+#include <hpp/fcl/distance.h>
+#include <map>
+#include <list>
+#include <utility>
+
+
+namespace se3
+{
+
+ inline GeometryModel::Index GeometryModel::addGeomObject( Index parent,const fcl::CollisionObject & co, const SE3 & placement, const std::string & geoName )
+ {
+
+ Index idx = (Index) (ngeom ++);
+
+
+ collision_objects .push_back(co);
+ geom_parents .push_back(parent);
+ geometryPlacement .push_back(placement);
+ geom_names .push_back( (geoName!="")?geoName:random(8));
+
+ return idx;
+ }
+
+ inline GeometryModel::Index GeometryModel::getGeomId( const std::string & name ) const
+ {
+ std::vector<std::string>::iterator::difference_type
+ res = std::find(geom_names.begin(),geom_names.end(),name) - geom_names.begin();
+ assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
+ assert( (res>=0)&&(res<(long)collision_objects.size())&&"The joint name you asked do not exist" );
+ return Index(res);
+ }
+
+ inline bool GeometryModel::existGeomName( const std::string & name ) const
+ {
+ return (geom_names.end() != std::find(geom_names.begin(),geom_names.end(),name));
+ }
+
+ inline const std::string& GeometryModel::getGeomName( Index index ) const
+ {
+ assert( index < (Index)collision_objects.size() );
+ return geom_names[index];
+ }
+
+ inline void GeometryModel::addInnerObject(Index joint, Index inner_object)
+ {
+ innerObjects[joint].push_back(inner_object);
+ }
+
+ inline void GeometryModel::addOutterObject(Index joint, Index outer_object)
+ {
+ outerObjects[joint].push_back(outer_object);
+ }
+
+ inline std::ostream& operator<<(std::ostream& os, const GeometryModel& model_geom)
+ {
+ os << "Nb collision objects = " << model_geom.ngeom << std::endl;
+
+ for(GeometryModel::Index i=0;i<(GeometryModel::Index)(model_geom.ngeom);++i)
+ {
+ os << "Object n " << i << " : " << model_geom.geom_names[i] << ": attached to joint = " << model_geom.geom_parents[i]
+ << "\nwith offset \n" << model_geom.geometryPlacement[i] <<std::endl;
+ }
+
+ return os;
+ }
+
+ inline std::ostream& operator<<(std::ostream& os, const GeometryData& data_geom)
+ {
+
+ for(GeometryData::Index i=0;i<(GeometryData::Index)(data_geom.model_geom.ngeom);++i)
+ {
+ os << "collision object oMi " << data_geom.oMg[i] << std::endl;
+ }
+
+ return os;
+ }
+
+ inline void GeometryData::addCollisionPair (Index co1, Index co2)
+ {
+ assert ( co1 < co2);
+ assert ( co2 < model_geom.ngeom);
+ CollisionPair_t pair(co1, co2);
+
+ addCollisionPair(pair);
+ }
+
+ inline void GeometryData::addCollisionPair (const CollisionPair_t& pair)
+ {
+ assert(pair.first < pair.second);
+ assert(pair.second < model_geom.ngeom);
+ collision_pairs.push_back(pair);
+ nCollisionPairs++;
+ }
+
+ inline void GeometryData::removeCollisionPair (Index co1, Index co2)
+ {
+ assert(co1 < co2);
+ assert(co2 < model_geom.ngeom);
+ assert(isCollisionPair(co1,co2));
+
+ removeCollisionPair (CollisionPair_t(co1,co2));
+ }
+
+ inline void GeometryData::removeCollisionPair (const CollisionPair_t& pair)
+ {
+ assert(pair.first < pair.second);
+ assert(pair.second < model_geom.ngeom);
+ assert(isCollisionPair(pair));
+
+ collision_pairs.erase(std::remove(collision_pairs.begin(), collision_pairs.end(), pair), collision_pairs.end());
+ nCollisionPairs--;
+ }
+
+ inline bool GeometryData::isCollisionPair (Index co1, Index co2) const
+ {
+ return isCollisionPair(CollisionPair_t(co1,co2));
+ }
+
+ inline bool GeometryData::isCollisionPair (const CollisionPair_t& pair) const
+ {
+ return (std::find_if ( collision_pairs.begin(), collision_pairs.end(),
+ IsSameCollisionPair(pair)) != collision_pairs.end());
+
+ }
+
+ inline void GeometryData::fillAllPairsAsCollisions()
+ {
+ for (Index i = 0; i < model_geom.ngeom; ++i)
+ {
+ for (Index j = i+1; j < model_geom.ngeom; ++j)
+ {
+ addCollisionPair(i,j);
+ }
+ }
+ }
+
+ // TODO : give a srdf file as argument, read it, and remove corresponding
+ // pairs from list collision_pairs
+ inline void GeometryData::desactivateCollisionPairs()
+ {
+
+ }
+
+ inline void GeometryData::initializeListOfCollisionPairs()
+ {
+ fillAllPairsAsCollisions();
+ desactivateCollisionPairs();
+ assert(nCollisionPairs == collision_pairs.size());
+ }
+
+ inline bool GeometryData::collide(Index co1, Index co2) const
+ {
+ fcl::CollisionRequest collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP);
+ fcl::CollisionResult collisionResult;
+
+ if (fcl::collide (model_geom.collision_objects[co1].collisionGeometry().get(), oMg_fcl[co1],
+ model_geom.collision_objects[co2].collisionGeometry().get(), oMg_fcl[co2],
+ collisionRequest, collisionResult) != 0)
+ {
+ return true;
+ }
+ return false;
+ }
+
+ inline bool GeometryData::isColliding() const
+ {
+ for (std::vector<CollisionPair_t>::const_iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
+ {
+ if (collide(it->first, it->second))
+ {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ inline fcl::DistanceResult GeometryData::computeDistance(Index co1, Index co2) const
+ {
+ fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
+ fcl::DistanceResult result;
+ fcl::distance ( model_geom.collision_objects[co1].collisionGeometry().get(), oMg_fcl[co1],
+ model_geom.collision_objects[co2].collisionGeometry().get(), oMg_fcl[co2],
+ distanceRequest, result);
+ return result;
+ }
+
+ inline void GeometryData::resetDistances()
+ {
+ std::fill(distances.begin(), distances.end(), DistanceResult( fcl::DistanceResult(), 0, 0) );
+ }
+
+ inline void GeometryData::computeDistances ()
+ {
+ std::size_t cpt = 0;
+ for (std::vector<CollisionPair_t>::iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
+ {
+ distances[cpt] = DistanceResult(computeDistance(it->first, it->second), it->first, it->second);
+ cpt++;
+ }
+ }
+
+} // namespace se3
+
+#endif // ifndef __se3_geom_hxx__
diff --git a/src/multibody/parser/from-collada-to-fcl.hpp b/src/multibody/parser/from-collada-to-fcl.hpp
index d46ec85c3..75938acb5 100644
--- a/src/multibody/parser/from-collada-to-fcl.hpp
+++ b/src/multibody/parser/from-collada-to-fcl.hpp
@@ -15,6 +15,10 @@
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_collada_to_fcl_hpp__
+#define __se3_collada_to_fcl_hpp__
+
#include <limits>
#include <boost/filesystem/fstream.hpp>
@@ -47,7 +51,18 @@ struct TriangleAndVertices
std::vector <fcl::Triangle> triangles_;
};
-void buildMesh (const ::urdf::Vector3& scale, const aiScene* scene, const aiNode* node,
+
+/**
+ * @brief Recursive procedure for building a mesh
+ *
+ * @param[in] scale Scale to apply when reading the ressource
+ * @param[in] scene Pointer to the assimp scene
+ * @param[in] node Current node of the scene
+ * @param subMeshIndexes Submesh triangles indexes interval
+ * @param[in] mesh The mesh that must be built
+ * @param tv Triangles and Vertices of the mesh submodels
+ */
+inline void buildMesh (const ::urdf::Vector3& scale, const aiScene* scene, const aiNode* node,
std::vector<unsigned>& subMeshIndexes, const PolyhedronPtrType& mesh,
TriangleAndVertices& tv)
{
@@ -109,7 +124,16 @@ void buildMesh (const ::urdf::Vector3& scale, const aiScene* scene, const aiNode
}
-void meshFromAssimpScene (const std::string& name, const ::urdf::Vector3& scale,
+
+/**
+ * @brief Convert an assimp scene to a mesh
+ *
+ * @param[in] name File (ressource) transformed into an assimp scene in loa
+ * @param[in] scale Scale to apply when reading the ressource
+ * @param[in] scene Pointer to the assimp scene
+ * @param[in] mesh The mesh that must be built
+ */
+inline void meshFromAssimpScene (const std::string& name, const ::urdf::Vector3& scale,
const aiScene* scene,const PolyhedronPtrType& mesh)
{
TriangleAndVertices tv;
@@ -138,7 +162,15 @@ void meshFromAssimpScene (const std::string& name, const ::urdf::Vector3& scale,
mesh->endModel ();
}
-void loadPolyhedronFromResource ( const std::string& resource_path, const ::urdf::Vector3& scale,
+
+/**
+ * @brief Read a mesh file and convert it to a polyhedral mesh
+ *
+ * @param[in] resource_path Path to the ressource mesh file to be read
+ * @param[in] scale Scale to apply when reading the ressource
+ * @param[in] polyhedron The resulted polyhedron
+ */
+inline void loadPolyhedronFromResource ( const std::string& resource_path, const ::urdf::Vector3& scale,
const PolyhedronPtrType& polyhedron)
{
Assimp::Importer importer;
@@ -154,7 +186,16 @@ void loadPolyhedronFromResource ( const std::string& resource_path, const ::urdf
meshFromAssimpScene (resource_path, scale, scene, polyhedron);
}
-std::string fromURDFMeshPathToAbsolutePath(std::string & urdf_mesh_path, std::string meshRootDir)
+
+/**
+ * @brief Transform a cURL readable path (package://..) to an absolute path for urdf collision path
+ *
+ * @param urdf_mesh_path The path given in the urdf file (package://..)
+ * @param[in] meshRootDir Root path to the directory where meshes are located
+ *
+ * @return The absolute path to the mesh file
+ */
+inline std::string fromURDFMeshPathToAbsolutePath(std::string & urdf_mesh_path, std::string meshRootDir)
{
std::string absolutePath = std::string(meshRootDir +
@@ -163,3 +204,4 @@ std::string fromURDFMeshPathToAbsolutePath(std::string & urdf_mesh_path, std::st
return absolutePath;
}
+#endif // __se3_collada_to_fcl_hpp__
diff --git a/src/multibody/parser/urdf-with-geometry.hpp b/src/multibody/parser/urdf-with-geometry.hpp
index 1fa726017..27c7227c1 100644
--- a/src/multibody/parser/urdf-with-geometry.hpp
+++ b/src/multibody/parser/urdf-with-geometry.hpp
@@ -47,95 +47,61 @@ namespace se3
{
-fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link, std::string meshRootDir)
- {
- boost::shared_ptr < ::urdf::Collision> collision = link->collision;
- boost::shared_ptr < fcl::CollisionGeometry > geometry;
-
- // Handle the case where collision geometry is a mesh
- if (collision->geometry->type == ::urdf::Geometry::MESH)
- {
- boost::shared_ptr < ::urdf::Mesh> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Mesh> (collision->geometry);
- std::string collisionFilename = collisionGeometry->filename;
-
- std::string full_path = fromURDFMeshPathToAbsolutePath(collisionFilename, meshRootDir);
+ /**
+ * @brief Get a CollisionObject from an urdf link, reading the
+ * corresponding mesh
+ *
+ * @param[in] link The input urdf link
+ * @param[in] meshRootDir Root path to the directory where meshes are located
+ *
+ * @return The mesh converted as a fcl::CollisionObject
+ */
+ inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link, std::string meshRootDir);
- ::urdf::Vector3 scale = collisionGeometry->scale;
-
- // Create FCL mesh by parsing Collada file.
- PolyhedronPtrType polyhedron (new PolyhedronType);
-
- loadPolyhedronFromResource (full_path, scale, polyhedron);
- geometry = polyhedron;
- }
-
- // Compute body position in world frame.
- // MatrixHomogeneousType position = computeBodyAbsolutePosition (link, collision->origin);
- if (!geometry)
- {
- throw std::runtime_error(std::string("The polyhedron retrived is empty"));
- }
- fcl::CollisionObject collisionObject (geometry, fcl::Transform3f());
-
- return collisionObject; // TO CHECK: what happens if geometry is empty ?
-
- }
-
- void parseTreeForGeom( ::urdf::LinkConstPtr link, Model & model,GeometryModel & model_geom, std::string meshRootDir) throw (std::invalid_argument)
-{
-
- ::urdf::JointConstPtr joint = link->parent_joint;
-
- if(joint!=NULL)
- {
- assert(link->getParent()!=NULL);
-
- if (link->collision)
- {
- fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, meshRootDir);
- SE3 geomPlacement = convertFromUrdf(link->collision->origin);
- std::string collision_object_name = link->name ;
- model_geom.addGeomObject(model.getJointId(joint->name), collision_object, geomPlacement, collision_object_name);
- }
- }
- else if (link->getParent() != NULL)
- {
- const std::string exception_message (link->name + " - joint information missing.");
- throw std::invalid_argument(exception_message);
- }
-
- BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
- {
- parseTreeForGeom(child, model, model_geom, meshRootDir);
- }
-}
-
+ /**
+ * @brief Recursive procedure for reading the URDF tree, looking for geometries
+ * This function fill the geometric model whith geometry objects retrieved from the URDF tree
+ *
+ * @param[in] link The current URDF link
+ * @param model The model to which is the Geometry Model associated
+ * @param model_geom The Geometry Model where the Collision Objects must be added
+ * @param[in] meshRootDir Root path to the directory where meshes are located
+ */
+ inline void parseTreeForGeom( ::urdf::LinkConstPtr link, Model & model,GeometryModel & model_geom, std::string meshRootDir) throw (std::invalid_argument);
+
+
+
+ /**
+ * @brief Build The Model and GeometryModel from a URDF file with a particular joint as root of the model tree
+ *
+ * @param[in] filename The URDF complete file path
+ * @param[in] meshRootDir Root path to the directory where meshes are located
+ *
+ * @return The pair <se3::Model, se3::GeometryModel> the Model tree and its geometric model associated
+ */
+ template <typename D>
+ std::pair<Model, GeometryModel > buildModelAndGeom( const std::string & filename, const std::string & meshRootDir, const JointModelBase<D> & root_joint );
- template <typename D>
- std::pair<Model, GeometryModel > buildModelAndGeom( const std::string & filename, const std::string & meshRootDir, const JointModelBase<D> & root_joint )
- {
- Model model; GeometryModel model_geom;
-
- ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
- parseTree(urdfTree->getRoot(), model, SE3::Identity(), root_joint);
- parseTreeForGeom(urdfTree->getRoot(), model, model_geom, meshRootDir);
- return std::make_pair(model, model_geom);
- }
-
- std::pair<Model, GeometryModel > buildModelAndGeom( const std::string & filename, const std::string & meshRootDir)
- {
- Model model; GeometryModel model_geom;
-
- ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
- parseTree(urdfTree->getRoot(), model, SE3::Identity());
- parseTreeForGeom(urdfTree->getRoot(), model, model_geom, meshRootDir);
- return std::make_pair(model, model_geom);
- }
+ /**
+ * @brief Build The Model and GeometryModel from a URDF file with no root joint added to the model tree
+ *
+ * @param[in] filename The URDF complete file path
+ * @param[in] meshRootDir Root path to the directory where meshes are located
+ *
+ * @return The pair <se3::Model, se3::GeometryModel> the Model tree and its geometric model associated
+ */
+ inline std::pair<Model, GeometryModel > buildModelAndGeom( const std::string & filename, const std::string & meshRootDir);
} // namespace urdf
} // namespace se3
+
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+#include "pinocchio/multibody/parser/urdf-with-geometry.hxx"
+
#endif // ifndef __se3_urdf_geom_hpp__
diff --git a/src/multibody/parser/urdf-with-geometry.hxx b/src/multibody/parser/urdf-with-geometry.hxx
new file mode 100644
index 000000000..7ff1fa337
--- /dev/null
+++ b/src/multibody/parser/urdf-with-geometry.hxx
@@ -0,0 +1,130 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_urdf_geom_hxx__
+#define __se3_urdf_geom_hxx__
+
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+#include <iostream>
+#include <boost/foreach.hpp>
+#include "pinocchio/multibody/model.hpp"
+
+#include <hpp/fcl/collision_object.h>
+#include <hpp/fcl/collision.h>
+#include <hpp/fcl/shape/geometric_shapes.h>
+#include "pinocchio/multibody/parser/from-collada-to-fcl.hpp"
+
+#include <exception>
+
+
+namespace se3
+{
+ namespace urdf
+ {
+
+
+ inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link, std::string meshRootDir)
+ {
+ boost::shared_ptr < ::urdf::Collision> collision = link->collision;
+ boost::shared_ptr < fcl::CollisionGeometry > geometry;
+
+ // Handle the case where collision geometry is a mesh
+ if (collision->geometry->type == ::urdf::Geometry::MESH)
+ {
+ boost::shared_ptr < ::urdf::Mesh> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Mesh> (collision->geometry);
+ std::string collisionFilename = collisionGeometry->filename;
+
+ std::string full_path = fromURDFMeshPathToAbsolutePath(collisionFilename, meshRootDir);
+
+ ::urdf::Vector3 scale = collisionGeometry->scale;
+
+ // Create FCL mesh by parsing Collada file.
+ PolyhedronPtrType polyhedron (new PolyhedronType);
+
+ loadPolyhedronFromResource (full_path, scale, polyhedron);
+ geometry = polyhedron;
+ }
+
+ if (!geometry)
+ {
+ throw std::runtime_error(std::string("The polyhedron retrived is empty"));
+ }
+ fcl::CollisionObject collisionObject (geometry, fcl::Transform3f());
+
+ return collisionObject; // TO CHECK: what happens if geometry is empty ?
+ }
+
+
+ inline void parseTreeForGeom( ::urdf::LinkConstPtr link, Model & model,GeometryModel & model_geom, std::string meshRootDir) throw (std::invalid_argument)
+ {
+
+ ::urdf::JointConstPtr joint = link->parent_joint;
+
+ if(joint!=NULL)
+ {
+ assert(link->getParent()!=NULL);
+
+ if (link->collision)
+ {
+ fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, meshRootDir);
+ SE3 geomPlacement = convertFromUrdf(link->collision->origin);
+ std::string collision_object_name = link->name ;
+ model_geom.addGeomObject(model.getJointId(joint->name), collision_object, geomPlacement, collision_object_name);
+ }
+ }
+ else if (link->getParent() != NULL)
+ {
+ const std::string exception_message (link->name + " - joint information missing.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
+ {
+ parseTreeForGeom(child, model, model_geom, meshRootDir);
+ }
+ }
+
+
+
+ template <typename D>
+ std::pair<Model, GeometryModel > buildModelAndGeom( const std::string & filename, const std::string & meshRootDir, const JointModelBase<D> & root_joint )
+ {
+ Model model; GeometryModel model_geom;
+
+ ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
+ parseTree(urdfTree->getRoot(), model, SE3::Identity(), root_joint);
+ parseTreeForGeom(urdfTree->getRoot(), model, model_geom, meshRootDir);
+ return std::make_pair(model, model_geom);
+ }
+
+ inline std::pair<Model, GeometryModel > buildModelAndGeom( const std::string & filename, const std::string & meshRootDir)
+ {
+ Model model; GeometryModel model_geom;
+
+ ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
+ parseTree(urdfTree->getRoot(), model, SE3::Identity());
+ parseTreeForGeom(urdfTree->getRoot(), model, model_geom, meshRootDir);
+ return std::make_pair(model, model_geom);
+ }
+
+ } // namespace urdf
+} // namespace se3
+
+#endif // ifndef __se3_urdf_geom_hxx__
+
--
GitLab