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 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 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 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 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 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): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the postrection 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 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 picknplace(): # define the initial positions of baxter's joints in a dictionary initial_position = {'left_e0': -1.69483279891317, 'left_e1': 1.8669726956453, 'left_s0': 0.472137005716569, 'left_s1': -0.38852045702393034, 'left_w0': -1.9770933862776057, 'left_w1': -1.5701993084642143, 'left_w2': -0.6339059781326424, 'right_e0': 1.7238109084167481, 'right_e1': 1.7169079948791506, 'right_s0': 0.36930587426147465, 'right_s1': -0.33249033539428713, 'right_w0': -1.2160632682067871, 'right_w1': 1.668587600115967, 'right_w2': -1.810097327636719} # create two lists, one of initial locations and one of goal locations # for BAXTER to move them from one to another locs_x = [] # initial locations locs_y = [] orien = [] goal_move = [] place_goal = [] # goal locations # using readlines() to read the plan.ipc file, the PDDL problem solution PDDL_solved = open('plan.ipc', 'r') Lines = PDDL_solved.readlines() # adding the goal positions from the PDDL solved file for line in Lines: orien.append(0); goal_move.append(line.split()[3][8:9]) cord_dest = int(line.split()[2][8:9]) # adding an offset to all location but the 5th if(cord_dest != 5): locs_x.append(BLOCKS[cord_dest - 1][0] + 0.025) locs_y.append(BLOCKS[cord_dest - 1][1] + 0.03) else: locs_x.append(BLOCKS[cord_dest - 1][0]) locs_y.append(BLOCKS[cord_dest - 1][1]) for i in goal_move: place_goal_i = geometry_msgs.msg.Pose() if(int(i) != 5): place_goal_i.position.x = BLOCKS[int(i) - 1][0] + 0.025 place_goal_i.position.y = BLOCKS[int(i) - 1][1] + 0.03 else: place_goal_i.position.x = BLOCKS[int(i) - 1][0] place_goal_i.position.y = BLOCKS[int(i) - 1][1] place_goal_i.position.z = BLOCKS[int(i) - 1][2] place_goal_i.orientation.x = 1.0 place_goal_i.orientation.y = 0.0 place_goal_i.orientation.z = 0.0 place_goal_i.orientation.w = 0.0 place_goal.append(place_goal_i) # the distance from the zero point in Moveit to the ground is 0.903 m (for the table we use) # the value is not always the same. (look in Readme) center_z_cube = -MOVEIT_Z_CONST + TABLE_SIZE[2] + BLOCK_SIZE[2] / 2 # initialize a list for the objects and the stacked cubes. objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] both_arms.set_joint_value_target(initial_position) both_arms.plan() both_arms.go(wait=True) # remove models from the scene on shutdown #rospy.on_shutdown(delete_gazebo_models) # wait for All Clear message from emulator startup. rospy.wait_for_message("/robot/sim/started", Empty) planningScene.clear() """ Attach a box into the planning scene. def moveit_python.planning_scene_interface.PlanningSceneInterface.attachBox (self, name, - Name of the object size_x, - The x-dimensions size of the box size_y, - The y-dimensions size of the box size_z, - The z-dimensions size of the box x, - The x position in link_name frame y, - The y position in link_name frame z, - The z position in link_name frame link_name, - Name of link to attach this object to touch_links = None, - Names of robot links that can touch this object detach_posture = None, weight = 0.0, wait = True - When true, we wait for planning scene to actually update, this provides immunity against lost messages. ) add the table as attached object """ planningScene.attachBox('table', TABLE_SIZE[0], TABLE_SIZE[1], TABLE_SIZE[2], CENTER[0], CENTER[1], CENTER[2], 'base', touch_links=['pedestal']) planningScene.waitForSync() rightgripper.open() # cProfile explanation - https://docs.python.org/2/library/profile.html#module-cProfile # cProfile to measure the performance (time) of the task. profile = cProfile.Profile() profile.enable() # loop performing "pick and place" till all objects are cleared from table. # locs_x,locs_y are the source fot he cubes. num_of_pick_n_place = 0 while num_of_pick_n_place < len(goal_move): # do the task only if there are still objects on the table moved_objects = 0 while moved_objects < len(locs_x): # clear planning scene planningScene.clear() # Add table as attached object. planningScene.attachBox('table', TABLE_SIZE[0], TABLE_SIZE[1], TABLE_SIZE[2], CENTER[0], CENTER[1], CENTER[2], 'base', touch_links=['pedestal']) # Initialize the data of the objects on the table. x_n = locs_x[moved_objects] y_n = locs_y[moved_objects] z_n = Z_N_INIT_VALUE th_n = orien[moved_objects] # if the angle is bigger than 45 degrees, I assume there will be problem with the pick, # therefore in that case we need to calibrate the angle to be in the range[-45,45] degrees. # I surmise the offset of the angle is always adding toward the positive axis for theta, # thus we need to calibrate theta toward the negative 45. if th_n > pi / 4: th_n = -1 * (th_n % (pi / 4)) # Add the detected objects into the planning scene. for i in range(1, len(locs_x)): planningScene.addBox(objlist[i], BLOCK_SIZE[0], BLOCK_SIZE[1], BLOCK_SIZE[2], locs_x[i], locs_y[i], center_z_cube) planningScene.waitForSync() # defien a middle point from initial point to goal, as "approach_pick_goal". # initalize it as equal to the actual pick_goal in z and y axis, and different in z axis approach_pick_goal = geometry_msgs.msg.Pose() approach_pick_goal.position.x = x_n approach_pick_goal.position.y = y_n approach_pick_goal.position.z = z_n + MIDDLE_POINT_Z # PoseStamped is a Pose with reference coordinate frame and timestamp approach_pick_goal_dummy = PoseStamped() """" Header is Standard metadata for higher-level stamped data types. This is generally used to communicate timestamped data in a particular coordinate frame. sequence ID: consecutively increasing ID uint32 seq Two-integer timestamp that is expressed as: * stamp.sec: seconds (stamp_secs) since epoch (called 'secs') * stamp.nsec: nanoseconds since stamp_secs (called 'nsecs') """ approach_pick_goal_dummy.header.frame_id = "base" approach_pick_goal_dummy.header.stamp = rospy.Time.now() approach_pick_goal_dummy.pose.position.x = x_n approach_pick_goal_dummy.pose.position.y = y_n approach_pick_goal_dummy.pose.position.z = z_n + MIDDLE_POINT_Z approach_pick_goal_dummy.pose.orientation.x = 1.0 approach_pick_goal_dummy.pose.orientation.y = 0.0 approach_pick_goal_dummy.pose.orientation.z = 0.0 approach_pick_goal_dummy.pose.orientation.w = 0.0 # orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles. approach_pick_goal_dummy.pose = rotate_pose_msg_by_euler_angles(approach_pick_goal_dummy.pose, 0.0, 0.0, th_n) approach_pick_goal.orientation.x = approach_pick_goal_dummy.pose.orientation.x approach_pick_goal.orientation.y = approach_pick_goal_dummy.pose.orientation.y approach_pick_goal.orientation.z = approach_pick_goal_dummy.pose.orientation.z approach_pick_goal.orientation.w = approach_pick_goal_dummy.pose.orientation.w # Move to the approach goal and the pick_goal. pick_goal = deepcopy(approach_pick_goal) # create an exact copy of approach_pick_goal pick_goal.position.z = z_n move('right', approach_pick_goal) # move is a function time.sleep(5) move('right', pick_goal) time.sleep(1) rightgripper.close() # PICK! time.sleep(1) # move back to the approach_pick_goal. move('right', approach_pick_goal) # define the approach_place_goal approached_place_goal = deepcopy(place_goal[num_of_pick_n_place]) approached_place_goal.position.z = approached_place_goal.position.z + MIDDLE_POINT_Z move('right', approached_place_goal) time.sleep(5) place_goal_val = place_goal[num_of_pick_n_place] place_goal_val.position.z = place_goal_val.position.z + BLOCK_SIZE[2] move('right', place_goal_val) time.sleep(1) rightgripper.open() # PLACE! time.sleep(1) # move to the approach_place_goal. move('right', approached_place_goal) # increase iterators num_of_pick_n_place += 1 moved_objects += 1 # move the arms to initial position. both_arms.set_joint_value_target(initial_position) both_arms.plan() both_arms.go(wait=True) profile.disable() # pstats documentation - https://docs.python.org/3/library/profile.html#module-pstats pstats.Stats(profile).sort_stats('cumulative').print_stats(0.0) # exit MoveIt and shutting down the process moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) time.sleep(10)
def picknplace(): # Define positions. rpos = geometry_msgs.msg.Pose() rpos.position.x = 0.555 rpos.position.y = 0.0 rpos.position.z = 0.206 rpos.orientation.x = 1.0 rpos.orientation.y = 0.0 rpos.orientation.z = 0.0 rpos.orientation.w = 0.0 lpos = geometry_msgs.msg.Pose() lpos.position.x = 0.65 lpos.position.y = 0.6 lpos.position.z = 0.206 lpos.orientation.x = 1.0 lpos.orientation.y = 0.0 lpos.orientation.z = 0.0 lpos.orientation.w = 0.0 placegoal = geometry_msgs.msg.Pose() placegoal.position.x = 0.55 placegoal.position.y = 0.28 placegoal.position.z = 0 placegoal.orientation.x = 1.0 placegoal.orientation.y = 0.0 placegoal.orientation.z = 0.0 placegoal.orientation.w = 0.0 # Define variables. offset_zero_point = 0.903 table_size_x = 0.714655654394 table_size_y = 1.05043717328 table_size_z = 0.729766045265 center_x = 0.457327827197 center_y = 0.145765166941 center_z = -0.538116977368 # The distance from the zero point in Moveit to the ground is 0.903 m. # The value is not allways the same. (look in Readme) center_z_cube = -offset_zero_point + table_size_z + 0.0275 / 2 pressure_ok = 0 j = 0 k = 0 start = 1 locs_x = [] # Initialize a list for the objects and the stacked cubes. objlist = [ 'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11' ] boxlist = [ 'box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08', 'box09', 'box10', 'box11' ] # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) p.waitForSync() # Move both arms to start state. right_arm.set_pose_target(rpos) left_arm.set_pose_target(lpos) right_arm.plan() left_arm.plan() right_arm.go(wait=True) left_arm.go(wait=True) # cProfile to measure the performance (time) of the task. pr = cProfile.Profile() pr.enable() # Loop to continue pick and place until all objects are cleared from table. while locs_x or start: # Only for the start. if start: start = 0 time.sleep(0.5) # Receive the data from all objects from the topic "detected_objects". temp = rospy.wait_for_message("detected_objects", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] # Add the data from the objects. for i in range(len(locs)): locs_x.append(locs[i].position.x) locs_y.append(locs[i].position.y) orien.append(locs[i].position.z * pi / 180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected locations for same objects. ind_rmv = [] for i in range(0, len(locs)): if (locs_y[i] > 0.24 or locs_x[i] > 0.75): ind_rmv.append(i) continue for j in range(i, len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2) < 0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Do the task only if there are still objects on the table. if locs_x: # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) # Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes. ig0 = itemgetter(0) sorted_lists = zip(*sorted( zip(size, locs_x, locs_y, orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Initialize the data of the biggest object on the table. xn = locs_x[0] yn = locs_y[0] # -0.16 is the z position to grip the objects on the table. zn = -0.16 thn = orien[0] sz = size[0] if thn > pi / 4: thn = -1 * (thn % (pi / 4)) # Add the detected objects into the planning scene. #for i in range(1,len(locs_x)): #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube) # Add the stacked objects as collision objects into the planning scene to avoid moving against them. #for e in range(0, k): #p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.position.x, placegoal.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes']) if k > 0: p.attachBox(boxlist[0], 0.07, 0.07, 0.0275 * k, placegoal.position.x, placegoal.position.y, center_z_cube, 'base', touch_links=['cubes']) p.waitForSync() # Initialize the approach pickgoal (5 cm to pickgoal). approach_pickgoal = geometry_msgs.msg.Pose() approach_pickgoal.position.x = xn approach_pickgoal.position.y = yn approach_pickgoal.position.z = zn + 0.05 approach_pickgoal_dummy = PoseStamped() approach_pickgoal_dummy.header.frame_id = "base" approach_pickgoal_dummy.header.stamp = rospy.Time.now() approach_pickgoal_dummy.pose.position.x = xn approach_pickgoal_dummy.pose.position.y = yn approach_pickgoal_dummy.pose.position.z = zn + 0.05 approach_pickgoal_dummy.pose.orientation.x = 1.0 approach_pickgoal_dummy.pose.orientation.y = 0.0 approach_pickgoal_dummy.pose.orientation.z = 0.0 approach_pickgoal_dummy.pose.orientation.w = 0.0 # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles. approach_pickgoal_dummy.pose = rotate_pose_msg_by_euler_angles( approach_pickgoal_dummy.pose, 0.0, 0.0, thn) approach_pickgoal.orientation.x = approach_pickgoal_dummy.pose.orientation.x approach_pickgoal.orientation.y = approach_pickgoal_dummy.pose.orientation.y approach_pickgoal.orientation.z = approach_pickgoal_dummy.pose.orientation.z approach_pickgoal.orientation.w = approach_pickgoal_dummy.pose.orientation.w # Move to the approach goal and the pickgoal. pickgoal = deepcopy(approach_pickgoal) pickgoal.position.z = zn move("right", approach_pickgoal, pickgoal) time.sleep(0.5) # Read the force in z direction. b = rightarm.endpoint_effort() z_ = b['force'] z_force = z_.z # Search again for objects, if the gripper isn't at the right position and presses on an object. #print("----->force in z direction:", z_force) if z_force > -4: rightgripper.close() attempts = 0 pressure_ok = 1 # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects. while (rightgripper.force() < 25 and pressure_ok == 1): time.sleep(0.04) attempts += 1 if (attempts > 50): rightgripper.open() pressure_ok = 0 print("----->pressure is to low<-----") else: print("----->gripper presses on an object<-----") # Move back to the approach pickgoal. pickgoal.position.z = zn + 0.05 move("right", pickgoal) if pressure_ok and z_force > -4: # Define the approach placegoal. # Increase the height of the tower every time by 2.75 cm. approached_placegoal = deepcopy(placegoal) approached_placegoal.position.z = -0.155 + (k * 0.0275) + 0.08 # Define the placegoal placegoal.position.z = -0.155 + (k * 0.0275) # Move to the placegoal and open the gripper 4 mm above the tip of the tower. move("right", approached_placegoal, placegoal) rightgripper.open() while (rightgripper.force() > 10): time.sleep(0.01) # Move to the approach placegoal. move("right", approached_placegoal) k += 1 # Move right arm to start position. right_arm.set_pose_target(rpos) right_arm.plan() right_arm.go(wait=True) pr.disable() sortby = 'cumulative' ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0) p.clear() moveit_commander.roscpp_shutdown() # Exit MoveIt. moveit_commander.os._exit(0)
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
def picknplace(): # Define initial parameters p = PlanningSceneInterface("base") g = MoveGroupInterface("both_arms", "base") gr = MoveGroupInterface("right_arm", "base") gl = MoveGroupInterface("left_arm", "base") leftgripper = baxter_interface.Gripper('left') leftgripper.calibrate() leftgripper.open() jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2'] pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519] lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384] rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] # Clear planning scene p.clear() # Add table as attached object p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) # Move both arms to start state g.moveToJointPosition(jts_both, pos1, plan_only=False) # Get obj locations temp = rospy.wait_for_message("Dpos", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(0.57 + locs[i].position.x) locs_y.append(-0.011 + locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene ind_rmv = [] for i in range(0,len(locs)): if (locs_y[i] > 0.42): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes if locs_x: ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Loop to continue pick and place until all objects are cleared from table j=0 k=0 while locs_x: p.clear() # CLear planning scene of all collision objects # Attach table as attached collision object to base of baxter p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) xn = locs_x[0] yn = locs_y[0] zn = -0.06 thn = orien[0] sz = size[0] if thn > pi/4: thn = -1*(thn%(pi/4)) # Add collision objects into planning scene objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] for i in range(1,len(locs_x)): p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05) p.waitForSync() # Move both arms to pos2 (Right arm away and left arm on table) g.moveToJointPosition(jts_both, pos2, plan_only=False) # Move left arm to pick object and pick object pickgoal = PoseStamped() pickgoal.header.frame_id = "base" pickgoal.header.stamp = rospy.Time.now() pickgoal.pose.position.x = xn pickgoal.pose.position.y = yn pickgoal.pose.position.z = zn+0.1 pickgoal.pose.orientation.x = 1.0 pickgoal.pose.orientation.y = 0.0 pickgoal.pose.orientation.z = 0.0 pickgoal.pose.orientation.w = 0.0 gl.moveToPose(pickgoal, "left_gripper", plan_only=False) # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn) gl.moveToPose(pickgoal, "left_gripper", plan_only=False) pickgoal.pose.position.z = zn gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False) leftgripper.close() pickgoal.pose.position.z = zn+0.1 gl.moveToPose(pickgoal, "left_gripper", plan_only=False) # Move both arms to pos1 g.moveToJointPosition(jts_both, pos1, plan_only=False) # Get obj locations temp = rospy.wait_for_message("Dpos", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(0.57 + locs[i].position.x) locs_y.append(-0.011 + locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene ind_rmv = [] for i in range(0,len(locs)): if (locs_y[i] > 0.42): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes if locs_x: ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Place object stleft = PoseStamped() stleft.header.frame_id = "base" stleft.header.stamp = rospy.Time.now() stleft.pose.position.x = 0.65 stleft.pose.position.y = 0.55 stleft.pose.position.z = 0.1 stleft.pose.orientation.x = 1.0 stleft.pose.orientation.y = 0.0 stleft.pose.orientation.z = 0.0 stleft.pose.orientation.w = 0.0 # Stack objects (large cubes) if size if greater than some threshold if sz > 16.: stleft.pose.position.x = 0.65 stleft.pose.position.y = 0.7 stleft.pose.position.z = -0.04+(k*0.05) k += 1 gl.moveToPose(stleft, "left_gripper", plan_only=False) leftgripper.open() stleft.pose.position.z = 0.3 gl.moveToPose(stleft, "left_gripper", plan_only=False)
def dyn_wedge(): pub = rospy.Publisher("finished", String, queue_size=10) # Initialize MoveIt scene p = PlanningSceneInterface("base") arms_group = MoveGroupInterface("both_arms", "base") rightarm_group = MoveGroupInterface("right_arm", "base") leftarm_group = MoveGroupInterface("left_arm", "base") # Create right arm instance right_arm = baxter_interface.limb.Limb('right') # Create right gripper instance right_gripper = baxter_interface.Gripper('right') right_gripper.calibrate() right_gripper.open() right_gripper.close() offset_zero_point=0.903 table_size_x = 0.714655654394 table_size_y = 1.05043717328 table_size_z = 0.729766045265 center_x = 0.457327827197 center_y = 0.145765166941 center_z = -0.538116977368 table_distance_from_gripper = -offset_zero_point+table_size_z+0.0275/2 j=0 k=0 # Initialize object list objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] p.clear() p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) p.waitForSync() # Move both arms to start state. # Initial pose rpos = PoseStamped() rpos.header.frame_id = "base" rpos.header.stamp = rospy.Time.now() rpos.pose.position.x = 0.555 rpos.pose.position.y = 0.0 rpos.pose.position.z = 0.206 #0.18 rpos.pose.orientation.x = 1.0 rpos.pose.orientation.y = 0.0 rpos.pose.orientation.z = 0.0 rpos.pose.orientation.w = 0.0 lpos = PoseStamped() lpos.header.frame_id = "base" lpos.header.stamp = rospy.Time.now() lpos.pose.position.x = 0.555 lpos.pose.position.y = 0.65 lpos.pose.position.z = 0.206#0.35 lpos.pose.orientation.x = 1.0 lpos.pose.orientation.y = 0.0 lpos.pose.orientation.z = 0.0 lpos.pose.orientation.w = 0.0 rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) leftarm_group.moveToPose(lpos, "left_gripper", max_velocity_scaling_factor=1, plan_only=False) # Get the middle point between the two centroids locs = rospy.wait_for_message("clasificacion", PoseArray) if(len(locs.poses)!=0): punto_medio1 = PoseStamped() punto_medio1.header.frame_id = "base" punto_medio1.header.stamp = rospy.Time.now() punto_medio1.pose.position.x = locs.poses[0].position.x punto_medio1.pose.position.y = locs.poses[0].position.y punto_medio1.pose.position.z = -0.08 punto_medio1.pose.orientation.x = locs.poses[0].orientation.x punto_medio1.pose.orientation.y = locs.poses[0].orientation.y punto_medio1.pose.orientation.z = locs.poses[0].orientation.z punto_medio1.pose.orientation.w = locs.poses[0].orientation.w else: sys.exit("No se encuentran los puntos") # Get the middle point again after a few seconds tiempo = 3 time.sleep(tiempo) locs = rospy.wait_for_message("clasificacion", PoseArray) if(len(locs.poses)!=0): punto_medio2 = PoseStamped() punto_medio2.header.frame_id = "base" punto_medio2.header.stamp = rospy.Time.now() punto_medio2.pose.position.x = locs.poses[0].position.x punto_medio2.pose.position.y = locs.poses[0].position.y punto_medio2.pose.position.z = -0.08 punto_medio2.pose.orientation.x = locs.poses[0].orientation.x punto_medio2.pose.orientation.y = locs.poses[0].orientation.y punto_medio2.pose.orientation.z = locs.poses[0].orientation.z punto_medio2.pose.orientation.w = locs.poses[0].orientation.w else: sys.exit("No se encuentran los puntos") # Calculate speed of objects vel = (punto_medio2.pose.position.y - punto_medio1.pose.position.y) / tiempo # Predict position within a certain time nuevo_tiempo = 2.1 posicion = nuevo_tiempo * vel * 2 if(posicion>=0): nueva_y = punto_medio2.pose.position.y - posicion else: nueva_y = punto_medio2.pose.position.y + posicion orientacion = (-1) * 1/punto_medio2.pose.orientation.x new_or = 0.26 if orientacion > 0: new_or = -0.26 # If there is time enough to sweep the objects if vel > -0.08: start = time.time() punto_medio2.pose.position.y = nueva_y - nueva_y/2 punto_medio2.pose.position.x = punto_medio1.pose.position.x - 0.03 punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion - new_or) rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) end = time.time() tiempo = end - start while(nuevo_tiempo - tiempo >= 1): end = time.time() nuevo_tiempo = end - start else: punto_medio2.pose.position.y = nueva_y print(punto_medio2.pose.position.y) punto_medio2.pose.position.x = punto_medio1.pose.position.x punto_medio2.pose.position.y += 0.05 punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion) rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) obj = punto_medio2.pose.position.y + 0.16 neg = 1 while punto_medio2.pose.position.y < obj: punto_medio2.pose.position.y += 0.03 # Shake arm punto_medio2.pose.position.x += neg * 0.09 rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) neg = (-1)*neg time.sleep(2) rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) freemem = "free" pub.publish(freemem)
def picknplace(): # Initialize the planning scene interface. p = PlanningSceneInterface("base") # Connect the arms to the move group. g = MoveGroupInterface("both_arms", "base") gr = MoveGroupInterface("right_arm", "base") gl = MoveGroupInterface("left_arm", "base") # Create baxter_interface limb instance. leftarm = baxter_interface.limb.Limb('left') # Create baxter_interface gripper instance. leftgripper = baxter_interface.Gripper('left') leftgripper.calibrate() leftgripper.open() # Define the joints for the positions. jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2'] pos1 = [-1.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] lpos1 = [-1.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024] lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384] rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] placegoal = PoseStamped() placegoal.header.frame_id = "base" placegoal.header.stamp = rospy.Time.now() placegoal.pose.position.x = 0.55 placegoal.pose.position.y = 0.28 placegoal.pose.position.z = 0 placegoal.pose.orientation.x = 1.0 placegoal.pose.orientation.y = 0.0 placegoal.pose.orientation.z = 0.0 placegoal.pose.orientation.w = 0.0 # Define variables. offset_zero_point = 0.903 table_size_x = 0.714655654394 table_size_y = 1.05043717328 table_size_z = 0.729766045265 center_x = 0.457327827197 center_y = 0.145765166941 center_z = -0.538116977368 # The distance from the zero point in Moveit to the ground is 0.903 m. # The value is not allways the same. (look in Readme) center_z_cube= -offset_zero_point+table_size_z+0.0275/2 pressure_ok=0 j=0 k=0 start=1 locs_x = [] # Initialize a list for the objects and the stacked cubes. objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] boxlist= ['box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08', 'box09', 'box10', 'box11'] # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) # Move both arms to start state where the right camera looks for objects. g.moveToJointPosition(jts_both, pos1, max_velocity_scaling_factor = 1, plan_only=False) time.sleep(0.5) # cProfile to measure the performance (time) of the task. pr = cProfile.Profile() pr.enable() # Loop to continue pick and place until all objects are cleared from table. while locs_x or start: # Only for the start. if start: start = 0 # Receive the data from all objects from the topic "detected_objects". temp = rospy.wait_for_message("detected_objects", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] # Adds the data from the objects. for i in range(len(locs)): locs_x.append(locs[i].position.x) locs_y.append(locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected locations for same objects. ind_rmv = [] for i in range(0,len(locs) ): if (locs_y[i] > 0.24 or locs_x[i] > 0.75): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Do the task only if there are still objects on the table. if locs_x: # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) # Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes. ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Initialize the data of the biggest object on the table. xn = locs_x[0] yn = locs_y[0] zn = -0.16 thn = orien[0] sz = size[0] if thn > pi/4: thn = -1*(thn%(pi/4)) # Add the detected objects into the planning scene. #for i in range(1,len(locs_x)): #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube) # Add the stacked objects as collision objects into the planning scene to avoid moving against them. #for e in range(0, k): #p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes']) if k>0: p.attachBox(boxlist[0], 0.07, 0.07, 0.0275*k, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube, 'base', touch_links=['cubes']) p.waitForSync() # Move both arms to the second position. g.moveToJointPosition(jts_both, pos2, max_velocity_scaling_factor = 1, plan_only=False) # Initialize the pickgoal. pickgoal = PoseStamped() pickgoal.header.frame_id = "base" pickgoal.header.stamp = rospy.Time.now() pickgoal.pose.position.x = xn pickgoal.pose.position.y = yn pickgoal.pose.position.z = zn pickgoal.pose.orientation.x = 1.0 pickgoal.pose.orientation.y = 0.0 pickgoal.pose.orientation.z = 0.0 pickgoal.pose.orientation.w = 0.0 # Move to the approach pickgoal. (5 cm to pickgoal) pickgoal.pose.position.z = zn+0.05 # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles. pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn) gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) pickgoal.pose.position.z = zn # Move to the pickgoal. gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.3, plan_only=False) # Read the force in z direction. b=leftarm.endpoint_effort() z_= b['force'] z_force= z_.z # Search again for objects, if the gripper isn't at the right position and presses on an object. #print("----->force in z direction:", z_force) if z_force>-4: leftgripper.close() attempts=0 pressure_ok=1 # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects. while(leftgripper.force()<25 and pressure_ok==1): time.sleep(0.04) attempts+=1 if(attempts>50): leftgripper.open() pressure_ok=0 print("----->pressure is to low<-----") else: print("----->gripper presses on an object<-----") # Move back to the approach pickgoal. pickgoal.pose.position.z = zn+0.05 gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) # Move both arms to position 1. g.moveToJointPosition(jts_both, pos1, max_velocity_scaling_factor = 1, plan_only=False) if pressure_ok and z_force>-4: # Define the approach placegoal. # Increase the height of the tower every time by 2.75 cm. placegoal.pose.position.z =-0.145+(k*0.0275)+0.08 # Move to the approach pickgoal. gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) # Define the placegoal. placegoal.pose.position.z =-0.145+(k*0.0275) gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) leftgripper.open() k += 1 # Move to the approach placegoal. placegoal.pose.position.z = -0.145+(k*0.0275)+0.08 gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) # Move left arm to start position pos1. gl.moveToJointPosition(jts_left, lpos1, max_velocity_scaling_factor = 1, plan_only=False) pr.disable() sortby = 'cumulative' ps=pstats.Stats(pr).sort_stats(sortby).print_stats(0.0) p.clear()
def picknplace(): # Define positions. pos1 = { 'left_e0': -1.441426162661994, 'left_e1': 0.8389151064712133, 'left_s0': 0.14240920034028015, 'left_s1': -0.14501001475655606, 'left_w0': -1.7630090377446503, 'left_w1': -1.5706376573674472, 'left_w2': 0.09225918246029519, 'right_e0': 1.7238109084167481, 'right_e1': 1.7169079948791506, 'right_s0': 0.36930587426147465, 'right_s1': -0.33249033539428713, 'right_w0': -1.2160632682067871, 'right_w1': 1.668587600115967, 'right_w2': -1.810097327636719 } rpos1 = { 'right_e0': 1.7238109084167481, 'right_e1': 1.7169079948791506, 'right_s0': 0.36930587426147465, 'right_s1': -0.33249033539428713, 'right_w0': -1.2160632682067871, 'right_w1': 1.668587600115967, 'right_w2': -1.810097327636719 } rpos2 = { 'right_e0': 1.8166167480533013, 'right_e1': 1.8768254939778037, 'right_s0': -0.6435049405179311, 'right_s1': -0.18867963690990588, 'right_w0': -1.2919953185964896, 'right_w1': 1.7215099392044055, 'right_w2': -2.557145973404985 } lpos1 = { 'left_e0': -1.441426162661994, 'left_e1': 0.8389151064712133, 'left_s0': 0.14240920034028015, 'left_s1': -0.14501001475655606, 'left_w0': -1.7630090377446503, 'left_w1': -1.5706376573674472, 'left_w2': 0.09225918246029519 } lpos2 = { 'left_e0': -0.14956312681882783, 'left_e1': 1.235238029444729, 'left_s0': 0.3186845086831947, 'left_s1': -0.0682621450609009, 'left_w0': 2.1586944637517487, 'left_w1': -1.5661943844310073, 'left_w2': -0.4962427848809313 } basket_start = geometry_msgs.msg.Pose() basket_start.position.x = 0.582905207564 basket_start.position.y = 0.428866088097 basket_start.position.z = -0.034942926096 basket_start.orientation.x = 0.334360832507 basket_start.orientation.y = 0.660214964821 basket_start.orientation.z = -0.352475264948 basket_start.orientation.w = 0.572782874667 placegoal = geometry_msgs.msg.Pose() placegoal.position.x = 0.6 placegoal.position.y = 0.6 placegoal.position.z = 0 placegoal.orientation.x = 1.0 placegoal.orientation.y = 0.0 placegoal.orientation.z = 0.0 placegoal.orientation.w = 0.0 # Define variables. offset_zero_point = 0.903 table_size_x = 0.714655654394 table_size_y = 1.05043717328 table_size_z = 0.729766045265 center_x = 0.457327827197 center_y = 0.145765166941 center_z = -0.538116977368 # The distance from the zero point in Moveit to the ground is 0.903 m. # The value is not allways the same. (look in Readme) center_z_cube = -offset_zero_point + table_size_z + 0.0275 / 2 center_z_basket = -offset_zero_point + table_size_z + 0.16 / 2 pressure_ok = 0 j = 0 k = 0 start = 1 locs_x = [] # Initialize a list for the objects and the stacked cubes. objlist = [ 'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11' ] boxlist = [ 'box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08', 'box09', 'box10', 'box11' ] # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) p.waitForSync() # Try to find and grip the basket till it is between the left gripper. while (pressure_ok == 0): # Move both arms to start state. both_arms.set_joint_value_target(pos1) both_arms.plan() both_arms.go(wait=True) time.sleep(0.5) # Wait till it detect the basket on the table. while (True): temp = rospy.wait_for_message("detected_basket", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(locs[i].position.x) locs_y.append(locs[i].position.y) if locs_x: break else: time.sleep(3) #sys.exit() # The first detected basket position isn't very precisly if the # basket isn't in an area under the right hand camera because # the function cvMinEnclosingCircle(c, ¢er, &radius) find # a circle around all the pink color. It moves to the position # and looks again with a better view. # Save the position where it normally looks from. rpos1_values = set_current_position("right") old_x_pos = rpos1_values.position.x old_y_pos = rpos1_values.position.y # Move to above the first detected position from the basket. rpos1_values.position.x = locs_x[0] rpos1_values.position.y = locs_y[0] right_arm.set_pose_target(rpos1_values) right_arm.plan() right_arm.go(wait=True) time.sleep(0.5) # Save the position where it looks the second time. second_view = set_current_position("right") new_x_pos = second_view.position.x new_y_pos = second_view.position.y # Calculate the offsets to the first position to get # the position in the baxter coordinate system. diff_x = new_x_pos - old_x_pos diff_y = new_y_pos - old_y_pos # Look the second time for the basket and add the offsets. temp = rospy.wait_for_message("detected_basket", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(locs[i].position.x + diff_x) locs_y.append(locs[i].position.y + diff_y) # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) #p.attachBox('basket', 0.05, 0.05, 0.16,locs_x[0], locs_y[0], center_z_basket, 'base') p.waitForSync() # Move the right arm next to the table to make more space for the # left arm. right_arm.set_joint_value_target(rpos2) right_arm.plan() right_arm.go(wait=True) # The offsets are an approximation to the right basket position. approach_basket = geometry_msgs.msg.Pose() approach_basket.position.x = locs_x[0] - 0.07 approach_basket.position.y = locs_y[0] + 0.14 approach_basket.position.z = -0.0780530728262 approach_basket.orientation.x = 0.355253135688 approach_basket.orientation.y = 0.64271546033 approach_basket.orientation.z = -0.311705465591 approach_basket.orientation.w = 0.60295252662 # The offsets cause the arm to move too far # that the basket can be better grip. basket = geometry_msgs.msg.Pose() basket.position.x = locs_x[0] + 0.02 basket.position.y = locs_y[0] - 0.03 basket.position.z = -0.0780530728262 basket.orientation.x = 0.32068927 basket.orientation.y = 0.6715956 basket.orientation.z = -0.3556723 basket.orientation.w = 0.565344935 # Move the left arm to the approach position. left_arm.set_pose_target(approach_basket) left_arm.plan() left_arm.go(wait=True) # Clear planning scene again to remove the attached basket. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) p.waitForSync() # Move the left arm to the basket position. move("left", basket) leftgripper.close() attempts = 0 pressure_ok = 1 # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects. while (leftgripper.force() < 25 and pressure_ok == 1): time.sleep(0.04) attempts += 1 if (attempts > 50): leftgripper.open() pressure_ok = 0 print("----->pressure is to low<-----") # Lift the basket up. basket_up = deepcopy(basket) basket_up.position.z = 0.025 move("left", basket_up) # Move to the start position with the basket. left_arm.set_joint_value_target(lpos2) left_arm.plan() left_arm.go(wait=True) # Move to the start position for the right arm. right_arm.set_joint_value_target(rpos1) right_arm.plan() right_arm.go(wait=True) # cProfile to measure the performance (time) of the task. pr = cProfile.Profile() pr.enable() # Loop to continue pick and place until all objects are cleared from table. while locs_x or start: # Only for the start. if start: start = 0 # Receive the data from all objects from the topic "detected_objects". time.sleep(1) temp = rospy.wait_for_message("detected_objects", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] # Adds the data from the objects. for i in range(len(locs)): locs_x.append(locs[i].position.x) locs_y.append(locs[i].position.y) orien.append(locs[i].position.z * pi / 180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected locations # for same objects and objects which are too far away. ind_rmv = [] for i in range(0, len(locs)): if (locs_y[i] > 0.3 or locs_x[i] > 0.75): ind_rmv.append(i) continue for j in range(i, len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2) < 0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Do the task only if there are still objects on the table. if locs_x: # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) # Initialize the data of the biggest object on the table. xn = locs_x[0] yn = locs_y[0] zn = -0.16 thn = orien[0] sz = size[0] if thn > pi / 4: thn = -1 * (thn % (pi / 4)) # Add the detected objects into the planning scene. #for i in range(1,len(locs_x)): #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube) p.waitForSync() # Initialize the approach pickgoal (5 cm to pickgoal). approach_pickgoal = geometry_msgs.msg.Pose() approach_pickgoal.position.x = xn approach_pickgoal.position.y = yn approach_pickgoal.position.z = zn + 0.05 approach_pickgoal_dummy = PoseStamped() approach_pickgoal_dummy.header.frame_id = "base" approach_pickgoal_dummy.header.stamp = rospy.Time.now() approach_pickgoal_dummy.pose.position.x = xn approach_pickgoal_dummy.pose.position.y = yn approach_pickgoal_dummy.pose.position.z = zn + 0.05 approach_pickgoal_dummy.pose.orientation.x = 1.0 approach_pickgoal_dummy.pose.orientation.y = 0.0 approach_pickgoal_dummy.pose.orientation.z = 0.0 approach_pickgoal_dummy.pose.orientation.w = 0.0 # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles. approach_pickgoal_dummy.pose = rotate_pose_msg_by_euler_angles( approach_pickgoal_dummy.pose, 0.0, 0.0, thn) approach_pickgoal.orientation.x = approach_pickgoal_dummy.pose.orientation.x approach_pickgoal.orientation.y = approach_pickgoal_dummy.pose.orientation.y approach_pickgoal.orientation.z = approach_pickgoal_dummy.pose.orientation.z approach_pickgoal.orientation.w = approach_pickgoal_dummy.pose.orientation.w # Move to the approach goal. right_arm.set_pose_target(approach_pickgoal) right_arm.plan() right_arm.go(wait=True) # Move to the pickgoal. pickgoal = deepcopy(approach_pickgoal) pickgoal.position.z = zn right_arm.set_pose_target(pickgoal) right_arm.plan() right_arm.go(wait=True) time.sleep(0.5) # Read the force in z direction. f_r = rightarm.endpoint_effort() z_r = f_r['force'] z_force = z_r.z # Search again for objects, if the gripper isn't at the right position and presses on an object. #print("----->force in z direction:", z_force) if z_force > -4: rightgripper.close() attempts = 0 pressure_ok = 1 # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects. while (rightgripper.force() < 25 and pressure_ok == 1): time.sleep(0.04) attempts += 1 if (attempts > 50): rightgripper.open() pressure_ok = 0 print("----->pressure is to low<-----") else: print("----->gripper presses on an object<-----") if pressure_ok and z_force > -4: # Move with the object in the gripper above the pickgoal. picked_object = set_current_position("right") picked_object.position.z = 0.2 right_arm.set_pose_target(picked_object) right_arm.plan() right_arm.go(wait=True) # Move with the basket under the gripper with the object. picked_object = set_current_position("right") picked_object.position.z = 0 picked_object.orientation.x = 0.334360832507 picked_object.orientation.y = 0.660214964821 picked_object.orientation.z = -0.352475264948 picked_object.orientation.w = 0.572782874667 move("left", picked_object) rightgripper.open() while (rightgripper.force() > 10): time.sleep(0.01) # Move the left arm back to the start position. move("left", basket_start) # Move the right arm back to the start position. right_arm.set_joint_value_target(rpos1) right_arm.plan() right_arm.go(wait=True) pr.disable() sortby = 'cumulative' ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0) p.clear() moveit_commander.roscpp_shutdown() # Exit MoveIt. moveit_commander.os._exit(0)
def picknplace(): # Define positions. pos1 = { 'left_e0': -1.69483279891317, 'left_e1': 1.8669726956453, 'left_s0': 0.472137005716569, 'left_s1': -0.38852045702393034, 'left_w0': -1.9770933862776057, 'left_w1': -1.5701993084642143, 'left_w2': -0.6339059781326424, 'right_e0': 1.7238109084167481, 'right_e1': 1.7169079948791506, 'right_s0': 0.36930587426147465, 'right_s1': -0.33249033539428713, 'right_w0': -1.2160632682067871, 'right_w1': 1.668587600115967, 'right_w2': -1.810097327636719 } lpos1 = { 'left_e0': -1.69483279891317, 'left_e1': 1.8669726956453, 'left_s0': 0.472137005716569, 'left_s1': -0.38852045702393034, 'left_w0': -1.9770933862776057, 'left_w1': -1.5701993084642143, 'left_w2': -0.6339059781326424 } rpos1 = { 'right_e0': 1.7238109084167481, 'right_e1': 1.7169079948791506, 'right_s0': 0.36930587426147465, 'right_s1': -0.33249033539428713, 'right_w0': -1.2160632682067871, 'right_w1': 1.668587600115967, 'right_w2': -1.810097327636719 } rpos = geometry_msgs.msg.Pose() rpos.position.x = 0.555 rpos.position.y = 0.0 rpos.position.z = 0.206 rpos.orientation.x = 1.0 rpos.orientation.y = 0.0 rpos.orientation.z = 0.0 rpos.orientation.w = 0.0 lpos = geometry_msgs.msg.Pose() lpos.position.x = 0.65 lpos.position.y = 0.6 lpos.position.z = 0.206 lpos.orientation.x = 1.0 lpos.orientation.y = 0.0 lpos.orientation.z = 0.0 lpos.orientation.w = 0.0 placegoal = geometry_msgs.msg.Pose() placegoal.position.x = 0.55 placegoal.position.y = 0.2 # Open the gripper 4 mm above the tip of the tower. placegoal.position.z = 0 placegoal.orientation.x = 1.0 placegoal.orientation.y = 0.0 placegoal.orientation.z = 0.0 placegoal.orientation.w = 0.0 # Define variables. table_size_x = 1.0 table_size_y = 0.929999123509 table_size_z = 0.75580154342 center_x = 0.6 center_y = -0.0441882472378 center_z = -0.52509922829 # The distance from the zero point in Moveit to the ground is 0.903 m. # The value is not allways the same. (look in Readme) center_z_cube = -0.903 + table_size_z + 0.0275 / 2 j = 0 k = 0 u = 0 locs_x = [] # Initialize a list for the objects and the stacked cubes. objlist = [ 'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11' ] boxlist = [ 'box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08', 'box09', 'box10', 'box11' ] #rightarm.move_to_joint_positions(rpos1) right_arm.set_pose_target(rpos) right_arm.plan() right_arm.go(wait=True) both_arms.set_joint_value_target(pos1) both_arms.plan() both_arms.go(wait=True) # Load Gazebo Models via Spawning Services. load_gazebo_models() # Remove models from the scene on shutdown. rospy.on_shutdown(delete_gazebo_models) # Wait for the All Clear from emulator startup. rospy.wait_for_message("/robot/sim/started", Empty) # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) p.waitForSync() rightgripper.open() # cProfile to measure the performance (time) of the task. pr = cProfile.Profile() pr.enable() # Loop to continue pick and place until all objects are cleared from table. while k < 4: locs_x = [0.6725 + 0.02, 0.55 + 0.02, 0.7 + 0.02, 0.58 + 0.02] locs_y = [0.0765 + 0.02, -0.2 + 0.02, -0.1 + 0.02, -0.03 + 0.02] orien = [0, 0, 0, 0] size = [5, 5, 5, 5] # Filter objects list to remove multiple detected locations for same objects. ind_rmv = [] for i in range(0, len(locs_x)): if (locs_y[i] > 0.24): ind_rmv.append(i) continue for j in range(i, len(locs_x)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2) < 0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Do the task only if there are still objects on the table. while u < len(locs_x): # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) # Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes. ig0 = itemgetter(0) sorted_lists = zip(*sorted( zip(size, locs_x, locs_y, orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Initialize the data of the biggest object on the table. xn = locs_x[u] yn = locs_y[u] # -0.16 is the z position to grip the objects on the table. zn = -0.143 thn = orien[u] sz = size[u] if thn > pi / 4: thn = -1 * (thn % (pi / 4)) # Add the detected objects into the planning scene. for i in range(1, len(locs_x)): p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube) # Add the stacked objects as collision objects into the planning scene to avoid moving against them. for e in range(0, k): p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.position.x, placegoal.position.y, center_z_cube + 0.0275 * (e - 1), 'base', touch_links=['cubes']) p.waitForSync() # Initialize the approach pickgoal (5 cm to pickgoal). approach_pickgoal = geometry_msgs.msg.Pose() approach_pickgoal.position.x = xn approach_pickgoal.position.y = yn approach_pickgoal.position.z = zn + 0.05 approach_pickgoal_dummy = PoseStamped() approach_pickgoal_dummy.header.frame_id = "base" approach_pickgoal_dummy.header.stamp = rospy.Time.now() approach_pickgoal_dummy.pose.position.x = xn approach_pickgoal_dummy.pose.position.y = yn approach_pickgoal_dummy.pose.position.z = zn + 0.05 approach_pickgoal_dummy.pose.orientation.x = 1.0 approach_pickgoal_dummy.pose.orientation.y = 0.0 approach_pickgoal_dummy.pose.orientation.z = 0.0 approach_pickgoal_dummy.pose.orientation.w = 0.0 # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles. approach_pickgoal_dummy.pose = rotate_pose_msg_by_euler_angles( approach_pickgoal_dummy.pose, 0.0, 0.0, thn) approach_pickgoal.orientation.x = approach_pickgoal_dummy.pose.orientation.x approach_pickgoal.orientation.y = approach_pickgoal_dummy.pose.orientation.y approach_pickgoal.orientation.z = approach_pickgoal_dummy.pose.orientation.z approach_pickgoal.orientation.w = approach_pickgoal_dummy.pose.orientation.w # Move to the approach goal and the pickgoal. pickgoal = deepcopy(approach_pickgoal) pickgoal.position.z = zn move('right', approach_pickgoal, pickgoal) time.sleep(1) rightgripper.close() time.sleep(0.5) # Move back to the approach pickgoal. move('right', approach_pickgoal) # Define the approach placegoal. # Increase the height of the tower every time by 2.75 cm. approached_placegoal = deepcopy(placegoal) approached_placegoal.position.z = -0.143 + (k * 0.0275) + 0.08 # Define the placegoal. placegoal.position.z = -0.143 + (k * 0.0275) move('right', approached_placegoal, placegoal) rightgripper.open() time.sleep(1) # Move to the approach placegoal. move('right', approached_placegoal) k += 1 u += 1 # Move right arm to start position. both_arms.set_joint_value_target(pos1) both_arms.plan() both_arms.go(wait=True) pr.disable() sortby = 'cumulative' ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0) moveit_commander.roscpp_shutdown() # Exit MoveIt. moveit_commander.os._exit(0) time.sleep(10)
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)
def picknplace(): # Define positions. pos1 = { 'left_e0': -1.69483279891317, 'left_e1': 1.8669726956453, 'left_s0': 0.472137005716569, 'left_s1': -0.38852045702393034, 'left_w0': -1.9770933862776057, 'left_w1': -1.5701993084642143, 'left_w2': -0.6339059781326424, 'right_e0': 1.7238109084167481, 'right_e1': 1.7169079948791506, 'right_s0': 0.36930587426147465, 'right_s1': -0.33249033539428713, 'right_w0': -1.2160632682067871, 'right_w1': 1.668587600115967, 'right_w2': -1.810097327636719 } lpos1 = { 'left_e0': -1.69483279891317, 'left_e1': 1.8669726956453, 'left_s0': 0.472137005716569, 'left_s1': -0.38852045702393034, 'left_w0': -1.9770933862776057, 'left_w1': -1.5701993084642143, 'left_w2': -0.6339059781326424 } rpos1 = { 'right_e0': 1.7238109084167481, 'right_e1': 1.7169079948791506, 'right_s0': 0.36930587426147465, 'right_s1': -0.33249033539428713, 'right_w0': -1.2160632682067871, 'right_w1': 1.668587600115967, 'right_w2': -1.810097327636719 } placegoal = geometry_msgs.msg.Pose() placegoal.position.x = 0.55 placegoal.position.y = 0.22 placegoal.position.z = 0 placegoal.orientation.x = 1.0 placegoal.orientation.y = 0.0 placegoal.orientation.z = 0.0 placegoal.orientation.w = 0.0 # Define variables. offset_zero_point = 0.903 table_size_x = 0.714655654394 table_size_y = 1.05043717328 table_size_z = 0.729766045265 center_x = 0.457327827197 center_y = 0.145765166941 center_z = -0.538116977368 # The distance from the zero point in Moveit to the ground is 0.903 m. # The value is not allways the same. (look in Readme) center_z_cube = -offset_zero_point + table_size_z + 0.0275 / 2 pressure_l_ok = 0 pressure_r_ok = 0 left_ready = 0 right_ready = 0 j = 0 k = 0 start = 1 locs_x_right = [] locs_x_left = [] # Initialize a list for the objects and the stacked cubes. objlist = [ 'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11' ] boxlist = [ 'box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08', 'box09', 'box10', 'box11' ] # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) p.waitForSync() # Move both arms to start state. both_arms.set_joint_value_target(pos1) both_arms.plan() both_arms.go(wait=True) # cProfile to measure the performance (time) of the task. pr = cProfile.Profile() pr.enable() # Loop to continue pick and place until all objects are cleared from table. while locs_x_right or locs_x_left or start: # Only for the start. if start: start = 0 time.sleep(0.5) # Receive the data from all objects from the topic "detected_objects_left". temp_left = rospy.wait_for_message("detected_objects_left", PoseArray) locs_left = temp_left.poses # Receive the data from all objects from the topic "detected_objects_right". temp_right = rospy.wait_for_message("detected_objects_right", PoseArray) locs_right = temp_right.poses locs_x_right = [] locs_y_right = [] orien_right = [] size_right = [] locs_x_left = [] locs_y_left = [] orien_left = [] size_left = [] # Adds the data from the objects from the left camera. for i in range(len(locs_left)): locs_x_left.append(locs_left[i].position.x) locs_y_left.append(locs_left[i].position.y) orien_left.append(locs_left[i].position.z * pi / 180) size_left.append(locs_left[i].orientation.x) # Adds the data from the objects from the right camera. for i in range(len(locs_right)): locs_x_right.append(locs_right[i].position.x) locs_y_right.append(locs_right[i].position.y) orien_right.append(locs_right[i].position.z * pi / 180) size_right.append(locs_right[i].orientation.x) # Filter objects list to remove multiple detected locations for same objects (left camera). ind_rmv = [] for i in range(0, len(locs_left)): if (locs_y_left[i] < 0.28 or locs_y_left[i] > 0.62 or locs_x_left[i] > 0.75): ind_rmv.append(i) continue for j in range(i, len(locs_left)): if not (i == j): if sqrt((locs_x_left[i] - locs_x_left[j])**2 + (locs_y_left[i] - locs_y_left[j])**2) < 0.018: ind_rmv.append(i) locs_x_left = del_meth(locs_x_left, ind_rmv) locs_y_left = del_meth(locs_y_left, ind_rmv) orien_left = del_meth(orien_left, ind_rmv) size_left = del_meth(size_left, ind_rmv) # Filter objects list to remove multiple detected locations for same objects (right camera). ind_rmv = [] for i in range(0, len(locs_right)): if (locs_y_right[i] > 0.16 or locs_x_right[i] > 0.75): ind_rmv.append(i) continue for j in range(i, len(locs_right)): if not (i == j): if sqrt((locs_x_right[i] - locs_x_right[j])**2 + (locs_y_right[i] - locs_y_right[j])**2) < 0.018: ind_rmv.append(i) locs_x_right = del_meth(locs_x_right, ind_rmv) locs_y_right = del_meth(locs_y_right, ind_rmv) orien_right = del_meth(orien_right, ind_rmv) size_right = del_meth(size_right, ind_rmv) # Do the task only if there are still objects on the table. if locs_x_right: # Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes. ig0_right = itemgetter(0) sorted_lists_right = zip(*sorted(zip(size_right, locs_x_right, locs_y_right, orien_right), reverse=True, key=ig0_right)) locs_x_right = list(sorted_lists_right[1]) locs_y_right = list(sorted_lists_right[2]) orien_right = list(sorted_lists_right[3]) size_right = list(sorted_lists_right[0]) # Initialize the data of the biggest object on the table. xn_right = locs_x_right[0] yn_right = locs_y_right[0] zn_right = -0.16 thn_right = orien_right[0] sz_right = size_right[0] if thn_right > pi / 4: thn_right = -1 * (thn_right % (pi / 4)) if locs_x_left: ig0_left = itemgetter(0) sorted_lists_left = zip( *sorted(zip(size_left, locs_x_left, locs_y_left, orien_left), reverse=True, key=ig0_left)) locs_x_left = list(sorted_lists_left[1]) locs_y_left = list(sorted_lists_left[2]) orien_left = list(sorted_lists_left[3]) size_left = list(sorted_lists_left[0]) # Initialize the data of the biggest object on the table. xn_left = locs_x_left[0] yn_left = locs_y_left[0] zn_left = -0.145 thn_left = orien_left[0] sz_left = size_left[0] if thn_left > pi / 4: thn_left = -1 * (thn_left % (pi / 4)) if locs_x_right or locs_x_left: # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) # Add the detected objects into the planning scene. #for i in range(1,len(locs_x_left)): #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x_left[i], locs_y_left[i], center_z_cube) #for i in range(1,len(locs_x_right)): #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x_right[i], locs_y_right[i], center_z_cube) # Add the stacked objects as collision objects into the planning scene to avoid moving against them. #for e in range(0, k): #p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.position.x, placegoal.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes']) if k > 0: p.attachBox(boxlist[0], 0.07, 0.07, 0.0275 * k, placegoal.position.x, placegoal.position.y, center_z_cube, 'base', touch_links=['cubes']) p.waitForSync() if left_ready == 0 and locs_x_left: # Initialize the approach pickgoal left (5 cm to pickgoal). approach_pickgoal_l = geometry_msgs.msg.Pose() approach_pickgoal_l.position.x = xn_left approach_pickgoal_l.position.y = yn_left approach_pickgoal_l.position.z = zn_left + 0.05 approach_pickgoal_l_dummy = PoseStamped() approach_pickgoal_l_dummy.header.frame_id = "base" approach_pickgoal_l_dummy.header.stamp = rospy.Time.now() approach_pickgoal_l_dummy.pose.position.x = xn_left approach_pickgoal_l_dummy.pose.position.y = yn_left approach_pickgoal_l_dummy.pose.position.z = zn_left + 0.05 approach_pickgoal_l_dummy.pose.orientation.x = 1.0 approach_pickgoal_l_dummy.pose.orientation.y = 0.0 approach_pickgoal_l_dummy.pose.orientation.z = 0.0 approach_pickgoal_l_dummy.pose.orientation.w = 0.0 # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles. approach_pickgoal_l_dummy.pose = rotate_pose_msg_by_euler_angles( approach_pickgoal_l_dummy.pose, 0.0, 0.0, thn_left) approach_pickgoal_l.orientation.x = approach_pickgoal_l_dummy.pose.orientation.x approach_pickgoal_l.orientation.y = approach_pickgoal_l_dummy.pose.orientation.y approach_pickgoal_l.orientation.z = approach_pickgoal_l_dummy.pose.orientation.z approach_pickgoal_l.orientation.w = approach_pickgoal_l_dummy.pose.orientation.w # Move to the approach goal. left_arm.set_pose_target(approach_pickgoal_l) left_arm.plan() left_arm.go(wait=True) # Move to the pickgoal. pickgoal_l = deepcopy(approach_pickgoal_l) pickgoal_l.position.z = zn_left left_arm.set_pose_target(pickgoal_l) left_arm.plan() left_arm.go(wait=True) time.sleep(0.5) # Read the force in z direction. f_l = leftarm.endpoint_effort() z_l = f_l['force'] z_force_l = z_l.z # Search again for objects, if the gripper isn't at the right position and presses on an object. #print("----->force in z direction:", z_force_l) if z_force_l > -4: leftgripper.close() attempts = 0 pressure_l_ok = 1 # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects. while (leftgripper.force() < 25 and pressure_l_ok == 1): time.sleep(0.04) attempts += 1 if (attempts > 50): leftgripper.open() pressure_l_ok = 0 print("----->pressure is to low<-----") else: print("----->gripper presses on an object<-----") # Move back to the approach pickgoal. left_arm.set_pose_target(approach_pickgoal_l) left_arm.plan() left_arm.go(wait=True) if pressure_l_ok and z_force_l > -4: left_ready = 1 else: # Move back to lpos1. left_arm.set_joint_value_target(lpos1) left_arm.plan() left_arm.go(wait=True) if (left_ready == 1 or not locs_x_left) and locs_x_right: # Initialize the approach pickgoal right (5 cm to pickgoal). approach_pickgoal_r = geometry_msgs.msg.Pose() approach_pickgoal_r.position.x = xn_right approach_pickgoal_r.position.y = yn_right approach_pickgoal_r.position.z = zn_right + 0.05 approach_pickgoal_r_dummy = PoseStamped() approach_pickgoal_r_dummy.header.frame_id = "base" approach_pickgoal_r_dummy.header.stamp = rospy.Time.now() approach_pickgoal_r_dummy.pose.position.x = xn_right approach_pickgoal_r_dummy.pose.position.y = yn_right approach_pickgoal_r_dummy.pose.position.z = zn_right + 0.05 approach_pickgoal_r_dummy.pose.orientation.x = 1.0 approach_pickgoal_r_dummy.pose.orientation.y = 0.0 approach_pickgoal_r_dummy.pose.orientation.z = 0.0 approach_pickgoal_r_dummy.pose.orientation.w = 0.0 # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles. approach_pickgoal_r_dummy.pose = rotate_pose_msg_by_euler_angles( approach_pickgoal_r_dummy.pose, 0.0, 0.0, thn_right) approach_pickgoal_r.orientation.x = approach_pickgoal_r_dummy.pose.orientation.x approach_pickgoal_r.orientation.y = approach_pickgoal_r_dummy.pose.orientation.y approach_pickgoal_r.orientation.z = approach_pickgoal_r_dummy.pose.orientation.z approach_pickgoal_r.orientation.w = approach_pickgoal_r_dummy.pose.orientation.w # Move to the approach goal. right_arm.set_pose_target(approach_pickgoal_r) right_arm.plan() right_arm.go(wait=True) # Move to the pickgoal. pickgoal_r = deepcopy(approach_pickgoal_r) pickgoal_r.position.z = zn_right right_arm.set_pose_target(pickgoal_r) right_arm.plan() right_arm.go(wait=True) time.sleep(0.5) # Read the force in z direction. f_r = rightarm.endpoint_effort() z_r = f_r['force'] z_force_r = z_r.z # Search again for objects, if the gripper isn't at the right position and presses on an object. #print("----->force in z direction:", z_force_l) if z_force_r > -4: rightgripper.close() attempts = 0 pressure_r_ok = 1 # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects. while (rightgripper.force() < 25 and pressure_r_ok == 1): time.sleep(0.04) attempts += 1 if (attempts > 50): rightgripper.open() pressure_r_ok = 0 print("----->pressure is to low<-----") else: print("----->gripper presses on an object<-----") # Move back to the approach pickgoal. right_arm.set_pose_target(approach_pickgoal_r) right_arm.plan() right_arm.go(wait=True) if pressure_r_ok and z_force_r > -4: right_ready = 1 else: # Move back to rpos1. right_arm.set_joint_value_target(rpos1) right_arm.plan() right_arm.go(wait=True) if (left_ready == 1 or not locs_x_left) and (right_ready == 1 or not locs_x_right): # Move both arms to start state. #both_arms.set_joint_value_target(pos1) #both_arms.plan() #both_arms.go(wait=True) if (left_ready == 1): # Define the approach placegoal for the left arm. # Increase the height of the tower every time by 2.75 cm. approached_placegoal = deepcopy(placegoal) approached_placegoal.position.z = -0.14 + (k * 0.0275) + 0.08 # Move to the approach placegoal. left_arm.set_pose_target(approached_placegoal) left_arm.plan() left_arm.go(wait=True) # Define the placegoal. placegoal.position.z = -0.14 + (k * 0.0275) placegoal.position.x = 0.54 left_arm.set_pose_target(placegoal) left_arm.plan() left_arm.go(wait=True) leftgripper.open() while (leftgripper.force() > 10): time.sleep(0.01) k += 1 # Move back to the approach placegoal. left_arm.set_pose_target(approached_placegoal) left_arm.plan() left_arm.go(wait=True) # Move back to lpos1. left_arm.set_joint_value_target(lpos1) left_arm.plan() left_arm.go(wait=True) left_ready = 0 if (right_ready == 1): # Define the approach placegoal for the right arm. # Increase the height of the tower every time by 2.75 cm. approached_placegoal = deepcopy(placegoal) approached_placegoal.position.z = -0.155 + (k * 0.0275) + 0.08 # Move to the approach placegoal. right_arm.set_pose_target(approached_placegoal) right_arm.plan() right_arm.go(wait=True) # Define the placegoal. placegoal.position.z = -0.155 + (k * 0.0275) placegoal.position.x = 0.53 right_arm.set_pose_target(placegoal) right_arm.plan() right_arm.go(wait=True) rightgripper.open() while (rightgripper.force() > 10): time.sleep(0.01) k += 1 # Move back to the approach placegoal. right_arm.set_pose_target(approached_placegoal) right_arm.plan() right_arm.go(wait=True) # Move back to rpos1. right_arm.set_joint_value_target(rpos1) right_arm.plan() right_arm.go(wait=True) right_ready = 0 pr.disable() sortby = 'cumulative' ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0) p.clear() moveit_commander.roscpp_shutdown() # Exit MoveIt. moveit_commander.os._exit(0)
def clasificar(): adaptative = True pr_b = False precision = 0.7 # Can free memory? pub = rospy.Publisher("finished", String, queue_size=10) # Initialize MoveIt! scene p = PlanningSceneInterface("base") arms_group = MoveGroupInterface("both_arms", "base") rightarm_group = MoveGroupInterface("right_arm", "base") leftarm_group = MoveGroupInterface("left_arm", "base") # Create right arm instance right_arm = baxter_interface.limb.Limb('right') # Create right gripper instance right_gripper = baxter_interface.Gripper('right') right_gripper.calibrate() right_gripper.open() right_gripper.close() offset_zero_point = 0.903 table_size_x = 0.714655654394 table_size_y = 1.05043717328 table_size_z = 0.729766045265 center_x = 0.457327827197 center_y = 0.145765166941 center_z = -0.538116977368 table_distance_from_gripper = -offset_zero_point + table_size_z + 0.0275 / 2 j = 0 k = 0 # Initialize object list objlist = [ 'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11' ] p.clear() p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) p.waitForSync() # Move both arms to start state. # Initial pos rpos = PoseStamped() rpos.header.frame_id = "base" rpos.header.stamp = rospy.Time.now() rpos.pose.position.x = 0.555 rpos.pose.position.y = 0.0 rpos.pose.position.z = 0.206 rpos.pose.orientation.x = 1.0 rpos.pose.orientation.y = 0.0 rpos.pose.orientation.z = 0.0 rpos.pose.orientation.w = 0.0 lpos = PoseStamped() lpos.header.frame_id = "base" lpos.header.stamp = rospy.Time.now() lpos.pose.position.x = 0.65 lpos.pose.position.y = 0.6 lpos.pose.position.z = 0.206 lpos.pose.orientation.x = 1.0 lpos.pose.orientation.y = 0.0 lpos.pose.orientation.z = 0.0 lpos.pose.orientation.w = 0.0 while True: # Move to initial position rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) leftarm_group.moveToPose(lpos, "left_gripper", max_velocity_scaling_factor=1, plan_only=False) # Get the middle point locs = PoseArray() locs = rospy.wait_for_message("clasificacion", PoseArray) if (len(locs.poses) != 0): punto_medio = PoseStamped() punto_medio.header.frame_id = "base" punto_medio.header.stamp = rospy.Time.now() punto_medio.pose.position.x = locs.poses[0].position.x punto_medio.pose.position.y = locs.poses[0].position.y punto_medio.pose.position.z = locs.poses[0].position.z punto_medio.pose.orientation.x = locs.poses[0].orientation.x punto_medio.pose.orientation.y = locs.poses[0].orientation.y punto_medio.pose.orientation.z = locs.poses[0].orientation.z punto_medio.pose.orientation.w = locs.poses[0].orientation.w alfa = 0.1 # Two parameters: # When pr_b == 1, the program will try to adapt the trajectory for getting the precision established in the parameter of the same name # When adaptative == 1, the program will try to get the best precision based on the precision of the last execution. if (pr_b and not adaptative): if precision >= 1: punto_medio.pose.position.x += 0.01 else: punto_medio.pose.position.x -= alfa * (1 - precision) else: print("If it is the first time executing, write -1") precision_value = input() if precision_value != -1.0: punto_medio.pose.position.x += alfa * (1 - precision_value) elif precision_value == 1.0: adaptative = False # Get the normal orientation for separating the objects orient = (-1) * (1 / punto_medio.pose.orientation.x) punto_medio.pose = rotate_pose_msg_by_euler_angles( punto_medio.pose, 0.0, 0.0, orient) rightarm_group.moveToPose(punto_medio, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) orient = 1.57 - orient punto_medio.pose = rotate_pose_msg_by_euler_angles( punto_medio.pose, 0.0, 0.0, orient) rightarm_group.moveToPose(punto_medio, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) punto_medio.pose.position.x += 0.15 rightarm_group.moveToPose(punto_medio, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) time.sleep(1) punto_medio.pose.position.x -= 0.3 rightarm_group.moveToPose(punto_medio, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) time.sleep(1) rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) # Free the memory when finished freemem = "free" if not adaptative: pub.publish(freemem) else: sys.exit("Cannot identify any object")