Commit 7700dee4 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #403 from florent-lamiraux/devel

Fix bug in parser urdf + add copy constructor in VectorSpaceOperation
parents 6ac2f57c 2a8b1d2f
......@@ -51,6 +51,15 @@ namespace se3
assert (Size == Eigen::Dynamic || size_ == Size);
}
/// Constructor
/// \param size size of the vector space: should be the equal to template
/// argument for static sized vector-spaces.
VectorSpaceOperation (const LieGroupDerived& other) : size_ (other.size_)
{
assert (size_ >= 0);
assert (Size == Eigen::Dynamic || size_ == Size);
}
Index nq () const
{
return size_;
......
......@@ -104,7 +104,19 @@ namespace se3
joint_name,
max_effort,max_velocity,min_config,max_config
);
FrameIndex jointFrameId = (FrameIndex) model.addJointFrame(idx, (int)parentFrameId); // C-style cast to remove polluting compilation warning. This is Bad practice. See issue #323 (rework indexes)
int res (model.addJointFrame(idx, (int)parentFrameId));
if (res == -1) {
std::ostringstream oss;
oss << joint_name << " already inserted as a frame. Current frames "
"are [";
for (container::aligned_vector<Frame>::const_iterator it =
model.frames.begin (); it != model.frames.end (); ++it) {
oss << "\"" << it->name << "\",";
}
oss << "]";
throw std::runtime_error (oss.str ().c_str ());
}
FrameIndex jointFrameId = (FrameIndex) res; // C-style cast to remove polluting compilation warning. This is Bad practice. See issue #323 (rework indexes)
appendBodyToJoint(model, jointFrameId, Y, SE3::Identity(), body_name);
}
......
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