Verified Commit 60f3750d authored by Justin Carpentier's avatar Justin Carpentier
Browse files

data: add starting_subspace_idx_fromRow field

parent 68896cea
......@@ -233,6 +233,9 @@ namespace pinocchio
/// \brief Dimension of the subtree motion space (for CRBA)
std::vector<int> nvSubtree;
/// \brief Starting index of the Joint motion subspace
std::vector<int> starting_subspace_idx_fromRow;
/// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition.
MatrixXs U;
......
......@@ -63,6 +63,7 @@ namespace pinocchio
, Fcrb((std::size_t)model.njoints)
, lastChild((std::size_t)model.njoints)
, nvSubtree((std::size_t)model.njoints)
, starting_subspace_idx_fromRow((std::size_t)model.nv)
, U(model.nv,model.nv)
, D(model.nv)
, Dinv(model.nv)
......@@ -170,14 +171,16 @@ namespace pinocchio
assert(idx_vj >= 0 && idx_vj < model.nv);
if(parent>0) parents_fromRow[(Index)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
else parents_fromRow[(Index)idx_vj] = -1;
nvSubtree_fromRow[(Index)idx_vj] = nvSubtree[joint];
if(parent>0) parents_fromRow[(size_t)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
else parents_fromRow[(size_t)idx_vj] = -1;
nvSubtree_fromRow[(size_t)idx_vj] = nvSubtree[joint];
starting_subspace_idx_fromRow[(size_t)idx_vj] = idx_vj;
for(int row=1;row<nvj;++row)
{
parents_fromRow[(Index)(idx_vj+row)] = idx_vj+row-1;
nvSubtree_fromRow[(Index)(idx_vj+row)] = nvSubtree[joint]-row;
parents_fromRow[(size_t)(idx_vj+row)] = idx_vj+row-1;
nvSubtree_fromRow[(size_t)(idx_vj+row)] = nvSubtree[joint]-row;
starting_subspace_idx_fromRow[(size_t)(idx_vj+row)] = idx_vj;
}
}
}
......
......@@ -13,6 +13,24 @@ using namespace pinocchio;
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_AUTO_TEST_CASE(test_data_starting_subspace_idx_fromRow)
{
Model model;
buildModels::humanoidRandom(model);
Data data(model);
for(Model::JointIndex joint_id = 1; joint_id < (Model::JointIndex)model.njoints; ++joint_id)
{
const int nv_joint = model.joints[joint_id].nv();
const int idx_joint = model.joints[joint_id].idx_v();
for(int k = 0; k < nv_joint; ++k)
BOOST_CHECK(data.starting_subspace_idx_fromRow[(size_t)(idx_joint+k)] == idx_joint);
}
}
BOOST_AUTO_TEST_CASE(test_data_supports_fromRow)
{
Model model;
......
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