To Whom It May Concern,

I ran across the following ros-industrial ticket in trying to solve the inverse kinematic issue I am having.

https://github.com/ros-industrial/universal_robot/issues/127 https://github.com/ros-industrial/universal_robot/issues/155 https://github.com/ros-industrial/universal_robot/issues/184 https://github.com/ros-industrial/universal_robot/issues/202 https://github.com/ros-industrial/universal_robot/issues/241

The response to these tickets was to rewrite portions of urkimematic/UR5KinematicsPlugin to allow the ur5arm to better plan and execute.

However, the main topic was RVIZ. RVIZ to the best of my knowledge is using forward kinematics due to the fact it already knows the ending joint angles. I attempted to perform inverse kinematic planning using Moveit!’s Python interface with the code below on a Clearpath Husky equiped with a UR5.

# Initialize the move group for the arm
ur5_arm = moveit_commander.MoveGroupCommander("ur5_arm")
# Get the name of the end-effector link
end_effector_link = ur5_arm.get_end_effector_link()
# Set the reference frame for pose targets
 reference_frame = "/base_link"
# Set the arm reference frame accordingly
ur5_arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
 ur5_arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
ur5_arm.set_goal_position_tolerance(0.01)
ur5_arm.set_goal_orientation_tolerance(0.05)
#Move the end effecor to the x , y, z positon
#Set the target pose in the base_link frame
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 1.0 #hole_pose.position.x
target_pose.pose.position.y = .1 #hole_pose.position.y
target_pose.pose.position.z = .7 #hole_pose.position.z
target_pose.pose.orientation.x = 0 #hole_pose.orientation.x
target_pose.pose.orientation.y = 0 #hole_pose.orientation.y
target_pose.pose.orientation.z = 0 #hole_pose.orientation.z
target_pose.pose.orientation.w = 1 #hole_pose.orientation.w
# Set the start state to the current state
ur5_arm.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
ur5_arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = ur5_arm.plan()
# Execute the planned trajectory
ur5_arm.execute(traj)
# Pause for a second
rospy.sleep(1)

However, the code gives the following errors:

[ERROR] [1467334302.962946004, 615.710000000]: Kin chain provided in model doesn't contain standard UR joint 'shoulder_lift_joint'.
[ERROR] [1467334302.963030369, 615.710000000]: Kinematics solver of type 'ur_kinematics/UR5KinematicsPlugin' could not be initialized for group 'ur5_arm'
[ERROR] [1467334302.963234727, 615.710000000]: Kinematics solver could not be instantiated for joint group ur5_arm.
[ INFO] [1467334303.757624243, 616.500000000]: Ready to take MoveGroup commands for group ur5_arm.
[ INFO] [1467334303.757756298, 616.500000000]: Replanning: yes
[ WARN] [1467334307.237704997, 619.980000000]: Fail: ABORTED: No motion plan found. No execution attempted.

The prefix errors do not seem to affect the planner in solving forward kinematic motion problem. As I have used similar code to go to group states set in the srdf, as well as other random valid joint configurations. However, when specifying a Cartesian point, the planner always fails. After reading the posts regarding urkimematic/UR5KinematicsPlugin, I think the problem is more likely that my code is not using the same configuration in regards to using limited joint angles when planning and not setting up the same namespace as huskyur5planningexecution.launch rather than the planner not being very good.

Should setting up a launch file like https://github.com/husky/husky/blob/indigo-devel/husky_ur5_moveit_config/launch/moveit_rviz.launch solve the issue. If so, are all the lines necessary and I just need to replace the RVIZ node with my appropriate node? Or is the issue more likely caused by the planner itself? If the latter, have you heard of trac_ik from TRAClabs. Their website quotes a 99.55 % success rate with an average time of .42ms, with similar performance for the UR10. Is this planner more likely to work?

Thanks for any help you are able to give.

Best, CMobley7

ur5_arm:
  kinematics_solver: ur_kinematics/UR5KinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  kinematics_solver_attempts: 3

ur5_arm:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_timeout: 0.05
  solve_type: Manipulation1