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:
[ 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
↧