diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1c6e7f3dc9d4f104e30a5200a28d5b01cdb855d9..81651f0e3289f5364da7cebab39b1321f954d822 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -63,6 +63,9 @@ ADD_OPTIONAL_DEPENDENCY("metapod >= 1.0.7")
ADD_OPTIONAL_DEPENDENCY("urdfdom >= 0.2.10")
ADD_OPTIONAL_DEPENDENCY("lua5.1")
ADD_OPTIONAL_DEPENDENCY("hpp-fcl")
+IF(HPP_FCL_FOUND AND URDFDOM_FOUND)
+ ADD_REQUIRED_DEPENDENCY("assimp")
+ENDIF(HPP_FCL_FOUND AND URDFDOM_FOUND)
SET(BOOST_COMPONENTS filesystem unit_test_framework)
SEARCH_FOR_BOOST()
@@ -178,6 +181,15 @@ IF(URDFDOM_FOUND)
multibody/parser/urdf.hpp
multibody/parser/urdf.hxx
)
+
+ IF(HPP_FCL_FOUND )
+ SET(${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
+ ${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
+ multibody/parser/from-collada-to-fcl.hpp
+ multibody/parser/urdf-with-geometry.hpp
+ )
+ ENDIF(HPP_FCL_FOUND)
+
LIST(APPEND HEADERS
${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index d1c10506648acb6d8e9f99c8c7bb7ee05a6bc788..3b12df4f8d666fe5812911da91ae49026c75d772 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -67,6 +67,13 @@ IF (UNIX)
SET_TARGET_PROPERTIES( ${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} eigen3)
+ IF(URDFDOM_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} urdfdom)
+ IF(HPP_FCL_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} assimp)
+ ENDIF(HPP_FCL_FOUND)
+ ENDIF(URDFDOM_FOUND)
+
IF(HPP_FCL_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} hpp-fcl)
ADD_TARGET_CFLAGS (${PROJECT_NAME} "-DWITH_HPP_FCL")
@@ -96,6 +103,9 @@ IF(EIGENPY_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} eigenpy)
IF(URDFDOM_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} urdfdom)
+ IF(HPP_FCL_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} assimp)
+ ENDIF(HPP_FCL_FOUND)
ENDIF(URDFDOM_FOUND)
IF(HPP_FCL_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} hpp-fcl)
diff --git a/src/multibody/parser/from-collada-to-fcl.hpp b/src/multibody/parser/from-collada-to-fcl.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d46ec85c335084ddbdf6ff6aa064b304cb735e12
--- /dev/null
+++ b/src/multibody/parser/from-collada-to-fcl.hpp
@@ -0,0 +1,165 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio and is mainly inspired
+// by software hpp-model-urdf
+// 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/>.
+#include <limits>
+
+#include <boost/filesystem/fstream.hpp>
+#include <boost/foreach.hpp>
+#include <boost/format.hpp>
+
+#include <assimp/DefaultLogger.h>
+
+#include <assimp/assimp.hpp>
+#include <assimp/aiScene.h>
+#include <assimp/aiPostProcess.h>
+#include <assimp/IOStream.h>
+#include <assimp/IOSystem.h>
+
+
+#include <hpp/fcl/BV/OBBRSS.h>
+#include <hpp/fcl/BVH/BVH_model.h>
+
+typedef fcl::BVHModel< fcl::OBBRSS > PolyhedronType;
+typedef boost::shared_ptr <PolyhedronType> PolyhedronPtrType;
+
+struct TriangleAndVertices
+{
+ void clear()
+ {
+ vertices_.clear ();
+ triangles_.clear ();
+ }
+ std::vector <fcl::Vec3f> vertices_;
+ std::vector <fcl::Triangle> triangles_;
+};
+
+void buildMesh (const ::urdf::Vector3& scale, const aiScene* scene, const aiNode* node,
+ std::vector<unsigned>& subMeshIndexes, const PolyhedronPtrType& mesh,
+ TriangleAndVertices& tv)
+{
+ if (!node) return;
+
+ aiMatrix4x4 transform = node->mTransformation;
+ aiNode *pnode = node->mParent;
+ while (pnode)
+ {
+ // Don't convert to y-up orientation, which is what the root node in
+ // Assimp does
+ if (pnode->mParent != NULL)
+ {
+ transform = pnode->mTransformation * transform;
+ }
+ pnode = pnode->mParent;
+ }
+
+ for (uint32_t i = 0; i < node->mNumMeshes; i++)
+ {
+ aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
+
+ unsigned oldNbPoints = mesh->num_vertices;
+ unsigned oldNbTriangles = mesh->num_tris;
+
+ // Add the vertices
+ for (uint32_t j = 0; j < input_mesh->mNumVertices; j++)
+ {
+ aiVector3D p = input_mesh->mVertices[j];
+ p *= transform;
+ tv.vertices_.push_back (fcl::Vec3f (p.x * scale.x,
+ p.y * scale.y,
+ p.z * scale.z));
+ }
+
+ // add the indices
+ for (uint32_t j = 0; j < input_mesh->mNumFaces; j++)
+ {
+ aiFace& face = input_mesh->mFaces[j];
+ // FIXME: can add only triangular faces.
+ tv.triangles_.push_back (fcl::Triangle( oldNbPoints + face.mIndices[0],
+ oldNbPoints + face.mIndices[1],
+ oldNbPoints + face.mIndices[2]));
+ }
+
+ // Save submesh triangles indexes interval.
+ if (subMeshIndexes.size () == 0)
+ {
+ subMeshIndexes.push_back (0);
+ }
+
+ subMeshIndexes.push_back (oldNbTriangles + input_mesh->mNumFaces);
+ }
+
+ for (uint32_t i=0; i < node->mNumChildren; ++i)
+ {
+ buildMesh(scale, scene, node->mChildren[i], subMeshIndexes, mesh, tv);
+ }
+}
+
+
+void meshFromAssimpScene (const std::string& name, const ::urdf::Vector3& scale,
+ const aiScene* scene,const PolyhedronPtrType& mesh)
+ {
+ TriangleAndVertices tv;
+
+ if (!scene->HasMeshes())
+ {
+ throw std::runtime_error (std::string ("No meshes found in file ")+
+ name);
+ }
+
+ std::vector<unsigned> subMeshIndexes;
+ int res = mesh->beginModel ();
+
+ if (res != fcl::BVH_OK)
+ {
+ std::ostringstream error;
+ error << "fcl BVHReturnCode = " << res;
+ throw std::runtime_error (error.str ());
+ }
+
+ tv.clear();
+
+ buildMesh (scale, scene, scene->mRootNode, subMeshIndexes, mesh, tv);
+ mesh->addSubModel (tv.vertices_, tv.triangles_);
+
+ mesh->endModel ();
+ }
+
+void loadPolyhedronFromResource ( const std::string& resource_path, const ::urdf::Vector3& scale,
+ const PolyhedronPtrType& polyhedron)
+{
+ Assimp::Importer importer;
+ const aiScene* scene = importer.ReadFile(resource_path.c_str(), aiProcess_SortByPType| aiProcess_GenNormals|
+ aiProcess_Triangulate|aiProcess_GenUVCoords|
+ aiProcess_FlipUVs);
+ if (!scene)
+ {
+ throw std::runtime_error (std::string ("Could not load resource ") + resource_path + std::string ("\n") +
+ importer.GetErrorString ());
+ }
+
+ meshFromAssimpScene (resource_path, scale, scene, polyhedron);
+}
+
+std::string fromURDFMeshPathToAbsolutePath(std::string & urdf_mesh_path, std::string meshRootDir)
+{
+
+ std::string absolutePath = std::string(meshRootDir +
+ urdf_mesh_path.substr(10, urdf_mesh_path.size()));
+
+ return absolutePath;
+}
+
diff --git a/src/multibody/parser/urdf-with-geometry.hpp b/src/multibody/parser/urdf-with-geometry.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f03a0ec1183604c47a2bfb248d2218357f22abba
--- /dev/null
+++ b/src/multibody/parser/urdf-with-geometry.hpp
@@ -0,0 +1,345 @@
+//
+// 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_hpp__
+#define __se3_urdf_geom_hpp__
+
+#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 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;
+}
+
+namespace se3
+{
+ namespace urdf
+ {
+ Inertia convertFromUrdf( const ::urdf::Inertial& Y )
+ {
+ const ::urdf::Vector3 & p = Y.origin.position;
+ const ::urdf::Rotation & q = Y.origin.rotation;
+
+ const Eigen::Vector3d com(p.x,p.y,p.z);
+ const Eigen::Matrix3d & R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();
+
+ Eigen::Matrix3d I; I << Y.ixx,Y.ixy,Y.ixz
+ , Y.ixy,Y.iyy,Y.iyz
+ , Y.ixz,Y.iyz,Y.izz;
+ return Inertia(Y.mass,com,R*I*R.transpose());
+ }
+
+ SE3 convertFromUrdf( const ::urdf::Pose & M )
+ {
+ const ::urdf::Vector3 & p = M.position;
+ const ::urdf::Rotation & q = M.rotation;
+ return SE3( Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix(), Eigen::Vector3d(p.x,p.y,p.z));
+ }
+
+ enum AxisCartesian { AXIS_X, AXIS_Y, AXIS_Z, AXIS_UNALIGNED };
+ AxisCartesian extractCartesianAxis( const ::urdf::Vector3 & axis )
+ {
+ if( (axis.x==1.0)&&(axis.y==0.0)&&(axis.z==0.0) )
+ return AXIS_X;
+ else if( (axis.x==0.0)&&(axis.y==1.0)&&(axis.z==0.0) )
+ return AXIS_Y;
+ else if( (axis.x==0.0)&&(axis.y==0.0)&&(axis.z==1.0) )
+ return AXIS_Z;
+ else
+ return AXIS_UNALIGNED;
+ }
+
+
+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;
+ }
+
+
+ // 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 parseTreeWithGeom( ::urdf::LinkConstPtr link, Model & model,GeometryModel & model_geom, std::string meshRootDir, const SE3 & placementOffset = SE3::Identity()) throw (std::invalid_argument)
+{
+
+
+ ::urdf::JointConstPtr joint = link->parent_joint;
+ SE3 nextPlacementOffset = SE3::Identity(); // OffSet of the next link. In case we encounter a fixed joint, we need to propagate the length of its attached body to next joint.
+
+ // std::cout << " *** " << link->name << " < attached by joint ";
+ // if(joint)
+ // std::cout << "#" << link->parent_joint->name << std::endl;
+ // else std::cout << "###ROOT" << std::endl;
+
+ // std::cout << "placementOffset: " << placementOffset << std::endl;
+
+ if(joint!=NULL)
+ {
+ assert(link->getParent()!=NULL);
+
+ if (!link->inertial && joint->type != ::urdf::Joint::FIXED)
+ {
+ const std::string exception_message (link->name + " - spatial inertia information missing.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ Model::Index parent = (link->getParent()->parent_joint==NULL) ? (model.existJointName("root_joint") ? model.getJointId("root_joint") : 0) :
+ model.getJointId( link->getParent()->parent_joint->name );
+ //std::cout << joint->name << " === " << parent << std::endl;
+
+ const SE3 & jointPlacement = placementOffset*convertFromUrdf(joint->parent_to_joint_origin_transform);
+
+ const Inertia & Y = (link->inertial) ? convertFromUrdf(*link->inertial) :
+ Inertia::Identity();
+
+ bool visual = (link->visual) ? true : false;
+
+
+ //std::cout << "Parent = " << parent << std::endl;
+ //std::cout << "Placement = " << (Matrix4)jointPlacement << std::endl;
+
+ switch(joint->type)
+ {
+ case ::urdf::Joint::REVOLUTE:
+ case ::urdf::Joint::CONTINUOUS: // Revolute with no joint limits
+ {
+
+ Eigen::VectorXd maxEffort;
+ Eigen::VectorXd velocity;
+ Eigen::VectorXd lowerPosition;
+ Eigen::VectorXd upperPosition;
+
+ if (joint->limits)
+ {
+ maxEffort.resize(1); maxEffort << joint->limits->effort;
+ velocity.resize(1); velocity << joint->limits->velocity;
+ lowerPosition.resize(1); lowerPosition << joint->limits->lower;
+ upperPosition.resize(1); upperPosition << joint->limits->upper;
+ }
+
+ Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
+ AxisCartesian axis = extractCartesianAxis(joint->axis);
+ switch(axis)
+ {
+ case AXIS_X:
+ model.addBody( parent, JointModelRX(), jointPlacement, Y,
+ maxEffort, velocity, lowerPosition, upperPosition,
+ joint->name,link->name, visual );
+ break;
+ case AXIS_Y:
+ model.addBody( parent, JointModelRY(), jointPlacement, Y,
+ maxEffort, velocity, lowerPosition, upperPosition,
+ joint->name,link->name, visual );
+ break;
+ case AXIS_Z:
+ model.addBody( parent, JointModelRZ(), jointPlacement, Y,
+ maxEffort, velocity, lowerPosition, upperPosition,
+ joint->name,link->name, visual );
+ break;
+ case AXIS_UNALIGNED:
+ jointAxis= Eigen::Vector3d( joint->axis.x,joint->axis.y,joint->axis.z );
+ jointAxis.normalize();
+ model.addBody( parent, JointModelRevoluteUnaligned(jointAxis),
+ jointPlacement, Y,
+ maxEffort, velocity, lowerPosition, upperPosition,
+ joint->name,link->name, visual );
+ break;
+ default:
+ assert( false && "Fatal Error while extracting revolute joint axis");
+ break;
+ }
+ break;
+ }
+ case ::urdf::Joint::PRISMATIC:
+ {
+ Eigen::VectorXd maxEffort = Eigen::VectorXd(0.);
+ Eigen::VectorXd velocity = Eigen::VectorXd(0.);
+ Eigen::VectorXd lowerPosition = Eigen::VectorXd(0.);
+ Eigen::VectorXd upperPosition = Eigen::VectorXd(0.);
+
+ if (joint->limits)
+ {
+ maxEffort.resize(1); maxEffort << joint->limits->effort;
+ velocity.resize(1); velocity << joint->limits->velocity;
+ lowerPosition.resize(1); lowerPosition << joint->limits->lower;
+ upperPosition.resize(1); upperPosition << joint->limits->upper;
+ }
+ AxisCartesian axis = extractCartesianAxis(joint->axis);
+ switch(axis)
+ {
+ case AXIS_X:
+ model.addBody( parent, JointModelPX(), jointPlacement, Y,
+ maxEffort, velocity, lowerPosition, upperPosition,
+ joint->name,link->name, visual );
+ break;
+ case AXIS_Y:
+ model.addBody( parent, JointModelPY(), jointPlacement, Y,
+ maxEffort, velocity, lowerPosition, upperPosition,
+ joint->name,link->name, visual );
+ break;
+ case AXIS_Z:
+ model.addBody( parent, JointModelPZ(), jointPlacement, Y,
+ maxEffort, velocity, lowerPosition, upperPosition,
+ joint->name,link->name, visual );
+ break;
+ case AXIS_UNALIGNED:
+ std::cerr << "Bad axis = (" << joint->axis.x <<"," << joint->axis.y << "," << joint->axis.z << ")" << std::endl;
+ assert(false && "Only X, Y or Z axis are accepted." );
+ break;
+ default:
+ assert( false && "Fatal Error while extracting prismatic joint axis");
+ break;
+ }
+ break;
+ }
+ case ::urdf::Joint::FIXED:
+ {
+ // In case of fixed joint, if link has inertial tag:
+ // -add the inertia of the link to his parent in the model
+ // Otherwise do nothing.
+ // In all cases:
+ // -let all the children become children of parent
+ // -inform the parser of the offset to apply
+ // -add fixed body in model to display it in gepetto-viewer
+ if (link->inertial)
+ {
+ model.mergeFixedBody(parent, jointPlacement, Y); //Modify the parent inertia in the model
+ }
+
+ SE3 ptjot_se3 = convertFromUrdf(link->parent_joint->parent_to_joint_origin_transform);
+
+ //transformation of the current placement offset
+ nextPlacementOffset = placementOffset*ptjot_se3;
+
+ //add the fixed Body in the model for the viewer
+ model.addFixedBody(parent,nextPlacementOffset,link->name,visual);
+
+ BOOST_FOREACH(::urdf::LinkPtr child_link,link->child_links)
+ {
+ child_link->setParent(link->getParent() ); //skip the fixed generation
+ }
+ break;
+ }
+ default:
+ {
+ std::cerr << "The joint type " << joint->type << " is not supported." << std::endl;
+ assert(false && "Only revolute, prismatic and fixed joints are accepted." );
+ break;
+ }
+ }
+ 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)
+ {
+ parseTree(child, model, nextPlacementOffset);
+ }
+}
+
+
+ template <typename D>
+ void parseTreeWithGeom( ::urdf::LinkConstPtr link, Model & model, GeometryModel & model_geom, std::string meshRootDir, const SE3 & placementOffset , const JointModelBase<D> & root_joint )
+ {
+ const Inertia & Y = (link->inertial) ?
+ convertFromUrdf(*link->inertial)
+ : Inertia::Identity();
+ model.addBody( 0, root_joint, placementOffset, Y , "root_joint", link->name, true );
+ BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
+ {
+ parseTreeWithGeom(child, model, model_geom, meshRootDir, SE3::Identity());
+ }
+ }
+
+
+ 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);
+ parseTreeWithGeom(urdfTree->getRoot(), model, model_geom, meshRootDir, SE3::Identity(), root_joint);
+ 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);
+ parseTreeWithGeom(urdfTree->getRoot(), model, model_geom, meshRootDir, SE3::Identity());
+ return std::make_pair(model, model_geom);
+ }
+
+ } // namespace urdf
+} // namespace se3
+
+#endif // ifndef __se3_urdf_geom_hpp__
+