Skip to content
GitLab
Commit 23631930 authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++][Geom parser] Removed boolean root_joint_added while parsing. Geom parser... 

[C++][Geom parser] Removed boolean root_joint_added while parsing. Geom parser modified consequently to look for parent of link in pinocchio model  now instead of urdf::tree parent joint.
parent 6f90a7f5
Hide whitespace changes
Inline Side-by-side
benchmark/timings-geometry.cpp
View file @ 23631930
......@@ -74,7 +74,7 @@ int main()
package_dirs.push_back(romeo_meshDir);
se3::Model model = se3::urdf::buildModel(romeo_filename, se3::JointModelFreeFlyer());
se3::GeometryModel geom_model = se3::urdf::buildGeom(romeo_model, romeo_filename, package_dirs, true);
se3::GeometryModel geom_model = se3::urdf::buildGeom(model, romeo_filename, package_dirs);
Data data(model);
GeometryData geom_data(data, geom_model);
......
src/multibody/parser/urdf-with-geometry.hpp
View file @ 23631930
......@@ -59,8 +59,7 @@ namespace se3
inline void parseTreeForGeom( ::urdf::LinkConstPtr link,
const Model & model,
GeometryModel & model_geom,
const std::vector<std::string> & package_dirs,
const bool rootJointAdded) throw (std::invalid_argument);
const std::vector<std::string> & package_dirs) throw (std::invalid_argument);
......@@ -82,8 +81,7 @@ namespace se3
*/
inline GeometryModel buildGeom(const Model & model,
const std::string & filename,
std::vector<std::string> & package_dirs,
const bool root_joint = false );
std::vector<std::string> & package_dirs);
/**
* @brief Build The GeometryModel from a URDF file. Search for meshes
......@@ -99,8 +97,7 @@ namespace se3
* given
*/
inline GeometryModel buildGeom(const Model & model,
const std::string & filename,
const bool root_joint = false );
const std::string & filename);
} // namespace urdf
......
src/multibody/parser/urdf-with-geometry.hxx
View file @ 23631930
......@@ -100,43 +100,32 @@ namespace se3
inline void parseTreeForGeom(::urdf::LinkConstPtr link,
const Model & model,
GeometryModel & geom_model,
const std::vector<std::string> & package_dirs,
const bool rootJointAdded) throw (std::invalid_argument)
const std::vector<std::string> & package_dirs) throw (std::invalid_argument)
{
// start with first link that is not empty
if(link->collision)
{
::urdf::JointConstPtr joint = link->parent_joint;
if (joint == NULL && rootJointAdded )
{
fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, package_dirs);
SE3 geomPlacement = convertFromUrdf(link->collision->origin);
std::string collision_object_name = link->name ;
geom_model.addGeomObject(model.getJointId("root_joint"), collision_object, geomPlacement, collision_object_name);
}
else if(joint!=NULL)
{
assert(link->getParent()!=NULL);
assert(link->getParent()!=NULL);
fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, package_dirs);
SE3 geomPlacement = convertFromUrdf(link->collision->origin);
std::string collision_object_name = link->name ;
geom_model.addGeomObject(model.getJointId(joint->name), collision_object, geomPlacement, collision_object_name);
}
else if (link->getParent() != NULL)
fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, package_dirs);
SE3 geomPlacement = convertFromUrdf(link->collision->origin);
std::string collision_object_name = link->name ;
geom_model.addGeomObject(model.parents[model.getBodyId(collision_object_name)], collision_object, geomPlacement, collision_object_name);
if (link->getParent() == NULL)
{
const std::string exception_message (link->name + " - joint information missing.");
throw std::invalid_argument(exception_message);
}
} // if(link->inertial)
} // if(link->collision)
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
parseTreeForGeom(child, model, geom_model, package_dirs, rootJointAdded);
parseTreeForGeom(child, model, geom_model, package_dirs);
}
}
......@@ -144,8 +133,7 @@ namespace se3
inline GeometryModel buildGeom(const Model & model,
const std::string & filename,
std::vector<std::string> & package_dirs,
const bool root_joint)
std::vector<std::string> & package_dirs)
{
GeometryModel model_geom(model);
......@@ -159,13 +147,12 @@ namespace se3
assert(!package_dirs.empty() && "You did not specify any package directory. Geometric parsing will crash" );
}
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
parseTreeForGeom(urdfTree->getRoot(), model, model_geom, package_dirs, root_joint);
parseTreeForGeom(urdfTree->getRoot(), model, model_geom, package_dirs);
return model_geom;
}
inline GeometryModel buildGeom(const Model & model,
const std::string & filename,
const bool root_joint)
const std::string & filename)
{
GeometryModel model_geom(model);
......@@ -181,7 +168,7 @@ namespace se3
}
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
parseTreeForGeom(urdfTree->getRoot(), model, model_geom, package_dirs, root_joint);
parseTreeForGeom(urdfTree->getRoot(), model, model_geom, package_dirs);
return model_geom;
}
......
src/python/parsers.hpp
View file @ 23631930
......@@ -88,11 +88,10 @@ namespace se3
static GeometryModelHandler
buildGeomFromUrdf(const ModelHandler & model,
const std::string & filename,
const bool root_added
const std::string & filename
)
{
GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename, root_added));
GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename));
return GeometryModelHandler(geometry_model, true);
}
......@@ -100,11 +99,10 @@ namespace se3
static GeometryModelHandler
buildGeomFromUrdf(const ModelHandler & model,
const std::string & filename,
std::vector<std::string> & package_dirs,
const bool root_added
std::vector<std::string> & package_dirs
)
{
GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename, package_dirs, root_added));
GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename, package_dirs));
return GeometryModelHandler(geometry_model, true);
}
......@@ -152,15 +150,15 @@ namespace se3
bp::to_python_converter<std::pair<ModelHandler, GeometryModelHandler>, PairToTupleConverter<ModelHandler, GeometryModelHandler> >();
bp::def("buildGeomFromUrdf",
static_cast <GeometryModelHandler (*) (const ModelHandler &, const std::string &, std::vector<std::string> &, const bool)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","filename (string)", "package_dirs (vector of strings)",
"bool stating if we added a custom root joint to the Model"),
static_cast <GeometryModelHandler (*) (const ModelHandler &, const std::string &, std::vector<std::string> &)> (&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).");
bp::def("buildGeomFromUrdf",
static_cast <GeometryModelHandler (*) (const ModelHandler &, const std::string &, const bool)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","filename (string)", "bool stating if we added a custom root joint to the Model"),
static_cast <GeometryModelHandler (*) (const ModelHandler &, const std::string &)> (&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).");
......
unittest/geom.cpp
View file @ 23631930
......@@ -210,7 +210,7 @@ BOOST_AUTO_TEST_CASE ( loading_model )
package_dirs.push_back(meshDir);
Model model = se3::urdf::buildModel(filename, se3::JointModelFreeFlyer());
GeometryModel geometry_model = se3::urdf::buildGeom(model, filename, package_dirs, true);
GeometryModel geometry_model = se3::urdf::buildGeom(model, filename, package_dirs);
Data data(model);
GeometryData geometry_data(data, geometry_model);
......@@ -251,7 +251,7 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
package_dirs.push_back(meshDir);
se3::Model model = se3::urdf::buildModel(filename, se3::JointModelFreeFlyer());
se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs, true);
se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs);
std::cout << model << std::endl;
......@@ -361,7 +361,7 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
package_dirs.push_back(meshDir);
se3::Model model = se3::urdf::buildModel(filename, se3::JointModelFreeFlyer());
se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs, true);
se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs);
std::cout << model << std::endl;
......
Supports Markdown
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