Commit 86675ad5 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

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

parent 64db3908
......@@ -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:
......
......@@ -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;
}
}
......
......@@ -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);
}
///
......
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