Commit 2b8433b9 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #407 from jmirabel/devel

Add overload of buildModel and buildGeom
parents 95d62eba 34ebae27
...@@ -49,6 +49,7 @@ SET(${PROJECT_NAME}_MULTIBODY_SOURCES ...@@ -49,6 +49,7 @@ SET(${PROJECT_NAME}_MULTIBODY_SOURCES
SET(${PROJECT_NAME}_PARSERS_SOURCES SET(${PROJECT_NAME}_PARSERS_SOURCES
parsers/sample-models.cpp parsers/sample-models.cpp
parsers/srdf.cpp
) )
IF(URDFDOM_FOUND) IF(URDFDOM_FOUND)
......
//
// Copyright (c) 2017 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_parser_srdf_hxx__
#define __se3_parser_srdf_hxx__
#include "pinocchio/parsers/srdf.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/geometry.hpp"
#include <iostream>
// Read XML file with boost
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <fstream>
#include <sstream>
#include <boost/foreach.hpp>
namespace se3
{
namespace srdf
{
#ifdef WITH_HPP_FCL
namespace details
{
inline void removeCollisionPairs(const Model& model,
GeometryModel & geomModel,
std::istream & stream,
const bool verbose = false)
{
// Read xml stream
using boost::property_tree::ptree;
ptree pt;
read_xml(stream, pt);
// Iterate over collision pairs
BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
{
if (v.first == "disable_collisions")
{
const std::string link1 = v.second.get<std::string>("<xmlattr>.link1");
const std::string link2 = v.second.get<std::string>("<xmlattr>.link2");
// Check first if the two bodies exist in model
if (!model.existBodyName(link1) || !model.existBodyName(link2))
{
if (verbose)
std::cout << "It seems that " << link1 << " or " << link2 << " do not exist in model. Skip." << std::endl;
continue;
}
const Model::JointIndex frame_id1 = model.getBodyId(link1);
const Model::JointIndex frame_id2 = model.getBodyId(link2);
// Malformed SRDF
if (frame_id1 == frame_id2)
{
if (verbose)
std::cout << "Cannot disable collision between " << link1 << " and " << link2 << std::endl;
continue;
}
typedef std::vector<CollisionPair> CollisionPairs_t;
bool didRemove = false;
for(CollisionPairs_t::iterator _colPair = geomModel.collisionPairs.begin();
_colPair != geomModel.collisionPairs.end(); ) {
const CollisionPair& colPair (*_colPair);
bool remove =
(
(geomModel.geometryObjects[colPair.first ].parentFrame == frame_id1)
&& (geomModel.geometryObjects[colPair.second].parentFrame == frame_id2)
) || (
(geomModel.geometryObjects[colPair.second].parentFrame == frame_id1)
&& (geomModel.geometryObjects[colPair.first ].parentFrame == frame_id2)
);
if (remove) {
_colPair = geomModel.collisionPairs.erase(_colPair);
didRemove = true;
} else {
++_colPair;
}
}
if(didRemove && verbose)
std::cout << "Remove collision pair (" << link1 << "," << link2 << ")" << std::endl;
}
} // BOOST_FOREACH
}
} // namespace details
void removeCollisionPairsFromSrdf(const Model& model,
GeometryModel & geomModel,
const std::string & filename,
const bool verbose) throw (std::invalid_argument)
{
// Check extension
const std::string extension = filename.substr(filename.find_last_of('.')+1);
if (extension != "srdf")
{
const std::string exception_message (filename + " does not have the right extension.");
throw std::invalid_argument(exception_message);
}
// Open file
std::ifstream srdf_stream(filename.c_str());
if (! srdf_stream.is_open())
{
const std::string exception_message (filename + " does not seem to be a valid file.");
throw std::invalid_argument(exception_message);
}
details::removeCollisionPairs(model, geomModel, srdf_stream, verbose);
}
void removeCollisionPairsFromSrdfString(
const Model& model,
GeometryModel & geomModel,
const std::string & xmlString,
const bool verbose)
{
std::istringstream srdf_stream(xmlString);
details::removeCollisionPairs(model, geomModel, srdf_stream, verbose);
}
#endif // ifdef WITH_HPP_FCL
Eigen::VectorXd getNeutralConfigurationFromSrdf(Model & model,
const std::string & filename,
const bool verbose) throw (std::invalid_argument)
{
// Check extension
const std::string extension = filename.substr(filename.find_last_of('.')+1);
if (extension != "srdf")
{
const std::string exception_message (filename + " does not have the right extension.");
throw std::invalid_argument(exception_message);
}
// Open file
std::ifstream srdf_stream(filename.c_str());
if (! srdf_stream.is_open())
{
const std::string exception_message (filename + " does not seem to be a valid file.");
throw std::invalid_argument(exception_message);
}
// Read xml stream
using boost::property_tree::ptree;
ptree pt;
read_xml(srdf_stream, pt);
// Iterate over all tags directly children of robot
BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
{
// if we encounter a tag group_state
if (v.first == "group_state")
{
const std::string name = v.second.get<std::string>("<xmlattr>.name");
// Ensure that it is the half_sitting tag
if( name == "half_sitting")
{
// Iterate over all the joint tags
BOOST_FOREACH(const ptree::value_type & joint, v.second)
{
if (joint.first == "joint")
{
std::string joint_name = joint.second.get<std::string>("<xmlattr>.name");
double joint_config = joint.second.get<double>("<xmlattr>.value");
if (verbose)
{
std::cout << "(" << joint_name << " , " << joint_config << ")" << std::endl;
}
// Search in model the joint and its config id
Model::JointIndex joint_id = model.getJointId(joint_name);
if (joint_id != model.joints.size()) // != model.njoints
{
const JointModel & joint = model.joints[joint_id];
model.neutralConfiguration(joint.idx_q()) = joint_config; // joint with 1 dof
// model.neutralConfiguration.segment(joint.idx_q(),joint.nq()) = joint_config; // joint with more than 1 dof
}
else
{
if (verbose) std::cout << "The Joint " << joint_name << " was not found in model" << std::endl;
}
}
}
return model.neutralConfiguration;
}
}
} // BOOST_FOREACH
assert(false && "no half_sitting configuration found in the srdf file"); // Should we throw something here ?
return Eigen::VectorXd::Constant(model.nq,NAN); // warning : uninitialized vector is returned
}
}
} // namespace se3
#endif // ifndef __se3_parser_srdf_hxx__
...@@ -20,13 +20,6 @@ ...@@ -20,13 +20,6 @@
#include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/geometry.hpp" #include "pinocchio/multibody/geometry.hpp"
#include <iostream>
// Read XML file with boost
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <fstream>
#include <boost/foreach.hpp>
namespace se3 namespace se3
...@@ -45,86 +38,23 @@ namespace se3 ...@@ -45,86 +38,23 @@ namespace se3
/// \param[in] filename The complete path to the SRDF file. /// \param[in] filename The complete path to the SRDF file.
/// \param[in] verbose Verbosity mode (print removed collision pairs and undefined link inside the model). /// \param[in] verbose Verbosity mode (print removed collision pairs and undefined link inside the model).
/// ///
inline void removeCollisionPairsFromSrdf(const Model& model, void removeCollisionPairsFromSrdf(const Model& model,
GeometryModel & geomModel, GeometryModel & geomModel,
const std::string & filename, const std::string & filename,
const bool verbose = false) throw (std::invalid_argument) const bool verbose = false) throw (std::invalid_argument);
{ ///
// Check extension /// \brief Deactive all possible collision pairs mentioned in the SRDF file.
const std::string extension = filename.substr(filename.find_last_of('.')+1); ///
if (extension != "srdf") /// \param[in] model Model of the kinematic tree.
{ /// \param[in] geomModel Model of the geometries.
const std::string exception_message (filename + " does not have the right extension."); /// \param[out] data_geom Data containing the active collision pairs.
throw std::invalid_argument(exception_message); /// \param[in] xmlString the SRDF string.
} /// \param[in] verbose Verbosity mode (print removed collision pairs and undefined link inside the model).
///
// Open file void removeCollisionPairsFromSrdfString(const Model& model,
std::ifstream srdf_stream(filename.c_str()); GeometryModel & geomModel,
if (! srdf_stream.is_open()) const std::string & xmlString,
{ const bool verbose = false);
const std::string exception_message (filename + " does not seem to be a valid file.");
throw std::invalid_argument(exception_message);
}
// Read xml stream
using boost::property_tree::ptree;
ptree pt;
read_xml(srdf_stream, pt);
// Iterate over collision pairs
BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
{
if (v.first == "disable_collisions")
{
const std::string link1 = v.second.get<std::string>("<xmlattr>.link1");
const std::string link2 = v.second.get<std::string>("<xmlattr>.link2");
// Check first if the two bodies exist in model
if (!model.existBodyName(link1) || !model.existBodyName(link2))
{
if (verbose)
std::cout << "It seems that " << link1 << " or " << link2 << " do not exist in model. Skip." << std::endl;
continue;
}
const Model::JointIndex frame_id1 = model.getBodyId(link1);
const Model::JointIndex frame_id2 = model.getBodyId(link2);
// Malformed SRDF
if (frame_id1 == frame_id2)
{
if (verbose)
std::cout << "Cannot disable collision between " << link1 << " and " << link2 << std::endl;
continue;
}
typedef std::vector<CollisionPair> CollisionPairs_t;
bool didRemove = false;
for(CollisionPairs_t::iterator _colPair = geomModel.collisionPairs.begin();
_colPair != geomModel.collisionPairs.end(); ) {
const CollisionPair& colPair (*_colPair);
bool remove =
(
(geomModel.geometryObjects[colPair.first ].parentFrame == frame_id1)
&& (geomModel.geometryObjects[colPair.second].parentFrame == frame_id2)
) || (
(geomModel.geometryObjects[colPair.second].parentFrame == frame_id1)
&& (geomModel.geometryObjects[colPair.first ].parentFrame == frame_id2)
);
if (remove) {
_colPair = geomModel.collisionPairs.erase(_colPair);
didRemove = true;
} else {
++_colPair;
}
}
if(didRemove && verbose)
std::cout << "Remove collision pair (" << link1 << "," << link2 << ")" << std::endl;
}
} // BOOST_FOREACH
}
#endif // ifdef WITH_HPP_FCL #endif // ifdef WITH_HPP_FCL
...@@ -138,75 +68,9 @@ namespace se3 ...@@ -138,75 +68,9 @@ namespace se3
/// \param[in] verbose Verbosity mode. /// \param[in] verbose Verbosity mode.
/// ///
/// \return The neutral configuration as an eigen vector /// \return The neutral configuration as an eigen vector
inline Eigen::VectorXd getNeutralConfigurationFromSrdf(Model & model, Eigen::VectorXd getNeutralConfigurationFromSrdf(Model & model,
const std::string & filename, const std::string & filename,
const bool verbose = false) throw (std::invalid_argument) const bool verbose = false) throw (std::invalid_argument);
{
// Check extension
const std::string extension = filename.substr(filename.find_last_of('.')+1);
if (extension != "srdf")
{
const std::string exception_message (filename + " does not have the right extension.");
throw std::invalid_argument(exception_message);
}
// Open file
std::ifstream srdf_stream(filename.c_str());
if (! srdf_stream.is_open())
{
const std::string exception_message (filename + " does not seem to be a valid file.");
throw std::invalid_argument(exception_message);
}
// Read xml stream
using boost::property_tree::ptree;
ptree pt;
read_xml(srdf_stream, pt);
// Iterate over all tags directly children of robot
BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
{
// if we encounter a tag group_state
if (v.first == "group_state")
{
const std::string name = v.second.get<std::string>("<xmlattr>.name");
// Ensure that it is the half_sitting tag
if( name == "half_sitting")
{
// Iterate over all the joint tags
BOOST_FOREACH(const ptree::value_type & joint, v.second)
{
if (joint.first == "joint")
{
std::string joint_name = joint.second.get<std::string>("<xmlattr>.name");
double joint_config = joint.second.get<double>("<xmlattr>.value");
if (verbose)
{
std::cout << "(" << joint_name << " , " << joint_config << ")" << std::endl;
}
// Search in model the joint and its config id
Model::JointIndex joint_id = model.getJointId(joint_name);
if (joint_id != model.joints.size()) // != model.njoints
{
const JointModel & joint = model.joints[joint_id];
model.neutralConfiguration(joint.idx_q()) = joint_config; // joint with 1 dof
// model.neutralConfiguration.segment(joint.idx_q(),joint.nq()) = joint_config; // joint with more than 1 dof
}
else
{
if (verbose) std::cout << "The Joint " << joint_name << " was not found in model" << std::endl;
}
}
}
return model.neutralConfiguration;
}
}
} // BOOST_FOREACH
assert(false && "no half_sitting configuration found in the srdf file"); // Should we throw something here ?
return Eigen::VectorXd::Constant(model.nq,NAN); // warning : uninitialized vector is returned
}
} }
} // namespace se3 } // namespace se3
......
...@@ -24,8 +24,6 @@ ...@@ -24,8 +24,6 @@
#include "pinocchio/multibody/geometry.hpp" #include "pinocchio/multibody/geometry.hpp"
#include "pinocchio/parsers/urdf/types.hpp" #include "pinocchio/parsers/urdf/types.hpp"
#include <urdf_model/model.h>
namespace se3 namespace se3
{ {
namespace urdf namespace urdf
...@@ -59,6 +57,37 @@ namespace se3 ...@@ -59,6 +57,37 @@ namespace se3
Model & model, Model & model,
const bool verbose = false) throw (std::invalid_argument); const bool verbose = false) throw (std::invalid_argument);
///
/// \brief Build the model from a URDF model with a particular joint as root of the model tree inside
/// the model given as reference argument.
///
/// \param[in] urdfTree the tree build from the URDF
/// \param[in] rootJoint The joint at the root of the model tree.
/// \param[in] verbose Print parsing info.
/// \param[out] model Reference model where to put the parsed information.
/// \return Return the reference on argument model for convenience.
///
/// \note urdfTree can be build from ::urdf::parseURDF
/// or ::urdf::parseURDFFile
Model & buildModel (const ::urdf::ModelInterfaceSharedPtr & urdfTree,
const JointModelVariant & rootJoint,
Model & model,
const bool verbose = false);
///
/// \brief Build the model from a URDF model
///
/// \param[in] urdfTree the tree build from the URDF
/// \param[in] verbose Print parsing info.
/// \param[out] model Reference model where to put the parsed information.
/// \return Return the reference on argument model for convenience.
///
/// \note urdfTree can be build from ::urdf::parseURDF
/// or ::urdf::parseURDFFile
Model & buildModel (const ::urdf::ModelInterfaceSharedPtr & urdfTree,
Model & model,
const bool verbose = false);
/** /**
* @brief Build The GeometryModel from a URDF file. Search for meshes * @brief Build The GeometryModel from a URDF file. Search for meshes
...@@ -87,6 +116,33 @@ namespace se3 ...@@ -87,6 +116,33 @@ namespace se3
const std::vector<std::string> & packageDirs = std::vector<std::string> ()) const std::vector<std::string> & packageDirs = std::vector<std::string> ())
throw (std::invalid_argument); throw (std::invalid_argument);
/**
* @brief Build The GeometryModel from a URDF model. Search for meshes
* in the directories specified by the user first and then in
* the environment variable ROS_PACKAGE_PATH
*
* @param[in] model The model of the robot, built with
* urdf::buildModel
* @param[in] urdfTree The URDF model
* @param[in] packageDirs A vector containing the different directories
* where to search for models and meshes, typically
* obtained from calling se3::rosPaths()
*
* @param[in] type The type of objects that must be loaded (must be VISUAL or COLLISION)
* @param[out] geomModel Reference where to put the parsed information.
*
* @return Returns the reference on geom model for convenience.
*
* \warning If hpp-fcl has not been found during compilation, COLLISION types can not be loaded
*
*/
GeometryModel& buildGeom(const Model & model,
const ::urdf::ModelInterfaceSharedPtr & urdfTree,
const GeometryType type,
GeometryModel & geomModel,
const std::vector<std::string> & packageDirs = std::vector<std::string> ())
throw (std::invalid_argument);
} // namespace urdf } // namespace urdf
} // namespace se3 } // namespace se3
......
...@@ -343,6 +343,17 @@ namespace se3 ...@@ -343,6 +343,17 @@ namespace se3
GeometryModel & geomModel, GeometryModel & geomModel,
const std::vector<std::string> & package_dirs) const std::vector<std::string> & package_dirs)
throw(std::invalid_argument) throw(std::invalid_argument)
{
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
return buildGeom (model, urdfTree, type, geomModel, package_dirs);
}
GeometryModel& buildGeom(const Model & model,
const ::urdf::ModelInterfaceSharedPtr & urdfTree,
const GeometryType type,
GeometryModel & geomModel,
const std::vector<std::string> & package_dirs)
throw(std::invalid_argument)
{ {
std::vector<std::string> hint_directories(package_dirs); std::vector<std::string> hint_directories(package_dirs);
...@@ -355,7 +366,6 @@ namespace se3 ...@@ -355,7 +366,6 @@ namespace se3
throw std::runtime_error("You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash"); throw std::runtime_error("You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash");
} }
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
details::parseTreeForGeom(urdfTree->getRoot(), model, geomModel, hint_directories,type); details::parseTreeForGeom(urdfTree->getRoot(), model, geomModel, hint_directories,type);
return geomModel; return geomModel;
} }
......
...@@ -587,8 +587,7 @@ namespace se3 ...@@ -587,8 +587,7 @@ namespace se3
if (urdfTree) if (urdfTree)
{ {
model.name = urdfTree->getName(); return buildModel (urdfTree, root_joint, model, verbose);
ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose);
} }
else else
{ {
...@@ -604,8 +603,7 @@ namespace se3 ...@@ -604,8 +603,7 @@ namespace se3
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename); ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
if (urdfTree) if (urdfTree)
{ {
model.name = urdfTree->getName(); return buildModel (urdfTree, model, verbose);
details::parseRootTree(urdfTree->getRoot(),model,verbose);
} }
else else
{ {
...@@ -616,5 +614,26 @@ namespace se3 ...@@ -616,5 +614,26 @@ namespace se3
return model; return model;
} }
Model& buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
const JointModelVariant & root_joint,
Model & model,
const bool verbose)
{
assert(urdfTree);
model.name = urdfTree->getName();
ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose);
return model;
}
Model& buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
Model& model,
const bool verbose)
{
assert(urdfTree);
model.name = urdfTree->getName();