Commit 3c029ada authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[C++] Fix issues related to frame "universe"

parent 1d53aec9
......@@ -124,6 +124,9 @@ namespace se3
, gravity( gravity981,Eigen::Vector3d::Zero() )
{
names[0] = "universe"; // Should be "universe joint (trivial)"
// FIXME Should the universe joint be a FIXED_JOINT even if it is
// in the list of joints ? See comment in definition of
// Model::addJointFrame and Model::addBodyFrame
addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
// Inertia of universe has no sense.
inertias[0].mass() = std::numeric_limits<double>::quiet_NaN();
......
......@@ -103,10 +103,11 @@ namespace se3
int fidx)
{
if (fidx < 0) {
fidx = getFrameId(names[parents[jidx]], JOINT);
// FIXED_JOINT is required because the parent can be the universe and its
// type is FIXED_JOINT
fidx = getFrameId(names[parents[jidx]], (FrameType)(JOINT | FIXED_JOINT));
}
if (fidx >= frames.size())
throw std::invalid_argument ("Frame not found");
assert(fidx < frames.size() && "Frame index out of bound");
// Add a the joint frame attached to itself to the frame vector - redundant information but useful.
return addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
}
......@@ -126,7 +127,9 @@ namespace se3
int previousFrame)
{
if (previousFrame < 0) {
previousFrame = getFrameId(names[parentJoint], JOINT);
// FIXED_JOINT is required because the parent can be the universe and its
// type is FIXED_JOINT
previousFrame = getFrameId(names[parentJoint], (FrameType)(JOINT | FIXED_JOINT));
}
assert(previousFrame < frames.size() && "Frame index out of bound");
return addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
......
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