Skip to content

Commit

Permalink
Merge pull request #447 from jcarpent/topic/urdf
Browse files Browse the repository at this point in the history 
Handle XML stream inside URDF parsers
  • Loading branch information
@jcarpent
jcarpent authored Mar 21, 2018
2 parents 7b5fa8b + c0776ba commit b67dc51
Show file tree
Hide file tree
Showing 5 changed files with 268 additions and 48 deletions.
7 changes: 6 additions & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,11 @@ It is built upon Eigen for linear algebra and FCL for collision detections. **Pi

**Pinocchio** can be easily installed on various Linux and Unix distributions. Please refer to the [installation procedure](http://stack-of-tasks.github.io/pinocchio/download.html).

## Tutorials

**Pinocchio** is comming with a large bunch of tutorials aiming at introducting the basic tools for robotics control.
The content of the tutorials are described [here](http://projects.laas.fr/gepetto/index.php/Teach/Supaero2018) and the code source of these tutorials is located [here](https://github.com/stack-of-tasks/pinocchio-tutorials).

## Dependencies

The Pinocchio software depends on several packages which
Expand Down Expand Up @@ -49,7 +54,7 @@ If you want to cite **Pinocchio** in your papers, please use the following bibte
author = {Justin Carpentier and Florian Valenza and Nicolas Mansard and others},
title = {Pinocchio: fast forward and inverse dynamics for poly-articulated systems},
howpublished = {https://stack-of-tasks.github.io/pinocchio},
year = {2015--2017}
year = {2015--2018}
}
```

Expand Down
53 changes: 41 additions & 12 deletions bindings/python/parsers/parsers.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,23 @@ namespace se3
se3::urdf::buildModel(filename, model);
return model;
}


static Model buildModelFromXML(const std::string & XMLstream,
bp::object & root_joint_object
)
{
JointModelVariant root_joint = bp::extract<JointModelVariant> (root_joint_object)();
Model model;
se3::urdf::buildModelFromXML(XMLstream, root_joint, model);
return model;
}

static Model buildModelFromXML(const std::string & XMLstream)
{
Model model;
se3::urdf::buildModelFromXML(XMLstream, model);
return model;
}

static GeometryModel
buildGeomFromUrdf(const Model & model,
Expand Down Expand Up @@ -151,36 +167,49 @@ namespace se3
bp::def("buildModelFromUrdf",
static_cast <Model (*) (const std::string &, bp::object &)> (&ParsersPythonVisitor::buildModelFromUrdf),
bp::args("Filename (string)","Root Joint Model"),
"Parse the urdf file given in input and return a pinocchio model starting with the given root joint model"
"(remember to create the corresponding data structure)."
"Parse the URDF file given in input and return a pinocchio model starting with the given root joint model"
"(remember to then create the corresponding Data structure associated to the model)."
);

bp::def("buildModelFromUrdf",
static_cast <Model (*) (const std::string &)> (&ParsersPythonVisitor::buildModelFromUrdf),
bp::args("Filename (string)"),
"Parse the urdf file given in input and return a pinocchio model"
"(remember to create the corresponding data structure)."
"Parse the URDF file given in input and return a pinocchio model"
"(remember to then create the corresponding Data structure associated to the model)."
);

bp::def("buildModelFromXML",
static_cast <Model (*) (const std::string &, bp::object &)> (&ParsersPythonVisitor::buildModelFromXML),
bp::args("XML stream (string)","Root Joint Model"),
"Parse the URDF XML stream given in input and return a pinocchio model starting with the given root joint model"
"(remember to then create the corresponding Data structure associated to the model)."
);

bp::def("buildModelFromXML",
static_cast <Model (*) (const std::string &)> (&ParsersPythonVisitor::buildModelFromXML),
bp::args("XML stream (string)"),
"Parse the URDF XML stream given in input and return a pinocchio model"
"(remember to then create the corresponding Data structure associated to the model)."
);

bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const std::vector<std::string> &, const GeometryType)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","filename (string)", "package_dirs (vector of strings)"
),
"Parse the urdf file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model "
"(remember to create the corresponding data structures).");
"Parse the URDF file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model "
"(remember to then create the corresponding Data structure associated to the geometry model).");

bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const GeometryType)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","filename (string)"),
"Parse the urdf file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model "
"(remember to create the corresponding data structures).");
"Parse the URDF file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model "
"(remember to then create the corresponding Data structure associated to the geometry model).");

#ifdef WITH_HPP_FCL
bp::def("removeCollisionPairsFromSrdf",removeCollisionPairsFromSrdf,
bp::args("Model", "GeometryModel (where pairs are removed)","srdf filename (string)", "verbosity"
),
"Parse an srdf file in order to desactivate collision pairs for a specific GeometryData and GeometryModel ");
"Parse an SRDF file in order to desactivate collision pairs for a specific GeometryData and GeometryModel ");

#endif // #ifdef WITH_HPP_FCL
#endif // #ifdef WITH_URDFDOM
Expand All @@ -190,7 +219,7 @@ namespace se3
bp::args("Filename (string)",
"Free flyer (bool, false for a fixed robot)",
"Verbose option "),
"Parse the urdf file given in input and return a proper pinocchio model "
"Parse the URDF file given in input and return a proper pinocchio model "
"(remember to create the corresponding data structure).");
#endif // #ifdef WITH_LUA5

Expand All @@ -202,7 +231,7 @@ namespace se3

bp::def("loadRotorParamsFromSrdf",loadRotorParamsFromSrdf,
bp::args("Model for which we are loading the rotor parameters",
"srdf filename (string)", "verbosity"),
"SRDF filename (string)", "verbosity"),
"Load the rotor parameters of a given model from an SRDF file.\n"
"Results are stored in model.rotorInertia and model.rotorGearRatio.");
}
Expand Down
105 changes: 99 additions & 6 deletions src/parsers/urdf.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
//
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
Expand Down Expand Up @@ -87,6 +87,39 @@ namespace se3
Model & buildModel (const ::urdf::ModelInterfaceSharedPtr & urdfTree,
Model & model,
const bool verbose = false);

///
/// \brief Build the model from an XML stream with a particular joint as root of the model tree inside
/// the model given as reference argument.
///
/// \param[in] xmlStream stream containing the URDF model.
/// \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 & buildModelFromXML(const std::string & xmlStream,
const JointModelVariant & rootJoint,
Model & model,
const bool verbose = false)
throw (std::invalid_argument);

///
/// \brief Build the model from an XML stream
///
/// \param[in] xmlStream stream containing the URDF model.
/// \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 & buildModelFromXML(const std::string & xmlStream,
Model & model,
const bool verbose = false)
throw (std::invalid_argument);


/**
Expand All @@ -97,7 +130,7 @@ namespace se3
* @param[in] model The model of the robot, built with
* urdf::buildModel
* @param[in] filename The URDF complete (absolute) file path
* @param[in] packageDirs A vector containing the different directories
* @param[in] packageDirs A vector containing the different directories
* where to search for models and meshes, typically
* obtained from calling se3::rosPaths()
*
Expand All @@ -106,7 +139,7 @@ namespace se3
*
* @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
* \warning If hpp-fcl has not been found during compilation, COLLISION objects can not be loaded
*
*/
GeometryModel& buildGeom(const Model & model,
Expand All @@ -115,6 +148,36 @@ namespace se3
GeometryModel & geomModel,
const std::vector<std::string> & packageDirs = std::vector<std::string> ())
throw (std::invalid_argument);

/**
* @brief Build The GeometryModel from a URDF file. 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] filename The URDF complete (absolute) file path
* @param[in] packageDir A string containing the path to the directories of the 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 objects can not be loaded
*
*/
inline GeometryModel& buildGeom(const Model & model,
const std::string & filename,
const GeometryType type,
GeometryModel & geomModel,
const std::string & packageDir)
throw (std::invalid_argument)
{
const std::vector<std::string> dirs(1,packageDir);
return buildGeom(model,filename,type,geomModel,dirs);
}

/**
* @brief Build The GeometryModel from a URDF model. Search for meshes
Expand All @@ -124,7 +187,7 @@ namespace se3
* @param[in] model The model of the robot, built with
* urdf::buildModel
* @param[in] xmlStream Stream containing the URDF model
* @param[in] packageDirs A vector containing the different directories
* @param[in] packageDirs A vector containing the different directories
* where to search for models and meshes, typically
* obtained from calling se3::rosPaths()
*
Expand All @@ -133,15 +196,45 @@ namespace se3
*
* @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
* \warning If hpp-fcl has not been found during compilation, COLLISION objects cannot be loaded
*
*/
GeometryModel& buildGeom(const Model & model,
const std::istream& xmlStream,
const std::istream & xmlStream,
const GeometryType type,
GeometryModel & geomModel,
const std::vector<std::string> & packageDirs = std::vector<std::string> ())
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] xmlStream Stream containing the URDF model
* @param[in] packageDir A string containing the path to the directories of the 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 objects cannot be loaded
*
*/
inline GeometryModel & buildGeom(const Model & model,
const std::istream & xmlStream,
const GeometryType type,
GeometryModel & geomModel,
const std::string & packageDir)
throw (std::invalid_argument)
{
const std::vector<std::string> dirs(1,packageDir);
return buildGeom(model,xmlStream,type,geomModel,dirs);
}


} // namespace urdf
Expand Down
45 changes: 40 additions & 5 deletions src/parsers/urdf/model.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
//
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
Expand Down Expand Up @@ -586,9 +586,9 @@ namespace se3
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);

if (urdfTree)
{
{
return buildModel (urdfTree, root_joint, model, verbose);
}
}
else
{
const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
Expand All @@ -602,9 +602,9 @@ namespace se3
{
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
if (urdfTree)
{
{
return buildModel (urdfTree, model, verbose);
}
}
else
{
const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
Expand All @@ -613,6 +613,41 @@ namespace se3

return model;
}

Model & buildModelFromXML(const std::string & xmlStream,
const JointModelVariant & rootJoint,
Model & model,
const bool verbose) throw (std::invalid_argument)
{
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlStream);

if (urdfTree)
return buildModel(urdfTree, rootJoint, model, verbose);
else
{
const std::string exception_message ("The XML stream does not contain a valid URDF model.");
throw std::invalid_argument(exception_message);
}

return model;
}

Model & buildModelFromXML(const std::string & xmlStream,
Model & model,
const bool verbose) throw (std::invalid_argument)
{
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlStream);

if (urdfTree)
return buildModel(urdfTree, model, verbose);
else
{
const std::string exception_message ("The XML stream does not contain a valid URDF model.");
throw std::invalid_argument(exception_message);
}

return model;
}

Model& buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
const JointModelVariant & root_joint,
Expand Down
Loading

0 comments on commit b67dc51

Please sign in to comment.