diff --git a/README.md b/README.md
index eb1daa3a13a14b08ac7bba4c200b382b3d16d825..206b88c31ee7242e89a9e9ec326493c2599542cc 100644
--- a/README.md
+++ b/README.md
@@ -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
@@ -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}
}
```
diff --git a/bindings/python/parsers/parsers.hpp b/bindings/python/parsers/parsers.hpp
index 9b3e53294851e08750a129ebf5c1a33b292a6ba4..3497385d855a0fc4635a9d1f18e474e4e5f0a9b6 100644
--- a/bindings/python/parsers/parsers.hpp
+++ b/bindings/python/parsers/parsers.hpp
@@ -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,
@@ -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
@@ -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
@@ -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.");
}
diff --git a/src/parsers/urdf.hpp b/src/parsers/urdf.hpp
index 16312e5cf0f1dd961feed0570fec086a440543de..9ba87da645fb51bde9b2b5895f4c3c2b2d647dc8 100644
--- a/src/parsers/urdf.hpp
+++ b/src/parsers/urdf.hpp
@@ -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
@@ -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);
/**
@@ -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()
*
@@ -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,
@@ -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
@@ -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()
*
@@ -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
diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp
index 67d08b120f93bca0c666ed8578e7f3ab3a1c3144..0c2914b733dd6d52e00636651fc6f6eab9138b07 100644
--- a/src/parsers/urdf/model.cpp
+++ b/src/parsers/urdf/model.cpp
@@ -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
@@ -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.");
@@ -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.");
@@ -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,
diff --git a/unittest/urdf.cpp b/unittest/urdf.cpp
index 1907368c13f03dddacb99689ef7b896dde31506b..7fd6e50903bc01751e994be026a8bc47fe0acfbe 100644
--- a/unittest/urdf.cpp
+++ b/unittest/urdf.cpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2015 CNRS
+// Copyright (c) 2015-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
@@ -16,41 +16,99 @@
// <http://www.gnu.org/licenses/>.
#include <iostream>
+#include <fstream>
+#include <streambuf>
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include <boost/test/unit_test.hpp>
+#include <urdf_parser/urdf_parser.h>
+
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_AUTO_TEST_CASE ( build_model )
{
- std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
-
- #ifndef NDEBUG
- std::cout << "Parse filename \"" << filename << "\"" << std::endl;
- #endif
- se3::Model model;
- se3::urdf::buildModel(filename, model);
- se3::GeometryModel geomModel;
- se3::urdf::buildGeom(model, filename, se3::COLLISION, geomModel);
- std::cout << "Robot's name:" << model.name << std::endl;
+ const std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo/urdf/romeo.urdf";
+ const std::string dir = PINOCCHIO_SOURCE_DIR"/models/romeo";
+
+ se3::Model model;
+ se3::urdf::buildModel(filename, model);
+ se3::GeometryModel geomModel;
+ se3::urdf::buildGeom(model, filename, se3::COLLISION, geomModel, dir);
+
+ BOOST_CHECK(model.nq == 31);
+}
+
+BOOST_AUTO_TEST_CASE ( build_model_from_XML )
+{
+ const std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo/urdf/romeo.urdf";
+
+ // Read file as XML
+ std::ifstream file;
+ file.open(filename.c_str());
+ std::string filestr((std::istreambuf_iterator<char>(file)),
+ std::istreambuf_iterator<char>());
+
+ se3::Model model;
+ se3::urdf::buildModelFromXML(filestr, model);
+
+ BOOST_CHECK(model.nq == 31);
+}
+
+BOOST_AUTO_TEST_CASE ( build_model_from_UDRFTree )
+{
+ const std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo/urdf/romeo.urdf";
+
+ ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
+
+ se3::Model model;
+ se3::urdf::buildModel(urdfTree, model);
+
+ BOOST_CHECK(model.nq == 31);
}
- BOOST_AUTO_TEST_CASE ( build_model_with_joint )
- {
- std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
-
-#ifndef NDEBUG
- std::cout << "Parse filename \"" << filename << "\"" << std::endl;
-#endif
- se3::Model model;
- se3::urdf::buildModel(filename, se3::JointModelFreeFlyer(), model);
- se3::GeometryModel geomModel;
- se3::urdf::buildGeom(model, filename, se3::COLLISION, geomModel);
- std::cout << "Robot's name:" << model.name << std::endl;
- }
+BOOST_AUTO_TEST_CASE ( build_model_with_joint )
+{
+ const std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo/urdf/romeo.urdf";
+ const std::string dir = PINOCCHIO_SOURCE_DIR"/models/romeo";
+
+ se3::Model model;
+ se3::urdf::buildModel(filename, se3::JointModelFreeFlyer(), model);
+ se3::GeometryModel geomModel;
+ se3::urdf::buildGeom(model, filename, se3::COLLISION, geomModel, dir);
+
+ BOOST_CHECK(model.nq == 38);
+}
+
+BOOST_AUTO_TEST_CASE ( build_model_with_joint_from_XML )
+{
+ const std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo/urdf/romeo.urdf";
+
+ // Read file as XML
+ std::ifstream file;
+ file.open(filename.c_str());
+ std::string filestr((std::istreambuf_iterator<char>(file)),
+ std::istreambuf_iterator<char>());
+
+ se3::Model model;
+ se3::urdf::buildModelFromXML(filestr, se3::JointModelFreeFlyer(), model);
+
+ BOOST_CHECK(model.nq == 38);
+}
+
+BOOST_AUTO_TEST_CASE ( build_model_with_joint_from_UDRFTree )
+{
+ const std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo/urdf/romeo.urdf";
+
+ ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
+
+ se3::Model model;
+ se3::urdf::buildModel(urdfTree, se3::JointModelFreeFlyer(), model);
+
+ BOOST_CHECK(model.nq == 38);
+}
BOOST_AUTO_TEST_SUITE_END()