UR5e moveit+ros target pose error

asked 2022-06-08 04:34:59 -0600

francirrapi gravatar image

updated 2022-06-08 05:04:12 -0600

Hi there, I am working on a UR5e robot with a gripper attached and sending pose target through a ros pc (ubuntu with melodic) + moveit. As I am using the ur5e_moveit_config package of fmauch_universal_robot repository, and there is not the robotiq attached, I modified the urdf "ur_macro.xacro" file in the flange link origin xyz with a 0.18 in the z axis. I tried the IK after this change and seems to work very well. But with some eef target positions, I don't understand why, the robot reach or try to reach a totally different position from the asked one.

This is the goal pose that I want to reach:

quat = quaternion_from_euler(math.radians(180), math.radians(0), math.radians(0))

self.goal = geometry_msgs.msg.Pose()
self.goal.position.x = 0.01165
self.goal.position.y = 0.4229
self.goal.position.z = -0.67351
self.goal.orientation.x = quat[0]
self.goal.orientation.y = quat[1]
self.goal.orientation.z = quat[2]
self.goal.orientation.w = quat[3]

Below I show the code I use for sending to the robot the desired pose to reach:

def go_to_pose_goal(self):

        print("POSE GOAL:")


        self.move_group.set_joint_value_target(self.goal, True)


        plan = self.move_group.go(wait=True)

        rospy.loginfo("move_group: goal sent")

        curr_pose = self.move_group.get_current_pose().pose
        curr_rpy = self.move_group.get_current_rpy()
        pose = [curr_pose.position.x, curr_pose.position.y, curr_pose.position.z, curr_rpy[0], curr_rpy[1], curr_rpy[2]]

        print(" ")
        print("CURRENT POSE:")
        print(" ")

The trajectory is fully executed from the robot, without errors or "ABORTED" messages, but is different from the desired one. This is the output of the program:

CURRENT POSE: [-0.0025498964402632233, 0.27602545190178573, 0.22645338195763132, 3.1413060975447378, 0.001050279172657526, -0.045771220441788774]

POSE GOAL: [0.01165, 0.4229, -0.67351, 3.141592653589793, 0.0, 0.0] [INFO] [1654603061.934200]: move_group: goal sent

CURRENT POSE: [-0.15265319399686922, 0.3118501764751299, 0.1503329903011287, -3.140611349653064, -0.0006975548158330148, 0.000968528534922659]

Could you help me to solve this problem?

edit retag flag offensive close merge delete