UR5KinematicsPlugin fails to plan in Cartesian space

[CMobley7]

To Whom It May Concern,

I ran across issues #127, #155, #184, #202, and #241 in which portions of ur_kimematic/UR5KinematicsPlugin were rewritten to allow the ur5_arm 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 ur_kimematic/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 husky_ur5_planning_execution.launch rather than issues with the planner itself.

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?

Thanks for any help you are able to give.

Best,
CMobley7

[gavanderhoorn]

Cross-posted: UR5KinematicsPlugin fails to plan in Cartesian space on Clearpath Husky equipped with UR5.

[CMobley7]

@gavanderhoorn should I have just posted here?

[CMobley7]

I just install trac-ik-kinematics-plugin and changed my kinematics.yaml file from

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

to

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

This fixed both the prefix error and has successfully solved every IK problem I have given it. So, the issue may lie in the planner. Consequently, my issue is solved but the overall issue probably isn't.

[gavanderhoorn]

Tagging this as a possible issue to investigate on WRID @ipa-nhg.

[schornakj]

It looks like this was more or less successfully addressed in husky/husky/issues/32.

I took a stab at replicating this while using the UR5 demo launch file. I tweaked the provided script to use the planning groups set up by the launch file, and to correct some important omitted parts like the package imports and the node initialization.

One problem is that the target point is outside the workspace of the UR5, which is why planning failed when I ran this code. However, I think that the source of the problem @CMobley7 was trying to solve was caused by some behind-the-scenes configuration and parameter setup which was done correctly in demo.launch but possibly not in their code.

roslaunch ur5_moveit_config demo.launch

Running the script "as-is":

import sys
import rospy
import moveit_commander
from geometry_msgs.msg import PoseStamped

rospy.init_node('ur_test_node',
                anonymous=True)

# Initialize the move group for the arm
ur5_arm = moveit_commander.MoveGroupCommander("manipulator")

# Get the name of the end-effector link
end_effector_link = ur5_arm.get_end_effector_link()
print "EE", end_effector_link

# Set the reference frame for pose targets
reference_frame = "/world"

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

moveit_commander script output:

[ INFO] [1531345351.522262836]: Loading robot model 'ur5'...
[ WARN] [1531345351.522344924]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not match the URDF frame 'world'
[ INFO] [1531345351.522389695]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1531345351.605858255]: Loading robot model 'ur5'...
[ WARN] [1531345351.605885301]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not match the URDF frame 'world'
[ INFO] [1531345351.605894549]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1531345352.461248465]: 
Deprecation warning: Trajectory execution service is deprecated (was replaced by an action).
Replace 'MoveGroupExecuteService' with 'MoveGroupExecuteTrajectoryAction' in move_group.launch
[ INFO] [1531345352.470502620]: Ready to take commands for planning group manipulator.
[ INFO] [1531345352.470624030]: Replanning: yes
[ WARN] [1531345357.546190722]: Fail: ABORTED: No motion plan found. No execution attempted.

Output from RVIZ console:

[ INFO] [1531345952.441763943]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1531345952.445423840]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1531345952.446524637]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1531345957.447696329]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1531345957.447772200]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1531345957.447790540]: No solution found after 5.001621 seconds
[ INFO] [1531345957.451973384]: Unable to solve the planning problem
[ INFO] [1531345957.453636968]: Received new trajectory execution service request...
[ WARN] [1531345957.453678534]: The trajectory to execute is empty

Running the script after changing the target pose:

target_pose.pose.position.x = .7 #hole_pose.position.x
target_pose.pose.position.y = .1 #hole_pose.position.y
target_pose.pose.position.z = .7 #hole_pose.position.z

moveit_commander script output:

[ INFO] [1531345781.471457489]: Loading robot model 'ur5'...
[ WARN] [1531345781.471540366]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not match the URDF frame 'world'
[ INFO] [1531345781.471585038]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1531345781.539599159]: Loading robot model 'ur5'...
[ WARN] [1531345781.539629378]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not match the URDF frame 'world'
[ INFO] [1531345781.539642060]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1531345782.504974892]: 
Deprecation warning: Trajectory execution service is deprecated (was replaced by an action).
Replace 'MoveGroupExecuteService' with 'MoveGroupExecuteTrajectoryAction' in move_group.launch
[ INFO] [1531345782.513952474]: Ready to take commands for planning group manipulator.
[ INFO] [1531345782.514071440]: Replanning: yes

Output from Rviz console:

[ INFO] [1531345880.565580537]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1531345880.569188273]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1531345880.570291332]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1531345880.909209551]: RRTConnect: Created 6 states (4 start + 2 goal)
[ INFO] [1531345880.909239437]: Solution found in 0.339302 seconds
[ INFO] [1531345880.931146122]: SimpleSetup: Path simplification took 0.021842 seconds and changed from 3 to 4 states
[ INFO] [1531345880.934497802]: Received new trajectory execution service request...
[ INFO] [1531345880.965122432]: Fake execution of trajectory
[ INFO] [1531345884.765649851]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1531345884.765868776]: Execution completed: SUCCEEDED

[gavanderhoorn]

With the extensive analysis by @schornakj I believe we can close this as something too localised. The plugin itself should be OK (to the extent needed for the use-case described), but local configuration seems to have interfered with its operation.

If it turns out this is still a problem for the OP or other users, we can re-open.