Inconsistent forward kinematics result

[kamikat]

Thank you for the fantastic library with an out-of-box ikfast integration experience.

I'm using the library with UniversalRobots/Universal_Robots_ROS_Driver and ur5e_moveit_config on UR5e robot.

I use IK result from the library for basic joint-based motion and found there always be some "offset" between target pose and the actual pose after motion execution.

So I checked the forward kinematics result against the one produced by moveit /compute_fk interface with the following script:

import math
import rospy
import moveit_commander
import moveit_msgs.srv

from ur_ikfast import ur_kinematics

rospy.init_node("fk_test")

joint_angles = [-3.1, -1.6, 1.6, -1.6, -1.6, 0.]

move_group = moveit_commander.MoveGroupCommander("manipulator")
compute_fk = rospy.ServiceProxy("compute_fk", moveit_msgs.srv.GetPositionFK)
req = moveit_msgs.srv.GetPositionFKRequest()
req.header.frame_id = "base_link"
req.fk_link_names = ["tool0"]  # <- the end effector?
req.robot_state = move_group.get_current_state()
req.robot_state.joint_state.position = joint_angles
moveit_pose = compute_fk(req).pose_stamped[0].pose
print("-- compute_fk --")
print(moveit_pose)

ki = ur_kinematics.URKinematics("ur5e")
ikfast_pose = ki.forward(joint_angles)
print("-- ur_ikfast --")
print(ikfast_pose)

d = math.dist(
    [moveit_pose.position.x, moveit_pose.position.y, moveit_pose.position.z], ikfast_pose[:3])
print("---")
print("distance:", d)

Produces following result:

-- compute_fk --
position:
  x: -0.4769431713580158
  y: -0.14787343593460026
  z: 0.49045141919270163
orientation:
  x: -0.694025987237262
  y: -0.7196358216867514
  z: -0.0038895792832481426
  w: 0.02090656254809776
-- ur_ikfast --
[-4.76598591e-01 -1.51027992e-01  4.90823984e-01  6.92255947e-01
  7.21356638e-01 -4.29355962e-04 -2.06427328e-02]
---
distance: 0.003195116089384994

How can I fix this?

[cambel]

Hi @kamikat

Unfortunately, the short answer is that there is no fix. This IKFast library only works for the ideal robot model. In practice, the real robot is imperfect and has some offsets on each joint, so a calibration procedure is required to obtain such values when using the ROS driver library. I don't know if it is possible to generate a custom IKFast dataset for a UR model + calibration data. You can find more info in this thread https://github.com/ros-industrial/universal_robot/issues/564

For millimetric precision, you will need to use a different IK solver, such as Trac-IK, that can account for the model with offsets easily by just loading it from the robot_description parameter.

[kamikat]

Thanks for help.

I've tried compiling IKFast with calibrated URDF but the compilation failed (the limitation addressed by @captain-yoshi).

Using trac-ik on my calibrated model cannot always get a solution (solve rate <10%). Then I came up with a combined approach by using ikfast result as search seed of trac-ik and use a bounded search for final result. The combined approach get >90% solve rate on my calibrated model.

[cambel]

That seems odd to me. Do you have a very constrained environment or something like that? I have been using Trac-ik a lot and have not had such issues with low solve rates. In any case, combining IK-FAST with TRAC-IK seems pretty useful.