Пример #1
0
def _object_plan(args):
    obj, object_list, space, robot_types = args

    space.temp_obstacle_list = [
        item['start'] for item in object_list if item['idx'] is not obj['idx']
    ]

    test_case = {
        'start_position': obj['start'],
        'end_position': obj['end'],
        'workspace': space,
        'robot_types': robot_types
    }

    try:
        path = Manager._execute_planning(test_case)
        segments = Analyser.slipt(path)

        obj['plan'] = {'path': path, 'segments': segments, 'total': len(path)}

    except Exception, e:
        print "Object: ", obj['idx'] + 1
        print e.message
    def execute(environment):
        Manager._create_object_plan(environment)

        space = environment['workspace']
        object_list = environment['object_list']
        robot = environment['robot_dict'].values()[0]

        total_object = 0
        total_robot = 0

        while True:
            work_list = [
                obj for obj in object_list if len(obj['plan']['segments']) != 0
            ]

            if len(work_list) == 0:
                break

            best_obj = None
            best_cost = 0
            best_move_size = 0

            best_prepare = None
            best_move = None

            for obj in work_list:
                segment = obj['plan']['segments'][0]

                # If there is another object in the segment,
                # its no possible to be executed

                in_place_objects = [
                    item for item in object_list
                    if (item['idx'] is not obj['idx']) and (
                        hash(item['current_pos']) in segment)
                ]

                if len(in_place_objects) != 0:
                    print 'obj', obj['idx'], 'impossible by', [
                        item['idx'] for item in in_place_objects
                    ]
                    continue

                segment_start = segment.node[segment.graph['start_id']]
                segment_end = segment.node[segment.graph['end_id']]

                # Prepare Movement

                space.temp_obstacle_list = [
                    item['current_pos'] for item in object_list
                ]

                robot_start = robot.copy()

                robot_end = robot.copy()
                robot_end.position = Planner.create_robot_position(
                    segment_start, 2).position

                try:
                    prepare_plan = Manager._execute_planning({
                        'start_position':
                        robot_start,
                        'end_position':
                        robot_end,
                        'workspace':
                        space,
                        'robot_type':
                        robot.robot_type
                    })
                except Exception, e:
                    print "Object: ", obj['idx'] + 1
                    print "Impossible prepare plan"
                    continue

                space.temp_obstacle_list = []

                # Move Movement

                space.temp_obstacle_list = [
                    item['current_pos'] for item in object_list
                    if item['idx'] is not obj['idx']
                ]

                robot_start.position = robot_end.position
                robot_end = robot.copy()
                robot_end.position = Planner.create_robot_position(
                    segment_end).position

                try:
                    moviment_plan = Manager._execute_planning({
                        'start_position':
                        robot_start,
                        'end_position':
                        robot_end,
                        'workspace':
                        space,
                        'robot_type':
                        robot.robot_type
                    })
                except Exception, e:
                    print "Object: ", obj['idx'] + 1
                    print "Impossible movement plan"
                    continue

                space.temp_obstacle_list = []

                # total_cost = len(prepare_plan) - obj['plan']['total']
                total_cost = obj['plan']['total'] - len(segment) + len(
                    prepare_plan)
                # total_cost = len(prepare_plan) + len(moviment_plan) - ( obj['plan']['total'] - len(segment) )
                # total_cost = -obj['plan']['total'] - len(segment) + len(prepare_plan)

                if best_obj is None or total_cost < best_cost:
                    best_obj = obj
                    best_cost = total_cost
                    best_move_size = len(prepare_plan) + len(moviment_plan)

                    best_prepare = prepare_plan
                    best_move = moviment_plan
