Esempio n. 1
0
    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'
Esempio n. 2
0
    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'
Esempio n. 3
0
    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'