Commit 30c8d021 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add argument to URDF loading functions.

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