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, taskdata): rospy.loginfo('Executing state PlaceObject') destination = taskdata.place_position if self._retry == 2: rospy.logwarn('Failed to place two times. Dropping object.') self._retry = 0 if not utils.manipulation.open_gripper(): #cant happen taskdata.failed_object = EurocObject() return 'fail' if taskdata.enable_movement: move_to_func = utils.manipulation.move_arm_and_base_to else: move_to_func = utils.manipulation.move_to co = utils.manipulation.get_planning_scene().get_attached_object() if co is None: self._retry = 0 taskdata.failed_object = EurocObject() return 'noObjectAttached' co = co.object rospy.logdebug("Placing: " + str(co)) destination = utils.manipulation.transform_to(destination) rospy.logdebug("at::: " + str(destination)) if taskdata.yaml.task_type == Task.TASK_5: destination.point.z = 0.4 dest_orientation = deepcopy(taskdata.yaml.puzzle_fixture.orientation) place_poses = get_place_position_for_puzzle(destination, dest_orientation) else: place_poses = get_place_position(co, destination, utils.manipulation.transform_to, taskdata.dist_to_obj, taskdata.grasp) print place_poses # place_poses = utils.map.filter_invalid_poses3(destination.point.x, destination.point.y, place_poses) if not taskdata.enable_movement: place_poses = utils.manipulation.filter_close_poses(place_poses) for place_pose in place_poses: rospy.logdebug("Try to place at: " + str(place_pose)) if taskdata.yaml.task_type != Task.TASK_5: if not move_to_func(get_pre_place_position(place_pose), do_not_blow_up_list=(co.id)): rospy.logwarn("Can't reach preplaceposition.") continue else: rospy.logdebug("preplaceposition taken") time.sleep(0.5) rospy.sleep(1) if not move_to_func(place_pose, do_not_blow_up_list=(co.id, "map")): rospy.logwarn("Can't reach placeposition.") continue else: rospy.logdebug("placeposition taken") time.sleep(0.5) rospy.sleep(1) self._retry = 0 gripper_target = min(utils.manipulation.get_current_gripper_state()[1] + 0.002, gripper_max_pose) if not utils.manipulation.open_gripper(gripper_target): #cant happen taskdata.failed_object = EurocObject() return 'fail' rospy.sleep(3) if not utils.manipulation.open_gripper(): #cant happen taskdata.failed_object = EurocObject() return 'fail' if taskdata.yaml.task_type == Task.TASK_5: utils.manipulation.get_planning_scene().remove_object(co.id) time.sleep(0.5) rospy.sleep(1) # post_place_pose = utils.manipulation.transform_to(place_pose, co.id) if not move_to_func(get_pre_grasp(place_pose), do_not_blow_up_list=(co.id, "map")) and \ not move_to_func(get_pre_place_position(place_pose), do_not_blow_up_list=(co.id, "map")): rospy.logwarn("Can't reach postplaceposition. Continue anyway") taskdata.failed_object = EurocObject() self._retry = 0 return 'success' else: rospy.logdebug("postplaceposition taken") rospy.sleep(0.25) rospy.loginfo("placed " + co.id) taskdata.failed_object = EurocObject() self._retry = 0 return 'success' #try to place the object where it currently is rospy.logdebug("Placement failed, to to place where we are.") taskdata.place_position = self.new_place_position() o = EurocObject o.mpe_object = CollisionObject() o.object = CollisionObject() o.object.id = co.id o.mpe_object.id = co.id o.c_centroid = Point(0,0,0) taskdata.failed_object = o self._retry += 1 return 'noPlacePosition'
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'