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