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
///
/// \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);
......
......@@ -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>
......
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