core/model: fix addFrame to handle existing Frame

src/multibody/model.hpp

@@ -509,8 +509,8 @@ namespace pinocchio
     ///
     /// \param[in] frame The frame to add to the kinematic tree.
     ///
-    /// \return Returns the index of the frame if it has been successfully added,
-    ///         -1 otherwise.
+    /// \return Returns the index of the frame if it has been successfully added or if it already exists in the kinematic tree.
+    ///         The function returns -1 when the frame.type did not match with the existing frame in the kinematic having the same name.
     ///
     int addFrame(const Frame & frame);
 

src/multibody/model.hxx

@@ -251,16 +251,21 @@ namespace pinocchio
   inline int ModelTpl<Scalar,Options,JointCollectionTpl>::
   addFrame(const Frame & frame)
   {
-    if(!existFrame(frame.name, frame.type))
+    // Check if the frame.name exists
+    if(existFrame(frame.name))
+    {
+      FrameIndex frame_id = getFrameId(frame.name);
+      if(frames[frame_id].type == frame.type)
+        return frame_id;
+      else
+        return -1;
+    }
+    else // does not exist, so add the Frame to the kinematic tree
     {
       frames.push_back(frame);
       nframes++;
       return nframes - 1;
     }
-    else
-    {
-      return -1;
-    }
   }
   
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>