Пример #3
0
    def execute(environment):
        # print "-------------------------------------------------"
        # print "Planning for objects..."

        Manager._create_object_plan(environment)

        allocation_list = []

        space = environment['workspace']
        object_list = environment['object_list']
        robots = environment['robot_dict']

        total_object = 0
        total_robot = 0

        allocator = Munkres()

        # While not all objects are transported
        while True:
            # Create the list with objects that there are not yet
            # fully transported
            work_list = [
                obj for obj in object_list if len(obj['plan']['segments']) != 0
            ]

            # With all objects were transported, finish
            if len(work_list) == 0:
                break

            # print "-------------------------------------------------"
            # print "Planning for robots..."

            # Create the allocation matrix
            # Square, for the Hungarian Method
            shape = (max(len(work_list), len(robots)), ) * 2
            cost_matrix = np.full(shape,
                                  ManagerHungarianMulti.inf,
                                  dtype=np.int)

            robot_id_list = robots.keys()
            robot_plan = {}

            # For each robot available
            for robot_index, robot_id in enumerate(robot_id_list):

                # print "# robot", robot_index
                robot = robots[robot_id]
                robot_plan[robot_id] = {}

                # For each object to be transported
                for obj_index, obj in enumerate(work_list):
                    # print "### object", obj['idx']

                    robot_plan[robot_id][obj['idx']] = {}

                    # Get first segment
                    segment = obj['plan']['segments'][0]

                    if segment.graph['type'] is not robot.robot_type:
                        continue

                    # If there is another object in the segment,
                    # its no possible to be executed
                    in_place_objects = [
                        item for item in object_list
                        if (item['idx'] is not obj['idx']) and (
                            hash(item['current_pos']) in segment)
                    ]

                    if len(in_place_objects) != 0:
                        continue

                    segment_start = segment.node[segment.graph['start_id']]
                    segment_end = segment.node[segment.graph['end_id']]

                    # Prepare Movement

                    space.temp_obstacle_list = [
                        item['current_pos'] for item in object_list
                    ]

                    robot_start = robot.copy()

                    robot_end = robot.copy()

                    if robot.robot_type == Robot.TYPES.aerial:
                        robot_end.position = Planner.create_robot_position(
                            segment_start, -1).position
                    else:
                        robot_end.position = Planner.create_robot_position(
                            segment_start, 2).position

                    try:
                        prepare_plan = Manager._execute_planning({
                            'start_position':
                            robot_start,
                            'end_position':
                            robot_end,
                            'workspace':
                            space,
                            'robot_type':
                            robot.robot_type
                        })
                    except Exception, e:
                        print "Object: ", obj['idx'] + 1
                        print "Impossible prepare plan"
                        continue

                    space.temp_obstacle_list = []

                    robot_plan[robot_id][
                        obj['idx']]['prepare_plan'] = prepare_plan

                    # Move Movement

                    space.temp_obstacle_list = [
                        item['current_pos'] for item in object_list
                        if item['idx'] is not obj['idx']
                    ]

                    robot_start.position = robot_end.position
                    robot_end = robot.copy()
                    robot_end.position = Planner.create_robot_position(
                        segment_end).position

                    try:
                        moviment_plan = Manager._execute_planning({
                            'start_position':
                            robot_start,
                            'end_position':
                            robot_end,
                            'workspace':
                            space,
                            'robot_type':
                            robot.robot_type
                        })
                    except Exception, e:
                        print "Object: ", obj['idx'] + 1
                        print "Impossible movement plan"
                        continue

                    space.temp_obstacle_list = []

                    robot_plan[robot_id][
                        obj['idx']]['moviment_plan'] = moviment_plan

                    # Total cost to movement this object
                    # by the current robot
                    total_cost = obj['plan']['total'] - len(segment) + len(
                        prepare_plan)

                    # Populate the cost matrix
                    cost_matrix[robot_index][obj_index] = total_cost
    def execute(environment):
        Manager._create_object_plan(environment)

        space = environment['workspace']
        object_list = environment['object_list']
        robot = environment['robot_dict'].values()[0]

        total_object = 0
        total_robot = 0

        # For each object
        for obj in object_list:

            # print "### object", obj['idx']

            # For each segment
            for segment in obj['plan']['segments']:

                segment_start_id = segment.graph['start_id']
                segment_end_id = segment.graph['end_id']

                segment_start = segment.node[segment_start_id]
                segment_end = segment.node[segment_end_id]

                # print "start"
                # print robot
                # print obj['current_pos']
                # print '---------------'

                # Prepare

                space.temp_obstacle_list = [
                    item['current_pos'] for item in object_list
                ]

                robot_end = robot.copy()
                robot_end.position = Planner.create_robot_position(
                    segment_start, 2).position

                prepare_plan = Manager._execute_planning({
                    'start_position':
                    robot,
                    'end_position':
                    robot_end,
                    'workspace':
                    space,
                    'robot_type':
                    robot.robot_type
                })

                Manager.robot_path.append(prepare_plan)

                # Execute
                # if ManagerSimple.ani_obj:
                #     data = {
                #         'robot_path': prepare_plan
                #     }
                #     ManagerSimple.ani_obj.set_data( data )

                space.temp_obstacle_list = []

                # Moviment

                robot.position = robot_end.position

                # print "pre move"
                # print robot
                # print obj['current_pos']
                # print '---------------'

                space.temp_obstacle_list = [
                    item['current_pos'] for item in object_list
                    if item['idx'] is not obj['idx']
                ]

                robot_end = robot.copy()
                robot_end.position = Planner.create_robot_position(
                    segment_end).position

                moviment_plan = Manager._execute_planning({
                    'start_position':
                    robot,
                    'end_position':
                    robot_end,
                    'workspace':
                    space,
                    'robot_type':
                    robot.robot_type
                })

                Manager.robot_path.append(moviment_plan)

                space.temp_obstacle_list = []

                robot.position = robot_end.position
                obj['current_pos'].position = segment_end['state'].position

                # print "move"
                # print robot
                # print obj['current_pos']
                # print '---------------'

                total_object += len(segment)
                total_robot += len(prepare_plan) + len(moviment_plan)

            space.temp_obstacle_list = []

        # print "Total object:", total_object
        # print "Total robot:", total_robot
        # print "Total:", (total_object + total_robot)

        return total_object, total_robot
