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 :(");
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).
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.