Verified Commit 181c4fc0 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

core/model: fix addFrame to handle existing Frame

parent edf6529e
...@@ -509,8 +509,8 @@ namespace pinocchio ...@@ -509,8 +509,8 @@ namespace pinocchio
/// ///
/// \param[in] frame The frame to add to the kinematic tree. /// \param[in] frame The frame to add to the kinematic tree.
/// ///
/// \return Returns the index of the frame if it has been successfully added, /// \return Returns the index of the frame if it has been successfully added or if it already exists in the kinematic tree.
/// -1 otherwise. /// 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); int addFrame(const Frame & frame);
......
...@@ -251,16 +251,21 @@ namespace pinocchio ...@@ -251,16 +251,21 @@ namespace pinocchio
inline int ModelTpl<Scalar,Options,JointCollectionTpl>:: inline int ModelTpl<Scalar,Options,JointCollectionTpl>::
addFrame(const Frame & frame) 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); frames.push_back(frame);
nframes++; nframes++;
return nframes - 1; return nframes - 1;
} }
else
{
return -1;
}
} }
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment