UR5KinematicsPlugin fails to plan in Cartesian space on Clearpath Husky equipped with UR5 [closed]

edit

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/uni... https://github.com/ros-industrial/uni... https://github.com/ros-industrial/uni... https://github.com/ros-industrial/uni... https://github.com/ros-industrial/uni...

The response to these tickets was to rewrite portions of ur_kimematic/UR5KinematicsPlugin 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 ... (more)