diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp index 1e6c6b64acb46871901deeeb7f18cfad3d855484..94b77e8e96ebbd978cff21f6c57eda613939d63b 100644 --- a/src/multibody/model.hpp +++ b/src/multibody/model.hpp @@ -102,6 +102,9 @@ namespace se3 /// \brief Default 3D gravity vector (=(0,0,-9.81)). static const Eigen::Vector3d gravity981; + /// \brief Model name; + std::string name; + /// \brief Default constructor. Builds an empty model with no joints. Model() : nq(0) diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp index a8822fac116708511353cec8f3a0ef409c3ba936..6f0d65b4fb8f0ff9ff6bab156791590533b650d1 100644 --- a/src/parsers/urdf/model.cpp +++ b/src/parsers/urdf/model.cpp @@ -501,7 +501,7 @@ namespace se3 { addFixedJointAndBody(model, 0, SE3::Identity(), "root_joint", root_link->inertial, root_link->name); - + BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links) { parseTree(child, model, verbose); @@ -572,8 +572,12 @@ namespace se3 throw (std::invalid_argument) { ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename); + if (urdfTree) - ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose); + { + model.name = urdfTree->getName(); + ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose); + } else { const std::string exception_message ("The file " + filename + " does not contain a valid URDF model."); @@ -587,7 +591,10 @@ namespace se3 { ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename); if (urdfTree) - details::parseRootTree(urdfTree->getRoot(),model,verbose); + { + model.name = urdfTree->getName(); + details::parseRootTree(urdfTree->getRoot(),model,verbose); + } else { const std::string exception_message ("The file " + filename + " does not contain a valid URDF model."); diff --git a/unittest/urdf.cpp b/unittest/urdf.cpp index c7504a08bd1c9b10056dd2cec40f65b1e608ceae..882d8a992e7c6fa0dfaf9c3f38a8ce0d76077838 100644 --- a/unittest/urdf.cpp +++ b/unittest/urdf.cpp @@ -34,6 +34,7 @@ BOOST_AUTO_TEST_CASE ( buildModel ) #endif se3::Model model; se3::urdf::buildModel(filename, model); + std::cout << "Robot's name:" << model.name << std::endl; } BOOST_AUTO_TEST_SUITE_END()