[C++] Model::addFrame return the index of the new frame or -1 when error.

src/multibody/model.hpp

@@ -170,10 +170,10 @@ namespace se3
     /// \param[in] frameIndex Index of the parent frame. If negative,
     ///            the parent frame is the frame of the parent joint.
     ///
-    /// \return The index of the new frame.
+    /// \return The index of the new frame or -1 in case of error.
     ///
-    FrameIndex addJointFrame (const JointIndex& jointIndex,
-                                    int         frameIndex = -1);
+    int addJointFrame (const JointIndex& jointIndex,
+                             int         frameIndex = -1);
 
     ///
     /// \brief Append a body to a given joint of the kinematic tree.
@@ -198,12 +198,12 @@ namespace se3
     /// \param[in] previousFrame Index of the parent frame. If negative,
     ///            the parent frame is the frame of the parent joint.
     ///
-    /// \return The index of the new frame.
+    /// \return The index of the new frame or -1 in case of error.
     ///
-    FrameIndex addBodyFrame (const std::string& body_name,
-                             const JointIndex& parentJoint,
-                             const SE3 & body_placement = SE3::Identity(),
-                                   int         previousFrame = -1);
+    int addBodyFrame (const std::string & body_name,
+                      const JointIndex  & parentJoint,
+                      const SE3         & body_placement = SE3::Identity(),
+                            int           previousFrame  = -1);
 
     ///
     /// \brief Return the index of a body given by its name.
@@ -357,9 +357,10 @@ namespace se3
     ///
     /// \param[in] frame The frame to add to the kinematic tree.
     ///
-    /// \return Returns true if the frame has been successfully added.
+    /// \return Returns the index of the frame if it has been successfully added,
+    ///         -1 otherwise.
     ///
-    bool addFrame(const Frame & frame);
+    int addFrame(const Frame & frame);
 
   protected:
     

src/multibody/model.hxx

@@ -87,8 +87,8 @@ namespace se3
     return idx;
   }
 
-  inline FrameIndex Model::addJointFrame (const JointIndex& jidx,
-                                                int         fidx)
+  inline int Model::addJointFrame (const JointIndex& jidx,
+                                         int         fidx)
   {
     if (fidx < 0) {
       fidx = getFrameId(names[parents[jidx]]);
@@ -96,8 +96,7 @@ namespace se3
     if (fidx >= frames.size())
       throw std::invalid_argument ("Frame not found");
     // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
-    addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
-    return frames.size() - 1;
+    return addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
   }
 
   inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
@@ -109,17 +108,16 @@ namespace se3
     nbody++;
   }
 
-  inline FrameIndex Model::addBodyFrame (const std::string& body_name,
-                                         const JointIndex& parentJoint,
-                                         const SE3 & body_placement,
-                                               int         previousFrame)
+  inline int Model::addBodyFrame (const std::string & body_name,
+                                  const JointIndex  & parentJoint,
+                                  const SE3         & body_placement,
+                                        int           previousFrame)
   {
     if (previousFrame < 0) {
       previousFrame = getFrameId(names[parentJoint]);
     }
     assert(previousFrame < frames.size() && "Frame index out of bound");
-    addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
-    return frames.size() - 1;
+    return addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
   }
   
   inline Model::JointIndex Model::getBodyId (const std::string & name) const
@@ -228,17 +226,17 @@ namespace se3
     return frames[index].placement;
   }
 
-  inline bool Model::addFrame ( const Frame & frame )
+  inline int Model::addFrame ( const Frame & frame )
   {
     if( !existFrame(frame.name) )
     {
       frames.push_back(frame);
       nFrames++;
-      return true;
+      return nFrames - 1;
     }
     else
     {
-      return false;
+      return -1;
     }
   }
   

src/parsers/urdf/model.cpp

@@ -124,12 +124,13 @@ namespace se3
         Model::JointIndex idx;
         Frame& frame = model.frames[parentFrameId];
 
-        model.addFrame(
+        int fid = model.addFrame(
             Frame (joint_name, frame.parent, parentFrameId,
               frame.placement, FIXED_JOINT)
             );
-        // TODO addFrame should return an index.
-        appendBodyToJoint(model, model.frames.size()-1, Y, SE3::Identity(), body_name);
+        if (fid < 0)
+          throw std::invalid_argument ("Fixed joint " + joint_name + " could not be added.");
+        appendBodyToJoint(model, (FrameIndex)fid, Y, SE3::Identity(), body_name);
       }
 
       ///