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.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
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()
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.
Thanks in advance, Sietse
How are you determining that the poses are perfectly reachable?
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.
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.