Commit 71a4b94e authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Fix setRootJointBounds when root joint is an anchor joint

parent 783b2465
......@@ -128,9 +128,9 @@ namespace hpp {
const JointIndex& rtIdx,
const std::string& rootType)
{
const std::size_t idx = model.joints[rtIdx].idx_q();
value_type b = std::numeric_limits<value_type>::infinity();
if (rootType == "freeflyer") {
value_type b = std::numeric_limits<value_type>::infinity();
const std::size_t idx = model.joints[rtIdx].idx_q();
model.upperPositionLimit.segment<3>(idx).setConstant(+b);
model.lowerPositionLimit.segment<3>(idx).setConstant(-b);
// Quaternion bounds
......@@ -139,7 +139,7 @@ namespace hpp {
model.upperPositionLimit.segment<4>(quat_idx).setConstant(+b);
model.lowerPositionLimit.segment<4>(quat_idx).setConstant(-b);
} else if (rootType == "planar") {
value_type b = std::numeric_limits<value_type>::infinity();
const std::size_t idx = model.joints[rtIdx].idx_q();
model.upperPositionLimit.segment<2>(idx).setConstant(+b);
model.lowerPositionLimit.segment<2>(idx).setConstant(-b);
// Unit complex bounds
......
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