ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

How good should inverse kinematics with KDL or Trac_ik work?

asked 2016-09-09 07:23:00 -0500

Sietse gravatar image

updated 2016-09-14 01:46:56 -0500

Hello List,

I have a basic 5DOF arm that basically works with moveit! and inverse kinematics. I adapted the script from the ROS by Example book and use phrases like:

  # Set the target pose.                                                                                                        
    target_pose = PoseStamped()
    target_pose.header.frame_id = reference_frame
    target_pose.header.stamp =
    target_pose.pose.position.x = -0.274
    target_pose.pose.position.y = -0.019
    target_pose.pose.position.z = 0.306
    target_pose.pose.orientation.x = 0.057
    target_pose.pose.orientation.y = 0.047
    target_pose.pose.orientation.z = 0.99
    target_pose.pose.orientation.w =  0.113                                                                                                                
    right_arm.set_pose_target(target_pose, end_effector_link)
    traj = right_arm.plan()

But the problem is that if I give perfectly reachable poses in the set_pose_target function, on average only 5 out of 6 calls results in success. The others result in

LBKPIECE1: Unable to sample any valid states for goal tree

Why does it fail so often?

I use a Core2 quad CPU so I would have thought that 5 seconds should be long enough to find the solution. This is both with KDL and Trac_ik. Am I missing something?

As said, the poses are perfectly reachable, on average 5 out of 6 times the solution is found.

UPDATE: Solved. The positions I used were not perfectly reachable because I left out some decimals in copying the values to the test program. See some comments with the answers below.

Thanks in advance, Sietse

edit retag flag offensive close merge delete


How are you determining that the poses are perfectly reachable?

cbames gravatar image cbames  ( 2016-09-12 08:38:05 -0500 )edit

I have a program (arbotix_gui) to set the joints and visualise with rviz. From the ROS_by_example book I use that gives the eef position. I think that should work. See also my comment below where trac_ik uses the ik_test program where 995 out of 1000 attemps succeed.

Sietse gravatar image Sietse  ( 2016-09-12 08:54:08 -0500 )edit

I believe the trac_ik sample uises 1e-3 eps, which again points to the fact that your goal pose may be be exactly reachable after you round it to 3 significant digits.

pbeeson gravatar image pbeeson  ( 2016-09-12 10:47:35 -0500 )edit

3 Answers

Sort by ยป oldest newest most voted

answered 2016-09-12 08:18:36 -0500

As @Airuno2L stated, the numerical solvers are optimized for with >= 6 DOF. A solution that worked well for us in some applications is performing some slight modifications on trac_ik to make it work better with < 6 DOF systems. You can find a fork here:

And a example robot configuration using the options here:

edit flag offensive delete link more


Trac_ik has a test program ik_tests that I just used on my system. With 1000 random (reachable) positioins trac_ik solves around 995! So that's OK. I now don't understand why this does not work with my program....

Sietse gravatar image Sietse  ( 2016-09-12 08:53:17 -0500 )edit

It could be a rounding issue, where the pose is not EXACTLY reachable, but one close to it is, and your joint values are getting rounded by whatever FK solution you are using. 1) Have you tried increasing the EPS in the trac_ik plugin from 1e-5 to something higher? This is hardcoded in plugin source

pbeeson gravatar image pbeeson  ( 2016-09-12 10:37:47 -0500 )edit

2) You could take the joints you know define this goal pose, and run KDL's FK solver for the chain and see if the pose you get out matches the pose you are sending to IK.

pbeeson gravatar image pbeeson  ( 2016-09-12 10:38:19 -0500 )edit

Yes, I did copy the values only to about 3 decimals in the (naive) assumption that a position so close would be reachable anyhow. I now copied the values more complete and it now almost ALWAYS works. So that was the problem, thanks!

Sietse gravatar image Sietse  ( 2016-09-13 01:48:01 -0500 )edit

answered 2016-09-12 07:56:55 -0500

Airuno2L gravatar image

The numerical solver that KDL and Trak_IK use are really just meant to work with arms that are 6 DOF or more. The fact that you're even getting solutions 5 out of 6 times is a little surprising to me and might just be a bit of luck.

In the KDL mailing lists you can find some suggested workarounds to using a 5 DOF arm, but personally I've tried them and haven't had any luck.

The best suggestion I can make from my experience is to use the IKFast solver from OpenRave and use the TranslationDirection5D solver type. Luckily this is something common enough that there is a nice way to do it with Moveit and there is a tutorial that describes the process here. Note that instead of --iktype=transform6d you'll want to use --iktype=translationdirection5d (I think it is translationdirection5d, the exact text might be something similar but different).

edit flag offensive delete link more


I've been trying that for some time, but I failed to get ikfast to generate my C++ code. I've ask about that on the openrave list, see here, and on this list. But no solution yet(: Any ideas?

Sietse gravatar image Sietse  ( 2016-09-12 08:42:59 -0500 )edit

I wouldn't say that these numerical solvers are MEANT to work only with arms of 6 chains or more. In fact, in my tests, I've run with chains of 2,3,4,5 and it works fine. The issue sounds more like the "achievable goal" isn't actually achievable to 1e-5 precision (which IS what MoveIt! assumes).

pbeeson gravatar image pbeeson  ( 2016-09-12 10:39:59 -0500 )edit

answered 2016-09-12 11:13:48 -0500

pbeeson gravatar image

This is probably something that can be fixed by using this:

I am REALLY busy the next 2 weeks, but I can prioritize this after that.

edit flag offensive delete link more


Thanks! One final question: I noticed that when using Moveit! the function set_joint_value_target cannot be used with the third parameter that makes it also do approximations. Is there a way to do something simular when using trac_ik? Would be very convenient for arms with < 6 DOF.

Sietse gravatar image Sietse  ( 2016-09-13 04:02:43 -0500 )edit

Question Tools



Asked: 2016-09-09 07:23:00 -0500

Seen: 1,477 times

Last updated: Sep 14 '16