Commit 643af185 authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++] Added parser that handle geometry

parent 7ce9721f
......@@ -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}
)
......
......@@ -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)
......
//
// 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;
}
//
// 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__
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment