def __grasp_with_group(self, collision_object_name, move_group, object_density): """ Deprecated. For testing only """ if type(collision_object_name) is CollisionObject: collision_object_name = collision_object_name.id collision_object = self.__planning_scene_interface.get_collision_object(collision_object_name) if collision_object is None: rospy.logwarn("Collision Object " + collision_object_name + " is not in planningscene.") return False grasp_positions = calculate_grasp_position(collision_object, self.tf.transform_to) grasp_positions = self.filter_low_poses(grasp_positions) grasp_positions = self.filter_close_poses(grasp_positions) if len(grasp_positions) == 0: rospy.logwarn("No grasppositions found for " + collision_object_name) grasp_positions.sort(cmp=lambda x, y: self.cmp_pose_stamped(collision_object, x, y)) visualize_poses(grasp_positions) # print grasp_positions self.open_gripper() for grasp in grasp_positions: if self.__move_group_to(get_pre_grasp(self.transform_to(grasp)), move_group, do_not_blow_up_list=("map", collision_object_name)): if not self.__move_group_to(grasp, move_group, do_not_blow_up_list=("map", collision_object_name)): continue rospy.sleep(1) self.close_gripper(collision_object, get_fingertip(self.transform_to(grasp))) # com = self.get_center_of_mass(collision_object) # com = self.tf.transform_to(com, "/tcp") # if com is None: # rospy.logwarn("TF failed") # return False # self.load_object(self.calc_object_weight(collision_object, object_density), # Vector3(com.point.x, com.point.y, com.point.z)) rospy.loginfo("grasped " + collision_object_name) # self.__grasp = self.tf.transform_to(grasp) # v1 = deepcopy(self.__grasp.pose.position) # v1.z = 0 # v2 = deepcopy(collision_object.primitive_poses[0].position) # v2.z = 0 # a = magnitude(subtract_point(v1, v2)) # b = abs(self.__grasp.pose.position.z - collision_object.primitive_poses[0].position.z) # c = sqrt(a ** 2 + b ** 2) # self.__d = abs(c) # print c rospy.logdebug("lift object") if not self.__move_group_to(get_pre_grasp(grasp), move_group, do_not_blow_up_list=("map", collision_object_name)): rospy.logdebug("couldnt lift object") return True rospy.logwarn("Grapsing failed.") return False
def execute(self, userdata): rospy.loginfo('Executing state ScanObstacles') userdata.sec_try_done = False if userdata.sec_try: current_region = self.classified_regions[self.next_cluster - 1][0] userdata.sec_try_done = True else: #get regions if len(self.classified_regions) == 0: obstacle_cluster = utils.map.get_obstacle_regions() rospy.logdebug( str(len(self.classified_regions)) + " regions found.") print '#####################################' self.classified_regions = utils.map.undercover_classifier( obstacle_cluster, userdata.yaml.objects) print self.classified_regions print '#####################################' base = utils.manipulation.get_base_origin().point self.classified_regions.sort(key=lambda x: euclidean_distance( Point(*(utils.map.index_to_coordinates(*x[0].get_avg())) + (0.0, )), base)) # x[0].get_number_of_cells()) if self.next_cluster >= len(self.classified_regions): rospy.loginfo("searched all cluster") return 'noRegionLeft' current_region = self.classified_regions[self.next_cluster][0] rospy.logdebug("current region: " + str(self.next_cluster) + "\n" + str(current_region) + "\nclassified as " + str(self.classified_regions[self.next_cluster][1])) self.next_cluster += 1 region_centroid = Point( *(utils.map.index_to_coordinates(*current_region.get_avg())) + (-0.065, )) dist_to_region = mathemagie.euclidean_distance(Point(0, 0, 0), region_centroid) #if userdata.yaml.task_type == Task.TASK_5: # fixture_position = mathemagie.add_point(userdata.yaml.puzzle_fixture.position, Point(0.115, 0.165, 0)) # dist_to_fixture = mathemagie.euclidean_distance(fixture_position, region_centroid) # if dist_to_fixture < 0.35: # rospy.logdebug("Region classified as puzzle fixture, skipping") # return 'mapScanned' # If the arm cannot move ignore distant regions # TODO find the best max distance if not userdata.enable_movement: rospy.logdebug('Distance of the current region to the arm: %s' % str(dist_to_region)) if dist_to_region > 1.1: rospy.logwarn('Current region is out of reach. Ignoring it.') return 'mapScanned' angle = 1.2 distance = 0.6 + current_region.get_number_of_cells() * 0.008 poses = make_scan_pose(region_centroid, distance, angle, n=16) if not userdata.enable_movement: poses = utils.manipulation.filter_close_poses(poses) poses = utils.map.filter_invalid_scan_poses2(region_centroid.x, region_centroid.y, poses) if userdata.sec_try: current_pose = utils.manipulation.get_eef_position().pose.position current_pose.z = 0 region_to_eef = subtract_point(region_centroid, current_pose) poses.sort(key=lambda pose: abs( get_angle( region_to_eef, subtract_point( Point(pose.pose.position.x, pose.pose.position.y, 0), region_centroid)) - pi / 2)) visualize_poses(poses) if userdata.enable_movement: # move = utils.manipulation.move_arm_and_base_to plan = utils.manipulation.plan_arm_and_base_to else: # move = utils.manipulation.move_to plan = utils.manipulation.plan_arm_to utils.manipulation.blow_up_objects(do_not_blow_up_list=("map")) for pose in poses: # utils.manipulation.set_planning_time_arm(2) if utils.manipulation.move_with_plan_to(plan(pose)): # utils.manipulation.set_planning_time_arm(5) userdata.focused_point = region_centroid rospy.logdebug('Wait for clock') time.sleep(0.5) rospy.sleep(2.5) return 'newImage' utils.manipulation.blow_down_objects() return 'mapScanned'
def execute(self, userdata): rospy.loginfo('Executing state ScanMapArmCam') if self.finished: return 'mapScanned' regions = utils.map.get_unknown_regions() regions.sort(key=lambda r: -r.get_number_of_cells()) for r in regions: r.set_boarder_cells(sorted(utils.map.get_boarder_cell_points(r), cmp=lambda p1, p2: utils.map.is_more_edge(p1.x, p1.y, p2.x, p2.y))) i = 0 r_id = 0 utils.manipulation.set_planning_time_arm_base(5) while len(regions) > 0 and utils.map.get_percent_cleared() < 0.985: if len(regions[r_id].get_boarder_cells()) < self.min_num_of_cells: regions.remove(regions[r_id]) if len(regions) == 0: break r_id = r_id % len(regions) i = 0 continue if i >= 2: r_id += 1 r_id = r_id % len(regions) i = 0 continue next_point = regions[r_id].get_boarder_cells()[-1] regions[r_id].get_boarder_cells().remove(next_point) cell_x = next_point.x cell_y = next_point.y utils.map.mark_cell(cell_x, cell_y, True) next_point.z = 0.2 poses = make_scan_pose(next_point, 0.7, 0.8, n=16) poses = utils.map.filter_invalid_scan_poses(cell_x, cell_y, poses) poses = utils.map.filter_invalid_scan_poses2(cell_x, cell_y, poses) poses.sort(key=lambda pose: -euclidean_distance( Point(*(utils.map.index_to_coordinates(*regions[r_id].get_avg()) + (0,))), Point(pose.pose.position.x, pose.pose.position.y, 0))) visualize_poses(poses) j = 0 move_successfull = False while j < len(poses) and not move_successfull: rospy.logdebug("try scan pose nr. " + str(j)) move_successfull = utils.manipulation.move_arm_and_base_to(poses[j], do_not_blow_up_list=("all")) j += 1 if move_successfull: rospy.sleep(utils.waiting_time_before_scan) arm_base = utils.manipulation.get_base_origin() map_updated = utils.map.add_point_cloud(arm_base.point, scene_cam=False) if not map_updated: rospy.logdebug("no map update") utils.map.mark_cell(next_point.x, next_point.y, False) i += 1 continue rospy.logdebug("published") utils.map.mark_cell(next_point.x, next_point.y, False) co = utils.map.to_collision_object() utils.manipulation.get_planning_scene().add_object(co) utils.manipulation.set_planning_time_arm_base(10) return 'newImage' utils.map.mark_cell(next_point.x, next_point.y, False) i += 1 rospy.loginfo("can't update map any further") utils.map.all_unknowns_to_obstacle() co = utils.map.to_collision_object() utils.manipulation.get_planning_scene().add_object(co) self.finished = True utils.manipulation.set_planning_time_arm_base(10) return 'mapScanned'
def execute(self, userdata): rospy.loginfo('Executing state ScanObstacles') userdata.sec_try_done = False if userdata.sec_try: current_region = self.classified_regions[self.next_cluster-1][0] userdata.sec_try_done = True else: #get regions if len(self.classified_regions) == 0: obstacle_cluster = utils.map.get_obstacle_regions() rospy.logdebug(str(len(self.classified_regions)) + " regions found.") print '#####################################' self.classified_regions = utils.map.undercover_classifier(obstacle_cluster, userdata.yaml.objects) print self.classified_regions print '#####################################' base = utils.manipulation.get_base_origin().point self.classified_regions.sort(key=lambda x: euclidean_distance(Point(*(utils.map.index_to_coordinates(*x[0].get_avg()))+(0.0,)), base)) # x[0].get_number_of_cells()) if self.next_cluster >= len(self.classified_regions): rospy.loginfo("searched all cluster") return 'noRegionLeft' current_region = self.classified_regions[self.next_cluster][0] rospy.logdebug("current region: " + str(self.next_cluster) + "\n" + str(current_region) + "\nclassified as " + str(self.classified_regions[self.next_cluster][1])) self.next_cluster += 1 region_centroid = Point(*(utils.map.index_to_coordinates(*current_region.get_avg()))+(-0.065,)) dist_to_region = mathemagie.euclidean_distance(Point(0, 0, 0), region_centroid) #if userdata.yaml.task_type == Task.TASK_5: # fixture_position = mathemagie.add_point(userdata.yaml.puzzle_fixture.position, Point(0.115, 0.165, 0)) # dist_to_fixture = mathemagie.euclidean_distance(fixture_position, region_centroid) # if dist_to_fixture < 0.35: # rospy.logdebug("Region classified as puzzle fixture, skipping") # return 'mapScanned' # If the arm cannot move ignore distant regions # TODO find the best max distance if not userdata.enable_movement: rospy.logdebug('Distance of the current region to the arm: %s' % str(dist_to_region)) if dist_to_region > 1.1: rospy.logwarn('Current region is out of reach. Ignoring it.') return 'mapScanned' angle = 1.2 distance = 0.6 + current_region.get_number_of_cells()*0.008 poses = make_scan_pose(region_centroid, distance, angle, n=16) if not userdata.enable_movement: poses = utils.manipulation.filter_close_poses(poses) poses = utils.map.filter_invalid_scan_poses2(region_centroid.x, region_centroid.y, poses) if userdata.sec_try: current_pose = utils.manipulation.get_eef_position().pose.position current_pose.z = 0 region_to_eef = subtract_point(region_centroid, current_pose) poses.sort(key=lambda pose: abs(get_angle(region_to_eef, subtract_point(Point(pose.pose.position.x, pose.pose.position.y, 0), region_centroid)) - pi/2)) visualize_poses(poses) if userdata.enable_movement: # move = utils.manipulation.move_arm_and_base_to plan = utils.manipulation.plan_arm_and_base_to else: # move = utils.manipulation.move_to plan = utils.manipulation.plan_arm_to utils.manipulation.blow_up_objects(do_not_blow_up_list=("map")) for pose in poses: # utils.manipulation.set_planning_time_arm(2) if utils.manipulation.move_with_plan_to(plan(pose)): # utils.manipulation.set_planning_time_arm(5) userdata.focused_point = region_centroid rospy.logdebug('Wait for clock') time.sleep(0.5) rospy.sleep(2.5) return 'newImage' utils.manipulation.blow_down_objects() return 'mapScanned'
def execute(self, userdata): rospy.loginfo('Executing state GraspObject') collision_object_name = userdata.object_to_move.mpe_object.id rospy.loginfo('Trying to grasp %s' % collision_object_name) if userdata.enable_movement: move_to_func = utils.manipulation.move_arm_and_base_to plan_to_func = utils.manipulation.plan_arm_and_base_to else: move_to_func = utils.manipulation.move_to plan_to_func = utils.manipulation.plan_arm_to #get the collisionobject out of the planningscene collision_object = utils.manipulation.get_planning_scene().get_collision_object(collision_object_name) if collision_object is None: rospy.logwarn("Collision Object " + collision_object_name + " is not in planningscene.") return 'objectNotInPlanningscene' rospy.logdebug("Grasping: " + str(collision_object)) grasp_positions = calculate_grasp_position(collision_object, utils.manipulation.transform_to) #filter out some invalid grasps grasp_positions = utils.manipulation.filter_low_poses(grasp_positions) if not userdata.enable_movement: grasp_positions = utils.manipulation.filter_close_poses(grasp_positions) if len(grasp_positions) == 0: rospy.logwarn("No grasppositions found for " + collision_object_name) userdata.failed_object = userdata.object_to_move return 'noGraspPosition' #sort to try the best grasps first grasp_positions.sort(cmp=lambda x, y: utils.manipulation.cmp_pose_stamped(collision_object, x, y)) visualize_poses(grasp_positions) utils.manipulation.open_gripper() grasp_positions = [utils.manipulation.transform_to(grasp) for grasp in grasp_positions] utils.manipulation.blow_up_objects(do_not_blow_up_list=(collision_object_name)) for grasp in grasp_positions: plan_pre_grasp = plan_to_func(get_pre_grasp(grasp)) if plan_pre_grasp is None: continue rospy.logdebug("Plan to pregraspposition found") utils.manipulation.blow_up_objects(do_not_blow_up_list=("map", collision_object_name)) plan_to_grasp = plan_to_func(grasp, start_state=utils.manipulation.get_end_state(plan_pre_grasp)) if plan_to_grasp is None or not utils.manipulation.move_with_plan_to(plan_pre_grasp): rospy.logdebug("Failed to move to Graspposition") continue rospy.sleep(0.5) if not utils.manipulation.move_with_plan_to(plan_to_grasp): rospy.logdebug("Failed to move to Graspposition") continue rospy.logdebug("Graspposition taken") time.sleep(0.5) rospy.sleep(1) if not utils.manipulation.close_gripper(collision_object, get_fingertip(utils.manipulation.transform_to(grasp))): # userdata.failed_object = userdata.object_to_move utils.manipulation.blow_down_objects() return 'fail' time.sleep(0.5) rospy.sleep(1.5) #calculate the center of mass and weight of the object and call the load object service com = utils.manipulation.get_center_of_mass(collision_object) com = utils.manipulation.transform_to(com, "/tcp") if com is None: rospy.logwarn("TF failed") # userdata.failed_object = userdata.object_to_move utils.manipulation.blow_down_objects() return 'fail' density = 1 for obj in userdata.yaml.objects: if obj.name == collision_object_name: density = obj.primitive_densities[0] if isinf(density): rospy.logerr("Infinite density! WTF. setting density to 7850") density = 7850 utils.manipulation.load_object(utils.manipulation.calc_object_weight(collision_object, density), Vector3(com.point.x, com.point.y, com.point.z)) rospy.loginfo("grasped " + collision_object_name) #save grasp data for placing userdata.grasp = grasp fingertip = get_fingertip(grasp) fingertip_to_tcp = subtract_point(grasp.pose.position, fingertip.point) userdata.dist_to_obj = magnitude(fingertip_to_tcp) rospy.logdebug("lift object") the_pre_grasp = get_pre_grasp(grasp) the_move_to_func = move_to_func(the_pre_grasp, do_not_blow_up_list=collision_object_name) if not the_move_to_func: rospy.logwarn("couldnt lift object. continue anyway") userdata.failed_object = None return 'success' rospy.logwarn("Grapsing failed.") userdata.failed_object = userdata.object_to_move utils.manipulation.blow_down_objects() return 'fail'
def __grasp(self, taskdata): rospy.loginfo('Executing state GraspObject') collision_object_name = taskdata.object_to_move.mpe_object.id rospy.loginfo('Trying to grasp %s' % collision_object_name) if taskdata.enable_movement: move_to_func = utils.manipulation.move_arm_and_base_to plan_to_func = utils.manipulation.plan_arm_and_base_to else: move_to_func = utils.manipulation.move_to plan_to_func = utils.manipulation.plan_arm_to #get the collisionobject out of the planningscene collision_object = utils.manipulation.get_planning_scene().get_collision_object(collision_object_name) if collision_object is None: rospy.logwarn("Collision Object " + collision_object_name + " is not in planningscene.") return 'objectNotInPlanningscene' rospy.logdebug("Grasping: " + str(collision_object)) grasp_positions = calculate_grasp_position(collision_object, utils.manipulation.transform_to) #PoseStamped[] #filter out some invalid grasps grasp_positions = utils.manipulation.filter_low_poses(grasp_positions) if not taskdata.enable_movement: grasp_positions = utils.manipulation.filter_close_poses(grasp_positions) if len(grasp_positions) == 0: rospy.logwarn("No grasppositions found for " + collision_object_name) taskdata.failed_object = taskdata.object_to_move return 'noGraspPosition' #sort to try the best grasps first grasp_positions.sort(cmp=lambda x, y: utils.manipulation.cmp_pose_stamped(collision_object, x, y)) visualize_poses(grasp_positions) utils.manipulation.open_gripper() grasp_positions = [utils.manipulation.transform_to(grasp) for grasp in grasp_positions] utils.manipulation.blow_up_objects(do_not_blow_up_list=(collision_object_name)) for grasp in grasp_positions: plan_pre_grasp = plan_to_func(get_pre_grasp(grasp)) if plan_pre_grasp is None: continue rospy.logdebug("Plan to pregraspposition found") utils.manipulation.blow_up_objects(do_not_blow_up_list=("map", collision_object_name)) plan_to_grasp = plan_to_func(grasp, start_state=utils.manipulation.get_end_state(plan_pre_grasp)) if plan_to_grasp is None or not utils.manipulation.move_with_plan_to(plan_pre_grasp): rospy.logdebug("Failed to move to Graspposition") continue rospy.sleep(0.5) if not utils.manipulation.move_with_plan_to(plan_to_grasp): rospy.logdebug("Failed to move to Graspposition") continue rospy.logdebug("Graspposition taken") time.sleep(0.5) rospy.sleep(1) if not utils.manipulation.close_gripper(collision_object, get_fingertip(utils.manipulation.transform_to(grasp))): # taskdata.failed_object = taskdata.object_to_move utils.manipulation.blow_down_objects() return 'fail' time.sleep(0.5) rospy.sleep(1.5) #calculate the center of mass and weight of the object and call the load object service com = utils.manipulation.get_center_of_mass(collision_object) com = utils.manipulation.transform_to(com, "/tcp") if com is None: rospy.logwarn("TF failed") # taskdata.failed_object = taskdata.object_to_move utils.manipulation.blow_down_objects() return 'fail' density = 1 for obj in taskdata.yaml.objects: if obj.name == collision_object_name: density = obj.primitive_densities[0] if isinf(density): rospy.logerr("Infinite density! WTF. setting density to 7850") density = 7850 utils.manipulation.load_object(utils.manipulation.calc_object_weight(collision_object, density), Vector3(com.point.x, com.point.y, com.point.z)) rospy.loginfo("grasped " + collision_object_name) #save grasp data for placing taskdata.grasp = grasp fingertip = get_fingertip(grasp) fingertip_to_tcp = subtract_point(grasp.pose.position, fingertip.point) taskdata.dist_to_obj = magnitude(fingertip_to_tcp) rospy.logdebug("lift object") the_pre_grasp = get_pre_grasp(grasp) the_move_to_func = move_to_func(the_pre_grasp, do_not_blow_up_list=collision_object_name) if not the_move_to_func: rospy.logwarn("couldnt lift object. continue anyway") taskdata.failed_object = EurocObject() return 'success' rospy.logwarn("Grapsing failed.") taskdata.failed_object = taskdata.object_to_move utils.manipulation.blow_down_objects() return 'fail'
def __grasp_with_group(self, collision_object_name, move_group, object_density): """ Deprecated. For testing only """ if type(collision_object_name) is CollisionObject: collision_object_name = collision_object_name.id collision_object = self.__planning_scene_interface.get_collision_object( collision_object_name) if collision_object is None: rospy.logwarn("Collision Object " + collision_object_name + " is not in planningscene.") return False grasp_positions = calculate_grasp_position(collision_object, self.tf.transform_to) grasp_positions = self.filter_low_poses(grasp_positions) grasp_positions = self.filter_close_poses(grasp_positions) if len(grasp_positions) == 0: rospy.logwarn("No grasppositions found for " + collision_object_name) grasp_positions.sort( cmp=lambda x, y: self.cmp_pose_stamped(collision_object, x, y)) visualize_poses(grasp_positions) # print grasp_positions self.open_gripper() for grasp in grasp_positions: if self.__move_group_to( get_pre_grasp(self.transform_to(grasp)), move_group, do_not_blow_up_list=("map", collision_object_name)): if not self.__move_group_to( grasp, move_group, do_not_blow_up_list=("map", collision_object_name)): continue rospy.sleep(1) self.close_gripper(collision_object, get_fingertip(self.transform_to(grasp))) # com = self.get_center_of_mass(collision_object) # com = self.tf.transform_to(com, "/tcp") # if com is None: # rospy.logwarn("TF failed") # return False # self.load_object(self.calc_object_weight(collision_object, object_density), # Vector3(com.point.x, com.point.y, com.point.z)) rospy.loginfo("grasped " + collision_object_name) # self.__grasp = self.tf.transform_to(grasp) # v1 = deepcopy(self.__grasp.pose.position) # v1.z = 0 # v2 = deepcopy(collision_object.primitive_poses[0].position) # v2.z = 0 # a = magnitude(subtract_point(v1, v2)) # b = abs(self.__grasp.pose.position.z - collision_object.primitive_poses[0].position.z) # c = sqrt(a ** 2 + b ** 2) # self.__d = abs(c) # print c rospy.logdebug("lift object") if not self.__move_group_to( get_pre_grasp(grasp), move_group, do_not_blow_up_list=("map", collision_object_name)): rospy.logdebug("couldnt lift object") return True rospy.logwarn("Grapsing failed.") return False