Verified Commit 202f84f6 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

data: add field end_idx_v_fromRow

parent 9518b555
......@@ -236,6 +236,9 @@ namespace pinocchio
/// \brief Starting index of the Joint motion subspace
std::vector<int> start_idx_v_fromRow;
/// \brief End index of the Joint motion subspace
std::vector<int> end_idx_v_fromRow;
/// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition.
MatrixXs U;
......
......@@ -64,6 +64,7 @@ namespace pinocchio
, lastChild((std::size_t)model.njoints)
, nvSubtree((std::size_t)model.njoints)
, start_idx_v_fromRow((std::size_t)model.nv)
, end_idx_v_fromRow((std::size_t)model.nv)
, U(model.nv,model.nv)
, D(model.nv)
, Dinv(model.nv)
......@@ -176,11 +177,13 @@ namespace pinocchio
nvSubtree_fromRow[(size_t)idx_vj] = nvSubtree[joint];
start_idx_v_fromRow[(size_t)idx_vj] = idx_vj;
end_idx_v_fromRow[(size_t)idx_vj] = idx_vj+nvj-1;
for(int row=1;row<nvj;++row)
{
parents_fromRow[(size_t)(idx_vj+row)] = idx_vj+row-1;
nvSubtree_fromRow[(size_t)(idx_vj+row)] = nvSubtree[joint]-row;
start_idx_v_fromRow[(size_t)(idx_vj+row)] = start_idx_v_fromRow[(size_t)idx_vj];
end_idx_v_fromRow[(size_t)(idx_vj+row)] = end_idx_v_fromRow[(size_t)idx_vj];
}
}
}
......
......@@ -27,7 +27,10 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
const int idx_joint = model.joints[joint_id].idx_v();
for(int k = 0; k < nv_joint; ++k)
{
BOOST_CHECK(data.start_idx_v_fromRow[(size_t)(idx_joint+k)] == idx_joint);
BOOST_CHECK(data.end_idx_v_fromRow[(size_t)(idx_joint+k)] == idx_joint+nv_joint-1);
}
}
}
......
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