My robot cannot pick a target with MoveIt
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 ...
Are you sure that your end-effector is configured correctly? Have you tried the tutorial?