[C++] Frame in Model are identified by their name + their type.

src/multibody/frame.hpp

@@ -33,11 +33,11 @@ namespace se3
   ///
   enum FrameType
   {
-    OP_FRAME, // operational frame type
-    JOINT, // joint frame type
-    FIXED_JOINT, // fixed joint frame type
-    BODY, // body frame type
-    SENSOR // sensor frame type
+    OP_FRAME     = 0x1 << 0, // operational frame type
+    JOINT        = 0x1 << 1, // joint frame type
+    FIXED_JOINT  = 0x1 << 2, // fixed joint frame type
+    BODY         = 0x1 << 3, // body frame type
+    SENSOR       = 0x1 << 4 // sensor frame type
   };
   
   ///

src/multibody/model.hpp

@@ -275,19 +275,21 @@ namespace se3
     /// (for example to get the id of a parent frame).
     /// 
     /// \param[in] name Name of the frame.
+    /// \param[in] type Type of the frame.
     ///
     /// \return Index of the frame.
     ///
-    FrameIndex getFrameId (const std::string & name) const;
+    FrameIndex getFrameId (const std::string & name, const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
     
     ///
     /// \brief Checks if a frame given by its name exists.
     ///
     /// \param[in] name Name of the frame.
+    /// \param[in] type Type of the frame.
     ///
     /// \return Returns true if the frame exists.
     ///
-    bool existFrame (const std::string & name) const;
+    bool existFrame (const std::string & name, const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
     
     ///
     /// \brief Get the name of a frame given by its index.

src/multibody/model.hxx

@@ -28,6 +28,18 @@
 
 namespace se3
 {
+  namespace details
+  {
+    struct FilterFrame {
+      const std::string& name;
+      const FrameType & typeMask;
+      FilterFrame (const std::string& name, const FrameType& typeMask)
+        : name (name), typeMask (typeMask) {}
+      bool operator() (const Frame& frame) const
+      { return (typeMask & frame.type) && (name == frame.name); }
+    };
+  }
+
   inline std::ostream& operator<< (std::ostream & os, const Model & model)
   {
     os << "Nb joints = " << model.njoint << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
@@ -91,7 +103,7 @@ namespace se3
                                          int         fidx)
   {
     if (fidx < 0) {
-      fidx = getFrameId(names[parents[jidx]]);
+      fidx = getFrameId(names[parents[jidx]], JOINT);
     }
     if (fidx >= frames.size())
       throw std::invalid_argument ("Frame not found");
@@ -114,7 +126,7 @@ namespace se3
                                         int           previousFrame)
   {
     if (previousFrame < 0) {
-      previousFrame = getFrameId(names[parentJoint]);
+      previousFrame = getFrameId(names[parentJoint], JOINT);
     }
     assert(previousFrame < frames.size() && "Frame index out of bound");
     return addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
@@ -122,12 +134,12 @@ namespace se3
   
   inline Model::JointIndex Model::getBodyId (const std::string & name) const
   {
-    return getFrameId(name);
+    return getFrameId(name, BODY);
   }
   
   inline bool Model::existBodyName (const std::string & name) const
   {
-    return existFrame(name);
+    return existFrame(name, BODY);
   }
 
   inline const std::string& Model::getBodyName (const Model::JointIndex index) const
@@ -156,18 +168,22 @@ namespace se3
     return names[index];
   }
 
-  inline Model::FrameIndex Model::getFrameId ( const std::string & name ) const
+  inline Model::FrameIndex Model::getFrameId ( const std::string & name, const FrameType & type ) const
   {
     std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                         , frames.end()
-                                                        , boost::bind(&Frame::name, _1) == name
+                                                        , details::FilterFrame (name, type)
                                                         );
+    assert (it != frames.end() && "Frame not found");
+    assert ((std::find_if( boost::next(it), frames.end(), details::FilterFrame (name, type)) == frames.end())
+        && "Several frames match the filter");
     return Model::FrameIndex(it - frames.begin());
   }
 
-  inline bool Model::existFrame ( const std::string & name ) const
+  inline bool Model::existFrame ( const std::string & name, const FrameType & type) const
   {
-    return std::find_if( frames.begin(), frames.end(), boost::bind(&Frame::name, _1) == name) != frames.end();
+    return std::find_if( frames.begin(), frames.end(),
+        details::FilterFrame (name, type)) != frames.end();
   }
 
   inline const std::string & Model::getFrameName ( const FrameIndex index ) const
@@ -192,7 +208,7 @@ namespace se3
 
   inline int Model::addFrame ( const Frame & frame )
   {
-    if( !existFrame(frame.name) )
+    if( !existFrame(frame.name, frame.type) )
     {
       frames.push_back(frame);
       nFrames++;

src/parsers/urdf/geometry.cpp

@@ -192,9 +192,9 @@ namespace se3
 
           std::vector< boost::shared_ptr< T > > geometries_array = getLinkGeometryArray<T>(link);
 
-          if (!model.existBodyName(link_name))
+          if (!model.existFrame(link_name, BODY))
             throw std::invalid_argument("No link " + link_name + " in model");
-          FrameIndex frame_id = model.getFrameId(link_name);
+          FrameIndex frame_id = model.getFrameId(link_name, BODY);
           SE3 body_placement = model.frames[frame_id].placement;
           assert(model.frames[frame_id].type == BODY);
 

src/parsers/urdf/model.cpp

@@ -33,19 +33,21 @@ namespace se3
   {
     namespace details
     {
+      const FrameType JOINT_OR_FIXED_JOINT = (FrameType) (JOINT | FIXED_JOINT);
+
       FrameIndex getParentJointFrame(::urdf::LinkConstPtr link, Model & model)
       {
         assert(link!=NULL && link->getParent()!=NULL);
 
         FrameIndex id;
         if (link->getParent()->parent_joint==NULL) {
-          if (model.existFrame("root_joint"))
-            id = model.getFrameId ("root_joint");
+          if (model.existFrame("root_joint",  JOINT_OR_FIXED_JOINT))
+            id = model.getFrameId ("root_joint", JOINT_OR_FIXED_JOINT);
           else
             id = 0;
         } else {
-          if (model.existFrame(link->getParent()->parent_joint->name))
-            id = model.getFrameId (link->getParent()->parent_joint->name);
+          if (model.existFrame(link->getParent()->parent_joint->name, JOINT_OR_FIXED_JOINT))
+            id = model.getFrameId (link->getParent()->parent_joint->name, JOINT_OR_FIXED_JOINT);
           else
             throw std::invalid_argument ("Model does not have any joints named "
                 + link->getParent()->parent_joint->name);
@@ -69,7 +71,7 @@ namespace se3
             && Y->mass > Eigen::NumTraits<double>::epsilon()) {
           model.appendBodyToJoint(frame.parent, convertFromUrdf(*Y), p);
         }
-        model.addBodyFrame(body_name, frame.parent, p, fid);
+        model.addBodyFrame(body_name, frame.parent, p, (int)fid);
         // Reference to model.frames[fid] can has changed because the vector
         // may have been reallocated.
         if (model.frames[fid].parent > 0) {