28 nrOfJoints(0), nrOfSegments(0), root_name(_root_name)
55 SegmentMap::iterator parent =
segments.find(hook_name);
59 std::pair<SegmentMap::iterator, bool> retval;
63 #ifdef KDL_USE_NEW_TREE_INTERFACE 65 #else //#ifdef KDL_USE_NEW_TREE_INTERFACE 67 #endif //#ifdef KDL_USE_NEW_TREE_INTERFACE 83 std::string parent_name = hook_name;
99 SegmentMap::const_iterator child;
122 std::vector<SegmentMap::key_type> parents_chain_root, parents_chain_tip;
124 parents_chain_root.push_back(s->first);
127 if (parents_chain_root.empty() || parents_chain_root.back() !=
root_name)
return false;
129 parents_chain_tip.push_back(s->first);
132 if (parents_chain_tip.empty() || parents_chain_tip.back() !=
root_name)
return false;
135 SegmentMap::key_type last_segment =
root_name;
136 while (!parents_chain_root.empty() && !parents_chain_tip.empty() &&
137 parents_chain_root.back() == parents_chain_tip.back()){
138 last_segment = parents_chain_root.back();
139 parents_chain_root.pop_back();
140 parents_chain_tip.pop_back();
142 parents_chain_root.push_back(last_segment);
146 for (
unsigned int s=0; s<parents_chain_root.size()-1; s++){
159 for (
auto rit=parents_chain_tip.rbegin(); rit != parents_chain_tip.rend(); ++rit){
168 SegmentMap::const_iterator root =
segments.find(segment_name);
172 tree =
Tree(root->first);
Vector JointOrigin() const
unsigned int nrOfSegments
#define GetTreeElementSegment(tree_element)
Tree & operator=(const Tree &arg)
SegmentMap::const_iterator getRootSegment() const
bool addTreeRecursive(SegmentMap::const_iterator root, const std::string &hook_name)
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body ine...
TreeElement TreeElementType
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
unsigned int getNrOfSegments() const
void addSegment(const Segment &segment)
const Segment & getSegment(unsigned int nr) const
#define GetTreeElementChildren(tree_element)
#define GetTreeElementParent(tree_element)
Rotation M
Orientation of the Frame.
const Joint & getJoint() const
bool addChain(const Chain &chain, const std::string &hook_name)
Frame Inverse() const
Gives back inverse transformation of a Frame.
Frame pose(const double &q) const
bool addTree(const Tree &tree, const std::string &hook_name)
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
const JointType & getType() const
static TreeElementType Root(const std::string &root_name)
bool getSubTree(const std::string &segment_name, Tree &tree) const
represents a frame transformation in 3D space (rotation + translation)
const std::string & getName() const
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
const std::string & getName() const
bool addSegment(const Segment &segment, const std::string &hook_name)
Tree(const std::string &root_name="root")
This class encapsulates a tree kinematic interconnection structure. It is built out of segments...
SegmentMap::const_iterator getSegment(const std::string &segment_name) const