Пример #5
0
def _robot_plan(args):
    robot_index, robot, cost_row, work_list, object_list, space = args

    robot_plan = {}

    for obj_index, obj in enumerate(work_list):
        robot_plan[obj['idx']] = {}

        segment = obj['plan']['segments'][0]

        in_place_objects = [
            item for item in object_list
            if (item['idx'] is not obj['idx']) and (
                hash(item['current_pos']) in segment)
        ]

        if len(in_place_objects) != 0:
            print 'obj', obj['idx'] + 1, 'impossible by', [
                item['idx'] + 1 for item in in_place_objects
            ]
            continue

        segment_start = segment.node[segment.graph['start_id']]
        segment_end = segment.node[segment.graph['end_id']]

        # Prepare Movement

        space.temp_obstacle_list = [
            item['current_pos'] for item in object_list
        ]

        robot_start = robot.copy()

        robot_end = robot.copy()
        robot_end.position = Planner.create_robot_position(segment_start,
                                                           2).position

        try:
            prepare_plan = Manager._execute_planning({
                'start_position':
                robot_start,
                'end_position':
                robot_end,
                'workspace':
                space,
                'robot_type':
                robot.robot_type
            })
        except Exception, e:
            print "Object: ", obj['idx'] + 1
            print "Impossible prepare plan"
            continue

        space.temp_obstacle_list = []

        robot_plan[obj['idx']]['prepare_plan'] = prepare_plan

        # Move Movement

        space.temp_obstacle_list = [
            item['current_pos'] for item in object_list
            if item['idx'] is not obj['idx']
        ]

        robot_start.position = robot_end.position
        robot_end = robot.copy()
        robot_end.position = Planner.create_robot_position(
            segment_end).position

        try:
            moviment_plan = Manager._execute_planning({
                'start_position':
                robot_start,
                'end_position':
                robot_end,
                'workspace':
                space,
                'robot_type':
                robot.robot_type
            })
        except Exception, e:
            print "Object: ", obj['idx'] + 1
            print "Impossible movement plan"
            continue