Verified Commit 68896cea authored by Justin Carpentier's avatar Justin Carpentier
Browse files

data: add supports_fromRow field

parent a138bd03
......@@ -230,6 +230,7 @@ namespace pinocchio
/// \brief Index of the last child (for CRBA)
std::vector<int> lastChild;
/// \brief Dimension of the subtree motion space (for CRBA)
std::vector<int> nvSubtree;
......@@ -248,6 +249,10 @@ namespace pinocchio
/// \brief First previous non-zero row in M (used in Cholesky Decomposition).
std::vector<int> parents_fromRow;
/// \brief Each element of this vector corresponds to the ordered list of indexes belonging to the supporting tree of the
/// given index at the row level. It may be helpful to retrieve the sparsity pattern through it.
std::vector< std::vector<int> > supports_fromRow;
/// \brief Subtree of the current row index (used in Cholesky Decomposition).
std::vector<int> nvSubtree_fromRow;
......@@ -347,6 +352,7 @@ namespace pinocchio
private:
void computeLastChild(const Model & model);
void computeParents_fromRow(const Model & model);
void computeSupports_fromRow(const Model & model);
};
......
......@@ -68,6 +68,7 @@ namespace pinocchio
, Dinv(model.nv)
, tmp(model.nv)
, parents_fromRow((std::size_t)model.nv)
, supports_fromRow((std::size_t)model.nv)
, nvSubtree_fromRow((std::size_t)model.nv)
, J(6,model.nv)
, dJ(6,model.nv)
......@@ -106,13 +107,14 @@ namespace pinocchio
{ Fcrb[i].resize(6,model.nv); }
computeLastChild(model);
/* Init for Coriolis */
C.setZero();
/* Init for Cholesky */
U.setIdentity();
computeParents_fromRow(model);
computeSupports_fromRow(model);
/* Init Jacobian */
J.setZero();
......@@ -180,6 +182,36 @@ namespace pinocchio
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline void DataTpl<Scalar,Options,JointCollectionTpl>
::computeSupports_fromRow(const Model & model)
{
typedef typename Model::JointIndex JointIndex;
for(JointIndex joint_id = 1;
joint_id < (JointIndex)(model.njoints);
joint_id++)
{
const int nvj = nv (model.joints[joint_id]);
const int idx_vj = idx_v(model.joints[joint_id]);
assert(idx_vj >= 0 && idx_vj < model.nv);
const int parent_fromRow = parents_fromRow[(size_t)idx_vj];
if(parent_fromRow >= 0)
supports_fromRow[(size_t)idx_vj] = supports_fromRow[(size_t)parent_fromRow];
supports_fromRow[(size_t)idx_vj].push_back(idx_vj);
for(int row = 1; row < nvj; ++row)
{
supports_fromRow[(size_t)(idx_vj+row)] = supports_fromRow[(size_t)(idx_vj+row-1)];
supports_fromRow[(size_t)(idx_vj+row)].push_back(idx_vj+row);
}
}
}
} // namespace pinocchio
/// @endcond
......
......@@ -96,6 +96,7 @@ IF(BUILD_PYTHON_INTERFACE)
ENDIF(BUILD_PYTHON_INTERFACE)
ADD_PINOCCHIO_UNIT_TEST(model)
ADD_PINOCCHIO_UNIT_TEST(data)
ADD_PINOCCHIO_UNIT_TEST(constraint)
ADD_PINOCCHIO_UNIT_TEST(joints)
ADD_PINOCCHIO_UNIT_TEST(compute-all-terms)
......
//
// Copyright (c) 2019 INRIA
//
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
using namespace pinocchio;
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_AUTO_TEST_CASE(test_data_supports_fromRow)
{
Model model;
buildModels::humanoidRandom(model);
Data data(model);
for(size_t k = 0; k < (size_t)model.nv; ++k)
{
const std::vector<int> & support = data.supports_fromRow[k];
const int parent_id = data.parents_fromRow[k];
if(parent_id >= 0)
{
const std::vector<int> & support_parent = data.supports_fromRow[(size_t)parent_id];
BOOST_CHECK(support.size() == support_parent.size()+1);
for(size_t j = 0; j < support_parent.size(); ++j)
{
BOOST_CHECK(support[j] == support_parent[j]);
}
}
BOOST_CHECK(support.back() == (int)k);
}
}
BOOST_AUTO_TEST_SUITE_END()
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