KDL kinematics plugin's forward kinematics

[eferre89]

Description

Up to my understanding, this is the current implementation of getPositionFK for the KDL kinematics plugin:

bool valid = true;
for (unsigned int i = 0; i < poses.size(); i++)
{
  if (fk_solver_->JntToCart(jnt_pos_in, p_out) >= 0)
  {
    poses[i] = tf2::toMsg(p_out);
  }
  else
  {
    ROS_ERROR_NAMED("kdl", "Could not compute FK for %s", link_names[i].c_str());
    valid = false;
  }
}
return valid;

It seems to me that, at each iteration, poses[i] always takes the same value, regardless of i (regardless of the specific links in the link_names parameter). I think that the purpose of this for loop is to give each pose[i] the pose of the link we are computing FK for.

Maybe the programmer intention was to provide an implementation that resembles the following snippet:

bool valid = true;

for (unsigned int i = 0; i < poses.size(); i++)
{
    KDL::Frame p_out;

    if (fk_solver_->JntToCart(jnt_pos_in, p_out, link_indices[i]+1) >= 0)
    {
        poses[i] = tf2::toMsg(p_out);
    }
    else
    {
        ROS_ERROR_NAMED("nsp", "Could not compute FK for %s", link_names[i].c_str());
        valid = false;
    }
}

return valid;

where link_indices is computed from link_names and the joint model group.

Your environment

• ROS Distro: Melodic
• OS Version: Ubuntu 18.04
• Binary build
• Maybe release version is not relevant, since I'm referring to source code

Steps to reproduce

For example you can assign link_names = getLinkNames() (all links) and call getPositionFK(link_names, joint_angles, poses).

Expected behaviour

poses should contain forward kinematics for each link passed in link_names.

Actual behaviour

poses contains forward kinematics for the last link repeated link_names.size() times.

Backtrace or Console output

N/A

[welcome]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[simonschmeisser]

This code indeed looks suspicious, please open a PR with your changes and ideally also a test like you described in "Steps to reproduce"

Personally I would rather use a RobotState, set the positions (joint values), call update and then get the FK positions from getGlobalLinkPose

[rhaschke]

You are right, the code currently doesn't handle multiple link names, primarily because the KDL plugin doesn't support non-chain kinematic trees:
https://github.com/ros-planning/moveit/blob/f69a324617a14dc5f90b897d8bf6ae9c995960fd/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp#L146-L150

@eferre89, do you just want to solve the FK? In this case, your proposed approach should be easily feasible and we welcome a corresponding PR. If you are asking for IK as well, this will require much more work, because the Jacobian matrix computation should be extended appropriately... However, we would very much welcome a PR on this as well 😉

[eferre89]

Thank you for your feedback, @simonschmeisser and @rhaschke. I'm going to submit a PR for FK, with related test case.

[eferre89]

This is the fix I'd propose:

https://github.com/unisa-acg/moveit/blob/7a71da79a1312f40913a892a8886999f59bc8318/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp#L542-L559

I added the following test case:

https://github.com/unisa-acg/moveit/blob/7a71da79a1312f40913a892a8886999f59bc8318/moveit_kinematics/test/test_kinematics_plugin.cpp#L330-L346

I have no issues with the Panda robot, that does not have any fake link, while the Fanuc robot does:

https://github.com/ros-planning/moveit_resources/blob/ae3fbec262db7aaeb892b813ef210e39a09693ae/fanuc_description/urdf/fanuc.urdf#L126

and this makes the test fail. Is there any specific reason why this additional link has been included in the Fanuc robot description? It makes the Fanuc kinematics a tree and not a chain. Indeed, we already know that the KDL plugin does not work with trees, so, in my opinion, we should not be testing the KDL kinematics plugin on the Fanuc robot. What do you think?

[v4hn]

and this makes the test fail. Is there any specific reason why this additional link has been included in the Fanuc robot description? It makes the Fanuc kinematics a tree and not a chain.

You highlight the link tool0 which is a fixed link extending from link_6. I don't know why you would say the kinematics become a tree because of that? The tip frame tool0 is a ros-industrial convention as far as I recall.

Looking at rqt_tf_tree I notice that base is actually a child link of base_link, which seems wrong.
I don't think it would affect anything here, but we might want to consider switching them.

[gavanderhoorn]

@v4hn wrote:

I notice that base is actually a child link of base_link, which seems wrong.

No, that's actually on purpose:

base_link shall follow ROS conventions for both chirality and orientation as set forth in REP 103. Discrepancies between a controller's internal Cartesian (world) frame and REP 103 shall be resolved by defining the correct transform between base_link (or any other convenient frame) and the base frame (see base).

[..]

The base frame shall be coincident with the default Cartesian coordinate system as defined by the industrial robot controller. Its purpose is to allow users to transform poses from a ROS application into the Cartesian base frame of the robot.

In the ros-industrial/fanuc model which was the source of the one here in MoveIt, the World coordinate frame of the controller is attached to the main kinematic chain of the model by means of the base_link->base transform.

[rhaschke]

It makes the Fanuc kinematics a tree and not a chain.

I agree with @v4hn that the Fanuc is a simple kinematic chain. There is no reason to drop a test for Fanuc.

[eferre89]

Hello everyone, sorry, I misread the URDF, I apologize for that. I agree with you that the joint model group manipulator we are considering is a chain and not a tree. I might have understood where the problem is, I'll try to explain to collect your opinion and then try to "fix my fix" ;P

When I call

int link_index = joint_model_group_->getLinkModel(link_names[i])->getLinkIndex();

the index I get is not the one in the group, but the one in the model. In fact, when I ask for index of tool0, I get 8, but the KDL chain correctly has no link beyond index 7. I might need to code a function that retrieves the link index in the group, if the API does not provide one already.

[rhaschke]

Usually kinematics plugins are tailored towards a fixed subtree / subchain of the RobotModel. In this case, the mapping from link names to an internal id/index can be established once during initialization.
In your case, I suggest computing this map once in initialize(), not for each call to getPositionFK().