Commit 1d53aec9 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[C++] Frame in Model are identified by their name + their type.

parent 26ca737a
......@@ -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
};
///
......
......@@ -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.
......
......@@ -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++;
......
......@@ -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);
......
......@@ -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) {
......
Supports Markdown
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