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(): # 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 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 main(): rospy.init_node("arm_obstacle_demo") wait_for_time() argv = rospy.myargv() 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.2 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 gripper = robot_api.Gripper() arm = robot_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 15, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': True, 'orientation_constraint': oc } planning_scene.removeAttachedObject('tray') gripper.open() 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') rospy.sleep(2) 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() gripper.close() rospy.sleep(2) 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') gripper.open() planning_scene.removeAttachedObject('tray') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') return
def main(): rospy.init_node('arm_obstacle_demo') wait_for_time() # argv = rospy.myargv() planning_scene = PlanningSceneInterface(frame='base_link') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') planning_scene.removeAttachedObject('tray') # Create table obstacle 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 # 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) # add 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 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 # # set torso to max height torso = robot_api.Torso() torso.set_height(robot_api.Torso.MAX_HEIGHT) arm = robot_api.Arm() # register shutdown method def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 20, 'execution_timeout': 20, 'num_planning_attempts': 10, 'replan': True } 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') # attach an object if pose1 is successfully reached 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() # close the gripper # gripper = robot_api.Gripper() # gripper.close() 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') # At the end of the program planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') planning_scene.removeAttachedObject('tray')
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()
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import argparse import rospy from moveit_python import PlanningSceneInterface if __name__ == "__main__": parser = argparse.ArgumentParser( description="Attach objects to link in the MoveIt planning scene.") parser.add_argument("--name", nargs="?", help="Name of object to be attached") parser.add_argument( "--link", nargs="?", help="Name of link to where the object should be attached") args = parser.parse_args() if args.name: if args.link: rospy.init_node("attache_objects") scene = PlanningSceneInterface("/base_link") print("Attach Object with name: %s to Link: %s" % (args.name, args.link)) scene.attachBox(args.name, 0.1, 0.1, 0.1, 0.0, 0.0, 0.0, args.link) else: parser.print_help()
class GripperInterfaceClient: def __init__(self): self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_controller...") self.client.wait_for_server() rospy.loginfo("...connected.") self.scene = PlanningSceneInterface("base_link") self.attached_object_publisher = rospy.Publisher( "attached_collision_object", AttachedCollisionObject, queue_size=10 ) self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True) 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.debug_index = -1 self.graspables = [] def execute_trajectory(self, trajectory): goal_position = trajectory.points[-1].positions print "==========", goal_position command_goal = GripperCommandGoal() command_goal.command.position = goal_position[0] + goal_position[1] command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this self.client.send_goal(command_goal) self.client.wait_for_result() def move_to(self, goal_position, max_effort): command_goal = GripperCommandGoal() command_goal.command.position = goal_position command_goal.command.max_effort = max_effort self.client.send_goal(command_goal) self.client.wait_for_result() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True 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, wait=False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, wait=False) rospy.sleep(0.5) # Gets rid of annoying error messages stemming from race condition. self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx self.scene.addSolidPrimitive( obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False ) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0] + 0.1, 1.5, # 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=False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size # if obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07 or \ # obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07 or \ # obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07: # continue # has to be on table # if obj.object.primitive_poses[0].position.z < 0.5: # continue return obj.object, obj.grasps # nothing detected return None, None def set_target_object(self, primitive_type, target_pose, dimensions): primitive_types = {"BOX": 1, "SPHERE": 2, "CYLINDER": 3, "CONE": 4} self.sought_object = Object() primitive = SolidPrimitive() primitive.type = primitive_types[primitive_type.upper()] primitive.dimensions = dimensions self.sought_object.primitives.append(primitive) p = Pose() p.position.x = target_pose[0] p.position.y = target_pose[1] p.position.z = target_pose[2] quaternion = quaternion_from_euler(target_pose[3], target_pose[4], target_pose[5]) p.orientation.x = quaternion[0] p.orientation.y = quaternion[1] p.orientation.z = quaternion[2] p.orientation.w = quaternion[3] self.sought_object.primitive_poses.append(p) def find_graspable_object(self, tolerance=0.01): self.updateScene() self.current_grasping_target = None for obj in self.objects: # Must have grasps if len(obj.grasps) < 1: continue # Compare to sought object detected_object_dimensions = np.array(obj.object.primitives[0].dimensions) sought_object_dimensions = np.array(self.sought_object.primitives[0].dimensions) try: difference = detected_object_dimensions - sought_object_dimensions print "===DEBUG: Difference: ", difference mag_diff = np.linalg.norm(difference) print "===DEBUG: mag_diff: ", mag_diff if mag_diff <= tolerance: self.current_grasping_target = obj tolerance = mag_diff print "===DEBUG: Tolerance: ", tolerance else: rospy.loginfo("Object dimensions do not match. Trying another object...") except: rospy.loginfo("Object geometries do not match. Trying another object...") if self.current_grasping_target is None: tolerance += 0.01 self.find_graspable_object(tolerance=tolerance) # Nothing detected def computeGraspToPickMatrix(self): pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w] grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w] pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion) rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion) translation = [pick_translation_distance, 0, 0] rotation_matrix[[0, 1, 2], 3] = translation pick_to_grasp_matrix = np.mat(rotation_matrix) grasp_to_pick_matrix = pick_to_grasp_matrix.getI() return grasp_to_pick_matrix def computePlaceToBaseMatrix(self, place): place_quaternion = place[3:] rotation_matrix = quaternion_matrix(place_quaternion) translation = place[0:3] rotation_matrix[[0, 1, 2], 3] = translation place_to_base_matrix = rotation_matrix return place_to_base_matrix def broadcastCurrentGraspingTargetTransform(self): pose = self.current_grasping_target.object.primitive_poses[0] br = tf2_ros.TransformBroadcaster() t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "base_link" t.child_frame_id = "current_grasping_target" t.transform.translation.x = pose.position.x t.transform.translation.y = pose.position.y t.transform.translation.z = pose.position.z t.transform.rotation.x = pose.orientation.x t.transform.rotation.y = pose.orientation.y t.transform.rotation.z = pose.orientation.z t.transform.rotation.w = pose.orientation.w br.sendTransform(t) def plan_pick(self): success, pick_result = self.pickplace.pick_with_retry( self.current_grasping_target.object.name, self.current_grasping_target.grasps, support_name=self.current_grasping_target.object.support_surface, scene=self.scene, allow_gripper_support_collision=False, planner_id="PRMkConfigDefault", ) self.pick_result = pick_result print "DEBUG: plan_pick(): ", self.scene.getKnownAttachedObjects() if success: rospy.loginfo("Planning succeeded.") trajectory_approved = get_user_approval() if trajectory_approved: return pick_result.trajectory_stages else: rospy.loginfo("Plan rejected. Getting new grasps for replan...") else: rospy.logwarn("Planning failed. Getting new grasps for replan...") self.find_graspable_object() return self.plan_pick() 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 recognizeAttachedObject(self): print "========= Recognizing attached object" name = self.current_grasping_target.object.name size_x = self.current_grasping_target.object.primitives[0].dimensions[0] size_y = self.current_grasping_target.object.primitives[0].dimensions[1] size_z = self.current_grasping_target.object.primitives[0].dimensions[2] (x, y, z) = (0.03, 0.0, 0.0) link_name = "gripper_link" touch_links = ["l_gripper_finger_link", "r_gripper_finger_link", "gripper_link"] detach_posture = None weight = 0.0 wait = True """ object_x = self.current_grasping_target.object.primitive_poses[0].position.x object_y = self.current_grasping_target.object.primitive_poses[0].position.y object_z = self.current_grasping_target.object.primitive_poses[0].position.z pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation grasp_position = self.pick_result.grasp.grasp_pose.pose.position pick_translation_distance = self.pick_result.grasp.post_grasp_retreat.desired_distance pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w] grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w] pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion) grasp_to_base_matrix_array = quaternion_matrix(grasp_quaternion) grasp_to_base_matrix = np.matrix(grasp_to_base_matrix_array) displacement = grasp_to_base_matrix.dot(np.matrix([[pick_translation_distance-.03], [0], [0], [0]])) print displacement attached_location = list() attached_location.append(object_x - displacement[0,0]) attached_location.append(object_y - displacement[1,0]) attached_location.append(object_z - displacement[2,0]) print attached_location quaternion = Quaternion() quaternion.x = pick_to_grasp_quaternion[0] quaternion.y = pick_to_grasp_quaternion[1] quaternion.z = pick_to_grasp_quaternion[2] quaternion.w = pick_to_grasp_quaternion[3] pose = Pose() pose.position.x = attached_location[0] pose.position.y = attached_location[1] pose.position.z = attached_location[2] pose.orientation = self.current_grasping_target.object.primitive_poses[0].orientation collision_object = self.scene.makeSolidPrimitive(self.current_grasping_target.object.name, self.current_grasping_target.object.primitives[0], pose) attached_collision_object = self.scene.makeAttached("gripper_link", collision_object, touch_links, detach_posture, 0.0) self.attached_object_publisher.publish(attached_collision_object) """ self.scene.attachBox( name, size_x, size_y, size_z, x, y, z, link_name, touch_links=touch_links, detach_posture=detach_posture, weight=weight, wait=wait, ) def removeCollisionObjects(self): collision_object_names = self.scene.getKnownCollisionObjects() print self.current_grasping_target.object.name print collision_object_names raw_input("press ENTER") x = self.scene.getKnownAttachedObjects() print x raw_input("press Enter") for name in collision_object_names: if name != self.current_grasping_target.object.name: self.scene.removeCollisionObject(name, wait=False)
def main(): rospy.init_node('arm_obstacle_demo') wait_for_time() 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) 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 torso = fetch_api.Torso() torso.set_height(fetch_api.Torso.MAX_HEIGHT) arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) # and add an orientation constraint to pose 2: 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 kwargs = { 'allowed_planning_time': 100, 'execution_timeout': 100, 'num_planning_attempts': 15, 'replan': True, 'replan_attempts': 10 } # Before moving to the first pose planning_scene.removeAttachedObject('tray') error = arm.move_to_pose(pose1, **kwargs) # If the robot reaches the first pose successfully, then "attach" an object there # Of course, you would have to close the gripper first and ensure that you grasped the object properly 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'] = 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')
class GripperInterfaceClient(): def __init__(self): self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_controller...") self.client.wait_for_server() rospy.loginfo("...connected.") self.scene = PlanningSceneInterface("base_link") self.attached_object_publisher = rospy.Publisher('attached_collision_object', AttachedCollisionObject, queue_size = 10) self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True) 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.debug_index = -1 self.graspables = [] def execute_trajectory(self, trajectory): goal_position = trajectory.points[-1].positions print '==========', goal_position command_goal = GripperCommandGoal() command_goal.command.position = goal_position[0]+goal_position[1] command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this self.client.send_goal(command_goal) self.client.wait_for_result() def move_to(self, goal_position, max_effort): command_goal = GripperCommandGoal() command_goal.command.position = goal_position command_goal.command.max_effort = max_effort self.client.send_goal(command_goal) self.client.wait_for_result() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True 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, wait=False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, wait=False) rospy.sleep(0.5) # Gets rid of annoying error messages stemming from race condition. self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d"%idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0] + 0.1, 1.5, # 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 = False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size #if obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07 or \ # obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07 or \ # obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07: # continue # has to be on table #if obj.object.primitive_poses[0].position.z < 0.5: # continue return obj.object, obj.grasps # nothing detected return None, None def set_target_object(self, primitive_type, target_pose, dimensions): primitive_types = {"BOX":1, "SPHERE":2, "CYLINDER":3, "CONE":4} self.sought_object = Object() primitive = SolidPrimitive() primitive.type = primitive_types[primitive_type.upper()] primitive.dimensions = dimensions self.sought_object.primitives.append(primitive) p = Pose() p.position.x = target_pose[0] p.position.y = target_pose[1] p.position.z = target_pose[2] quaternion = quaternion_from_euler(target_pose[3], target_pose[4], target_pose[5]) p.orientation.x = quaternion[0] p.orientation.y = quaternion[1] p.orientation.z = quaternion[2] p.orientation.w = quaternion[3] self.sought_object.primitive_poses.append(p) def find_graspable_object(self, tolerance=0.01): self.updateScene() self.current_grasping_target = None for obj in self.objects: # Must have grasps if len(obj.grasps) < 1: continue # Compare to sought object detected_object_dimensions = np.array(obj.object.primitives[0].dimensions) sought_object_dimensions = np.array(self.sought_object.primitives[0].dimensions) try: difference = detected_object_dimensions - sought_object_dimensions print "===DEBUG: Difference: " , difference mag_diff = np.linalg.norm(difference) print "===DEBUG: mag_diff: ", mag_diff if mag_diff <= tolerance: self.current_grasping_target = obj tolerance = mag_diff print "===DEBUG: Tolerance: ", tolerance else: rospy.loginfo("Object dimensions do not match. Trying another object...") except: rospy.loginfo("Object geometries do not match. Trying another object...") if self.current_grasping_target is None: tolerance += .01 self.find_graspable_object(tolerance = tolerance) # Nothing detected def computeGraspToPickMatrix(self): pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w] grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w] pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion) rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion) translation = [pick_translation_distance, 0, 0] rotation_matrix[[0,1,2],3] = translation pick_to_grasp_matrix = np.mat(rotation_matrix) grasp_to_pick_matrix = pick_to_grasp_matrix.getI() return grasp_to_pick_matrix def computePlaceToBaseMatrix(self, place): place_quaternion = place[3:] rotation_matrix = quaternion_matrix(place_quaternion) translation = place[0:3] rotation_matrix[[0,1,2], 3] = translation place_to_base_matrix = rotation_matrix return place_to_base_matrix def broadcastCurrentGraspingTargetTransform(self): pose = self.current_grasping_target.object.primitive_poses[0] br = tf2_ros.TransformBroadcaster() t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "base_link" t.child_frame_id = "current_grasping_target" t.transform.translation.x = pose.position.x t.transform.translation.y = pose.position.y t.transform.translation.z = pose.position.z t.transform.rotation.x = pose.orientation.x t.transform.rotation.y = pose.orientation.y t.transform.rotation.z = pose.orientation.z t.transform.rotation.w = pose.orientation.w br.sendTransform(t) def plan_pick(self): success, pick_result = self.pickplace.pick_with_retry(self.current_grasping_target.object.name, self.current_grasping_target.grasps, support_name = self.current_grasping_target.object.support_surface, scene=self.scene, allow_gripper_support_collision=False, planner_id='PRMkConfigDefault') self.pick_result = pick_result print "DEBUG: plan_pick(): ", self.scene.getKnownAttachedObjects() if success: rospy.loginfo("Planning succeeded.") trajectory_approved = get_user_approval() if trajectory_approved: return pick_result.trajectory_stages else: rospy.loginfo("Plan rejected. Getting new grasps for replan...") else: rospy.logwarn("Planning failed. Getting new grasps for replan...") self.find_graspable_object() return self.plan_pick() 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 recognizeAttachedObject(self): print "========= Recognizing attached object" name = self.current_grasping_target.object.name size_x = self.current_grasping_target.object.primitives[0].dimensions[0] size_y = self.current_grasping_target.object.primitives[0].dimensions[1] size_z = self.current_grasping_target.object.primitives[0].dimensions[2] (x, y, z) = (0.03, 0.0, 0.0) link_name = "gripper_link" touch_links = ["l_gripper_finger_link", "r_gripper_finger_link", "gripper_link"] detach_posture = None weight = 0.0 wait = True """ object_x = self.current_grasping_target.object.primitive_poses[0].position.x object_y = self.current_grasping_target.object.primitive_poses[0].position.y object_z = self.current_grasping_target.object.primitive_poses[0].position.z pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation grasp_position = self.pick_result.grasp.grasp_pose.pose.position pick_translation_distance = self.pick_result.grasp.post_grasp_retreat.desired_distance pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w] grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w] pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion) grasp_to_base_matrix_array = quaternion_matrix(grasp_quaternion) grasp_to_base_matrix = np.matrix(grasp_to_base_matrix_array) displacement = grasp_to_base_matrix.dot(np.matrix([[pick_translation_distance-.03], [0], [0], [0]])) print displacement attached_location = list() attached_location.append(object_x - displacement[0,0]) attached_location.append(object_y - displacement[1,0]) attached_location.append(object_z - displacement[2,0]) print attached_location quaternion = Quaternion() quaternion.x = pick_to_grasp_quaternion[0] quaternion.y = pick_to_grasp_quaternion[1] quaternion.z = pick_to_grasp_quaternion[2] quaternion.w = pick_to_grasp_quaternion[3] pose = Pose() pose.position.x = attached_location[0] pose.position.y = attached_location[1] pose.position.z = attached_location[2] pose.orientation = self.current_grasping_target.object.primitive_poses[0].orientation collision_object = self.scene.makeSolidPrimitive(self.current_grasping_target.object.name, self.current_grasping_target.object.primitives[0], pose) attached_collision_object = self.scene.makeAttached("gripper_link", collision_object, touch_links, detach_posture, 0.0) self.attached_object_publisher.publish(attached_collision_object) """ self.scene.attachBox(name, size_x, size_y, size_z, x, y, z, link_name, touch_links=touch_links, detach_posture=detach_posture, weight=weight, wait=wait) def removeCollisionObjects(self): collision_object_names = self.scene.getKnownCollisionObjects() print self.current_grasping_target.object.name print collision_object_names raw_input("press ENTER") x = self.scene.getKnownAttachedObjects() print x raw_input("press Enter") for name in collision_object_names: if name != self.current_grasping_target.object.name: self.scene.removeCollisionObject(name, wait=False)
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import argparse import rospy from moveit_python import PlanningSceneInterface if __name__ == "__main__": parser = argparse.ArgumentParser( description="Attach objects to link in the MoveIt planning scene.") parser.add_argument("name", help="Name of the box to be attached") parser.add_argument("link", help="Name of link to where the box should be attached") parser.add_argument("x", type=float, help="X coordinate of center of box") parser.add_argument("y", type=float, help="Y coordinate of center of box") parser.add_argument("z", type=float, help="Z coordinate of center of box") parser.add_argument("size_x", type=float, help="Size of box in x dimension") parser.add_argument("size_y", type=float, help="Size of box in y dimension") parser.add_argument("size_z", type=float, help="Size of box in z dimension") args = parser.parse_args() if args.name: if args.link: rospy.init_node("attach_box") scene = PlanningSceneInterface("/base_link") print("Attach Object with name: %s to Link: %s" % (args.name, args.link)) scene.attachBox(args.name, args.size_x, args.size_y, args.size_z, args.x, args.y, args.z, args.link) else: parser.print_help()
def main(): print('We are running the main of arm_obstable_demo') rospy.init_node('arm_obstacle_demo') wait_for_time() print( "If planning scene fails to init, make sure you have launched move_group.launch" ) planning_scene = PlanningSceneInterface("base_link") # Create table object 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 object planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.2 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) # Before moving to the first pose planning_scene.removeAttachedObject('tray') 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) kwargs = { 'allowed_planning_time': 15, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': False } error = arm.move_to_pose(pose1, **kwargs) # If the robot reaches the first pose successfully, then "attach" an object there # Of course, you would have to close the gripper first and ensure that you grasped the object properly 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')
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 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()