Unverified Commit 0ec0a56c authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #143 from jmirabel/devel

Add argument to URDF loading functions.
parents 5fa3815b 30c8d021
Pipeline #13572 failed with stage
in 5 minutes and 15 seconds
...@@ -22,6 +22,8 @@ ...@@ -22,6 +22,8 @@
#ifndef HPP_PINOCCHIO_URDF_UTIL #ifndef HPP_PINOCCHIO_URDF_UTIL
# define HPP_PINOCCHIO_URDF_UTIL # define HPP_PINOCCHIO_URDF_UTIL
#include <pinocchio/spatial/se3.hpp>
#include <hpp/pinocchio/fwd.hh> #include <hpp/pinocchio/fwd.hh>
#include <hpp/pinocchio/deprecated.hh> #include <hpp/pinocchio/deprecated.hh>
...@@ -44,7 +46,7 @@ namespace hpp ...@@ -44,7 +46,7 @@ namespace hpp
/// \param modelName robot model name /// \param modelName robot model name
/// \param urdfSuffix suffix for urdf file /// \param urdfSuffix suffix for urdf file
/// \param srdfSuffix suffix for srdf file /// \param srdfSuffix suffix for srdf file
/// \param bMr position of the root joint of the new model in the base frame.
/// \note This function reads the following files: /// \note This function reads the following files:
/// \li \c package://${package}/urdf/${modelName}${urdfSuffix}.urdf /// \li \c package://${package}/urdf/${modelName}${urdfSuffix}.urdf
/// \li \c package://${package}/srdf/${modelName}${srdfSuffix}.srdf /// \li \c package://${package}/srdf/${modelName}${srdfSuffix}.srdf
...@@ -55,7 +57,8 @@ namespace hpp ...@@ -55,7 +57,8 @@ namespace hpp
const std::string& package, const std::string& package,
const std::string& modelName, const std::string& modelName,
const std::string& urdfSuffix, const std::string& urdfSuffix,
const std::string& srdfSuffix); const std::string& srdfSuffix,
const SE3& bMr = SE3::Identity());
void loadRobotModel (const DevicePtr_t& robot, void loadRobotModel (const DevicePtr_t& robot,
const std::string& rootJointType, const std::string& rootJointType,
const std::string& package, const std::string& package,
...@@ -88,7 +91,8 @@ namespace hpp ...@@ -88,7 +91,8 @@ namespace hpp
const std::string& prefix, const std::string& prefix,
const std::string& rootJointType, const std::string& rootJointType,
const std::string& package, const std::string& package,
const std::string& filename); const std::string& filename,
const SE3& bMr = SE3::Identity());
void loadUrdfModel (const DevicePtr_t& robot, void loadUrdfModel (const DevicePtr_t& robot,
const std::string& rootJointType, const std::string& rootJointType,
const std::string& package, const std::string& package,
...@@ -103,7 +107,8 @@ namespace hpp ...@@ -103,7 +107,8 @@ namespace hpp
const std::string& prefix, const std::string& prefix,
const std::string& rootType, const std::string& rootType,
const std::string& urdfPath, const std::string& urdfPath,
const std::string& srdfPath); const std::string& srdfPath,
const SE3& bMr = SE3::Identity());
/// Read URDF and, optionnally SRDF, as XML string. /// Read URDF and, optionnally SRDF, as XML string.
/// \param srdfString if empty, do not try to parse SRDF. /// \param srdfString if empty, do not try to parse SRDF.
...@@ -112,7 +117,8 @@ namespace hpp ...@@ -112,7 +117,8 @@ namespace hpp
const std::string& prefix, const std::string& prefix,
const std::string& rootType, const std::string& rootType,
const std::string& urdfString, const std::string& urdfString,
const std::string& srdfString); const std::string& srdfString,
const SE3& bMr = SE3::Identity());
/// Read SRDF file /// Read SRDF file
void loadSRDFModel (const DevicePtr_t& robot, void loadSRDFModel (const DevicePtr_t& robot,
......
...@@ -49,7 +49,7 @@ namespace hpp { ...@@ -49,7 +49,7 @@ namespace hpp {
// Iterate over collision pairs // Iterate over collision pairs
auto last = geom_model.collisionPairs.end(); auto last = geom_model.collisionPairs.end();
BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot")) for (const ptree::value_type & v : pt.get_child("robot"))
{ {
if (v.first == "disable_collisions") if (v.first == "disable_collisions")
{ {
...@@ -87,7 +87,7 @@ namespace hpp { ...@@ -87,7 +87,7 @@ namespace hpp {
} }
last = nlast; last = nlast;
} }
} // BOOST_FOREACH }
if(last != geom_model.collisionPairs.end()) { if(last != geom_model.collisionPairs.end()) {
hppDout(info, "Removing " << std::distance(last, geom_model.collisionPairs.end()) hppDout(info, "Removing " << std::distance(last, geom_model.collisionPairs.end())
<< " collision pairs."); << " collision pairs.");
...@@ -221,9 +221,8 @@ namespace hpp { ...@@ -221,9 +221,8 @@ namespace hpp {
::pinocchio::Frame& f = model.frames[i]; ::pinocchio::Frame& f = model.frames[i];
f.name = prefix + f.name; f.name = prefix + f.name;
} }
BOOST_FOREACH(::pinocchio::GeometryObject& go, geomModel.geometryObjects) { for (::pinocchio::GeometryObject& go : geomModel.geometryObjects)
go.name = prefix + go.name; go.name = prefix + go.name;
}
} }
void setRootJointBounds(Model& model, void setRootJointBounds(Model& model,
...@@ -309,6 +308,7 @@ namespace hpp { ...@@ -309,6 +308,7 @@ namespace hpp {
template <bool srdfAsXmlString> template <bool srdfAsXmlString>
void _loadModel (const DevicePtr_t& robot, void _loadModel (const DevicePtr_t& robot,
const FrameIndex& baseFrame, const FrameIndex& baseFrame,
const SE3& bMr,
std::string prefix, std::string prefix,
const std::string& rootType, const std::string& rootType,
const ::urdf::ModelInterfaceSharedPtr urdfTree, const ::urdf::ModelInterfaceSharedPtr urdfTree,
...@@ -362,7 +362,7 @@ namespace hpp { ...@@ -362,7 +362,7 @@ namespace hpp {
GeomModelPtr_t gm (new GeomModel); GeomModelPtr_t gm (new GeomModel);
::pinocchio::appendModel(robot->model(), *model, ::pinocchio::appendModel(robot->model(), *model,
robot->geomModel(), geomModel, robot->geomModel(), geomModel,
baseFrame, Transform3f::Identity(), baseFrame, bMr,
*m, *gm); *m, *gm);
robot->setModel (m); robot->setModel (m);
robot->setGeomModel (gm); robot->setGeomModel (gm);
...@@ -403,13 +403,15 @@ namespace hpp { ...@@ -403,13 +403,15 @@ namespace hpp {
const std::string& package, const std::string& package,
const std::string& modelName, const std::string& modelName,
const std::string& urdfSuffix, const std::string& urdfSuffix,
const std::string& srdfSuffix) const std::string& srdfSuffix,
const SE3& bMr)
{ {
loadModel (robot, baseFrame, loadModel (robot, baseFrame,
(prefix.empty() ? "" : prefix + "/"), (prefix.empty() ? "" : prefix + "/"),
rootJointType, rootJointType,
makeModelPath(package, "urdf", modelName, urdfSuffix), makeModelPath(package, "urdf", modelName, urdfSuffix),
makeModelPath(package, "srdf", modelName, srdfSuffix)); makeModelPath(package, "srdf", modelName, srdfSuffix),
bMr);
} }
void setupHumanoidRobot (const HumanoidRobotPtr_t& robot, void setupHumanoidRobot (const HumanoidRobotPtr_t& robot,
...@@ -426,12 +428,13 @@ namespace hpp { ...@@ -426,12 +428,13 @@ namespace hpp {
const std::string& prefix, const std::string& prefix,
const std::string& rootJointType, const std::string& rootJointType,
const std::string& package, const std::string& package,
const std::string& filename) const std::string& filename,
const SE3& bMr)
{ {
loadModel (robot, baseFrame, loadModel (robot, baseFrame,
(prefix.empty() ? "" : prefix + "/"), (prefix.empty() ? "" : prefix + "/"),
rootJointType, rootJointType,
makeModelPath(package, "urdf", filename), ""); makeModelPath(package, "urdf", filename), "", bMr);
} }
void loadUrdfModel (const DevicePtr_t& robot, void loadUrdfModel (const DevicePtr_t& robot,
...@@ -448,7 +451,8 @@ namespace hpp { ...@@ -448,7 +451,8 @@ namespace hpp {
const std::string& prefix, const std::string& prefix,
const std::string& rootType, const std::string& rootType,
const std::string& urdfPath, const std::string& urdfPath,
const std::string& srdfPath) const std::string& srdfPath,
const SE3& bMr)
{ {
std::vector<std::string> baseDirs = ::pinocchio::rosPaths(); std::vector<std::string> baseDirs = ::pinocchio::rosPaths();
...@@ -469,7 +473,7 @@ namespace hpp { ...@@ -469,7 +473,7 @@ namespace hpp {
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(urdfFileName); ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(urdfFileName);
std::ifstream urdfStream (urdfFileName.c_str()); std::ifstream urdfStream (urdfFileName.c_str());
_loadModel <false> (robot, baseFrame, prefix, rootType, _loadModel <false> (robot, baseFrame, bMr, prefix, rootType,
urdfTree, urdfStream, srdfFileName); urdfTree, urdfStream, srdfFileName);
} }
...@@ -478,11 +482,12 @@ namespace hpp { ...@@ -478,11 +482,12 @@ namespace hpp {
const std::string& prefix, const std::string& prefix,
const std::string& rootType, const std::string& rootType,
const std::string& urdfString, const std::string& urdfString,
const std::string& srdfString) const std::string& srdfString,
const SE3& bMr)
{ {
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(urdfString); ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(urdfString);
std::istringstream urdfStream (urdfString); std::istringstream urdfStream (urdfString);
_loadModel <true> (robot, baseFrame, prefix, rootType, _loadModel <true> (robot, baseFrame, bMr, prefix, rootType,
urdfTree, urdfStream, srdfString); urdfTree, urdfStream, srdfString);
} }
......
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