Ask Your Question
0

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

asked 2017-04-05 10:42:53 -0500

olchandra gravatar image

updated 2017-04-06 10:36:06 -0500

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 flag offensive close merge delete

Comments

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).

Stefan Kohlbrecher gravatar image Stefan Kohlbrecher  ( 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.

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

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-10-19 10:43:03 -0500

billus gravatar image

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;
tree.getChain("base_link", "xtion_wrist_link", 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]

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2017-04-05 10:42:53 -0500

Seen: 468 times

Last updated: Apr 06 '17