Commit 4460c5a8 authored by jcarpent's avatar jcarpent
Browse files

[C++] Add subtree info

parent 68f67ae9
......@@ -101,6 +101,7 @@ namespace se3
/// \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.
std::vector<IndexVector> subtrees;
/// \brief Spatial gravity of the model.
......@@ -121,6 +122,7 @@ namespace se3
, joints(1)
, parents(1)
, names(1)
, subtrees(1)
, gravity( gravity981,Eigen::Vector3d::Zero() )
{
names[0] = "universe"; // Should be "universe joint (trivial)"
......@@ -340,6 +342,13 @@ namespace se3
///
bool addFrame(const std::string & name, const JointIndex parent, const SE3 & placement, const FrameType type = OP_FRAME);
protected:
/// \brief Add the joint_id to its parent subtrees.
///
/// \param[in] joint_id The id of the joint to add to the subtrees
///
void addJointIndexToParentSubtrees(const JointIndex joint_id);
};
class Data
......
......@@ -83,6 +83,10 @@ namespace se3
// Add a the joint frame attached to itself to the frame vector - redundant information but useful.
addFrame(names[idx],idx,SE3::Identity(),JOINT);
// Init and add joint index to its parent subtrees.
subtrees.push_back(IndexVector(1));
subtrees[idx][0] = idx;
addJointIndexToParentSubtrees(idx);
return idx;
}
......@@ -225,6 +229,12 @@ namespace se3
else
return false;
}
inline void Model::addJointIndexToParentSubtrees(const JointIndex joint_id)
{
for(JointIndex parent = parents[joint_id]; parent>0; parent = parents[parent])
subtrees[parent].push_back(joint_id);
}
inline Data::Data (const Model & ref)
......@@ -244,7 +254,7 @@ namespace se3
,ddq(ref.nv)
,Yaba((std::size_t)ref.nbody)
,u(ref.nv)
,Ag(6, ref.nv)
,Ag(6,ref.nv)
,Fcrb((std::size_t)ref.nbody)
,lastChild((std::size_t)ref.nbody)
,nvSubtree((std::size_t)ref.nbody)
......@@ -282,7 +292,8 @@ namespace se3
computeParents_fromRow(ref);
/* Init Jacobian */
J.fill(0);
J.setZero();
Ag.setZero();
/* Init universe states relatively to itself */
......
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