Merge pull request #388 from olivier-stasse/devel

Add name to model and set it when loading a urdf file.

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)

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.");

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()