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

[C++] Add subtree info

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