My robot cannot pick a target by moveit

asked 2019-05-24 06:42:34 -0600

balabala gravatar image

updated 2019-05-24 07:21:32 -0600

I want use moveit to control my robot for a simple pick_and_plce task in rviz, and I use ur10 with a robotiq85 gripper. But it plans always fail. The information shows that there is a contact between 'target' and 'wrist_2_link' (the link of ur10) and the IK was failed. So the robot cannot plan to pick and it didn't move.I changed the place of target for some times. but the results were same. I don't know what's wrong with it. the information in the terminal as follows: there is the main part of my python script:

import sys from copy import deepcopy import rospy import moveit_commander from geometry_msgs.msg import PoseStamped, Pose from moveit_commander import MoveGroupCommander, PlanningSceneInterface from moveit_msgs.msg import PlanningScene, ObjectColor from moveit_msgs.msg import Grasp, GripperTranslation, MoveItErrorCodes from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from tf.transformations import quaternion_from_euler

GROUP_NAME_ARM = 'arm'

GROUP_NAME_GRIPPER = 'gripper'

GRIPPER_FRAME = 'tool0'

GRIPPER_OPEN = [0.0]

GRIPPER_CLOSED = [0.05]

REFERENCE_FRAME = 'base_link"

class MoveItPickAndPlaceDemo:

def __init__(self):

    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('moveit_pick_and_place_demo')
    scene = PlanningSceneInterface()
    self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
    self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
    self.colors = dict()
    arm = MoveGroupCommander(GROUP_NAME_ARM)
    gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
    arm.set_goal_position_tolerance(0.05)
    arm.set_goal_orientation_tolerance(0.1)
    arm.allow_replanning(True)
    arm.set_pose_reference_frame(REFERENCE_FRAME)
    target_size = [0.05, 0.05, 0.15]
    target_pose = PoseStamped()
    target_pose.header.frame_id = REFERENCE_FRAME
    target_pose.pose.position.x = -1.0
    target_pose.pose.position.y = 0.3
    target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 + 0.1
    target_pose.pose.orientation.w = 1.0
    scene.add_box(target_id, target_pose, target_size)
    self.setColor(target_id, 0.9, 0.9, 0, 1.0)
    self.sendColors()
    grasp_pose = target_pose
    grasps = self.make_grasps(grasp_pose, [target_id])

    for grasp in grasps:
        self.gripper_pose_pub.publish(grasp.grasp_pose)
        rospy.sleep(0.2)

    result = None
    n_attempts = 0

    while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
        n_attempts += 1
        rospy.loginfo("Pick attempt: " +  str(n_attempts))
        result = arm.pick(target_id, grasps)
        rospy.sleep(0.2)

    if result == MoveItErrorCodes.SUCCESS:
        result = None
        n_attempts = 0

       places = self.make_places(place_pose)
      # ...........................

def make_gripper_posture(self, joint_positions):  

    t = JointTrajectory()
    t.joint_names = ['robotiq_85_left_knuckle_joint']
    tp = JointTrajectoryPoint()
    tp.positions = joint_positions
    tp.effort = [1.0]
    tp.time_from_start = rospy.Duration(1.0)
    t.points.append(tp)
    return t

def make_gripper_translation(self, min_dist, desired, vector):

    g = GripperTranslation()
    g.direction.vector.x = vector[0]
    g.direction.vector.y = vector[1]
    g.direction.vector.z = vector[2]
    g.direction.header.frame_id = GRIPPER_FRAME
    g.min_distance = min_dist
    g.desired_distance = desired
    return g

def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
    g = Grasp()
    g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
    g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
    g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.1, [1.0, 0.0, 0.0])
    g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, -1.0, 1.0])
    g.grasp_pose = initial_pose_stamped
    pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]
    yaw_vals = [0]

    grasps = []
    for y in yaw_vals:
        for p in pitch_vals:
            q = quaternion_from_euler(0, p, y)
            g.grasp_pose.pose.orientation.x = q[0]
            g.grasp_pose.pose.orientation.y = q[1]
            g.grasp_pose.pose.orientation.z = q ...
(more)
edit retag flag offensive close merge delete