def main(): rospy.init_node('part1_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('floor') planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2) table_height = 0.767 table_width = 0.7 table_x = 0.95 planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height / 2) planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37 / 2) microwave_height = 0.28 microwave_width = 0.48 # microwave_depth = 0.33 microwave_depth = 0.27 microwave_x = 0.97 microwave_z = 0.06 microwave_y = 0.18 planning_scene.addBox('microwave', microwave_depth, microwave_width, microwave_height, microwave_x, microwave_y, table_height + microwave_z + microwave_height / 2) rospy.sleep(2) rospy.spin()
def main(): rospy.init_node('a5_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('top_shelf') planning_scene.removeCollisionObject('bottom_shelf') planning_scene.addBox('top_shelf', 0.32, 1, 0.4, 0.99, 0, 1.64) planning_scene.addBox('bottom_shelf', 0.5, 1, 1.09, 0.9, 0, 0.545) rospy.sleep(2)
def main(): rospy.init_node('a5_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('floor') planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2) planning_scene.addBox('table', 0.5, 1, 0.72, 1, 0, 0.72 / 2) rospy.sleep(2)
def main(): rospy.init_node('part2_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('floor') planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01/2) table_height = 0.767 table_width = 0.7 table_x = 0.95 planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height/2) planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37/2) rospy.sleep(2) rospy.spin()
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 main(): rospy.init_node('pour_scene') wait_for_time() target_name = 'cup1' x_pose = 0.329202820349 y_pose = -0.01 z_pose = 0.060 x_ori, y_ori, z_ori, w_ori = cvt_e2q(0, 0, 0) x, y, z, _ = getState('cup2') head = fetch_api.Head() arm = fetch_api.Arm() torso = fetch_api.Torso() torso.set_height(fetch_api.Torso.MAX_HEIGHT) head.look_at('base_link', x, y, z) sess = tensorflow.Session() model = load_model(sess) x, y, z, _ = getState(target_name) # observation test get_observation() move_arm_to(arm, x_pose, y + y_pose, z + z_pose, x_ori, y_ori, z_ori, w_ori) x, y, z, _ = getState('table1') base = fetch_api.Base() if x > 1: base.go_forward(0.6, speed=0.2) # collision in motion planning planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() length_table = 1 width_table = 1 height_table = 0.04 x_table, y_table, z_table, _ = getState('table1') z_table = z + 0.7 planning_scene.addBox('table', length_table, width_table, height_table, x_table, y_table, z_table) length_box = 0.05 width_box = 0.05 height_box = 0.2 x, y, z, _ = getState('cup1') x_box = x y_box = y z_box = z planning_scene.addBox('mug_1', length_box, width_box, height_box, x_box, y_box, z_box) length_box = 0.03 width_box = 0.05 height_box = 0.2 x, y, z, _ = getState('cup2') x_box = x y_box = y z_box = z planning_scene.addBox('mug_2', length_box, width_box, height_box, x_box, y_box, z_box) # the initial position of gripper is (-0.5, 0, 0), and # the final position of gripper is (-0.5+x_pose, y_pose, z_pose). x, y, z, _ = getState(target_name) move_arm_to(arm, x - 0.5 + x_pose, y + y_pose, z + z_pose, x_ori, y_ori, z_ori, w_ori) planning_scene.removeCollisionObject('mug_1') # planning_scene.removeCollisionObject('mug_2') # planning_scene.removeCollisionObject('mug') # planning_scene.removeCollisionObject('table') gripper = fetch_api.Gripper() effort = gripper.MAX_EFFORT gripper.close(effort) x, y, z, _ = getState(target_name) move_arm_to(arm, x - 0.5 + x_pose, y + y_pose, z + z_pose + 0.06, x_ori, y_ori, z_ori, w_ori) x, y, z, _ = getState('cup2') head.look_at('base_link', x, y, z) for _ in xrange(50): obs = get_observation() act = model.choose_action(obs) print('obs: {}, action: {}'.format(obs, act)) execute_action(act, arm) time.sleep(0.5)
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 main(): rospy.init_node('arm_obstacle_demo') wait_for_time() argv = rospy.myargv() torso = fetch_api.Torso() torso.set_height(fetch_api.Torso.MAX_HEIGHT) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 planning_scene = PlanningSceneInterface('base_link') # Create table obstacle planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.4 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) planning_scene.removeAttachedObject('tray') arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 20, 'execution_timeout': 40, 'num_planning_attempts': 50, 'replan': False, } error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) #kwargs['orientation_constraint'] = oc error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray') planning_scene.clear()
def main(): # Create table obstacle rospy.init_node('arm_obstacle_demo') planning_scene = PlanningSceneInterface('base_link') planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle # planning_scene.removeCollisionObject('divider') # size_x = 0.3 # size_y = 0.01 # size_z = 0.4 # x = table_x - (table_size_x / 2) + (size_x / 2) # y = 0 # z = table_z + (table_size_z / 2) + (size_z / 2) # planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) #Orientation constraint oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 # move to pose args kwargs = { 'allowed_planning_time': 20, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': True, 'orientation_constraint': oc } # Before moving to the first pose planning_scene.removeAttachedObject('tray') error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray') # Was working without this but now its needed planning_scene.clear()
class GraspingClient(object): def __init__(self, sim=False): self.scene = PlanningSceneInterface("base_link") self.dof = rospy.get_param('~jaco_dof') self.move_group = MoveGroupInterface("upper_body", "base_link") self.lmove_group = MoveGroupInterface("left_arm", "base_link") self.rmove_group = MoveGroupInterface("right_arm", "base_link") self.move_group.setPlannerId("RRTConnectkConfigDefault") self.lmove_group.setPlannerId("RRTConnectkConfigDefault") self.rmove_group.setPlannerId("RRTConnectkConfigDefault") if "6dof" == self.dof: self._upper_body_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint", "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint" ] self.tucked = [ -2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571, 0.0371, 0.0, 0.0 ] self.constrained_stow = [ 2.28, 2.17, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56, 0.09, -0.15, 2.06, 0.42, 0.0, 0.0 ] self.larm_const_stow = [-2.28, -2.17, 2.56, 0.09, -0.15, 2.08] self.rarm_const_stow = [2.28, 2.17, -2.56, -0.09, 0.15, 1.06] self.tableDist = 0.7 elif "7dof" == self.dof: self._upper_body_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint", "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint" ] self.tucked = [ -1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7, 0.0, -0.5, 1.7, 0.04, 0, 0 ] self.constrained_stow = [ -2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0 ] self.larm_const_stow = [2.6, -2.0, 0.0, -2.0, 0.0, 0.0, 1.0] self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0] self.tableDist = 0.8 else: rospy.logerr("DoF needs to be set 6 or 7, aborting demo") return self.pickplace = [None] * 2 self.pickplace[0] = PickPlaceInterface("left_side", "left_gripper", verbose=True) self.pickplace[0].planner_id = "RRTConnectkConfigDefault" self.pickplace[1] = PickPlaceInterface("right_side", "right_gripper", verbose=True) self.pickplace[1].planner_id = "RRTConnectkConfigDefault" self.pick_result = [None] * 2 self._last_gripper_picked = 0 self.place_result = [None] * 2 self._last_gripper_placed = 0 self._objs_to_keep = [] self._listener = tf.TransformListener() self._lgripper = GripperActionClient('left') self._rgripper = GripperActionClient('right') find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() self.scene.clear() # This is a simulation so need to adjust gripper parameters if sim: self._gripper_closed = 0.96 self._gripper_open = 0.00 else: self._gripper_closed = 0.01 self._gripper_open = 0.165 def add_objects_to_keep(self, obj): self._objs_to_keep.append(obj) def clearScene(self): self.scene.clear() def getPickCoordinates(self): self.updateScene(0, False) beer, grasps = self.getGraspableBeer(False) pringles, grasps = self.getGraspablePringles(False) if (None == beer) or (None == pringles): return None center_objects = (beer.primitive_poses[0].position.y + pringles.primitive_poses[0].position.y) / 2 surface = self.getSupportSurface(beer.support_surface) tmp1 = surface.primitive_poses[ 0].position.x - surface.primitives[0].dimensions[0] / 2 surface = self.getSupportSurface(pringles.support_surface) tmp2 = surface.primitive_poses[ 0].position.x - surface.primitives[0].dimensions[0] / 2 front_edge = (tmp1 + tmp2) / 2 coords = Pose2D(x=(front_edge - self.tableDist), y=center_objects, theta=0.0) return coords def updateScene(self, gripper=0, plan=True): # find objects rospy.loginfo("Updating scene...") goal = FindGraspableObjectsGoal() goal.plan_grasps = plan goal.gripper = gripper self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, True) # insert objects to scene idx = -1 for obj in find_result.objects: if obj.object.primitive_poses[ 0].position.z < 0.5 or obj.object.primitive_poses[ 0].position.x > 2.0 or obj.object.primitive_poses[ 0].position.y > 0.5: continue idx += 1 obj.object.name = "object%d_%d" % (idx, gripper) self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=True) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view if obj.primitive_poses[0].position.z < 0.5: continue height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0], obj.primitives[0].dimensions[1], # wider obj.primitives[0].dimensions[2] + height ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=True) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableBeer(self, planned=True): for obj in self.objects: # need grasps if len(obj.grasps) < 1 and planned: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5 or \ obj.object.primitive_poses[0].position.z > 1.0 or \ obj.object.primitive_poses[0].position.x > 2.0: continue elif (obj.object.primitives[0].type == sp.CYLINDER): if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.102 or \ obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.142: continue elif (obj.object.primitives[0].type == sp.BOX): if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.102 or \ obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.142: continue else: continue print "beer: ", obj.object.primitive_poses[0] return obj.object, obj.grasps # nothing detected return None, None def getGraspablePringles(self, planned=True): for obj in self.objects: # need grasps if len(obj.grasps) < 1 and planned: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5 or \ obj.object.primitive_poses[0].position.z > 1.0 or \ obj.object.primitive_poses[0].position.x > 2.0: continue elif (obj.object.primitives[0].type == sp.CYLINDER): if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.21 or \ obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.28: continue elif (obj.object.primitives[0].type == sp.BOX): if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.21 or \ obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.28: continue else: continue return obj.object, obj.grasps # nothing detected return None, None def getSupportSurface(self, name): for surface in self.surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps, gripper=0): success, pick_result = self.pickplace[gripper].pick_with_retry( block.name, grasps, retries=1, support_name=block.support_surface, scene=self.scene) self.pick_result[gripper] = pick_result self._last_gripper_picked = gripper 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 goto_tuck(self): # remove previous objects while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition( self._upper_body_joints, self.tucked, 0.05) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def goto_plan_grasp(self): while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition( self._upper_body_joints, self.constrained_stow, 0.05) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def left_arm_constrained_stow(self): c1 = Constraints() c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.stamp = rospy.get_rostime() c1.orientation_constraints[0].header.frame_id = "base_link" c1.orientation_constraints[0].link_name = "left_ee_link" c1.orientation_constraints[0].orientation.w = 1.0 c1.orientation_constraints[ 0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2 c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28 c1.orientation_constraints[0].weight = 1.0 while not rospy.is_shutdown(): result = self.lmove_group.moveToJointPosition( self._left_arm_joints, self.larm_const_stow, 0.05, path_constraints=c1, planning_time=120.0) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def right_arm_constrained_stow(self): c1 = Constraints() c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.stamp = rospy.get_rostime() c1.orientation_constraints[0].header.frame_id = "base_link" c1.orientation_constraints[0].link_name = "right_ee_link" c1.orientation_constraints[0].orientation.w = 1.0 c1.orientation_constraints[ 0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2 c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28 c1.orientation_constraints[0].weight = 1.0 while not rospy.is_shutdown(): result = self.rmove_group.moveToJointPosition( self._right_arm_joints, self.rarm_const_stow, 0.05, path_constraints=c1, planning_time=120.0) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def open_gripper(self): self._lgripper.command(self._gripper_open, block=True) self._rgripper.command(self._gripper_open, block=True) def close_gripper(self): self._lgripper.command(self._gripper_closed, block=True) self._rgripper.command(self._gripper_closed, block=True)
gripper_goal.command.max_effort = 10.0 scene = PlanningSceneInterface('base_link') p = Pose() p.position.x = 0.4 + 0.15 p.position.y = -0.4 p.position.z = 0.7 + 0.15 p.orientation = Quaternion(*quaternion_from_euler(0, 1, 1)) arm.set_pose_target(p) while True: if arm.go(True): break clear_octomap() scene.clear() look_at_bin() while not rospy.is_shutdown(): rate.sleep() try: t = tf_listener.getLatestCommonTime('/base_link', item_frame) if (rospy.Time.now() - t).to_sec() > 0.2: rospy.sleep(0.1) continue item_translation, item_orientation = tf_listener.lookupTransform( '/base_link', item_frame, t) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue
def main(): rospy.init_node('arm_obstacle_demo') wait_for_time() argv = rospy.myargv() # Create the arm and safety measures arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() return rospy.on_shutdown(shutdown) planning_scene = PlanningSceneInterface(frame='base_link') planning_scene.clear() # add table to the scene planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.4 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.4 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 kwargs1 = { 'allowed_planning_time': 15, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': False, } kwargs2 = { 'allowed_planning_time': 15, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': False, 'orientation_constraint': oc } gripper = fetch_api.Gripper() planning_scene.removeAttachedObject('tray') while True: # Before moving to the first pose error = arm.move_to_pose(pose1, **kwargs1) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs2) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeAttachedObject('tray')
def main(): rospy.init_node('part1_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('floor') planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2) table_height = 0.767 table_width = 0.7 table_x = 0.95 planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height / 2) planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37 / 2) microwave_height = 0.28 microwave_width = 0.48 microwave_depth = 0.27 microwave_x = 0.97 microwave_y = 0.18 microwave_z = 0.06 microwave_side_height = 0.2 microwave_r_width = 0.135 microwave_r_y = microwave_y - 0.175 microwave_l_width = 0.035 microwave_l_y = microwave_y + 0.222 microwave_bottom_height = 0.05 microwave_top_height = 0.04 microwave_back_depth = 0.03 microwave_back_x = table_x + (microwave_depth / 2) + (microwave_back_depth / 2) microwave_door_width = 0.09 microwave_door_x = microwave_x - 0.33 microwave_door_y = microwave_l_y + 0.027 planning_scene.addBox( 'microwave_top', microwave_depth, microwave_width, microwave_top_height, microwave_x, microwave_y, table_height + microwave_z + microwave_bottom_height + microwave_side_height + (microwave_top_height / 2)) planning_scene.addBox( 'microwave_bottom', microwave_depth, microwave_width, microwave_bottom_height, microwave_x, microwave_y, table_height + microwave_z + (microwave_bottom_height / 2)) planning_scene.addBox( 'microwave_side_r', microwave_depth, microwave_r_width, microwave_side_height, microwave_x, microwave_r_y, table_height + microwave_z + microwave_bottom_height + (microwave_side_height / 2)) planning_scene.addBox( 'microwave_side_l', microwave_depth, microwave_l_width, microwave_side_height, microwave_x, microwave_l_y, table_height + microwave_z + microwave_bottom_height + microwave_side_height / 2) planning_scene.addBox('microwave_back', microwave_back_depth, microwave_width, microwave_height, microwave_back_x, microwave_y, table_height + microwave_z + microwave_height / 2) planning_scene.addBox( 'microwave_door', 0.39, microwave_door_width, microwave_height + 0.01, microwave_door_x, microwave_door_y, table_height + microwave_z + microwave_height / 2 + 0.005) rospy.sleep(2) rospy.spin()
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")
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)
req.ik_request.timeout = rospy.Duration(3.0) res = self.ik_service(req) joint_state = res.solution.joint_state joints = [] for name, position in zip(joint_state.name, joint_state.position): if name in fetch_api.ArmJoints.names(): joints.append((position)) return joints if __name__ == "__main__": rospy.init_node("pick_demo") dist = 0 robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('cube') planning_scene.removeCollisionObject('table') group = moveit_commander.MoveGroupCommander("arm") planning_frame = group.get_planning_frame() pd = pick_demo() grasp = pd.calculate_grasp() base = fetch_api.Base() arm = fetch_api.Arm() torso = fetch_api.Torso() gripper = fetch_api.Gripper() torso.set_height(0.4) joint_goal = group.get_current_joint_values() joint_goal = [0,0,0,-math.pi/2,0,math.pi/2,0] group.go(joint_goal, wait=True) group.stop()
class GraspingClient(object): def __init__(self, sim=False): # Define planning groups self.scene = PlanningSceneInterface("base_link") self.dof = rospy.get_param('~jaco_dof') self.move_group = MoveGroupInterface("upper_body", "base_link") self.rmove_group = MoveGroupInterface("right_arm", "base_link") planner_id = "RRTConnectkConfigDefault" self.move_group.setPlannerId(planner_id) self.rmove_group.setPlannerId(planner_id) self.objects_heights = [0.122, 0.240] self.objects_heights_tolerances = [0.02, 0.03] # Define joints and positions for 6 and 7 DOF if "6dof" == self.dof: self._upper_body_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint", "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint" ] self.tucked = [ -2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571, 0.0371, 0.0, 0.0 ] self.constrained_stow = [ 2.28, 2.00, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56, 0.09, -0.15, 2.06, 0.42, 0.0, 0.0 ] self.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06] #self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32] elif "7dof" == self.dof: self._upper_body_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint", "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint" ] self.tucked = [ -1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7, 0.0, -0.5, 1.7, 0.04, 0, 0 ] self.constrained_stow = [ -2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0 ] self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0] else: rospy.logerr("DoF needs to be set 6 or 7, aborting demo") return # Define the MoveIt pickplace pipeline self.pickplace = None self.pickplace = PickPlaceInterface("left_side", "left_gripper", verbose=True) self.pickplace.planner_id = planner_id self.pick_result = None self.place_result = None self._listener = tf.TransformListener() self._lgripper = GripperActionClient('left') find_grasp_planning_topic = "grasp_planner/plan" rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic) self.find_grasp_planning_client = actionlib.SimpleActionClient( find_grasp_planning_topic, GraspPlanningAction) self.find_grasp_planning_client.wait_for_server() rospy.loginfo("...connected") self.scene.clear() # This is a simulation so need to adjust gripper parameters if sim: self._gripper_closed = 0.96 self._gripper_open = 0.00 else: self._gripper_closed = 0.01 self._gripper_open = 0.165 def __del__(self): self.scene.clear() def add_objects_to_keep(self, obj): self._objs_to_keep.append(obj) def clearScene(self): self.scene.clear() def calculateGraspForObject(self, object_to_grasp, gripper): goal = GraspPlanningGoal() goal.object = object_to_grasp goal.gripper = gripper self.find_grasp_planning_client.send_goal(goal) self.find_grasp_planning_client.wait_for_result(rospy.Duration(5.0)) return self.find_grasp_planning_client.get_result( ).grasps #moveit_msgs/Grasp[] def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, retries=1, scene=self.scene) self.pick_result = pick_result 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, retries=1, scene=self.scene) self.place_result = place_result return success def goto_tuck(self): # remove previous objects while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition( self._upper_body_joints, self.tucked, 0.05) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def goto_plan_grasp(self): while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition( self._upper_body_joints, self.constrained_stow, 0.05) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def left_arm_constrained_stow(self): c1 = Constraints() c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.stamp = rospy.get_rostime() c1.orientation_constraints[0].header.frame_id = "base_link" c1.orientation_constraints[0].link_name = "left_ee_link" c1.orientation_constraints[0].orientation.w = 1.0 c1.orientation_constraints[ 0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2 c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28 c1.orientation_constraints[0].weight = 1.0 while not rospy.is_shutdown(): result = self.lmove_group.moveToJointPosition( self._left_arm_joints, self.larm_const_stow, 0.05, path_constraints=c1, planning_time=120.0) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def right_arm_constrained_stow(self): # c1 = Constraints() # c1.orientation_constraints.append(OrientationConstraint()) # c1.orientation_constraints[0].header.stamp = rospy.get_rostime() # c1.orientation_constraints[0].header.frame_id = "base_link" # c1.orientation_constraints[0].link_name = "right_ee_link" # c1.orientation_constraints[0].orientation.w=1.0 # c1.orientation_constraints[0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link # c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2 # c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28 # c1.orientation_constraints[0].weight = 1.0 while not rospy.is_shutdown(): result = self.rmove_group.moveToJointPosition( self._right_arm_joints, self.rarm_const_stow, 0.05, planning_time=120.0) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def open_gripper(self): self._lgripper.command(self._gripper_open, block=True) def close_gripper(self): self._lgripper.command(self._gripper_closed, block=True)
class RoboEatsServer(object): DEFAULT_TORSO_HEIGHT = 0.4 MICROWAVE_LOCATION_NAME = "microwave_location" DROPOFF_LOCATION_NAME = "dropoff_location" FOOD_ALIAS = "food_id" def __init__(self, save_file_path="food_items.pkl", nav_file_path="annotator_positions.pkl"): self._food_items_pub = rospy.Publisher(FOOD_ITEMS_TOPIC, FoodItems, queue_size=10, latch=True) rospy.loginfo("Given save file path: " + save_file_path) if os.path.isfile(save_file_path): rospy.loginfo("File already exists, loading saved positions.") with open(save_file_path, "rb") as save_file: try: self._food_items = pickle.load(save_file) except EOFError: # this can be caused if the file is empty. self._food_items = {} rospy.loginfo("File loaded...") else: rospy.loginfo("File doesn't exist.") self._food_items = {} self.__print_food_items__() self._save_file_path = save_file_path self.__pub_food_items__() rospy.loginfo("initializing arm...") self.arm = robot_api.Arm() rospy.loginfo("initializing gripper...") self.gripper = robot_api.Gripper() rospy.loginfo("initializing head...") self.head = robot_api.Head() rospy.loginfo("initializing torso...") self.torso = robot_api.Torso() rospy.loginfo("initializing planning scene...") self.planning_scene = PlanningSceneInterface('base_link') self.curr_obstacle = None rospy.loginfo("Starting map annotator...") # We should expect the nav file given to contain the annotated positions: # MICROWAVE_LOCATION_NAME - starting location in front of the microwave. # DROPOFF_LOCATION_NAME - ending dropoff location. # TODO: Remember to uncomment this section when we get the map working. # self._map_annotator = Annotator(save_file_path=nav_file_path) # if not self._map_annotator.exists(self.MICROWAVE_LOCATION_NAME): # rospy.logwarn("Annotator is missing location '%s'" % # (self.MICROWAVE_LOCATION_NAME)) # if not self._map_annotator.exists(self.DROPOFF_LOCATION_NAME): # rospy.logwarn("Annotator is missing location '%s'" % # (self.DROPOFF_LOCATION_NAME)) rospy.loginfo("Initialization finished...") def __print_food_items__(self): rospy.loginfo("Current food items:") for f in self._food_items.values(): rospy.loginfo("\t" + str(f)) def __save_file__(self): """ Saves the pickle file containing food item information. """ with open(self._save_file_path, "wb") as save_file: pickle.dump(self._food_items, save_file, protocol=pickle.HIGHEST_PROTOCOL) # flush() saves file immediately instead of buffering changes. save_file.flush() def __pub_food_items__(self): """ Publishes the current list of food items to the FOOD_ITEMS_TOPIC topic. """ food_items = FoodItems() if len(self._food_items) > 0: food_items.names, food_items.descriptions, food_items.ids = zip(*[(f.name, f.description, f.id) for f in self._food_items.values()]) self._food_items_pub.publish(food_items) def __food_list_modified__(self): """ Helper method which does any actions that're needed after the food item dict has been modified. """ self.__save_file__() self.__pub_food_items__() def __remove_food_item__(self, id): if id in self._food_items: self._food_items.pop(id) self.__food_list_modified__() def handle_create_food_item(self, request): """ input: request(name, description, id) Creates and adds the given food item. If a food item with the same ID already exists, then it will be overridden by this food item. """ rospy.loginfo(request) self._food_items[request.id] = FoodItem(request.name, request.description, request.id) self.__food_list_modified__() return CreateFoodItemResponse() def handle_remove_food_item(self, request): """ input: request(id) Removes the food item with the given id iff it exists. """ self.__remove_food_item__(request.id) return RemoveFoodItemResponse() def __food_id_to_ar_frame__(self, id): return "ar_marker_" + str(id) def __load_program_and_run__(self, program_fp, id): """Load a pickle file from the given file path and run it with respect to the given food id. Arguments: program_fp {str} -- program file path id {int} -- food id """ if os.path.isfile(program_fp): rospy.loginfo("File " + program_fp + " exists. Loading...") with open(program_fp, "rb") as load_file: program = Program(self.arm, self.gripper, self.head, self.torso) program.commands = pickle.load(load_file) rospy.loginfo("Program loaded...") ar_marker_frame = self.__food_id_to_ar_frame__(id) rospy.loginfo("Program before changes:") program.print_program() # Make the program relative to this food item rospy.loginfo("Program after changes:") program.replace_frame(self.FOOD_ALIAS, ar_marker_frame) program.run() else: rospy.logerr("Program from given file path does not exist: " + program_fp) raise Exception def init_robot(self): """ 0a. Move torso to default position 0b. reset head 0c. open gripper 0d. move arm to starting pos (start_pos.pkl) """ rospy.loginfo("STARTING SEGMENT 1") rospy.loginfo("0a. Move torso to default position") torso = robot_api.Torso() torso.set_height(0.4) rospy.sleep(2) rospy.loginfo("0b. reset head") head = robot_api.Head() head.pan_tilt(-0.1, 0.57) rospy.sleep(2) rospy.loginfo("0c. open gripper") self.gripper.open() rospy.loginfo("0d. Move arm to starting pos") self.__load_program_and_run__("start_pos.pkl", id) rospy.sleep(1.5) def clear_obstacles(self): self.planning_scene.clear() self.planning_scene.removeCollisionObject('table') self.planning_scene.removeCollisionObject('floor') self.planning_scene.removeCollisionObject('lunchbox') self.planning_scene.removeAttachedObject('lunchbox') def start_obstacles_1(self): """ This scene has the microwave in a closed position. """ if self.curr_obstacle == 1: # don't change it if it's already set to this obstacle return self.planning_scene.clear() self.planning_scene.removeCollisionObject('table') self.planning_scene.removeCollisionObject('floor') self.planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01/2) table_height = 0.767 table_width = 0.7 table_x = 0.95 self.planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height/2) self.planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37/2) microwave_height = 0.28 microwave_width = 0.48 # microwave_depth = 0.33 microwave_depth = 0.27 microwave_x = 0.97 microwave_z = 0.06 microwave_y = 0.18 self.planning_scene.addBox('microwave', microwave_depth, microwave_width, microwave_height, microwave_x, microwave_y, table_height + microwave_z + microwave_height/2) self.curr_obstacle = 1 rospy.sleep(3.5) def start_obstacles_2(self): """ this scene has the microwave in an open position with the lid open. """ if self.curr_obstacle == 2: # don't change it if it's already set to this obstacle return self.planning_scene.clear() self.planning_scene.removeCollisionObject('table') self.planning_scene.removeCollisionObject('floor') self.planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01/2) table_height = 0.767 table_width = 0.7 table_x = 0.95 self.planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height/2) self.planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37/2) microwave_height = 0.28 microwave_width = 0.48 microwave_depth = 0.27 microwave_x = 0.97 microwave_y = 0.18 microwave_z = 0.06 microwave_side_height = 0.2 microwave_r_width = 0.04 + 0.05 microwave_r_y = microwave_y - 0.19 microwave_l_width = 0.035 microwave_l_y = microwave_y + 0.2 microwave_bottom_height = 0.05 microwave_top_height = 0.04 microwave_back_depth = 0.03 microwave_back_x = table_x + (microwave_depth / 2) + (microwave_back_depth/2) microwave_door_width = 0.045 microwave_door_x = microwave_x - 0.33 microwave_door_y = microwave_l_y + 0.05 self.planning_scene.addBox('microwave_top', microwave_depth, microwave_width, microwave_top_height, microwave_x, microwave_y, table_height + microwave_z + microwave_bottom_height + microwave_side_height + (microwave_top_height/2)) self.planning_scene.addBox('microwave_bottom', microwave_depth, microwave_width, microwave_bottom_height, microwave_x, microwave_y, table_height + microwave_z + (microwave_bottom_height/2) - 0.015) self.planning_scene.addBox('microwave_side_r', microwave_depth, microwave_r_width, microwave_side_height, microwave_x, microwave_r_y, table_height + microwave_z + microwave_bottom_height + (microwave_side_height/2)) self.planning_scene.addBox('microwave_side_l', microwave_depth, microwave_l_width, microwave_side_height, microwave_x, microwave_l_y, table_height + microwave_z + microwave_bottom_height + microwave_side_height/2) self.planning_scene.addBox('microwave_back', microwave_back_depth, microwave_width, microwave_height, microwave_back_x, microwave_y, table_height + microwave_z + microwave_height/2) self.planning_scene.addBox('microwave_door', 0.42, microwave_door_width, microwave_height + 0.01, microwave_door_x, microwave_door_y, table_height + microwave_z + microwave_height/2 + 0.005) self.curr_obstacle = 2 rospy.sleep(3.5) def attach_lunchbox(self): self.remove_lunchbox() frame_attached_to = 'gripper_link' frames_okay_collide_with = ['gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link', 'wrist_roll_link'] lunchbox_x_offset = 0.1 self.planning_scene.attachBox("lunchbox", 0.10, 0.10, 0.02, lunchbox_x_offset, 0, 0, frame_attached_to, frames_okay_collide_with) def remove_lunchbox(self): self.planning_scene.removeAttachedObject('lunchbox') self.planning_scene.removeCollisionObject('lunchbox') def start_segment1a(self, id): """ (Segment 1a) 0. Initialize robot 1. (OMITTED) Move to start pose 2. Open microwave (p2.pkl) (done) 2b. Move microwave lid (p2b.pkl) (done) """ # if id in self._food_items: rospy.loginfo("0. Initialize robot") self.init_robot() self.start_obstacles_2() # rospy.loginfo("1. Move to start pose") # self._map_annotator.goto_position(self.MICROWAVE_LOCATION_NAME) # rospy.sleep(2) rospy.loginfo("2. Open microwave") self.__load_program_and_run__("p2.pkl", id) rospy.sleep(1.5) rospy.loginfo("2b. Move microwave lid") self.__load_program_and_run__("p2b.pkl", id) rospy.sleep(1.5) rospy.loginfo("FINISHED SEGMENT 1a") # else: # print("Food item " + str(id) + " does not exist.") def start_segment1b(self, id): """ (Segment 1b) 3. Grab lunchbox (p1.pkl) (done - but redo if have enough time) 4. Put it into microwave (p3.pkl) (done-iffy) """ self.start_obstacles_2() rospy.loginfo("STARTING SEGMENT 1b") rospy.loginfo("3. Grab lunchbox") self.__load_program_and_run__("p1.pkl", id) rospy.sleep(1.5) self.attach_lunchbox() rospy.sleep(2) rospy.loginfo("4. Put it into microwave") self.__load_program_and_run__("p3.pkl", id) rospy.sleep(1.5) self.remove_lunchbox() # else: # print("Food item " + str(id) + " does not exist.") def start_segment1c(self, id): self.start_obstacles_2() rospy.loginfo("5a. Close microwave pt. 1") self.__load_program_and_run__("p4a.pkl", id) rospy.sleep(1.5) rospy.loginfo("5b. Changing obstacles...") self.start_obstacles_1() rospy.loginfo("5b. Close microwave pt. 2") self.__load_program_and_run__("p4b.pkl", id) rospy.sleep(1.5) rospy.loginfo("FINISHED SEGMENT 1c") def start_segment2(self, id): """ (Segment 2) 6. Enter time (1 min) & 7. Start microwave (p5.pkl) 8. Wait for food to finish microwaving 9. Wait for cooldown """ # if id in self._food_items: rospy.loginfo("STARTING SEGMENT 2") rospy.loginfo("6. Enter time(1 min)") self.__load_program_and_run__("p5.pkl", id) rospy.loginfo("8. Wait for food to finish microwaving (in seconds)") rospy.sleep(5) rospy.loginfo("9. Wait for cooldown (in seconds)") # rospy.sleep(40) rospy.loginfo("FINISHED SEGMENT 2") # else: # print("Food item " + str(id) + " does not exist.") def start_segment3a(self, id): """ (Segment 3) 10a. Open microwave (p2.pkl) (done) 10b. Move microwave lid (p2b.pkl) (done) """ # if id in self._food_items: self.start_obstacles_2() rospy.loginfo("PRE INITIALIZING FOR SEGMENT 3") self.__load_program_and_run__("segment3a-pre.pkl", id) self.__load_program_and_run__("start_pos.pkl", id) rospy.loginfo("STARTING SEGMENT 3") rospy.loginfo("10a. Open microwave") self.__load_program_and_run__("p2.pkl", id) rospy.loginfo("10b. Move microwave lid ") self.__load_program_and_run__("p2b.pkl", id) # else: # print("Food item " + str(id) + " does not exist.") def start_segment3b(self, id): """ (Segment 3) 11. Grab lunchbox (p6a.pkl, p6b.pkl) """ self.start_obstacles_2() rospy.loginfo("11. Grab lunchbox") # print('lowering torso for better view') # self.torso.set_height(0.35) self.__load_program_and_run__("p6a.pkl", id) self.attach_lunchbox() rospy.sleep(2) self.__load_program_and_run__("p6b.pkl", id) def start_segment3c(self, id): """ (Segment 3) 12. (OMITTED) Move to dropoff pose 13. Put down lunchbox (p7.pkl) """ self.start_obstacles_2() # rospy.loginfo("12. Move to dropoff pose") # self._map_annotator.goto_position(self.DROPOFF_LOCATION_NAME) # rospy.sleep(2) rospy.loginfo("13. Put down lunchbox") self.__load_program_and_run__("p7.pkl", id) self.remove_lunchbox() rospy.loginfo("FINISHED SEGMENT 3") def start_segment4(self, id): """ (Segment 4) 14. (OMITTED) Move to start pose 15. Close microwave (p4a.pkl, p4b.pkl) """ # if id in self._food_items: rospy.loginfo("STARTING SEGMENT 4") # rospy.loginfo("14. Move to start pose") # self._map_annotator.goto_position(self.MICROWAVE_LOCATION_NAME) # rospy.sleep(2) self.start_obstacles_2() rospy.loginfo("15a. Close microwave pt. 1") self.__load_program_and_run__("p4a.pkl", id) rospy.sleep(1.5) rospy.loginfo("15b. Changing obstacles...") self.start_obstacles_1() rospy.loginfo("15b. Close microwave pt. 2") self.__load_program_and_run__("p4b.pkl", id) rospy.sleep(1.5) rospy.loginfo("FINISHED SEGMENT 4") # else: # print("Food item " + str(id) + " does not exist.") def handle_start_sequence(self, request): """ input: request(id) Starts the entire food sequence and removes the food item from the dictionary after it has been finished. """ id = request.id # if id in self._food_items: rospy.loginfo("Starting sequence for food item: " + str(self._food_items[id])) self.start_segment1a(id) self.start_segment1b(id) self.start_segment1c(id) self.start_segment2(id) self.start_segment3a(id) self.start_segment3b(id) self.start_segment3c(id) # we decioded that we don't need to close the door :) self.start_segment4(id) rospy.loginfo("Remove food item from the list.") # self.__remove_food_item__(id) rospy.loginfo("Finished sequence.") # else: # print("Food item " + str(id) + " does not exist.") return StartSequenceResponse()