Verified Commit 6ea9b78b authored by Justin Carpentier's avatar Justin Carpentier
Browse files

mode: add the notion of subtrees

Will be useful for certain algorithms dealing with second order derivatives
parent 06995d18
......@@ -128,6 +128,11 @@ namespace pinocchio
/// \brief Vector of operational frames registered on the model.
FrameVector frames;
/// \brief Vector of supports.
/// supports[j] corresponds to the collection of all joints located on the path between body *j* and the world.
/// The last element of supports[j] is the index of the joint *j* itself.
std::vector<IndexVector> supports;
/// \brief Vector of subtrees.
/// subtree[j] corresponds to the subtree supported by the joint *j*.
/// The first element of subtree[j] is the index of the joint *j* itself.
......@@ -154,6 +159,7 @@ namespace pinocchio
, joints(1)
, parents(1, 0)
, names(1)
, supports(1,IndexVector(1,0))
, subtrees(1)
, gravity(gravity981,Vector3::Zero())
{
......
......@@ -122,6 +122,11 @@ namespace pinocchio
subtrees.push_back(IndexVector(1));
subtrees[idx][0] = idx;
addJointIndexToParentSubtrees(idx);
// Init and add joint index to the supports
supports.push_back(supports[parent]);
supports[idx].push_back(idx);
return idx;
}
......
......@@ -43,6 +43,30 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
}
}
BOOST_AUTO_TEST_CASE(test_model_support)
{
Model model;
buildModels::humanoidRandom(model);
const Model::IndexVector support0_ref(1,0);
BOOST_CHECK(model.supports[0] == support0_ref);
// Check that i ends supports[i]
for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
{
BOOST_CHECK(model.supports[joint_id].back() == joint_id);
Model::IndexVector & support = model.supports[joint_id];
size_t current_id = support.size()-2;
for(JointIndex parent_id = model.parents[joint_id];
parent_id > 0;
parent_id = model.parents[parent_id],
current_id--)
{
BOOST_CHECK(parent_id == support[current_id]);
}
}
}
BOOST_AUTO_TEST_CASE(comparison)
{
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