# Why am I getting different result from KDL forward kinematics and TF transform enquiry?

Hi. I am trying to perform forward kinematics for one of the arm using KDL Chainfksolverpos_recursive. The code works fine and I am getting a reasonable result. But when I compare the results with the "rosrun tf tf_echo /frame1 /frame2" result for the same root and tip links, the results are different. Has anyone come across this problem and know the reason for this discrepancy.

bool gotTree=kdl_parser::treeFromFile (file_path, tree_);
if (!gotTree)
ROS_ERROR("Failed to parse urdf file");
else
{
ROS_DEBUG_STREAM("Successfully created kdl tree");
std::cerr << "number of joint: " << tree_.getNrOfJoints() << std::endl;
std::cerr << "number of links: " << tree_.getNrOfSegments() << std::endl;
}
const sensor_msgs::JointStateConstPtr initJoints = ros::topic::waitForMessage<sensor_msgs::JointState>("/robot/joint_states", n);

ROS_INFO("Joint States published");
// ROS_INFO("length of joint states %d", initJoints->name.size());

tree_.getChain("base", "right_upper_elbow", chain_);
// tree_.getChain("base", "right_wrist", chain_);

if (chain_.getNrOfJoints() == 0)
{
ROS_INFO("Failed to initialize kinematic chain");
}
else
{
ROS_INFO("No of joints in the chain: %u", chain_.getNrOfJoints());
}
KDL::JntArray joint_pos= KDL::JntArray(chain_.getNrOfJoints());
// joint_pos.resize(chain_.getNrOfSegments());
KDL::Frame cart_pos;
// ROS_INFO("total Joints(i): %d",  initJoints->name.size());
ROS_INFO("chain_.getNrOfSegments(j): %d",  chain_.getNrOfSegments());
ROS_INFO("chain_.getNrOfJoints(): %d",  chain_.getNrOfJoints());
int k=0;
for (int i=0; i<initJoints->name.size(); i++)
{
for(int j=0; j< chain_.getNrOfSegments(); j++)
{
// ROS_INFO("just checking1: i= %d, j:%d", i, j);
KDL::Segment segmnt=chain_.getSegment(j);

ROS_INFO("%d.getName(): %s", j, segmnt.getJoint().getName().c_str());
if (!segmnt.getJoint().getName().compare(initJoints->name[i]) )
{
if(segmnt.getJoint().getType()!=KDL::Joint::None)
{
ROS_INFO("segment %s went fine", segmnt.getJoint().getName().c_str());
joint_pos(k)=initJoints->position[i];
ROS_INFO("k: %d, name: %s, initJoints->position[%d]: %f",k,  initJoints->name[i].c_str(), i, initJoints->position[i]);
k++;
}
}
// ROS_INFO("just checking3: i= %d, j:%d", i, j);
}
}
KDL::ChainFkSolverPos_recursive fksolver = KDL::ChainFkSolverPos_recursive(chain_);
KDL::Frame cartpos;
bool kinematics_status;

kinematics_status = fksolver.JntToCart(joint_pos,cartpos);
if(kinematics_status>=0)
{
// ROS_INFO("fk: x: %f, y: %f, z: %f", cartpos.p[0], cartpos.p[1], cartpos.p[2]);
ROS_INFO("fk: x: %f, y: %f, z: %f", cartpos.p.x(), cartpos.p.y(), cartpos.p.z());
}
else
ROS_INFO("JntToCart did not work :(");


edit retag close merge delete

An example would be helpful I think. Is there some small discrepancy or are both results completely different? In any case, both KDL and tf are used in a lot of projects, so it is likely that something is off with the invocation (for which, again, code would be good to see).

( 2017-04-06 09:26:01 -0500 )edit

In the code that I have added to the question, I am using Baxter robot in gazebo. The output from the JntToCart seems logical but not the same. I have tried with different tip links and the discrepancy increases with the number of joints in the chain. For just one joint it is almost the same as TF.

( 2017-04-06 10:41:12 -0500 )edit

Sort by » oldest newest most voted

Hi,

It seems to be a bit late but if anyone else need this. I followed your method and after getting bad results too, I changed two things.

The first, which I think cause your problem is the condition !segmnt.getJoint().getName().compare(initJoints->name[i]), which I transform as segmnt.getJoint().getName().compare(initJoints->name[i] == 0), because String1.compare(String2) does not return a bool.

The second, which is not important for good results (but I'm telling it to understand my code below), is the order of your loops. If your joint_state message, like me, is not necessarily ordered like the chain and could include other joints, it is more "optimized" i think to invert these two loops "for" and break when you find the match. Anyway, this doesn't really matter and this is my code which seems to work :

    urdf::Model model;
if (!model.initParam("robot_description"))
return -1;

KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(model, tree)) {
ROS_ERROR("Failed to extract kdl tree from xml robot description");
return -1;
}

const sensor_msgs::JointStateConstPtr initJoints = ros::topic::waitForMessage<sensor_msgs::JointState>("/joint_states", n);

KDL::Chain chain;

KDL::JntArray joint_pos= KDL::JntArray(chain.getNrOfJoints());

int k = 0;
for(int i = 0; i < chain.getNrOfSegments(); i++)
{
KDL::Segment segment = chain.getSegment(i);
for (int j = 0; j < initJoints->name.size(); j++)
{
if (segment.getJoint().getName().compare(initJoints->name[j]) == 0)
{
if(segment.getJoint().getType() != KDL::Joint::None)
{
joint_pos(k) = initJoints->position[j];
k++;
break;
}
}
}
}

KDL::ChainFkSolverPos_recursive fksolver = KDL::ChainFkSolverPos_recursive(chain);

KDL::Frame frame;
bool kinematics_status;

kinematics_status = fksolver.JntToCart(joint_pos, frame);
if (kinematics_status >= 0)
{
cout << "fk ->    x : " << frame.p.x() << " y : " << frame.p.y() << " z : " << frame.p.z() << endl;
}
else
cout << "JntToCart did not work" << kinematics_status << endl;


code output : fk -> x : 0.426696 y : -0.426238 z : 0.887562

rrun tf tf echo output : At time 122.154 - Translation: [0.427, -0.426, 0.888]

more