def latlon2xy(self, coord):
np_coord = (np.hstack((coord.npArray(), 1))[np.newaxis]).transpose()
translated_coord = np.matmul(self.translate_transform, np_coord)
rotated_coord = np.matmul(self.rotate_transform, translated_coord)
final_coord = np.matmul(self.scaling_transform, rotated_coord)
return Location(xlon=float(final_coord[0]), ylat=float(final_coord[1]))
def fromFile(cls, filename):
with open(filename,'rb') as f:
transform = yaml.load(f.read())
region_center = Location(xlon=transform['center']['longitude'], ylat=transform['center']['latitude'])
region_heading = transform['heading']
return cls(region_heading, region_center)
def __init__(self, region_heading, region_center):
# Region Heading = Heading in Degrees from the Vertical, Positive angle is CCW
# Region Center = Center of Target Region in Decimal Degrees Latitude and Longitude
self.rotate_theta = math.radians(region_heading)
self.translate_x, self.translate_y = (Location(0, 0) - region_center).npArray() # DX, DY in terms of Degrees Lon / Lat, respectively
# Assuming the world is flat nearby the region center
scale_x = haversine.haversine((region_center.lat, region_center.lon), (region_center.lat, region_center.lon + 1.)) # KM per Degree Longitude
scale_y = haversine.haversine((region_center.lat, region_center.lon), (region_center.lat + 1., region_center.lon)) # KM per Degree Latitude
self.rotate_transform = np.array([[math.cos(self.rotate_theta), -math.sin(self.rotate_theta), 0],
[math.sin(self.rotate_theta), math.cos(self.rotate_theta), 0],
[0, 0, 1]])
self.translate_transform = np.array([[1, 0, self.translate_x],
[0, 1, self.translate_y],
[0, 0, 1]])
self.scaling_transform = np.array([[scale_x, 0, 0],
[0, scale_y, 0],
[0, 0, 1]])
self.reverse_rotate_transform = np.array([[math.cos(-self.rotate_theta), -math.sin(-self.rotate_theta), 0],
[math.sin(-self.rotate_theta), math.cos(-self.rotate_theta), 0],
[0, 0, 1]])
self.reverse_translate_transform = np.array([[1, 0, -self.translate_x],
[0, 1, -self.translate_y],
[0, 0, 1]])
self.reverse_scaling_transform = np.array([[1./scale_x, 0, 0],
[0, 1./scale_y, 0],
[0, 0, 1]])
def updateCenter(self, new_center):
self.translate_x, self.translate_y = (new_center - Location(0, 0)).npArray() # DX, DY in terms of Degrees Lon / Lat, respectively
# Assuming the world is flat nearby the region center
scale_x = haversine.haversine((new_center.lat, new_center.lon), (new_center.lat, new_center.lon + 1.)) # KM per Degree Longitude
scale_y = haversine.haversine((new_center.lat, new_center.lon), (new_center.lat + 1., new_center.lon)) # KM per Degree Latitude
self.translate_transform = np.array([[1, 0, self.translate_x],
[0, 1, self.translate_y],
[0, 0, 1]])
self.scaling_transform = np.array([[scale_x, 0, 0],
[0, scale_y, 1],
[0, 0, 1]])
self.reverse_translate_transform = np.array([[1, 0, -self.translate_x],
[0, 1, -self.translate_y],
[0, 0, 1]])
self.reverse_scaling_transform = np.array([[1./scale_x, 0, 0],
[0, 1./scale_y, 0],
[0, 0, 1]])
def main():
parser = argparse.ArgumentParser(description='Parser for MIP testing')
parser.add_argument(
'-i', '--infile_path',
nargs='?',
type=str,
default="/home/mlfrantz/Documents/MIP_Research/mip_research/test_fields/test_field_2.csv",
help='Input file that represents the world',
)
parser.add_argument(
'-o', '--outfile_path',
nargs='?',
type=str,
default="/home/mlfrantz/Documents/MIP_Research/mip_research/Pictures/",
help='Directory where pictures are stored',
)
parser.add_argument(
'-g','--gradient',
action='store_true',
help='By adding this flag you will compute the gradient of the input field.',
)
parser.add_argument(
'-r', '--robots',
nargs='*',
type=str,
default='glider1',
help='List of robots to plan for. Must be in the robots.yaml file.',
)
parser.add_argument(
'--robots_cfg',
nargs='?',
type=str,
default='cfg/robots.yaml',
help='Configuration file of robots availalbe for planning.',
)
parser.add_argument(
'--sim_cfg',
nargs='?',
type=str,
default='cfg/sim.yaml',
help='Simulation-specific configuration file name.',
)
parser.add_argument(
'-n', '--planning_time',
nargs='?',
type=float,
default=5,
help='Length of the path to be planned in (units).',
)
parser.add_argument(
'-s', '--start_point',
nargs='*',
type=int,
default=(0,0),
help='Starting points for robots for planning purposes, returns list [x0,y0,x1,y1,...,xN,yN] for 1...N robots.',
)
parser.add_argument(
'-e', '--end_point',
nargs=2,
type=int,
default=[],
help='Ending point for planning purposes, returns list [x,y].',
)
parser.add_argument(
'-t', '--time_limit',
nargs='?',
type=float,
default=0.0,
help='Time limit in seconds you want to stop the simulation. Default lets it run until completion.',
)
parser.add_argument(
'-d', '--direction_constr',
nargs='?',
type=str,
default='8_direction',
help='Sets the direction constraint. Default allows it to move in any of the 8 directions each move. \
"nsew" only lets it move north-south-east-west. \
"diag" only lets it move diagonally (NW,NE,SW,SE).',
)
parser.add_argument(
'--same_point',
action='store_false',
help='By default it will not allow a point to be visited twice in the same planning period.',
)
parser.add_argument(
'--gen_image',
action='store_true',
help='Set to true if you want the image to be saved to file.',
)
parser.add_argument(
'--test',
action='store_true',
help='Will load ROMS maps by default, otherwise loads a test map.',
)
parser.add_argument(
'--experiment_name',
nargs='?',
type=str,
default="Test Experiment",
help='Name of the Experiement you are running',
)
args = parser.parse_args()
# Path lenth in time (hours).
Np = args.planning_time
# Load the map from either ROMS data or test file
if not args.test:
# ROMS map
# Loading Simulation-Specific Parameters
with open(os.path.expandvars(args.sim_cfg),'rb') as f:
yaml_sim = yaml.load(f.read())
fieldSavePath = '/home/mlfrantz/Documents/MIP_Research/mip_research/cfg/normal_field_{}_{}.npy'.format(str(abs(yaml_sim['sim_world']['center_longitude'])),yaml_sim['sim_world']['center_latitude'])
try:
field = np.load(fieldSavePath)
norm_field = np.load(fieldSavePath)
print("Loaded Map Successfully")
except IOError:
wd = World.roms(
datafile_path=yaml_sim['roms_file'],
xlen = yaml_sim['sim_world']['width'],
ylen = yaml_sim['sim_world']['height'],
center = Location(xlon=yaml_sim['sim_world']['center_longitude'], ylat=yaml_sim['sim_world']['center_latitude']),
feature = yaml_sim['science_variable'],
resolution = (yaml_sim['sim_world']['resolution'],yaml_sim['sim_world']['resolution']),
)
# This is the scalar_field in a static word.
# The '0' is the first time step and goes up to some max time
# field = np.copy(wd.scalar_field[:,:,0])
field = np.copy(wd.scalar_field)
norm_field = normalize(field)
field = normalize(field) # This will normailze the field between 0-1
# fieldSavePath = '/home/mlfrantz/Documents/MIP_Research/mip_research/cfg/normal_field.npy'
np.save(fieldSavePath, field)
# Example of an obstacle, make the value very low in desired area
# field[int(len(field)/4):int(3*len(field)/4),int(len(field)/4):int(3*len(field)/4)] = -100
field_resolution = (yaml_sim['sim_world']['resolution'],yaml_sim['sim_world']['resolution'])
else:
# Problem data, matrix transposed to allow for proper x,y coordinates to be mapped wih i,j
field = np.genfromtxt(args.infile_path, delimiter=',', dtype=float).transpose()
field_resolution = (1,1)
if args.gradient:
grad_field = np.gradient(field)
mag_grad_field = np.sqrt(grad_field[0]**2 + grad_field[1]**2)
# Load the robots.yaml Configuration file.
with open(os.path.expandvars(args.robots_cfg),'rb') as f:
yaml_mission = yaml.load(f.read())
# Get the speed of each robot that we are planning for.
steps = []
colors = []
for key,value in [(k,v) for k,v in yaml_mission.items() if k in args.robots]:
# Number of 1Km steps the planner can plan for.
# The expresseion solves for the number of waypoints so a +1 is needed for range.
# For instance, a glider going 0.4m/s would travel 1.44Km in 1 hour so it needs at least 2 waypoints, start and end.
plan_range = int(np.round(value['vel']*Np*60*60*0.001*(1/min(field_resolution))))+1
if plan_range > 0:
steps.append(range(plan_range))
else:
steps.append(range(2))
colors.append(value['color'])
temp_len = [len(s) for s in steps]
steps = [steps[np.argmax(temp_len)]]*len(steps) # This makes everything operate at the same time
velocity_correction = [t/max(temp_len) for t in temp_len] # To account for time difference between arriving to waypoints
# Make time correction for map forward propagation
max_steps = max([len(s) for s in steps])
field_delta = int(max_steps/Np)
t_step = 0
k_step = 0
field_time_steps = []
for i in range(max_steps):
field_time_steps.append(t_step)
k_step += 1
if k_step == field_delta:
k_step = 0
t_step += 1
# Number of robots we are planning for.
robots = range(len(args.robots))
DX = np.arange(field.shape[0]) # Integer values for range of X coordinates
DY = np.arange(field.shape[1]) # Integer values for range of Y coordinates
# Starting position contraint
start = args.start_point
if len(start) > 2:
# More than one robot.
start = [start[i:i + 2] for i in range(0, len(start), 2)]
else:
# One robot, extra list needed for nesting reasons.
start = [start]
# Greedy one step look ahead
# Build the direction vectors for checking values
if args.direction_constr == '8_direction':
# Check each of the 8 directions (N,S,E,W,NE,NW,SE,SW)
directions = [(0,1), (0,-1), (1,0), (-1,0), (1,1), (-1,1), (1,-1), (-1,-1)]
elif args.direction_constr == 'nsew':
directions = [(0,1), (0,-1), (1,0), (-1,0)] # N-S-E-W
elif args.direction_constr == 'diag':
directions = [(1,1), (-1,1), (1,-1), (-1,-1)] # Diag
startTime = time.time()
paths = []
for r in robots:
for s in steps[r]:
# Check each of the directions
if s == 0:
path = [start[r]]
continue
values = np.zeros(len(directions))
for i,d in enumerate(directions):
try:
if args.same_point:
move = [path[-1][0] + velocity_correction[r]*d[0], path[-1][1] + velocity_correction[r]*d[1]]
if move[0] >= 0 and move[0] <= field.shape[0]-1 and move[1] >= 0 and move[1] <= field.shape[1]-1:
# print(move,move[0], move[1],move[0] >= 0 and move[0] < field.shape[0] and move[1] >= 0 and move[1] < field.shape[1])
# Makes sure we are in
if [round(move[0],3),round(move[1],3)] not in [[round(p[0],3),round(p[1],3)] for p in path]:
# print(move)
values[i] = bilinear_interpolation(move, field)
# values[i] = field[path[-1][0] + d[0], path[-1][1] + d[1], 0]
else:
continue
else:
# print(move[0], move[1],move[0] >= 0 and move[0] < field.shape[0] and move[1] >= 0 and move[1] < field.shape[1])
# Makes sure we are in
continue
else:
move = [path[-1][0] + velocity_correction[r]*d[0], path[-1][1] + velocity_correction[r]*d[1]]
if move[0] >= 0 and move[0] <= field.shape[0]-1 and move[1] >= 0 and move[1] <= field.shape[1]-1:
values[i] = bilinear_interpolation(move, field)
except:
continue
# print(values)
new_point = [path[-1][0] + velocity_correction[r]*directions[np.argmax(values)][0], path[-1][1] + velocity_correction[r]*directions[np.argmax(values)][1]]
# print(new_point, values, np.argmax(values), directions[np.argmax(values)])
path.append(new_point)
paths.append(path)
# print(paths)
runTime = time.time() - startTime
if args.gen_image:
wd = World.roms(
datafile_path=yaml_sim['roms_file'],
xlen = yaml_sim['sim_world']['width'],
ylen = yaml_sim['sim_world']['height'],
center = Location(xlon=yaml_sim['sim_world']['center_longitude'], ylat=yaml_sim['sim_world']['center_latitude']),
feature = yaml_sim['science_variable'],
resolution = (yaml_sim['sim_world']['resolution'],yaml_sim['sim_world']['resolution']),
)
if args.gradient:
# plt.imshow(mag_grad_field.transpose())#, interpolation='gaussian', cmap= 'gnuplot')
if not args.test:
plt.imshow(wd.scalar_field[:,:,0].transpose(), interpolation='gaussian', cmap= 'gnuplot')
plt.xticks(np.arange(0,len(wd.lon_ticks), (1/min(field_resolution))), np.around(wd.lon_ticks[0::int(1/min(field_resolution))], 2))
plt.yticks(np.arange(0,len(wd.lat_ticks), (1/min(field_resolution))), np.around(wd.lat_ticks[0::int(1/min(field_resolution))], 2))
plt.xlabel('Longitude', fontsize=20)
plt.ylabel('Latitude', fontsize=20)
plt.text(1.25, 0.5, str(yaml_sim['science_variable']),{'fontsize':20}, horizontalalignment='left', verticalalignment='center', rotation=90, clip_on=False, transform=plt.gca().transAxes)
else:
plt.imshow(field.transpose(), interpolation='gaussian', cmap= 'gnuplot')
else:
if not args.test:
plt.imshow(norm_field[:,:,0].transpose(), interpolation='gaussian', cmap= 'gnuplot')
plt.xticks(np.arange(0,len(wd.lon_ticks), (1/min(field_resolution))), np.around(wd.lon_ticks[0::int(1/min(field_resolution))], 2))
plt.yticks(np.arange(0,len(wd.lat_ticks), (1/min(field_resolution))), np.around(wd.lat_ticks[0::int(1/min(field_resolution))], 2))
plt.xlabel('Longitude', fontsize=20)
plt.ylabel('Latitude', fontsize=20)
plt.text(1.25, 0.5, "normalized " + str(yaml_sim['science_variable']),{'fontsize':20}, horizontalalignment='left', verticalalignment='center', rotation=90, clip_on=False, transform=plt.gca().transAxes)
else:
plt.imshow(field.transpose(), interpolation='gaussian', cmap= 'gnuplot')
plt.colorbar()
for i,path in enumerate(paths):
path_x = [x for x,y in path]
path_y = [y for x,y in path]
plt.plot(path_x, path_y, color=colors[i], linewidth=2.0)
plt.plot(path[0][0], path[0][1], color='g', marker='o')
plt.plot(path[-1][0], path[-1][1], color='r', marker='o')
points = []
for path in paths:
for i, point in enumerate(path):
points.append(points)
if point in path and point not in point:
plt.annotate(i+1, (path[i][0], path[i][1]))
robots_str = '_robots_%d' % len(robots)
path_len_str = '_pathLen_%d' % len(steps[0])
if len(args.end_point) > 0 :
end_point_str = '_end%d%d' % (args.end_point[0], args.end_point[1])
else:
end_point_str = ''
if args.gradient:
grad_str = '_gradient'
else:
grad_str = ''
if args.time_limit > 0:
time_lim_str = '_timeLim_%d' % args.time_limit
else:
time_lim_str = ''
if args.direction_constr == 'nsew':
dir_str = '_%s' % args.direction_constr
elif args.direction_constr == 'diag':
dir_str = '_%s' % args.direction_constr
else:
dir_str = ''
# print(sum([field[p[0],p[1],0] for p in path]))
try:
score_str = '_score_%f' % sum([bilinear_interpolation(p, field) for path in paths for p in path])
except TypeError:
score_str = '_no_solution'
file_string = 'greedy_' + time.strftime("%Y%m%d-%H%M%S") + \
robots_str + \
path_len_str + \
end_point_str + \
grad_str + \
time_lim_str + \
dir_str + \
score_str + \
'.png'
print(file_string)
plt.savefig(args.outfile_path + file_string)
plt.show()
else:
filename = args.outfile_path
check_empty = os.path.exists(filename)
if args.direction_constr == 'nsew':
dir_str = '_%s' % args.direction_constr
elif args.direction_constr == 'diag':
dir_str = '_%s' % args.direction_constr
else:
dir_str = ''
constraint_string = dir_str
try:
score_str = sum([bilinear_interpolation(p, field) for path in paths for p in path])
except TypeError:
score_str = 0#'_no_solution'
with open(filename, 'a', newline='') as csvfile:
fieldnames = [ 'Experiment', \
'Algorithm', \
'Map', \
'Map Center', \
'Map Resolution', \
'Start Point', \
'End Point', \
'Score', \
'Run Time (sec)', \
'Budget (hours)', \
'Number of Robots', \
'Constraints']
writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
if not check_empty:
print("File is empty")
writer.writeheader()
writer.writerow({ 'Experiment': args.experiment_name, \
'Algorithm': 'Greedy', \
'Map': str(yaml_sim['roms_file']), \
'Map Center': Location(xlon=yaml_sim['sim_world']['center_longitude'], ylat=yaml_sim['sim_world']['center_latitude']).__str__(), \
'Map Resolution': (yaml_sim['sim_world']['resolution'],yaml_sim['sim_world']['resolution']), \
'Start Point': args.start_point, \
'End Point': args.end_point if len(args.end_point) > 0 else 'NA' , \
'Score': score_str, \
'Run Time (sec)': runTime, \
'Budget (hours)': args.planning_time, \
'Number of Robots': len(args.robots), \
'Constraints': constraint_string})
def main():
parser = argparse.ArgumentParser(description='Parser for MIP testing')
parser.add_argument(
'-i',
'--infile_path',
nargs='?',
type=str,
default=
"/home/mlfrantz/Documents/MIP_Research/mip_research/test_fields/fast_time_vary.npy",
help='Input file that represents the world',
)
parser.add_argument(
'-o',
'--outfile_path',
nargs='?',
type=str,
default="/home/mlfrantz/Documents/MIP_Research/mip_research/Pictures/",
help='Directory where pictures are stored',
)
parser.add_argument(
'-g',
'--gradient',
action='store_true',
help=
'By adding this flag you will compute the gradient of the input field.',
)
parser.add_argument(
'--time_vary',
action='store_true',
help=
'By adding this flag you will vary the time input field monotonically.',
)
parser.add_argument(
'-r',
'--robots',
nargs='*',
type=str,
default='glider1',
help='List of robots to plan for. Must be in the robots.yaml file.',
)
parser.add_argument(
'--robots_cfg',
nargs='?',
type=str,
default='cfg/robots.yaml',
help='Configuration file of robots available for planning.',
)
parser.add_argument(
'--sim_cfg',
nargs='?',
type=str,
default='cfg/sim.yaml',
help='Simulation-specific configuration file name.',
)
parser.add_argument(
'-n',
'--planning_time',
nargs='?',
type=float,
default=5,
help='Length of the path to be planned in time (hours).',
)
parser.add_argument(
'-s',
'--start_point',
nargs='*',
type=int,
default=(0, 0),
help=
'Starting points for robots for planning purposes, returns list [x0,y0,x1,y1,...,xN,yN] for 1...N robots.',
)
parser.add_argument(
'-e',
'--end_point',
nargs=2,
type=int,
default=[],
help='Ending point for planning purposes, returns list [x,y].',
)
parser.add_argument(
'-t',
'--time_limit',
nargs='?',
type=float,
default=0.0,
help=
'Real time limit in seconds you want to stop the simulation. Default lets it run until completion.',
)
parser.add_argument(
'-d',
'--direction_constr',
nargs='?',
type=str,
default='8_direction',
help=
'Sets the direction constraint. Default allows it to move in any of the 8 directions each move. \
"nsew" only lets it move north-south-east-west. \
"diag" only lets it move diagonally (NW,NE,SW,SE).',
)
parser.add_argument(
'--sync',
nargs='?',
type=str,
default='',
help=
'Sets the direction of the Synchronization constraint. Default is no Synchronization. \
"ns" goes from north to south. \
"sn" goes from south to north. \
"ew" goes from east to west. \
"we" goes from west to east. \
"diag" only lets it move diagonally (NW,NE,SW,SE).',
)
parser.add_argument(
'--same_point',
action='store_false',
help=
'By default it will not allow a point to be visited twice in the same planning period. Include this flag to allow, might allow for more flexible planning.',
)
parser.add_argument(
'--anti_curl',
action='store_true',
help='By default it will not consider the anti-curling constraints.',
)
parser.add_argument(
'--force_curl',
action='store_true',
help='By default it will not consider the force-curling constraints.',
)
parser.add_argument(
'--straight_line',
# action='store_true',
# help='By default it will not consider the straight line constraints.',
nargs=2,
type=int,
default=[],
help='Sets how long you want each straight to be, returns list [x,y].',
)
parser.add_argument(
'-a',
'--rect_area',
nargs=4,
type=int,
default=[],
help=
'Coordinates for recangular are to restrict planning to, returns list [x1, x2, y1, y2].',
)
parser.add_argument(
'-c',
'--collision_rad',
nargs='?',
type=float,
default=0.0,
help='Collision Radius between robots.',
)
parser.add_argument(
'--gen_image',
action='store_true',
help='Set to true if you want the image to be saved to file.',
)
parser.add_argument(
'--test',
action='store_true',
help='Will load ROMS maps by default, otherwise loads a test map.',
)
parser.add_argument(
'--experiment_name',
nargs='?',
type=str,
default="Test Experiment",
help='Name of the Experiement you are running',
)
args = parser.parse_args()
# Path lenth in time (hours).
Np = args.planning_time
# Load the map from either ROMS data or test file
if not args.test:
# ROMS map
# Loading Simulation-Specific Parameters
with open(os.path.expandvars(args.sim_cfg), 'rb') as f:
yaml_sim = yaml.load(f.read())
# If running many test on the same map this will save you time by not having to reload the map from scratch each run. Saved me probably 10s of hours.
fieldSavePath = '/home/mlfrantz/Documents/MIP_Research/mip_research/cfg/normal_field_{}_{}.npy'.format(
str(abs(yaml_sim['sim_world']['center_longitude'])),
yaml_sim['sim_world']['center_latitude'])
try:
field = np.load(fieldSavePath)
norm_field = np.load(fieldSavePath)
print("Loaded Map Successfully")
except IOError:
wd = World.roms(
datafile_path=yaml_sim['roms_file'],
xlen=yaml_sim['sim_world']['width'],
ylen=yaml_sim['sim_world']['height'],
center=Location(xlon=yaml_sim['sim_world']['center_longitude'],
ylat=yaml_sim['sim_world']['center_latitude']),
feature=yaml_sim['science_variable'],
resolution=(yaml_sim['sim_world']['resolution'],
yaml_sim['sim_world']['resolution']),
)
# This is the scalar_field in a static word.
# The '0' is the first time step and goes up to some max time
# field = np.copy(wd.scalar_field[:,:,0])
field = np.copy(wd.scalar_field)
# pdb.set_trace()
# norm_field = normalize(field)
# field = normalize(field) # This will normailze the field between 0-1
norm_field = np.array(
[normalize(field, i) for i in range(field.shape[2])])
norm_field = np.moveaxis(norm_field, 0, -1)
field = np.copy(norm_field)
np.save(fieldSavePath, field)
# Example of an obstacle, make the value very low in desired area
# field[int(len(field)/4):int(3*len(field)/4),int(len(field)/4):int(3*len(field)/4)] = -100
field_resolution = (yaml_sim['sim_world']['resolution'],
yaml_sim['sim_world']['resolution'])
else:
# Problem data, matrix transposed to allow for proper x,y coordinates to be mapped wih i,j
# field = np.genfromtxt(args.infile_path, delimiter=',', dtype=float).transpose()
field_resolution = (1, 1)
field = np.load(args.infile_path)
field = np.moveaxis(field, 0, -1)
# field = np.transpose(field)
norm_field = np.load(args.infile_path)
norm_field = np.moveaxis(norm_field, 0, -1)
pdb.set_trace()
print("Loaded Map Successfully")
if args.gradient:
# I would advise not using this as I don't believe it was doing what I thought it was.
# pdb.set_trace()
grad_field = np.gradient(field[:, :, 0])
mag_grad_field = np.dot(
grad_field[0],
grad_field[1]) #np.sqrt(grad_field[1]**2)# + grad_field[1]**2)
# Load the robots.yaml Configuration file.
with open(os.path.expandvars(args.robots_cfg), 'rb') as f:
yaml_mission = yaml.load(f.read())
# Get the speed of each robot that we are planning for.
steps = []
colors = []
for key, value in [(k, v) for k, v in yaml_mission.items()
if k in args.robots]:
# Number of 1Km steps the planner can plan for.
# The expresseion solves for the number of waypoints so a +1 is needed for range.
# For instance, a glider going 0.4m/s would travel 1.44Km in 1 hour so it needs at least 2 waypoints, start and end.
plan_range = int(
np.round(value['vel'] * Np * 60 * 60 * 0.001 *
(1 / min(field_resolution)))) + 1
print(plan_range)
if plan_range > 0:
steps.append(range(plan_range))
else:
steps.append(range(2))
colors.append(value['color'])
temp_len = [len(s) for s in steps]
steps = [steps[np.argmax(temp_len)]] * len(
steps) # This makes everything operate at the same time
velocity_correction = [
t / max(temp_len) for t in temp_len
] # To account for time difference between arriving to waypoints
# velocity_correction = [1 for t in temp_len] # To account for time difference between arriving to waypoints
# Make time correction for map forward propagation. This is only used on time-varying maps.
max_steps = max([len(s) for s in steps])
field_delta = int(max_steps / Np)
t_step = 0
k_step = 0
field_time_steps = []
for i in range(max_steps):
field_time_steps.append(t_step)
k_step += 1
if k_step == field_delta:
k_step = 0
t_step += 1
# Number of robots we are planning for.
robots = range(len(args.robots))
DX = np.arange(field.shape[0]) # Integer values for range of X coordinates
DY = np.arange(field.shape[1]) # Integer values for range of Y coordinates
m = Model() # This defines a model inside Gurobi.
if args.time_limit > 0:
# Sets a runtime limit. Default is to run to completion.
m.Params.TIME_LIMIT = args.time_limit
# m.Params.MIPGap = 0.01 # Allows the model to run until the solution is within a certain range of the optimal.
# Add variables
pairs = tuplelist([(r, s) for r in robots for s in steps[r]])
x = m.addVars(pairs, lb=DX[0], ub=DX[-1], vtype=GRB.CONTINUOUS, name='x')
y = m.addVars(pairs, lb=DY[0], ub=DY[-1], vtype=GRB.CONTINUOUS, name='y')
# If we are taking the gradient of the field
if args.gradient:
f = m.addVars(pairs,
lb=np.min(mag_grad_field),
ub=np.max(mag_grad_field),
vtype=GRB.CONTINUOUS,
name='f')
else:
f = m.addVars(pairs,
lb=np.min(field),
ub=np.max(field),
vtype=GRB.CONTINUOUS,
name='f')
lx = m.addVars(pairs, DX, lb=0, ub=1, vtype=GRB.CONTINUOUS, name='lx')
ly = m.addVars(pairs, DY, lb=0, ub=1, vtype=GRB.CONTINUOUS, name='ly')
lxy = m.addVars(pairs, DX, DY, vtype=GRB.CONTINUOUS, name='lxy')
for r in robots:
for t in steps[r]:
m.addSOS(GRB.SOS_TYPE2, [lx[r, t, i] for i in DX])
m.addSOS(GRB.SOS_TYPE2, [ly[r, t, j] for j in DY])
budget = m.addVars(pairs,
lb=0,
ub=len(steps[0]),
vtype=GRB.CONTINUOUS,
name="budget")
m.addConstrs((budget[r, steps[r][0]] == len(steps[0]) for r in robots),
name="Initial Budget")
# Add main constraints
# Starting position contraint
start = args.start_point
if len(start) > 2:
# More than one robot.
start = [start[i:i + 2] for i in range(0, len(start), 2)]
else:
# One robot, extra list needed for nesting reasons.
start = [start]
m.addConstrs((x[r, steps[r][0]] == start[r][0] for r in robots),
name="Initial x")
m.addConstrs((y[r, steps[r][0]] == start[r][1] for r in robots),
name="Initial y")
if args.gradient:
m.addConstrs(
(f[r, steps[r][0]] == mag_grad_field[start[r][0], start[r][1]]
for r in robots),
name="Initial f")
else:
m.addConstrs((f[r, steps[r][0]] == field[start[r][0], start[r][1], 0]
for r in robots),
name="Initial f")
# Optional end position constraint. Could implement additional position constaint
if len(args.end_point) > 0:
end = args.end_point
m.addConstrs((x[r, steps[r][-1]] == end[0] for r in robots),
name="End x")
m.addConstrs((y[r, steps[r][-1]] == end[1] for r in robots),
name="End y")
# m.addConstr((f[r, steps[r][-1]] == field[end[0], end[1], -1] for r in robots), name="End f")
for r in robots:
for t in steps[r]:
m.addConstrs(
quicksum(lxy[r, t, i, j] for j in DY) == lx[r, t, i]
for i in DX)
m.addConstrs(
quicksum(lxy[r, t, i, j] for i in DX) == ly[r, t, j]
for j in DY)
m.addConstrs((quicksum(lx[r, t, i] for i in DX) == 1 for r in robots
for t in steps[r]))
m.addConstrs((quicksum(ly[r, t, j] for j in DY) == 1 for r in robots
for t in steps[r]))
m.addConstrs((quicksum(DX[i] * lx[r, t, i] for i in DX) == x[r, t]
for r in robots for t in steps[r]))
m.addConstrs((quicksum(DY[j] * ly[r, t, j] for j in DY) == y[r, t]
for r in robots for t in steps[r]))
if args.gradient:
m.addConstrs((quicksum(mag_grad_field[i, j] * lxy[r, t, i, j]
for i in DX for j in DY) == f[r, t]
for r in robots for t in steps[r]))
else:
if args.time_vary:
# This map is time varying
m.addConstrs(
(quicksum(field[i, j, field_time_steps[t]] * lxy[r, t, i, j]
for i in DX for j in DY) == f[r, t] for r in robots
for t in steps[r]))
else:
# This map is static
m.addConstrs((quicksum(field[i, j, 0] * lxy[r, t, i, j] for i in DX
for j in DY) == f[r, t] for r in robots
for t in steps[r]))
# Primary Motion constraints
# There is something to this but it doesnt work as is
#
# if len(args.straight_line) > 0:
# # Now we need to correct our previous velocity_correction by making sure the edges are the lengths of the edges are all equal.
# path_len_x = args.straight_line[0]
# path_len_y = args.straight_line[1]
#
# # if path_len_x > 1 or path_len_y > 1:
# # print(steps)
# # pdb.set_trace()
# # for r in robots:
# # steps = [s for s in steps[r]]
#
# else:
path_len_x = 1
path_len_y = 1
# for r in robots:
# v = velocity_correction[r]
# m.addQConstr( (quicksum( (((x[r,t-1]-x[r,t])*(x[r,t-1]-x[r,t])) + \
# ((y[r,t-1]-y[r,t])*(y[r,t-1]-y[r,t]))) for t in steps[r][1:])) <= (len(steps[r])) ) # -1 becuase we added a step above
# m.addConstrs(( quicksum( ((x[r,t-1]-x[r,t])*(x[r,t-1]-x[r,t])) + ((y[r,t-1]-y[r,t])*(y[r,t-1]-y[r,t])) ) <= len(steps[r]) for r in robots for t in steps[r][1:] ))
# Binary variables for motion constraints
b_range = range(4)
b = m.addVars(pairs, b_range, vtype=GRB.BINARY, name='b%d' % t)
for r in robots:
v = velocity_correction[r]
for t in steps[r][1:]:
# pdb.set_trace()
m.addConstr(x[r, t - 1] + v * path_len_x * b[r, t, 0] -
v * path_len_x * b[r, t, 1] == x[r, t])
m.addConstr(b[r, t, 0] + b[r, t, 1] <= 1)
m.addConstr(y[r, t - 1] + v * path_len_y * b[r, t, 2] -
v * path_len_y * b[r, t, 3] == y[r, t])
m.addConstr(b[r, t, 2] + b[r, t, 3] <= 1)
# m.addConstr(budget[r,t-1] - (b[r,t,0] + b[r,t,1] + b[r,t,2] + b[r,t,3]) == budget[r,t])
# Add option constraints
# Direction constraints PCA/Eignevector/Cardinal constraints
if args.direction_constr == '8_direction':
m.addConstrs(b[r, t, 0] + b[r, t, 1] + b[r, t, 2] + b[r, t, 3] >= 1
for r in robots for t in steps[r][1:])
elif args.direction_constr == 'nsew':
m.addConstrs(b[r, t, 0] + b[r, t, 1] + b[r, t, 2] + b[r, t, 3] == 1
for r in robots for t in steps[r][1:]) #PCA N-S-E-W
elif args.direction_constr == 'diag':
m.addConstrs(b[r, t, 0] + b[r, t, 1] + b[r, t, 2] + b[r, t, 3] == 2
for r in robots for t in steps[r][1:]) #PCA Diag
# Constraint to prevent going through the same point twice
if args.same_point:
M = 100
for r in robots:
for i, t in enumerate(steps[r][1:]):
t1 = m.addVars(t, range(4), vtype=GRB.BINARY, name='t%d' % t)
for j, s in enumerate(steps[r][:i + 1]):
m.addConstr(x[r, t] - x[r, s] >= 0.1 - M * t1[j, 0])
m.addConstr(x[r, s] - x[r, t] >= 0.1 - M * t1[j, 1])
m.addConstr(y[r, t] - y[r, s] >= 0.1 - M * t1[j, 2])
m.addConstr(y[r, s] - y[r, t] >= 0.1 - M * t1[j, 3])
m.addConstr(t1[j, 0] + t1[j, 1] + t1[j, 2] + t1[j, 3] <= 3)
# Synchronization Constraint: Specific path or 8 direction [NS, EW, NE-SW, NW-SE]
if args.sync == 'ns':
for r in robots:
v = velocity_correction[r]
for t in steps[r][1:]:
m.addConstr(x[r, t - 1] - x[r, t] == 0)
m.addConstr(y[r, t] - y[r, t - 1] == v)
elif args.sync == 'sn':
for r in robots:
v = velocity_correction[r]
for t in steps[r][1:]:
m.addConstr(x[r, t - 1] - x[r, t] == 0)
m.addConstr(y[r, t - 1] - y[r, t] == v)
elif args.sync == 'ew':
for r in robots:
v = velocity_correction[r]
for t in steps[r][1:]:
m.addConstr(x[r, t - 1] - x[r, t] == v)
m.addConstr(y[r, t - 1] - y[r, t] == 0)
elif args.sync == 'we':
for r in robots:
v = velocity_correction[r]
for t in steps[r][1:]:
m.addConstr(x[r, t] - x[r, t - 1] == v)
m.addConstr(y[r, t - 1] - y[r, t] == 0)
elif args.sync == 'nwse':
for r in robots:
v = velocity_correction[r]
for t in steps[r][1:]:
m.addConstr(x[r, t] - x[r, t - 1] == v)
m.addConstr(y[r, t] - y[r, t - 1] == v)
elif args.sync == 'nesw':
for r in robots:
v = velocity_correction[r]
for t in steps[r][1:]:
m.addConstr(x[r, t - 1] - x[r, t] == v)
m.addConstr(y[r, t] - y[r, t - 1] == v)
elif args.sync == 'senw':
for r in robots:
v = velocity_correction[r]
for t in steps[r][1:]:
m.addConstr(x[r, t - 1] - x[r, t] == v)
m.addConstr(y[r, t - 1] - y[r, t] == v)
elif args.sync == 'swne':
for r in robots:
v = velocity_correction[r]
for t in steps[r][1:]:
m.addConstr(x[r, t] - x[r, t - 1] == v)
m.addConstr(y[r, t - 1] - y[r, t] == v)
elif args.sync == 'path':
#path = [(0,5), (1,4), (2,3), (3,2), (4,1), (5,0), (6,1), (7,2), (8,3), (9,4), (10,5), (9,6), (8,7), \
#(7,8), (6,9), (5,10), (4,9), (3,8), (2,7), (1,6), (0,5)]
path = [(0, 0), (0, 1), (-1, 1), (-1, 2), (-2, 2), (-2, 1)]
path_len = np.sum(np.abs(np.subtract(path[:-1], path[1:])))
for r in robots:
v = velocity_correction[r]
for i, t in enumerate(steps[r][4:3 + len(path)]):
print(i, t)
m.addConstr(x[r, t] - x[r, t - 1] == path[i + 1][0] -
path[i][0])
m.addConstr(y[r, t] - y[r, t - 1] == path[i + 1][1] -
path[i][1])
elif args.sync == 'path_time':
#path = [(0,5), (1,4), (2,3), (3,2), (4,1), (5,0), (6,1), (7,2), (8,3), (9,4), (10,5), (9,6), (8,7), \
#(7,8), (6,9), (5,10), (4,9), (3,8), (2,7), (1,6), (0,5)]
path = [(4, 3, 7), (5, 4, 8), (6, 5, 9), (7, 6, 10),
(7, 7, 11)] #, (2,6,10)]
path_len = np.sum(np.abs(np.subtract(path[:-1], path[1:])))
for r in robots:
v = velocity_correction[r]
times = [p[2] for p in path]
# pdb.set_trace()
for i, t in enumerate(times):
m.addConstr(x[r, t] == path[i][0])
m.addConstr(y[r, t] == path[i][1])
# Constraint to prevent colliding with other robots.
if args.collision_rad > 0:
M = 100
t2 = m.addVars(len(robots),
len(robots),
range(4),
vtype=GRB.BINARY,
name='t2')
for r in robots[1:]:
for s in range(r):
m.addConstrs(
x[r, t] - x[s, t] >= args.collision_rad - M * t2[r, s, 0]
for t in steps[s][1:])
m.addConstrs(
x[s, t] - x[r, t] >= args.collision_rad - M * t2[r, s, 1]
for t in steps[s][1:])
m.addConstrs(
y[r, t] - y[s, t] >= args.collision_rad - M * t2[r, s, 2]
for t in steps[s][1:])
m.addConstrs(
y[s, t] - y[r, t] >= args.collision_rad - M * t2[r, s, 3]
for t in steps[s][1:])
m.addConstr(
t2[r, s, 0] + t2[r, s, 1] + t2[r, s, 2] + t2[r, s, 3] <= 3)
# Anti-Curling constraints go from [3,...,Np] and enforce ani-curling (this one works!)
if args.anti_curl:
for r in robots:
v = velocity_correction[r]
t_range = range(4)
t1 = m.addVars(steps[r], t_range, vtype=GRB.BINARY, name='t1')
delta = 2
t_delta = 3
M = 100
for t in steps[r][t_delta:]:
m.addConstr(x[r, t - 2] - x[r, t] >= v * delta - M * t1[t, 0])
m.addConstr(x[r, t] - x[r, t - 2] >= v * delta - M * t1[t, 1])
m.addConstr(y[r, t - 2] - y[r, t] >= v * delta - M * t1[t, 2])
m.addConstr(y[r, t] - y[r, t - 2] >= v * delta - M * t1[t, 3])
m.addConstr(t1[t, 0] + t1[t, 1] + t1[t, 2] + t1[t, 3] <= 3)
# Curling constraints go from [2,...,Np] and enforce curing (Questionable functionality)
if args.force_curl:
for r in robots:
v = velocity_correction[r]
x_delta = 1
x_t_delta = 2
y_delta = 1
y_t_delta = 2
# This for x motion
m.addConstrs(x[r, t - x_t_delta] - x[r, t] <= v * x_delta
for t in steps[r][x_t_delta:])
m.addConstrs(x[r, t] - x[r, t - x_t_delta] <= v * x_delta
for t in steps[r][x_t_delta:])
# t_range = range(2)
# M = 1000
# t1 = m.addVars(steps[r][y_t_delta:], t_range, vtype=GRB.BINARY, name='t1')
# m.addConstrs(y[r,t-y_t_delta] - y[r,t] >= v*y_delta - M*t1[t,0] for t in steps[r][y_t_delta:])
# m.addConstrs(y[r,t] - y[r,t-y_t_delta] >= v*y_delta - M*t1[t,1] for t in steps[r][y_t_delta:])
# m.addConstrs(t1[t,0] + t1[t,1] <= 1 for t in steps[r][y_t_delta:])
m.addConstrs(y[r, t - y_t_delta] - y[r, t] <= v * y_delta
for t in steps[r][y_t_delta:])
m.addConstrs(y[r, t] - y[r, t - y_t_delta] <= v * y_delta
for t in steps[r][y_t_delta:])
# This is for y motion
# m.addConstrs(y[r,t-y_t_delta] - y[r,t] <= v*y_delta for t in steps[r][y_t_delta:])
# m.addConstrs(y[r,t] - y[r,t-y_t_delta] <= v*y_delta for t in steps[r][y_t_delta:])
# t_range = range(2)
# M = 1000
# t1 = m.addVars(steps[r][x_t_delta:], t_range, vtype=GRB.BINARY, name='t1')
# m.addConstrs(x[r,t-x_t_delta] - x[r,t] >= v*x_delta - M*t1[t,0] for t in steps[r][x_t_delta:])
# m.addConstrs(x[r,t] - x[r,t-x_t_delta] >= v*x_delta - M*t1[t,1] for t in steps[r][x_t_delta:])
# m.addConstrs(t1[t,0] + t1[t,1] <= 1 for t in steps[r][x_t_delta:])
# m.addConstrs(y[r,t-t_delta] - y[r,t] <= v*y_delta for t in steps[r][t_delta:])
# m.addConstrs(y[r,t] - y[r,t-t_delta] <= v*y_delta for t in steps[r][t_delta:])
# Straight path constraints (Questionable functionality)
if len(args.straight_line) > 0:
# Now we need to correct our previous velocity_correction by making sure the edges are the lengths of the edges are all equal.
path_len_x = args.straight_line[0]
path_len_y = args.straight_line[1]
delta = max(path_len_x, path_len_y)
print(delta, path_len_y)
M = 1000
for r in robots:
for v in velocity_correction:
inv_v = 1 / v
if inv_v > 1:
tv = m.addVars(steps[r][delta:],
range(4),
vtype=GRB.BINARY,
name='t1')
for t in steps[r][delta:]:
m.addConstr(x[r, t - delta] -
x[r, t] >= path_len_x * v - M * tv[t, 0])
m.addConstr(
x[r, t] - x[r, t - delta] >= path_len_x * v -
M * tv[t, 1])
m.addConstr(y[r, t - delta] -
y[r, t] >= path_len_y * v - M * tv[t, 2])
m.addConstr(
y[r, t] - y[r, t - delta] >= path_len_y * v -
M * tv[t, 3])
m.addConstr(tv[t, 0] + tv[t, 1] + tv[t, 2] +
tv[t, 3] == 3)
else:
# Working better
ts = m.addVars(steps[r][delta:],
range(4),
vtype=GRB.BINARY,
name='ts')
for t in steps[r][delta:]:
m.addConstr(x[r, t - path_len_x] -
x[r, t] >= path_len_x - M * ts[t, 0])
m.addConstr(
x[r, t] - x[r, t - path_len_x] >= path_len_x -
M * ts[t, 1])
m.addConstr(y[r, t - path_len_y] -
y[r, t] >= path_len_y - M * ts[t, 2])
m.addConstr(
y[r, t] - y[r, t - path_len_y] >= path_len_y -
M * ts[t, 3])
m.addConstr(ts[t, 0] + ts[t, 1] + ts[t, 2] +
ts[t, 3] == 3)
#
# Area Constraint (Start with square, expand to more complex shapes)
if len(args.rect_area) > 0:
for r in robots:
area = args.rect_area
m.addConstrs(x[r, t] >= area[0] for t in steps[r][1:])
m.addConstrs(x[r, t] <= area[1] for t in steps[r][1:])
m.addConstrs(y[r, t] >= area[2] for t in steps[r][1:])
m.addConstrs(y[r, t] <= area[3] for t in steps[r][1:])
# End constraint updates, seting objective
m.update()
obj = quicksum(f[r, t] for r in robots for t in steps[r])
m.setObjective(obj, GRB.MAXIMIZE)
# Run the optimizer
m.optimize()
# Print the variable values
# path = np.zeros(Np)
# print(m.getAttr('X', pos).values())
# for v in m.getVars():
# if v.X != 0:
# print("%s %f" % (v.varName, v.X))
if args.gen_image:
# Plotting Code
path_x = m.getAttr('X', x).values()
path_y = m.getAttr('X', y).values()
# print('Obj: %g' % obj.getValue())
_paths = list(zip(path_x, path_y))
paths = []
for r in robots:
paths.append(_paths[0:len(steps[r])])
_paths = _paths[len(steps[r]):]
print(paths)
dist = paths[0][:]
print(
sum([
np.sqrt((dist[i][0] - dist[i + 1][0])**2 +
(dist[i][1] - dist[i + 1][1])**2)
for i, p in enumerate(dist[:-1])
]))
plt.figure(figsize=(10, 10))
if args.gradient:
# plt.imshow(mag_grad_field.transpose())#, interpolation='gaussian', cmap= 'gnuplot')
if not args.test:
wd = World.roms(
datafile_path=yaml_sim['roms_file'],
xlen=yaml_sim['sim_world']['width'],
ylen=yaml_sim['sim_world']['height'],
center=Location(
xlon=yaml_sim['sim_world']['center_longitude'],
ylat=yaml_sim['sim_world']['center_latitude']),
feature=yaml_sim['science_variable'],
resolution=(yaml_sim['sim_world']['resolution'],
yaml_sim['sim_world']['resolution']),
)
plt.imshow(mag_grad_field[:, :].transpose(),
interpolation='gaussian',
cmap='jet')
plt.imshow(norm_field[:, :, 0].transpose(),
interpolation='gaussian',
cmap='jet')
plt.xticks(
np.arange(0, len(wd.lon_ticks),
(1 / min(field_resolution))),
np.around(wd.lon_ticks[0::int(1 / min(field_resolution))],
2))
plt.yticks(
np.arange(0, len(wd.lat_ticks),
(1 / min(field_resolution))),
np.around(wd.lat_ticks[0::int(1 / min(field_resolution))],
2))
plt.xlabel('Longitude', fontsize=20)
plt.ylabel('Latitude', fontsize=20)
plt.text(1.25,
0.5,
str(yaml_sim['science_variable']), {'fontsize': 20},
horizontalalignment='left',
verticalalignment='center',
rotation=90,
clip_on=False,
transform=plt.gca().transAxes)
else:
plt.imshow(field.transpose(),
interpolation='gaussian',
cmap='gnuplot')
else:
if not args.test:
wd = World.roms(
datafile_path=yaml_sim['roms_file'],
xlen=yaml_sim['sim_world']['width'],
ylen=yaml_sim['sim_world']['height'],
center=Location(
xlon=yaml_sim['sim_world']['center_longitude'],
ylat=yaml_sim['sim_world']['center_latitude']),
feature=yaml_sim['science_variable'],
resolution=(yaml_sim['sim_world']['resolution'],
yaml_sim['sim_world']['resolution']),
)
plt.imshow(norm_field[:, :, 0].transpose(),
interpolation='gaussian',
cmap='jet')
plt.xticks(
np.arange(0, len(wd.lon_ticks),
(1 / min(field_resolution))),
np.around(wd.lon_ticks[0::int(1 / min(field_resolution))],
2))
# plt.xticks(rotation=30)
plt.yticks(
np.flip(np.arange(0, len(wd.lat_ticks),
(1 / min(field_resolution))),
axis=0),
np.around(wd.lat_ticks[0::int(1 / min(field_resolution))],
2))
# plt.xticks(np.arange(11),np.arange(1,12), fontsize=20)
# plt.yticks(np.arange(11), np.arange(1,12), fontsize=20)
plt.xlabel('Longitude', fontsize=20)
plt.ylabel('Latitude', fontsize=20)
# plt.xlabel('Local X Coordinate', fontsize=20)
# plt.ylabel('Local Y Coordinate', fontsize=20)
plt.text(1.25,
0.5,
"normalized " + str(yaml_sim['science_variable']),
{'fontsize': 20},
horizontalalignment='left',
verticalalignment='center',
rotation=90,
clip_on=False,
transform=plt.gca().transAxes)
# plt.grid(b=True, which='both', axis='both')
else:
plt.imshow(field[:, :, int(args.planning_time)],
interpolation='gaussian',
cmap='jet')
plt.colorbar()
for i, path in enumerate(paths):
path_x = [x for x, y in path]
path_y = [y for x, y in path]
plt.plot(path_x, path_y, color=colors[i], linewidth=2.0)
plt.plot(path[0][0], path[0][1], color='g', marker='o')
plt.plot(path[-1][0], path[-1][1], color='r', marker='o')
points = []
for path in paths:
for i, point in enumerate(path):
points.append(points)
if point in path and point not in point:
plt.annotate(i + 1, (path[i][0], path[i][1]))
robots_str = '_robots_%d' % len(robots)
path_len_str = '_pathLen_%d' % len(steps[0])
if len(args.end_point) > 0:
end_point_str = '_end%d%d' % (args.end_point[0], args.end_point[1])
else:
end_point_str = ''
if args.gradient:
grad_str = '_gradient'
else:
grad_str = ''
if args.time_limit > 0:
time_lim_str = '_timeLim_%d' % args.time_limit
else:
time_lim_str = ''
if args.direction_constr == 'nsew':
dir_str = '_%s' % args.direction_constr
elif args.direction_constr == 'diag':
dir_str = '_%s' % args.direction_constr
else:
dir_str = ''
if args.collision_rad > 0:
collision_str = '_collRad_%d' % args.collision_rad
else:
collision_str = ''
if args.anti_curl:
anti_curl_str = '_antiCurl'
else:
anti_curl_str = ''
if args.force_curl:
force_curl_str = '_forceCurl'
else:
force_curl_str = ''
if len(args.straight_line) > 0:
straight_line_str = '_straight_%d_%d' % (args.straight_line[0],
args.straight_line[1])
else:
straight_line_str = ''
if len(args.rect_area) > 0:
area = args.rect_area
rect_area_str = '_rect_x%d_%dy%d_%d' % (area[0], area[1], area[2],
area[3])
else:
rect_area_str = ''
if args.time_vary:
obj = m.getObjective()
score_str = '_score_%f' % obj.getValue()
else:
# m.addConstrs((quicksum(field[i,j,field_time_steps[t]]*lxy[r,t,i,j] for i in DX for j in DY) == f[r,t] for r in robots for t in steps[r]))
score_str = '_score_%f' % sum([
bilinear_interpolation(p, field) for path in paths
for p in path
])
file_string = 'mip_run_' + time.strftime("%Y%m%d-%H%M%S") + \
robots_str + \
path_len_str + \
end_point_str + \
collision_str + \
grad_str + \
time_lim_str + \
dir_str + anti_curl_str + \
force_curl_str + \
straight_line_str + \
score_str + \
rect_area_str + \
'.eps'
print(file_string)
plt.savefig(args.outfile_path + file_string, format='eps')
plt.show()
else:
# Plotting Code
path_x = m.getAttr('X', x).values()
path_y = m.getAttr('X', y).values()
# print('Obj: %g' % obj.getValue())
_paths = list(zip(path_x, path_y))
paths = []
for r in robots:
paths.append(_paths[0:len(steps[r])])
_paths = _paths[len(steps[r]):]
print(paths)
filename = args.outfile_path
check_empty = os.path.exists(filename)
if args.direction_constr == 'nsew':
dir_str = '_%s' % args.direction_constr
elif args.direction_constr == 'diag':
dir_str = '_%s' % args.direction_constr
else:
dir_str = ''
if args.collision_rad > 0:
collision_str = '_collRad_%d' % args.collision_rad
else:
collision_str = ''
if args.anti_curl:
anti_curl_str = '_antiCurl'
else:
anti_curl_str = ''
if args.force_curl:
force_curl_str = '_forceCurl'
else:
force_curl_str = ''
if len(args.straight_line) > 0:
straight_line_str = '_straight_%d_%d' % (args.straight_line[0],
args.straight_line[1])
else:
straight_line_str = ''
if len(args.rect_area) > 0:
area = args.rect_area
rect_area_str = '_rect_x%d_%dy%d_%d' % (area[0], area[1], area[2],
area[3])
else:
rect_area_str = ''
if len(args.sync) > 0:
sync_str = 'sync'
else:
sync_str = ''
constraint_string = collision_str + \
dir_str + anti_curl_str + \
force_curl_str + \
straight_line_str + \
rect_area_str + \
sync_str
if args.time_vary:
obj = m.getObjective()
score_str = obj.getValue()
alg_str = "MIP_Time_Vary"
else:
score_str = sum([
bilinear_interpolation(p, field, field_time_steps[t])
for path in paths for t, p in enumerate(path)
])
alg_str = "MIP"
# obj = m.getObjective()
# score_str = obj.getValue()
with open(filename, 'a', newline='') as csvfile:
fieldnames = [ 'Experiment', \
'Algorithm', \
'Map', \
'Map Center', \
'Map Resolution', \
'Start Point', \
'End Point', \
'Score', \
'Run Time (sec)', \
'Budget (hours)', \
'Number of Robots', \
'Constraints']
writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
if not check_empty:
print("File is empty")
writer.writeheader()
writer.writerow({ 'Experiment': args.experiment_name, \
'Algorithm': alg_str, \
'Map': str(yaml_sim['roms_file']), \
'Map Center': Location(xlon=yaml_sim['sim_world']['center_longitude'], ylat=yaml_sim['sim_world']['center_latitude']).__str__(), \
'Map Resolution': (yaml_sim['sim_world']['resolution'],yaml_sim['sim_world']['resolution']), \
'Start Point': args.start_point, \
'End Point': args.end_point if len(args.end_point) > 0 else 'NA' , \
'Score': score_str, \
'Run Time (sec)': m.Runtime, \
'Budget (hours)': args.planning_time, \
'Number of Robots': len(args.robots), \
'Constraints': constraint_string})
return global_heading - math.degrees(self.rotate_theta)
def uv2planningFrame(self, uv):
np_uv = (np.hstack((uv.npArray(), 1))[np.newaxis]).transpose()
final_uv = np.matmul(self.rotate_transform, np_uv)
return LocDelta(d_xlon=float(final_uv[0]), d_ylat=float(final_uv[1]))
def uv2globalFrame(self, uv):
np_uv = (np.hstack((uv.npArray(), 1))[np.newaxis]).transpose()
final_uv = np.matmul(self.reverse_rotate_transform, np_uv)
return LocDelta(d_xlon=float(final_uv[0]), d_ylat=float(final_uv[1]))
if __name__ == '__main__':
ref_coord = Location(xlon= -2.0, ylat = 2.0)
frame_heading = 30.0
box_size = 1.0
latlon_coord = Location(xlon=1., ylat=1.)
xy_coord = Location(xlon=50., ylat=50.)
global_frame_current = LocDelta(d_xlon=.3, d_ylat=.3)
planning_frame_current = LocDelta(d_xlon=30, d_ylat=30)
global_frame_heading = math.atan2(global_frame_current.d_ylat, global_frame_current.d_xlon)
planning_frame_heading = math.atan2(planning_frame_current.d_ylat, planning_frame_current.d_xlon)
r = np.array([[math.cos(math.radians(frame_heading)), -math.sin(math.radians(frame_heading))],
[math.sin(math.radians(frame_heading)), math.cos(math.radians(frame_heading))]])