How good should inverse kinematics with KDL or Trac_ik work?

Hello List,

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

  # Set the target pose.
target_pose = PoseStamped()
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
traj = right_arm.plan()
right_arm.execute(traj)


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.

edit retag close merge delete

How are you determining that the poses are perfectly reachable?

( 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 get_arm_pose.py 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.

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

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

Sort by » oldest newest most voted

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: https://github.com/tu-darmstadt-ros-p...

And a example robot configuration using the options here: https://github.com/tu-darmstadt-ros-p...

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

( 2016-09-12 08:53:17 -0500 )edit
1

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

( 2016-09-12 10:37:47 -0500 )edit
1

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.

( 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!

( 2016-09-13 01:48:01 -0500 )edit

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

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?

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

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

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

https://bitbucket.org/traclabs/trac_i...

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

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.

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