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 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'