diff --git a/src/multibody/frame.hpp b/src/multibody/frame.hpp
index b2fec585ba5174620f731d930dda2f79e24d5d51..68fd090453f40c15e8a6be7ede6dd6dec3b84e92 100644
--- a/src/multibody/frame.hpp
+++ b/src/multibody/frame.hpp
@@ -33,11 +33,11 @@ namespace se3
///
enum FrameType
{
- OP_FRAME, // operational frame type
- JOINT, // joint frame type
- FIXED_JOINT, // fixed joint frame type
- BODY, // body frame type
- SENSOR // sensor frame type
+ OP_FRAME = 0x1 << 0, // operational frame type
+ JOINT = 0x1 << 1, // joint frame type
+ FIXED_JOINT = 0x1 << 2, // fixed joint frame type
+ BODY = 0x1 << 3, // body frame type
+ SENSOR = 0x1 << 4 // sensor frame type
};
///
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index efdd1569951a97145a7143dd9328dd842403824a..8f41750ad8a875788d58642157f1c3f11d72724a 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -275,19 +275,21 @@ namespace se3
/// (for example to get the id of a parent frame).
///
/// \param[in] name Name of the frame.
+ /// \param[in] type Type of the frame.
///
/// \return Index of the frame.
///
- FrameIndex getFrameId (const std::string & name) const;
+ FrameIndex getFrameId (const std::string & name, const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
///
/// \brief Checks if a frame given by its name exists.
///
/// \param[in] name Name of the frame.
+ /// \param[in] type Type of the frame.
///
/// \return Returns true if the frame exists.
///
- bool existFrame (const std::string & name) const;
+ bool existFrame (const std::string & name, const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
///
/// \brief Get the name of a frame given by its index.
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index 10ca4422f0eb3cd0bce5788464d65a640a06a0f6..a87695d6a059bd58f2f28ed194266811c36082cf 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -28,6 +28,18 @@
namespace se3
{
+ namespace details
+ {
+ struct FilterFrame {
+ const std::string& name;
+ const FrameType & typeMask;
+ FilterFrame (const std::string& name, const FrameType& typeMask)
+ : name (name), typeMask (typeMask) {}
+ bool operator() (const Frame& frame) const
+ { return (typeMask & frame.type) && (name == frame.name); }
+ };
+ }
+
inline std::ostream& operator<< (std::ostream & os, const Model & model)
{
os << "Nb joints = " << model.njoint << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
@@ -91,7 +103,7 @@ namespace se3
int fidx)
{
if (fidx < 0) {
- fidx = getFrameId(names[parents[jidx]]);
+ fidx = getFrameId(names[parents[jidx]], JOINT);
}
if (fidx >= frames.size())
throw std::invalid_argument ("Frame not found");
@@ -114,7 +126,7 @@ namespace se3
int previousFrame)
{
if (previousFrame < 0) {
- previousFrame = getFrameId(names[parentJoint]);
+ previousFrame = getFrameId(names[parentJoint], JOINT);
}
assert(previousFrame < frames.size() && "Frame index out of bound");
return addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
@@ -122,12 +134,12 @@ namespace se3
inline Model::JointIndex Model::getBodyId (const std::string & name) const
{
- return getFrameId(name);
+ return getFrameId(name, BODY);
}
inline bool Model::existBodyName (const std::string & name) const
{
- return existFrame(name);
+ return existFrame(name, BODY);
}
inline const std::string& Model::getBodyName (const Model::JointIndex index) const
@@ -156,18 +168,22 @@ namespace se3
return names[index];
}
- inline Model::FrameIndex Model::getFrameId ( const std::string & name ) const
+ inline Model::FrameIndex Model::getFrameId ( const std::string & name, const FrameType & type ) const
{
std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
, frames.end()
- , boost::bind(&Frame::name, _1) == name
+ , details::FilterFrame (name, type)
);
+ assert (it != frames.end() && "Frame not found");
+ assert ((std::find_if( boost::next(it), frames.end(), details::FilterFrame (name, type)) == frames.end())
+ && "Several frames match the filter");
return Model::FrameIndex(it - frames.begin());
}
- inline bool Model::existFrame ( const std::string & name ) const
+ inline bool Model::existFrame ( const std::string & name, const FrameType & type) const
{
- return std::find_if( frames.begin(), frames.end(), boost::bind(&Frame::name, _1) == name) != frames.end();
+ return std::find_if( frames.begin(), frames.end(),
+ details::FilterFrame (name, type)) != frames.end();
}
inline const std::string & Model::getFrameName ( const FrameIndex index ) const
@@ -192,7 +208,7 @@ namespace se3
inline int Model::addFrame ( const Frame & frame )
{
- if( !existFrame(frame.name) )
+ if( !existFrame(frame.name, frame.type) )
{
frames.push_back(frame);
nFrames++;
diff --git a/src/parsers/urdf/geometry.cpp b/src/parsers/urdf/geometry.cpp
index b858ee0d31a31bbe2da407daa49d6c9c3dbb9c5a..874fd8575f74188e8155e411bd9d2f83d7424fb3 100644
--- a/src/parsers/urdf/geometry.cpp
+++ b/src/parsers/urdf/geometry.cpp
@@ -192,9 +192,9 @@ namespace se3
std::vector< boost::shared_ptr< T > > geometries_array = getLinkGeometryArray<T>(link);
- if (!model.existBodyName(link_name))
+ if (!model.existFrame(link_name, BODY))
throw std::invalid_argument("No link " + link_name + " in model");
- FrameIndex frame_id = model.getFrameId(link_name);
+ FrameIndex frame_id = model.getFrameId(link_name, BODY);
SE3 body_placement = model.frames[frame_id].placement;
assert(model.frames[frame_id].type == BODY);
diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp
index c8116cc627bcbf084041ffcee268b97aa942df36..cc48058ee91c4d2f109a9f0017230862fc11bc88 100644
--- a/src/parsers/urdf/model.cpp
+++ b/src/parsers/urdf/model.cpp
@@ -33,19 +33,21 @@ namespace se3
{
namespace details
{
+ const FrameType JOINT_OR_FIXED_JOINT = (FrameType) (JOINT | FIXED_JOINT);
+
FrameIndex getParentJointFrame(::urdf::LinkConstPtr link, Model & model)
{
assert(link!=NULL && link->getParent()!=NULL);
FrameIndex id;
if (link->getParent()->parent_joint==NULL) {
- if (model.existFrame("root_joint"))
- id = model.getFrameId ("root_joint");
+ if (model.existFrame("root_joint", JOINT_OR_FIXED_JOINT))
+ id = model.getFrameId ("root_joint", JOINT_OR_FIXED_JOINT);
else
id = 0;
} else {
- if (model.existFrame(link->getParent()->parent_joint->name))
- id = model.getFrameId (link->getParent()->parent_joint->name);
+ if (model.existFrame(link->getParent()->parent_joint->name, JOINT_OR_FIXED_JOINT))
+ id = model.getFrameId (link->getParent()->parent_joint->name, JOINT_OR_FIXED_JOINT);
else
throw std::invalid_argument ("Model does not have any joints named "
+ link->getParent()->parent_joint->name);
@@ -69,7 +71,7 @@ namespace se3
&& Y->mass > Eigen::NumTraits<double>::epsilon()) {
model.appendBodyToJoint(frame.parent, convertFromUrdf(*Y), p);
}
- model.addBodyFrame(body_name, frame.parent, p, fid);
+ model.addBodyFrame(body_name, frame.parent, p, (int)fid);
// Reference to model.frames[fid] can has changed because the vector
// may have been reallocated.
if (model.frames[fid].parent > 0) {