def place(self, block, pose_stamped, gripper=0): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result[ gripper].grasp.pre_grasp_posture l.pre_place_approach = self.pick_result[ gripper].grasp.pre_grasp_approach l.post_place_retreat = self.pick_result[ gripper].grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace[gripper].place_with_retry( block.name, places, retries=1, scene=self.scene) self.place_result[gripper] = place_result self._last_gripper_placed = gripper return success
def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 90 degrees in yaw direction l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 1.57) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 1.57) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 1.57) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry( block.name, places, scene=self.scene) return success
def place(self, block, pose_stamped, placePos): rospy.sleep(1) #creates a list of place positons for the block, with a specified x, y, and z. places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = "base_link" l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat #this x and y value are input as placePos through the function call. l.place_pose.pose.position.x = placePos[0] l.place_pose.pose.position.y = placePos[1] places.append(copy.deepcopy(l)) #success = self.pickplace.place_with_retry(block.name, places, scene = self.scene) #return success ## create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry( block.name, places, scene=self.scene) return success
def create_placings_from_object_pose(self, posestamped): """ Create a list of PlaceLocation of the object rotated every 15deg""" place_locs = [] pre_grasp_posture = JointTrajectory() # Actually ignored.... pre_grasp_posture.header.frame_id = self._grasp_pose_frame_id pre_grasp_posture.joint_names = [ name for name in self._gripper_joint_names.split()] jtpoint = JointTrajectoryPoint() jtpoint.positions = [ float(pos) for pos in self._gripper_pre_grasp_positions.split()] jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture) pre_grasp_posture.points.append(jtpoint) # Generate all the orientations every step_degrees_yaw deg for yaw_angle in np.arange(0.0, 2.0 * pi, radians(self._step_degrees_yaw)): pl = PlaceLocation() pl.place_pose = posestamped newquat = quaternion_from_euler(0.0, 0.0, yaw_angle) pl.place_pose.pose.orientation = Quaternion( newquat[0], newquat[1], newquat[2], newquat[3]) # TODO: the frame is ignored, this will always be the frame of the gripper # so arm_tool_link pl.pre_place_approach = self.createGripperTranslation( Vector3(1.0, 0.0, 0.0)) pl.post_place_retreat = self.createGripperTranslation( Vector3(-1.0, 0.0, 0.0)) pl.post_place_posture = pre_grasp_posture place_locs.append(pl) return place_locs
def generate_place_poses(self, initial_place_pose): places = list() l = PlaceLocation() l.id = "dupadupa" l.place_pose.header.frame_id = "summit_xl_base_footprint" q = Quaternion(initial_place_pose.grasp_pose.pose.orientation.w, initial_place_pose.grasp_pose.pose.orientation.x, initial_place_pose.grasp_pose.pose.orientation.y, initial_place_pose.grasp_pose.pose.orientation.z) # Load successful grasp pose l.place_pose.pose.position = initial_place_pose.grasp_pose.pose.position l.place_pose.pose.orientation.w = q.elements[0] l.place_pose.pose.orientation.x = q.elements[1] l.place_pose.pose.orientation.y = q.elements[2] l.place_pose.pose.orientation.z = q.elements[3] # Move 20cm to the right l.place_pose.pose.position.y += 0.2 # Fill rest of the msg with some data l.post_place_posture = initial_place_pose.grasp_posture l.post_place_retreat = initial_place_pose.post_grasp_retreat l.pre_place_approach = initial_place_pose.pre_grasp_approach places.append(copy.deepcopy(l)) # Rotate place pose to generate more possible configurations for the planner m = 16 # Number of possible place poses for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * math.pi / m) places.append(copy.deepcopy(l)) return places
def generate_place_locations(self, goal_pose): pls = [] # get euler axis values axis = euler_from_quaternion([ goal_pose.pose.orientation.x, goal_pose.pose.orientation.y, goal_pose.pose.orientation.z, goal_pose.pose.orientation.w ]) tolerance = round(math.pi / 3, 4) step = 0.01 z_axis_angle = round(axis[2], 4) # create a range of possible place locations for x in arange(round((z_axis_angle - tolerance), 0), round((z_axis_angle + tolerance), 0), step, dtype=float): # for x in arange(0, 360, step, dtype=float): pl = PlaceLocation() pl.place_pose = goal_pose quat = quaternion_from_euler(0.0, 0.0, round(x, 4)) pl.place_pose.pose.orientation = Quaternion(*quat) # gripper comes from negative z axis pl.pre_place_approach = self.create_gripper_translation( Vector3(0.0, 0.0, -1.0)) # gripper leaves place position on positive z axis pl.post_place_retreat = self.create_gripper_translation( Vector3(0.0, 0.0, 1.0)) # gripper joint values pl.post_place_posture = self.get_post_place_posture() pls.append(copy.deepcopy(pl)) return pls
def plan_place(self, desired_pose): places = list() ps = PoseStamped() # ps.pose = self.current_grasping_target.object.primitive_poses[0] ps.header.frame_id = self.current_grasping_target.object.header.frame_id grasp_to_pick_matrix = self.computeGraspToPickMatrix() place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose) grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix position_vector = grasp2_to_place_matrix[0:-1, 3] quaternion = quaternion_from_matrix(grasp2_to_place_matrix) position_array = position_vector.getA1() l = PlaceLocation() direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector l.place_pose.header.frame_id = ps.header.frame_id l.place_pose.pose.position.x = position_array[0] l.place_pose.pose.position.y = position_array[1] l.place_pose.pose.position.z = position_array[2] l.place_pose.pose.orientation.x = quaternion[0] l.place_pose.pose.orientation.y = quaternion[1] l.place_pose.pose.orientation.z = quaternion[2] l.place_pose.pose.orientation.w = quaternion[3] # copy the posture, approach and retreat from the grasp used approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach) approach.desired_distance /= 2.0 l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 90 degrees in roll direction l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) print "DEBUG: Places: ", places[0] success, place_result = self.pickplace.place_with_retry( self.current_grasping_target.object.name, places, scene=self.scene, allow_gripper_support_collision=False, planner_id="PRMkConfigDefault", ) if success: rospy.loginfo("Planning succeeded.") trajectory_approved = get_user_approval() if trajectory_approved: return place_result.trajectory_stages else: rospy.loginfo("Plan rejected. Replanning...") else: rospy.logwarn("Planning failed. Replanning...") desired_pose[2] += 0.05 return self.plan_place(desired_pose)
def plan_place(self, desired_pose): places = list() ps = PoseStamped() #ps.pose = self.current_grasping_target.object.primitive_poses[0] ps.header.frame_id = self.current_grasping_target.object.header.frame_id grasp_to_pick_matrix = self.computeGraspToPickMatrix() place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose) grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix position_vector = grasp2_to_place_matrix[0:-1,3] quaternion = quaternion_from_matrix(grasp2_to_place_matrix) position_array = position_vector.getA1() l = PlaceLocation() direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector l.place_pose.header.frame_id = ps.header.frame_id l.place_pose.pose.position.x = position_array[0] l.place_pose.pose.position.y = position_array[1] l.place_pose.pose.position.z = position_array[2] l.place_pose.pose.orientation.x = quaternion[0] l.place_pose.pose.orientation.y = quaternion[1] l.place_pose.pose.orientation.z = quaternion[2] l.place_pose.pose.orientation.w = quaternion[3] # copy the posture, approach and retreat from the grasp used approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach) approach.desired_distance /= 2.0 l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 90 degrees in roll direction l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) print "DEBUG: Places: ", places[0] success, place_result = self.pickplace.place_with_retry(self.current_grasping_target.object.name, places, scene=self.scene, allow_gripper_support_collision=False, planner_id='PRMkConfigDefault') if success: rospy.loginfo("Planning succeeded.") trajectory_approved = get_user_approval() if trajectory_approved: return place_result.trajectory_stages else: rospy.loginfo("Plan rejected. Replanning...") else: rospy.logwarn("Planning failed. Replanning...") desired_pose[2] += 0.05 return self.plan_place(desired_pose)
def createPlaceLocations(posestamped, deg_step=15): """Create a list of PlaceLocation of the object rotated every deg_step, defaults to 15 degrees""" place_locs = [] for yaw_angle in np.arange(0, 2 * pi, radians(deg_step)): pl = PlaceLocation() pl.place_pose = posestamped newquat = quaternion_from_euler(0.0, 0.0, yaw_angle) pl.place_pose.pose.orientation = Quaternion(newquat[0], newquat[1], newquat[2], newquat[3]) pl.pre_place_approach = createGripperTranslation(Vector3(0.0, 0.0, -1.0)) pl.post_place_retreat = createGripperTranslation(Vector3(0.0, 0.0, 1.0)) pl.post_place_posture = getPreGraspPosture() place_locs.append(pl) return place_locs
def generate_place_locations( position): pls = [] pl = PlaceLocation() pl.place_pose = position for x in range(0, 360, 2): quat = quaternion_from_euler(0.0, 0.0, round(math.radians(x), 4)) pl.place_pose.pose.orientation = Quaternion(*quat) # take object along negative z-axis pl.pre_place_approach = create_gripper_translation(Vector3(0.0, 0.0, -1.0)) # go with object along positive z-axis pl.post_place_retreat = create_gripper_translation(Vector3(0.0, 0.0, 1.0)) pl.post_place_posture = get_post_place_posture() pls.append(copy.deepcopy(pl)) return pls
def createPlaceLocations(posestamped, deg_step=15): """Create a list of PlaceLocation of the object rotated every deg_step, defaults to 15 degrees""" place_locs = [] for yaw_angle in np.arange(0, 2 * pi, radians(deg_step)): pl = PlaceLocation() pl.place_pose = posestamped newquat = quaternion_from_euler(0.0, 0.0, yaw_angle) pl.place_pose.pose.orientation = Quaternion(newquat[0], newquat[1], newquat[2], newquat[3]) pl.pre_place_approach = createGripperTranslation( Vector3(0.0, 0.0, -1.0)) pl.post_place_retreat = createGripperTranslation(Vector3( 0.0, 0.0, 1.0)) pl.post_place_posture = getPreGraspPosture() place_locs.append(pl) return place_locs
def generate_place_locations(position): pls = [] pl = PlaceLocation() pl.place_pose = position for x in range(0, 360, 2): quat = quaternion_from_euler(0.0, 0.0, round(math.radians(x), 4)) pl.place_pose.pose.orientation = Quaternion(*quat) # take object along negative z-axis pl.pre_place_approach = create_gripper_translation( Vector3(0.0, 0.0, -1.0)) # go with object along positive z-axis pl.post_place_retreat = create_gripper_translation( Vector3(0.0, 0.0, 1.0)) pl.post_place_posture = get_post_place_posture() pls.append(copy.deepcopy(pl)) return pls
def make_places(self, init_pose, allowed_touch_objects, pre, post, set_rpy=0, roll_vals=[0], pitch_vals=[0], yaw_vals=[0]): place = PlaceLocation() place.post_place_posture = self.make_gripper_posture(GRIPPER_OPEN) place.pre_place_approach = self.make_gripper_translation( pre[0], pre[1], pre[2]) place.post_place_retreat = self.make_gripper_translation( post[0], post[1], post[2]) place.place_pose = init_pose x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] places = [] for yaw in yaw_vals: for pitch in pitch_vals: for y in y_vals: for x in x_vals: place.place_pose.pose.position.x = init_pose.pose.position.x + x place.place_pose.pose.position.y = init_pose.pose.position.y + y q = quaternion_from_euler(0, pitch, yaw) if set_rpy: place.place_pose.pose.orientation.x = q[0] place.place_pose.pose.orientation.y = q[1] place.place_pose.pose.orientation.z = q[2] place.place_pose.pose.orientation.w = q[3] place.id = str(len(places)) place.allowed_touch_objects = allowed_touch_objects places.append(deepcopy(place)) return places
def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m-1): l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry(block.name, places, scene=self.scene) return success
def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 90 degrees in yaw direction l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry(block.name, places, scene=self.scene) return success
def generate_place_locations(self, goal_pose): pls = [] # get euler axis values axis = euler_from_quaternion([goal_pose.pose.orientation.x, goal_pose.pose.orientation.y, goal_pose.pose.orientation.z, goal_pose.pose.orientation.w]) tolerance = round(math.pi/3, 4) step = 0.01 z_axis_angle = round(axis[2], 4) # create a range of possible place locations for x in arange(round((z_axis_angle - tolerance), 0), round((z_axis_angle + tolerance), 0), step, dtype=float): # for x in arange(0, 360, step, dtype=float): pl = PlaceLocation() pl.place_pose = goal_pose quat = quaternion_from_euler(0.0, 0.0, round(x, 4)) pl.place_pose.pose.orientation = Quaternion(*quat) # gripper comes from negative z axis pl.pre_place_approach = self.create_gripper_translation(Vector3(0.0, 0.0, -1.0)) # gripper leaves place position on positive z axis pl.post_place_retreat = self.create_gripper_translation(Vector3(0.0, 0.0, 1.0)) # gripper joint values pl.post_place_posture = self.get_post_place_posture() pls.append(copy.deepcopy(pl)) return pls
def make_places(self, pose_stamped, mega_angle=False): # setup default of place location l = PlaceLocation() l.post_place_posture = self.make_gripper_posture(GRIPPER_OPEN) l.pre_place_approach = self.make_gripper_translation(0.1, 0.15) l.post_place_retreat = self.make_gripper_translation(0.1, 0.15, -1.0) l.place_pose = pose_stamped pitch_vals = [0, 0.2, -0.2, 0.4, -0.4] if mega_angle: pitch_vals += [0.3, -0.3, 0.5, -0.5, 0.6, -0.6] # generate list of place locations places = [] for y in [-1.57, -0.78, 0, 0.78, 1.57]: for p in pitch_vals: q = quaternion_from_euler(0, p, y) # now in object frame l.place_pose.pose.orientation.x = q[0] l.place_pose.pose.orientation.y = q[1] l.place_pose.pose.orientation.z = q[2] l.place_pose.pose.orientation.w = q[3] l.id = str(len(places)) places.append(copy.deepcopy(l)) return places
# call move_group to pick the object rospy.loginfo("Beginning to pick.") success, pick_result = pickplace.pick_with_retry(obj_name, grasps, support_name=support_surface, scene=scene) if not success: exit(-1) # create a set of place locations for the cube places = list() l = PlaceLocation() l.place_pose.pose = find_result.objects[the_object].object.primitive_poses[0] l.place_pose.header.frame_id = find_result.objects[the_object].object.header.frame_id # invert the y of the pose l.place_pose.pose.position.y = -l.place_pose.pose.position.y # copy the posture, approach and retreat from the grasp used l.post_place_posture = pick_result.grasp.pre_grasp_posture l.pre_place_approach = pick_result.grasp.pre_grasp_approach l.post_place_retreat = pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 90 degrees in yaw direction l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57) places.append(copy.deepcopy(l)) # drop it like it's hot rospy.loginfo("Beginning to place.") while not rospy.is_shutdown(): # can't fail here or moveit needs to be restarted success, place_result = pickplace.place_with_retry(obj_name, places, support_name=support_surface, scene=scene)
def __init__(self): rospy.init_node('urb_pickup', anonymous=False) rospy.on_shutdown(self.shutdown) # Get Goal point where we pick up goal_x = rospy.get_param("~goal_x", 3.96) # meters goal_y = rospy.get_param("~goal_y", 4.01) goal_z = rospy.get_param("~goal_z", pi/2) parser = argparse.ArgumentParser(description="Simple demo of pick and place") parser.add_argument("--objects", help="Just do object perception", action="store_true") parser.add_argument("--all", help="Just do object perception, but insert all objects", action="store_true") parser.add_argument("--once", help="Run once.", action="store_true") parser.add_argument("--ready", help="Move the arm to the ready position.", action="store_true") parser.add_argument("--plan", help="Only do planning, no execution", action="store_true") parser.add_argument("--x", help="Recommended x offset, how far out an object should roughly be.", type=float, default=0.5) args, unknown = parser.parse_known_args() move_group = MoveGroupInterface("arm", "base_link", plan_only = args.plan) # Create a list to hold the target quaternions (orientations) quaternions = list() # Create a list to hold the waypoint poses waypoints = list() q_angle = quaternion_from_euler(0, 0, goal_z, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) waypoints.append(Pose(Point(goal_x, goal_y, 0.0), quaternions[0])) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") rospy.loginfo("Starting navigation test") # Intialize the waypoint goal goal = MoveBaseGoal() # Use the map frame to define goal poses goal.target_pose.header.frame_id = 'map' # Set the time stamp to "now" goal.target_pose.header.stamp = rospy.Time.now() # Set the goal pose to the i-th waypoint goal.target_pose.pose = waypoints[0] # Start the robot moving toward the goal self.move(goal) # if all we want to do is prepare the arm, do it now if args.ready: self.move_to_ready(move_group) exit(0) scene = PlanningSceneInterface("base_link") pickplace = PickPlaceInterface("arm", "gripper", plan_only = args.plan, verbose = True) rospy.loginfo("Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") while not rospy.is_shutdown(): goal = FindGraspableObjectsGoal() goal.plan_grasps = True find_objects.send_goal(goal) find_objects.wait_for_result(rospy.Duration(5.0)) find_result = find_objects.get_result() rospy.loginfo("Found %d objects" % len(find_result.objects)) # remove all previous objects for name in scene.getKnownCollisionObjects(): scene.removeCollisionObject(name, False) for name in scene.getKnownAttachedObjects(): scene.removeAttachedObject(name, False) scene.waitForSync() # clear colors scene._colors = dict() # insert objects, find the one to grasp the_object = None the_object_dist = 0.35 count = -1 for obj in find_result.objects: count += 1 scene.addSolidPrimitive("object%d"%count, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) # object must have usable grasps if len(obj.grasps) < 1: continue # choose object in front of robot dx = obj.object.primitive_poses[0].position.x - args.x dy = obj.object.primitive_poses[0].position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < the_object_dist: the_object_dist = d the_object = count if the_object == None: rospy.logerr("Nothing to grasp! try again...") continue # insert table for obj in find_result.support_surfaces: # extend surface to floor height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 2.0, # make table wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # add to scene scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) obj_name = "object%d"%the_object # sync scene.waitForSync() # set color of object we are grabbing scene.setColor(obj_name, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange scene.setColor(find_result.objects[the_object].object.support_surface, 0, 0, 0) # black scene.sendColors() # exit now if we are just doing object update if args.objects: if args.once: exit(0) else: continue # get grasps (we checked that they exist above) grasps = find_result.objects[the_object].grasps support_surface = find_result.objects[the_object].object.support_surface # call move_group to pick the object rospy.loginfo("Beginning to pick.") success, pick_result = pickplace.pick_with_retry(obj_name, grasps, support_name=support_surface, scene=scene) if not success: exit(-1) # create a set of place locations for the cube places = list() l = PlaceLocation() l.place_pose.pose = find_result.objects[the_object].object.primitive_poses[0] l.place_pose.header.frame_id = find_result.objects[the_object].object.header.frame_id # invert the y of the pose l.place_pose.pose.position.y = -l.place_pose.pose.position.y # copy the posture, approach and retreat from the grasp used l.post_place_posture = pick_result.grasp.pre_grasp_posture l.pre_place_approach = pick_result.grasp.pre_grasp_approach l.post_place_retreat = pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 90 degrees in yaw direction l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57) places.append(copy.deepcopy(l)) # drop it like it's hot rospy.loginfo("Beginning to place.") while not rospy.is_shutdown(): # can't fail here or moveit needs to be restarted success, place_result = pickplace.place_with_retry(obj_name, places, support_name=support_surface, scene=scene) if success: break # place arm back at side self.move_to_ready(move_group) rospy.loginfo("Ready...") # rinse and repeat if args.once: exit(0)