My robot cannot pick a target with MoveIt
I want use moveit to control my robot for a simple pickandplace 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 'wrist2link' (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 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[2]
g.grasp_pose.pose.orientation.w = q[3]
g.id = str(len(grasps))
g.allowed_touch_objects = allowed_touch_objects
grasps.append(deepcopy(g))
return grasps
Asked by balabala on 2019-05-24 06:42:34 UTC
Comments
Are you sure that your end-effector is configured correctly? Have you tried the tutorial?
Asked by fvd on 2020-03-03 08:44:36 UTC