Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, SegmentMap::value_type const* it, SegmentMap::value_type const* baseit)
{
//gets the frame for the current element (segment)
const TreeElement& currentElement = it->second;
if(it == baseit){
return KDL::Frame::Identity();
}
else{
Frame currentFrame = currentElement.segment.pose(((JntArray&)q_in)(currentElement.q_nr));
return recursiveFk(q_in, currentElement.parent, baseit) * currentFrame;
}
}
int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName)
{
SegmentMap::const_iterator it = tree.getSegment(segmentName);
if(q_in.rows() != tree.getNrOfJoints())
return -1;
else if(it == tree.getSegments().end()) //if the segment name is not found
return -2;
else{
p_out = recursiveFk(q_in, it);
return 0;
}
}
Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it)
{
//gets the frame for the current element (segment)
const TreeElementType& currentElement = it->second;
Frame currentFrame = GetTreeElementSegment(currentElement).pose(q_in(GetTreeElementQNr(currentElement)));
SegmentMap::const_iterator rootIterator = tree.getRootSegment();
if(it == rootIterator){
return currentFrame;
}
else{
SegmentMap::const_iterator parentIt = GetTreeElementParent(currentElement);
return recursiveFk(q_in, parentIt) * currentFrame;
}
}
int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)
{
SegmentMap::value_type const* it = tree.getSegmentPtr(segmentName);
SegmentMap::value_type const* baseit = tree.getSegmentPtr(baseName);
if(q_in.rows() != tree.getNrOfJoints())
return -1;
else if(!it) //if the segment name is not found
return -2;
else if(!baseit) //if the base segment name is not found
return -3;
else{
p_out = recursiveFk(q_in, it, baseit);
return 0;
}
}
