From 86675ad5bf14036975e0d946b1d768c2a6569694 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 7 Sep 2016 13:16:20 +0200
Subject: [PATCH] [C++] Model::addFrame return the index of the new frame or -1
when error.
---
src/multibody/model.hpp | 21 +++++++++++----------
src/multibody/model.hxx | 24 +++++++++++-------------
src/parsers/urdf/model.cpp | 7 ++++---
3 files changed, 26 insertions(+), 26 deletions(-)
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 28f241c5a..ab2c71da1 100644
--- a/src/multibody/model.hpp
+++ b/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:
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index 756a28239..3dada82b4 100644
--- a/src/multibody/model.hxx
+++ b/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;
}
}
diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp
index bd149bd26..c8116cc62 100644
--- a/src/parsers/urdf/model.cpp
+++ b/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);
}
///
--
GitLab