My robot cannot pick a target with MoveIt

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

balabala gravatar image

updated 2020-03-03 07:42:23 -0500

fvd gravatar image

I want use moveit to control my robot for a simple pick_and_place task in rviz, and I use ur10 with a robotiq85 gripper. But its plans always fail. The information shows that there is a contact between 'target' and 'wrist_2_link' (the link of ur10) and the IK 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 the same. I don't know what's wrong with it.

The information in the terminal is as follows:

[ INFO] [1558698830.071226740]: Collision checking is considered complete (collision was found and 0 contacts are stored)[ INFO] [1558698830.071521909]: Found a contact between 'target' (type 'Object') and 'wrist_2_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1558698830.071596110]: Collision checking is considered complete (collision was found and 0 contacts are stored)[ INFO] [1558698830.071945170]: IK failed[ INFO] [1558698830.072022409]: Sampler failed to produce a state[ INFO] [1558698830.072103597]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 0[ INFO] [1558698830.072665101]: Pickup planning completed after 0.009758 seconds

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Are you sure that your end-effector is configured correctly? Have you tried the tutorial?

fvd gravatar image fvd  ( 2020-03-03 07:44:36 -0500 )edit