std::string getDOFName(KDL::Tree & tree, const unsigned int index)
{
std::string retVal = "";
for (KDL::SegmentMap::const_iterator it=tree.getSegments().begin(); it!=tree.getSegments().end(); ++it)
{
if( GetTreeElementQNr(it->second) == index )
{
retVal = GetTreeElementSegment(it->second).getJoint().getName();
}
}
return retVal;
}
std::vector<std::string> getLinkAttachedToFixedJoints(KDL::Tree & tree)
{
std::vector<std::string> ret;
for(KDL::SegmentMap::const_iterator seg=tree.getSegments().begin();
seg != tree.getSegments().end();
seg++)
{
if( GetTreeElementSegment(seg->second).getJoint().getType() == KDL::Joint::None )
{
ret.push_back(GetTreeElementSegment(seg->second).getName());
}
}
return ret;
}
bool framesFromKDLTree(const KDL::Tree& tree,
std::vector<std::string>& framesNames,
std::vector<std::string>& parentLinkNames)
{
framesNames.clear();
parentLinkNames.clear();
KDL::SegmentMap::iterator seg;
KDL::SegmentMap segs;
KDL::SegmentMap::const_iterator root_seg;
root_seg = tree.getRootSegment();
segs = tree.getSegments();
for( seg = segs.begin(); seg != segs.end(); seg++ )
{
if( GetTreeElementChildren(seg->second).size() == 0 &&
GetTreeElementSegment(seg->second).getJoint().getType() == KDL::Joint::None &&
GetTreeElementSegment(seg->second).getInertia().getMass() == 0.0 )
{
std::string frameName = GetTreeElementSegment(seg->second).getName();
std::string parentLinkName = GetTreeElementSegment(GetTreeElementParent(seg->second)->second).getName();
framesNames.push_back(frameName);
parentLinkNames.push_back(parentLinkName);
//also check parent
KDL::Segment parent = GetTreeElementSegment(GetTreeElementParent(seg->second)->second);
if (parent.getJoint().getType() == KDL::Joint::None &&
parent.getInertia().getMass() == 0.0)
{
framesNames.push_back(parentLinkName);
}
}
}
return true;
}
bool leatherman::getSegmentOfJoint(const KDL::Tree &tree, std::string joint, std::string &segment)
{
KDL::SegmentMap smap = tree.getSegments();
for(std::map<std::string, KDL::TreeElement>::const_iterator iter = smap.begin(); iter != smap.end(); ++iter)
{
if(iter->second.segment.getJoint().getName().compare(joint) == 0)
{
segment = iter->second.segment.getName();
return true;
}
}
return false;
}
bool kdl_urdf_tools::initialize_kinematics_from_urdf(
const std::string &robot_description,
const std::string &root_link,
const std::string &tip_link,
unsigned int &n_dof,
KDL::Chain &kdl_chain,
KDL::Tree &kdl_tree,
urdf::Model &urdf_model)
{
if(robot_description.length() == 0) {
ROS_ERROR("URDF string is empty.");
return false;
}
// Construct an URDF model from the xml string
urdf_model.initString(robot_description);
// Get a KDL tree from the robot URDF
if (!kdl_parser::treeFromUrdfModel(urdf_model, kdl_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
// Populate the KDL chain
if(!kdl_tree.getChain(root_link, tip_link, kdl_chain))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_link<<" --> "<<tip_link);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin();
it != segment_map.end();
it++ )
{
ROS_ERROR_STREAM( " "<<(*it).first);
}
return false;
}
// Store the number of degrees of freedom of the chain
n_dof = kdl_chain.getNrOfJoints();
return true;
}
bool JacoIKSolver::initFromURDF(const std::string urdf, const std::string root_name,
const std::string tip_name, unsigned int max_iter, std::string error)
{
urdf::Model robot_model;
KDL::Tree tree;
if (!robot_model.initFile(urdf))
{
error += "Could not initialize robot model";
return false;
}
if (!kdl_parser::treeFromFile(urdf, tree))
{
error += "Could not initialize tree object";
return false;
}
if (tree.getSegment(root_name) == tree.getSegments().end())
{
error += "Could not find root link '" + root_name + "'.";
return false;
}
if (tree.getSegment(tip_name) == tree.getSegments().end())
{
error += "Could not find tip link '" + tip_name + "'.";
return false;
}
if (!tree.getChain(root_name, tip_name, chain_))
{
error += "Could not initialize chain object";
return false;
}
// Get the joint limits from the robot model
q_min_.resize(chain_.getNrOfJoints());
q_max_.resize(chain_.getNrOfJoints());
q_seed_.resize(chain_.getNrOfJoints());
joint_names_.resize(chain_.getNrOfJoints());
unsigned int j = 0;
for(unsigned int i = 0; i < chain_.getNrOfSegments(); ++i)
{
const KDL::Joint& kdl_joint = chain_.getSegment(i).getJoint();
if (kdl_joint.getType() != KDL::Joint::None)
{
// std::cout << chain_.getSegment(i).getName() << " -> " << kdl_joint.getName() << " -> " << chain_.getSegment(i + 1).getName() << std::endl;
boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(kdl_joint.getName());
if (joint && joint->limits)
{
q_min_(j) = joint->limits->lower;
q_max_(j) = joint->limits->upper;
q_seed_(j) = (q_min_(j) + q_max_(j)) / 2;
}
else
{
q_min_(j) = -1e9;
q_max_(j) = 1e9;
q_seed_(j) = 0;
}
joint_names_[j] = kdl_joint.getName();
++j;
}
}
;
fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_));
ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(chain_));
ik_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(chain_, q_min_, q_max_, *fk_solver_, *ik_vel_solver_, max_iter));
std::cout << "Using normal solver" << std::endl;
jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));
return true;
}
bool treeToUrdfModel(const KDL::Tree& tree, const std::string & robot_name, urdf::ModelInterface& robot_model)
{
robot_model.clear();
robot_model.name_ = robot_name;
//Add all links
KDL::SegmentMap::iterator seg;
KDL::SegmentMap segs;
KDL::SegmentMap::const_iterator root_seg;
root_seg = tree.getRootSegment();
segs = tree.getSegments();
for( seg = segs.begin(); seg != segs.end(); seg++ ) {
if (robot_model.getLink(seg->first))
{
std::cerr << "[ERR] link " << seg->first << " is not unique." << std::endl;
robot_model.clear();
return false;
}
else
{
urdf::LinkPtr link;
resetPtr(link, new urdf::Link);
//add name
link->name = seg->first;
//insert link
robot_model.links_.insert(make_pair(seg->first,link));
std::cerr << "[DEBUG] successfully added a new link " << link->name << std::endl;
}
//inserting joint
//The fake root segment has no joint to add
if( seg->first != root_seg->first ) {
KDL::Joint jnt;
jnt = GetTreeElementSegment(seg->second).getJoint();
if (robot_model.getJoint(jnt.getName()))
{
std::cerr << "[ERR] joint " << jnt.getName() << " is not unique." << std::endl;
robot_model.clear();
return false;
}
else
{
urdf::JointPtr joint;
urdf::LinkPtr link = robot_model.links_[seg->first];
//This variable will be set by toUrdf
KDL::Frame H_new_old_successor;
KDL::Frame H_new_old_predecessor = getH_new_old(GetTreeElementSegment(GetTreeElementParent(seg->second)->second));
urdf::resetPtr(joint, new urdf::Joint());
//convert joint
*joint = toUrdf(jnt, GetTreeElementSegment(seg->second).getFrameToTip(),H_new_old_predecessor,H_new_old_successor);
//insert parent
joint->parent_link_name = GetTreeElementParent(seg->second)->first;
//insert child
joint->child_link_name = seg->first;
//insert joint
robot_model.joints_.insert(make_pair(seg->first,joint));
std::cerr << "[DEBUG] successfully added a new joint" << jnt.getName() << std::endl;
//add inertial, taking in account an eventual change in the link frame
resetPtr(link->inertial, new urdf::Inertial());
*(link->inertial) = toUrdf(H_new_old_successor * GetTreeElementSegment(seg->second).getInertia());
}
}
}
// every link has children links and joints, but no parents, so we create a
// local convenience data structure for keeping child->parent relations
std::map<std::string, std::string> parent_link_tree;
parent_link_tree.clear();
// building tree: name mapping
//try
//{
robot_model.initTree(parent_link_tree);
//}
/*
catch(ParseError &e)
{
logError("Failed to build tree: %s", e.what());
robot_model.clear();
return false;
}*/
// find the root link
//try
//{
robot_model.initRoot(parent_link_tree);
//}
/*
catch(ParseError &e)
{
logError("Failed to find root link: %s", e.what());
robot_model.reset();
return false;
}
*/
return true;
}
bool Kinematics::build_chain(FINGERS f, const KDL::Tree &kdl_tree_){
std::string root_name, tip_name;
se_finger finger_info;
std::string finger_name;
try{
finger_info = root_tip.at(f);
root_name = finger_info.root_name;
tip_name = finger_info.tip_name;
}catch(const std::out_of_range& oor){
ROS_ERROR("Kinematics::build_chain no such finger type");
return false;
}
// joint_limits& joint_limits_ = finger_joint_limits.at(f);
KDL::Chain& kdl_chain_ = kdl_chains[f];
finger_name = f_enum2str.at(f);
// Populate the KDL chain
if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_name<<" --> "<<tip_name);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree_.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin(); it != segment_map.end(); it++ )
ROS_ERROR_STREAM( " "<<(*it).first);
return false;
}
ROS_INFO("=== Loading %s KChain ===",finger_name.c_str());
ROS_INFO("root: %s\t tip: %s",root_name.c_str(),tip_name.c_str());
ROS_INFO("Number of segments: %d",kdl_chain_.getNrOfSegments());
ROS_INFO("Number of joints in chain: %d",kdl_chain_.getNrOfJoints());
// Parsing joint limits from urdf model
/* boost::shared_ptr<const urdf::Link> link_ = model.getLink(tip_name);
boost::shared_ptr<const urdf::Joint> joint_;
joint_limits_.min.resize(kdl_chain_.getNrOfJoints());
joint_limits_.max.resize(kdl_chain_.getNrOfJoints());
joint_limits_.center.resize(kdl_chain_.getNrOfJoints());*/
/*int index;
for (int i = 0; i < kdl_chain_.getNrOfJoints() && link_; i++)
{
joint_ = model.getJoint(link_->parent_joint->name);
index = kdl_chain_.getNrOfJoints() - i - 1;
joint_limits_.min(index) = joint_->limits->lower;
joint_limits_.max(index) = joint_->limits->upper;
joint_limits_.center(index) = (joint_limits_.min(index) + joint_limits_.max(index))/2;
link_ = model.getLink(link_->getParent()->name);
}
*/
/* std::cout<< "segment names" << std::endl;
int index;
for(std::size_t i = 0; i < kdl_chain_.getNrOfJoints();i++){
// std::cout<< kdl_chain_.segments[i].getName() << " " << kdl_chain_.segments[i].getJoint().getName() << " " << kdl_chain_.segments[i].getJoint().getTypeName() << std::endl;
joint_ = model.getJoint(kdl_chain_.segments[i].getJoint().getName());
index = i;
switch(joint_->type){
case urdf::Joint::REVOLUTE:
{
joint_limits_.min(i) = joint_->limits->lower;
joint_limits_.max(i) = joint_->limits->upper;
joint_limits_.center(i) = (joint_limits_.min(i) + joint_limits_.max(i))/2;
ROS_INFO("joint name: %s \t type: %d \t %f %f",joint_->name.c_str(),joint_->type,joint_->limits->lower,joint_->limits->upper);
break;
}
case urdf::Joint::FIXED:
{
ROS_INFO("joint name: %s \t type: %d",joint_->name.c_str(),joint_->type);
break;
}
default:
{
std::cerr<< "Kinematics::build_chain no such joint type: " << joint_->type << " implemented" << std::endl;
break;
}
}
}*/
}
void drawMyTree(KDL::Tree& a_tree)
{
//graphviz stuff
/****************************************/
Agraph_t *g;
GVC_t *gvc;
/* set up a graphviz context */
gvc = gvContext();
/* Create a simple digraph */
g = agopen("robot-structure", AGDIGRAPH);
//create vector to hold nodes
std::vector<Agnode_t*> nodeVector;
nodeVector.resize(a_tree.getSegments().size());
// printf("size of segments in tree map %d\n", a_tree.getSegments().size());
// printf("size of segments in tree %d\n", a_tree.getNrOfSegments());
//create vector to hold edges
std::vector<Agedge_t*> edgeVector;
edgeVector.resize(a_tree.getNrOfJoints() + 1);
int jointIndex = a_tree.getNrOfJoints() + 1;
// printf("size of joint array %d %d\n", jointIndex, a_tree.getNrOfJoints());
int segmentIndex = 0;
// fill in the node vector by iterating over tree segments
for (SegmentMap::const_iterator iter = a_tree.getSegments().begin(); iter != a_tree.getSegments().end(); ++iter)
{
//it would have been very useful if one could access list of joints of a tree
//list of segments is already possible
int stringLength = iter->second.segment.getName().size();
char name[stringLength + 1];
strcpy(name, iter->second.segment.getName().c_str());
//q_nr returned is the same value for the root and the its child. this is a bug
nodeVector[iter->second.q_nr] = agnode(g, name);
agsafeset(nodeVector[iter->second.q_nr], "color", "red", "");
agsafeset(nodeVector[iter->second.q_nr], "shape", "box", "");
std::cout << "index parent " << iter->second.q_nr << std::endl;
std::cout << "name parent " << iter->second.segment.getName() << std::endl;
std::cout << "joint name parent " << iter->second.segment.getJoint().getName() << std::endl;
std::cout << "joint type parent " << iter->second.segment.getJoint().getType() << std::endl;
// if (iter->second.segment.getJoint().getType() == Joint::None) //equals to joint type None
// {
// int stringLength = iter->second.segment.getJoint().getName().size();
// char name[stringLength + 1];
// strcpy(name, iter->second.segment.getJoint().getName().c_str());
// edgeVector[iter->second.q_nr] = agedge(g, nodeVector[iter->second.q_nr], nodeVector[iter->second.q_nr]);
// agsafeset(edgeVector[iter->second.q_nr], "label", name, "");
// }
if (segmentIndex < a_tree.getSegments().size())
segmentIndex++;
}
//fill in edge vector by iterating over joints in the tree
for (SegmentMap::const_iterator iter = a_tree.getSegments().begin(); iter != a_tree.getSegments().end(); ++iter)
{
//TODO: Fix node-edge connection relation
int stringLength = iter->second.segment.getJoint().getName().size();
// std::cout << "Joint name " << iter->second.segment.getJoint().getName() << std::endl;
char name[stringLength + 1];
strcpy(name, iter->second.segment.getJoint().getName().c_str());
for (std::vector<KDL::SegmentMap::const_iterator>::const_iterator childIter = iter->second.children.begin(); childIter != iter->second.children.end(); childIter++)
{
edgeVector[iter->second.q_nr] = agedge(g, nodeVector[iter->second.q_nr], nodeVector[(*childIter)->second.q_nr]);
agsafeset(edgeVector[iter->second.q_nr], "label", name, "");
}
// if (jointIndex != 0)
// {
// edgeVector[jointIndex] = agedge(g, nodeVector[segmentIndex], nodeVector[jointIndex]);
// agsafeset(edgeVector[jointIndex], "label", name, "");
// }
}
/* Compute a layout using layout engine from command line args */
// gvLayoutJobs(gvc, g);
gvLayout(gvc, g, "dot");
/* Write the graph according to -T and -o options */
//gvRenderJobs(gvc, g);
gvRenderFilename(gvc, g, "ps", "test-invdynamics2.ps");
/* Free layout data */
gvFreeLayout(gvc, g);
/* Free graph structures */
agclose(g);
gvFreeContext(gvc);
/* close output file, free context, and return number of errors */
return;
}
bool TreeKinematics::readJoints(urdf::Model &robot_model,
KDL::Tree &kdl_tree,
std::string &tree_root_name,
unsigned int &nr_of_jnts,
KDL::JntArray &joint_min,
KDL::JntArray &joint_max,
KDL::JntArray &joint_vel_max)
{
KDL::SegmentMap segmentMap;
segmentMap = kdl_tree.getSegments();
tree_root_name = kdl_tree.getRootSegment()->second.segment.getName();
nr_of_jnts = kdl_tree.getNrOfJoints();
ROS_DEBUG( "the tree's number of joints: [%d]", nr_of_jnts );
joint_min.resize(nr_of_jnts);
joint_max.resize(nr_of_jnts);
joint_vel_max.resize(nr_of_jnts);
info_.joint_names.resize(nr_of_jnts);
info_.limits.resize(nr_of_jnts);
// The following walks through all tree segments, extracts their joints except joints of KDL::None and extracts
// the information about min/max position and max velocity of the joints not of type urdf::Joint::UNKNOWN or
// urdf::Joint::FIXED
ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None.");
boost::shared_ptr<const urdf::Joint> joint;
for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); ++seg_it)
{
if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None)
{
// check, if joint can be found in the URDF model of the robot
joint = robot_model.getJoint(seg_it->second.segment.getJoint().getName().c_str());
if (!joint)
{
ROS_FATAL("Joint '%s' has not been found in the URDF robot model!", joint->name.c_str());
return false;
}
// extract joint information
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
{
ROS_DEBUG( "getting information about joint: [%s]", joint->name.c_str() );
double lower = 0.0, upper = 0.0, vel_limit = 0.0;
unsigned int has_pos_limits = 0, has_vel_limits = 0;
if ( joint->type != urdf::Joint::CONTINUOUS )
{
ROS_DEBUG("joint is not continuous.");
lower = joint->limits->lower;
upper = joint->limits->upper;
has_pos_limits = 1;
if (joint->limits->velocity)
{
has_vel_limits = 1;
vel_limit = joint->limits->velocity;
ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
}
else
{
has_vel_limits = 0;
vel_limit = 0.0;
ROS_DEBUG("joint has no velocity limit.");
}
}
else
{
ROS_DEBUG("joint is continuous.");
lower = -M_PI;
upper = M_PI;
has_pos_limits = 0;
if(joint->limits && joint->limits->velocity)
{
has_vel_limits = 1;
vel_limit = joint->limits->velocity;
ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
}
else
{
has_vel_limits = 0;
vel_limit = 0.0;
ROS_DEBUG("joint has no velocity limit.");
}
}
joint_min(seg_it->second.q_nr) = lower;
joint_max(seg_it->second.q_nr) = upper;
joint_vel_max(seg_it->second.q_nr) = vel_limit;
ROS_DEBUG("pos_min = %f, pos_max = %f, vel_max = %f", lower, upper, vel_limit);
info_.joint_names[seg_it->second.q_nr] = joint->name;
info_.limits[seg_it->second.q_nr].joint_name = joint->name;
info_.limits[seg_it->second.q_nr].has_position_limits = has_pos_limits;
info_.limits[seg_it->second.q_nr].min_position = lower;
info_.limits[seg_it->second.q_nr].max_position = upper;
info_.limits[seg_it->second.q_nr].has_velocity_limits = has_vel_limits;
info_.limits[seg_it->second.q_nr].max_velocity = vel_limit;
}
}
}
return true;
}
void computeTemplatedDynamicsForTree(KDL::Tree& twoBranchTree, KDL::Vector& grav, std::vector<kdle::JointState>& jointState,
std::vector<kdle::SegmentState>& linkState, std::vector<kdle::SegmentState>& linkState2)
{
printf("Templated dynamics values for Tree \n");
kdle::transform<kdle::kdl_tree_iterator, kdle::pose> _comp1;
kdle::transform<kdle::kdl_tree_iterator, kdle::twist> _comp2;
kdle::transform<kdle::kdl_tree_iterator, kdle::accTwist> _comp3;
kdle::balance<kdle::kdl_tree_iterator, kdle::force> _comp4;
kdle::project<kdle::kdl_tree_iterator, kdle::wrench> _comp5;
#ifdef VERBOSE_CHECK_MAIN
std::cout << "Transform initial state" << std::endl << linkState[0].X << std::endl;
std::cout << "Twist initial state" << std::endl << linkState[0].Xdot << std::endl;
std::cout << "Acc Twist initial state" << std::endl << linkState[0].Xdotdot << std::endl;
std::cout << "Wrench initial state" << std::endl << linkState[0].F << std::endl << std::endl;
#endif
#ifdef VERBOSE_CHECK_MAIN
std::cout << "Transform L1" << linkState[1].X << std::endl;
std::cout << "Twist L1" << linkState[1].Xdot << std::endl;
std::cout << "Acc Twist L1" << linkState[1].Xdotdot << std::endl;
std::cout << "Wrench L1" << linkState[1].F << std::endl << std::endl;
#endif
#ifdef VERBOSE_CHECK_MAIN
std::cout << "Transform L2" << linkState[2].X << std::endl;
std::cout << "Twist L2" << linkState[2].Xdot << std::endl;
std::cout << "Acc Twist L2" << linkState[2].Xdotdot << std::endl;
std::cout << "Wrench L2" << linkState[2].F << std::endl << std::endl;
#endif
//typedef Composite<kdle::func_ptr(myTestComputation), kdle::func_ptr(myTestComputation) > compositeType0;
typedef kdle::Composite< kdle::transform<kdle::kdl_tree_iterator, kdle::twist>, kdle::transform<kdle::kdl_tree_iterator, kdle::pose> > compositeType1;
typedef kdle::Composite< kdle::balance<kdle::kdl_tree_iterator, kdle::force>, kdle::transform<kdle::kdl_tree_iterator, kdle::accTwist> > compositeType2;
typedef kdle::Composite<compositeType2, compositeType1> compositeType3;
// compositeType1 composite1 = kdle::compose(_comp2, _comp1);
compositeType3 composite2 = kdle::compose(kdle::compose(_comp4, _comp3), kdle::compose(_comp2, _comp1));
//kdle::DFSPolicy<KDL::Tree> mypolicy;
kdle::DFSPolicy<KDL::Tree, kdle::inward> mypolicy1;
kdle::DFSPolicy<KDL::Tree, kdle::outward> mypolicy2;
std::cout << std::endl << std::endl << "FORWARD TRAVERSAL" << std::endl << std::endl;
// traverseGraph(twoBranchTree, composite2, mypolicy2)(jointState, jointState, linkState, linkState2);
traverseGraph(twoBranchTree, composite2, mypolicy2)(jointState, linkState, linkState2);
#ifdef VERBOSE_CHECK_MAIN
std::cout << std::endl << std::endl << "LSTATE" << std::endl << std::endl;
for (unsigned int i = 0; i < twoBranchTree.getNrOfSegments(); i++)
{
std::cout << linkState[i].segmentName << std::endl;
std::cout << std::endl << linkState[i].X << std::endl;
std::cout << linkState[i].Xdot << std::endl;
std::cout << linkState[i].Xdotdot << std::endl;
std::cout << linkState[i].F << std::endl;
}
std::cout << std::endl << std::endl << "LSTATE2" << std::endl << std::endl;
for (unsigned int i = 0; i < twoBranchTree.getNrOfSegments(); i++)
{
std::cout << linkState2[i].segmentName << std::endl;
std::cout << std::endl << linkState2[i].X << std::endl;
std::cout << linkState2[i].Xdot << std::endl;
std::cout << linkState2[i].Xdotdot << std::endl;
std::cout << linkState2[i].F << std::endl;
}
#endif
std::vector<kdle::SegmentState> linkState3;
linkState3.resize(twoBranchTree.getNrOfSegments()+1);
std::cout << std::endl << std::endl << "REVERSE TRAVERSAL" << std::endl << std::endl;
std::vector<kdle::JointState> jstate1;
jstate1.resize(twoBranchTree.getNrOfSegments() + 1);
traverseGraph(twoBranchTree, _comp5, mypolicy1)(jointState, jstate1, linkState2, linkState3);
//version 1 traversal
//traverseGraph(twoBranchTree, kdl_extensions::func_ptr(myTestComputation), mypolicy)(1, 2, 3);
#ifdef VERBOSE_CHECK_MAIN
std::cout << std::endl << std::endl << "LSTATE3" << std::endl << std::endl;
for (KDL::SegmentMap::const_reverse_iterator iter = twoBranchTree.getSegments().rbegin(); iter != twoBranchTree.getSegments().rend(); ++iter)
{
std::cout << std::endl << iter->first << std::endl;
std::cout << linkState3[iter->second.q_nr].X << std::endl;
std::cout << linkState3[iter->second.q_nr].Xdot << std::endl;
std::cout << linkState3[iter->second.q_nr].Xdotdot << std::endl;
std::cout << linkState3[iter->second.q_nr].F << std::endl;
std::cout << "Joint index and torque " << iter->second.q_nr << " " << jstate1[iter->second.q_nr].torque << std::endl;
}
#endif
return;
}
void IkSolver::init(const KDL::Tree& tree,
const std::vector<std::string>& endpoint_names,
const std::vector<EndpointCoupling>& endpoint_couplings)
{
// Preconditions
assert(endpoint_names.size() == endpoint_couplings.size() && "endpoints and coupling vectors size mismatch");
// TODO: Verify that all endpoints are contained in tree
// Enpoint names vector
endpoint_names_ = endpoint_names;
// Problem size
const size_t q_dim = tree.getNrOfJoints(); // Joint space dimension
size_t x_dim = 0; // Task space dimension, value assigned below
// Populate coupled directions vector
coupled_dirs_.resize(endpoint_names_.size());
for (size_t i = 0; i < coupled_dirs_.size(); ++i)
{
if ((endpoint_couplings[i] & Position) == Position)
{
coupled_dirs_[i].push_back(0);
coupled_dirs_[i].push_back(1);
coupled_dirs_[i].push_back(2);
x_dim += 3;
}
if ((endpoint_couplings[i] & Orientation) == Orientation)
{
coupled_dirs_[i].push_back(3);
coupled_dirs_[i].push_back(4);
coupled_dirs_[i].push_back(5);
x_dim += 3;
}
}
// Initialize kinematics solvers
fk_solver_.reset(new FkSolver(tree));
jac_solver_.reset(new JacSolver(tree));
inverter_.reset(new Inverter(x_dim, q_dim));
// Matrix inversion parameters TODO: Expose!
inverter_->setLsInverseThreshold(1e-5); // NOTE: Magic values
inverter_->setDlsInverseThreshold(1e-4);
inverter_->setMaxDamping(0.05);
// Default values of position solver parameters
delta_twist_max_ = Eigen::NumTraits<double>::highest();
delta_joint_pos_max_ = Eigen::NumTraits<double>::highest();
velik_gain_ = 1.0;
eps_ = Eigen::NumTraits<double>::epsilon();
max_iter_ = 1;
// Populate map from joint names to KDL tree indices
joint_name_to_idx_.clear();
const KDL::SegmentMap& tree_segments = tree.getSegments();
for (KDL::SegmentMap::const_iterator it = tree_segments.begin(); it != tree_segments.end(); ++it)
{
const KDL::Joint& joint = it->second.segment.getJoint();
if (joint.getType() != KDL::Joint::None)
{
joint_name_to_idx_[joint.getName()] = it->second.q_nr;
}
}
// Joint space weights updater
limits_avoider_.reset(new JointPositionLimitsAvoider(q_dim));
limits_avoider_->setSmoothing(0.8); // NOTE: Magic value
// Preallocate IK resources
delta_twist_ = VectorXd::Zero(x_dim);
delta_q_ = VectorXd::Zero(q_dim);
q_tmp_ = VectorXd::Zero(q_dim);
jacobian_ = Eigen::MatrixXd(x_dim, q_dim);
jacobian_tmp_ = KDL::Jacobian(q_dim);
Wx_ = VectorXd::Ones(x_dim);
q_posture_ = KDL::JntArray(q_dim);
nullspace_projector_ = Eigen::MatrixXd(q_dim, q_dim);
identity_qdim_ = Eigen::MatrixXd::Identity(q_dim, q_dim);
}