def workspace_plot(outfile='highdim_workspace.png'):
'''Plot planar workspace for the high-dimensional case study.'''
# define boundary: unit hypercube
boundary = BoxBoundary([[0, 1], [0, 1]])
# define boundary style
boundary.style = {'color': 'black'}
# create robot's workspace
wspace = Workspace(boundary=boundary)
# create robot object
initConf = Point2D((0.1, 0.1)) # start close to the origin
robot = FullyActuatedRobot('Robot-highdim', init=initConf, wspace=wspace)
robot.diameter = 0
# create simulation object
sim = Simulate2D(wspace, robot, None)
# regions of interest
R1 = (BoxRegion2D([[.00, .20], [.00, .20]], ['r1']), 'brown')
R2 = (BoxRegion2D([[.25, .40], [.40, .55]], ['r2']), 'green')
R3 = (BoxRegion2D([[.70, 1.00], [.40, .60]], ['r3']), 'red')
R4 = (BoxRegion2D([[.00, .50], [.90, 1.00]], ['r4']), 'magenta')
# global obstacles
O1 = (BoxRegion2D([[.20, .30], [.30, .35]], ['o1']), 'gray')
O2 = (BoxRegion2D([[.15, .20], [.40, .60]], ['o2']), 'gray')
O3 = (BoxRegion2D([[.50, .55], [.30, .80]], ['o3']), 'gray')
# local obstacles
L1 = (BoxRegion2D([[.45, .50], [.75, .80]], ['LO']), to_rgba('gray', .6))
L2 = (BoxRegion2D([[.90, 1.00], [.50, .55]], ['LO']), to_rgba('gray', .6))
L3 = (BoxRegion2D([[.75, .80], [.20, .25]], ['LO']), to_rgba('gray', .6))
# add all regions
regions = [R1, R2, R3, R4, O1, O2, O3, L1, L2, L3]
# add regions to workspace
for r, c in regions:
# add styles to region
addStyle(r, style={'facecolor': c})
r.textStyle['fontsize'] = 24
# add region to workspace
sim.workspace.addRegion(r)
# set the robot's sensor
sensingShape = BallBoundary([0, 0], 0.001)
robot.sensor = BoundingBoxSimulatedSensor(robot, sensingShape, [], [])
# display workspace
sim.display(save=outfile, figsize=(7, 7))
def postprocessing(logfilename,
ts_filename,
outdir,
lts_index,
rrg_iterations,
lts_iterations,
local_traj_iterations,
generate=()):
'''Parses log file and generate statistics and figures.'''
if not os.path.isdir(outdir):
os.makedirs(outdir)
with open(logfilename, 'r') as logfile:
# first process general data
data = dict()
line = ''
for line in logfile:
prefix, line_data = line.split('--')
if prefix.lower().rfind('info') >= 0:
data.update(eval(line_data))
if line_data.lower().find('start global planning') >= 0:
break
print 'general data:', len(data)
# second process data on global planner
rrg_data = []
iteration_data = None
for line in logfile:
prefix, line_data = line.split('--')
if prefix.lower().rfind('info') >= 0:
if line_data.lower().rfind('found solution') >= 0:
rrg_data.append(iteration_data)
break
if line_data.lower().find('iteration') >= 0:
if iteration_data is not None:
rrg_data.append(iteration_data)
iteration_data = dict()
iteration_data.update(eval(line_data))
print 'rrg data:', len(rrg_data)
rrg_stat_data = eval(line_data)
for line in logfile:
prefix, line_data = line.split('--')
if prefix.lower().rfind('info') >= 0:
if line_data.lower().find('end global planning') >= 0:
break
rrg_stat_data.update(eval(line_data))
print 'rrg_stat_data', len(rrg_stat_data)
assert rrg_stat_data['Iterations'] == len(rrg_data)
# third process data on local planner
rrt_stat_data = dict()
for line in logfile:
prefix, line_data = line.split('--')
if prefix.lower().rfind('info') >= 0:
if line_data.lower().find('initialize local planner') >= 0:
break
rrt_stat_data.update(eval(line_data))
print 'rrt_stat_data:', len(rrt_stat_data)
rrt_data = []
execution_data = dict()
for line in logfile:
prefix, line_data = line.split('--')
if prefix.lower().rfind('info') >= 0:
if line_data.lower().find(
'local online planning finished') >= 0:
rrt_data.append(execution_data)
break
# NOTE: second condition is for compatibility with Cozmo logs
if (line_data.lower().find('start local planning step') >= 0
or line_data.lower().find('plan start time') >= 0):
rrt_data.append(execution_data)
execution_data = dict()
execution_data.update(eval(line_data))
print 'rrt data:', len(rrt_data)
# save useful information
with open(os.path.join(outdir, 'general_data.txt'), 'w') as f:
for key in [
'Robot initial configuration', 'Robot step size',
'Global specification', 'Buchi size', 'Local specification'
]:
print >> f, key, ':', data[key]
for key in [
'Iterations', 'global planning runtime', 'Size of TS',
'Size of PA'
]:
print >> f, key, ':', rrg_stat_data[key]
pre, suff = rrg_stat_data['global policy']
print >> f, 'Global policy size :', (len(pre), len(suff))
# key = 'Computing potential function runtime'
# print>>f, key, ':', rrt_stat_data[key]
# get workspace
wspace, style = data['Workspace']
wspace.boundary.style = style
ewspace, style = data['Expanded workspace']
ewspace.boundary.style = style
# get robot
robot_name = data['Robot name']
initConf = data['Robot initial configuration']
stepsize = data['Robot step size']
robot = eval(data['Robot constructor'])
robot.diameter = data['Robot diameter']
robot.localObst = data['Local obstacle label']
# create simulation object
sim = Simulate2D(wspace, robot, ewspace)
sim.config['output-dir'] = outdir
# add regions to workspace
for key, value in data.iteritems():
if isinstance(key, tuple) and key[0] == "Global region":
r, style = value
# add styles to region
addStyle(r, style=style)
# add region to workspace
sim.workspace.addRegion(r)
# create expanded region
er = expandRegion(r, robot.diameter / 2)
# add style to the expanded region
addStyle(er, style=style)
# add expanded region to the expanded workspace
sim.expandedWorkspace.addRegion(er)
# get local requests and obstacles
localSpec = data['Local specification']
requests = []
obstacles = []
for key, value in data.iteritems():
if isinstance(key, tuple) and key[0] == "Local region":
r, style, path, is_request = value
# add styles to region
addStyle(r, style=style)
# add path
if path:
r.path = it.cycle(path)
r.original_path = path[:]
# add to requests or obstacles lists
if is_request:
name = next(iter(r.symbols))
requests.append(Request(r, name, localSpec[name]))
else:
obstacles.append(r)
# get robot sensor
robot.sensor = eval(data['Robot sensor constructor'])
# create RRG planner and load transition system, and global policy
sim.offline = RRGPlanner(robot, None, 1)
prefix, suffix = rrg_stat_data['global policy']
sim.solution = (prefix, suffix[1:])
print 'Global policy size:', len(prefix), len(suffix)
if any(option in generate for option in ('workspace', 'expanded workspace',
'global solution')):
# set larger font for saving figures
for r in sim.workspace.regions | sim.expandedWorkspace.regions:
r.fontsize_orig = r.textStyle.get('fontsize', 12)
r.textStyle['fontsize'] = 24
# display workspace
if 'workspace' in generate:
sim.display(save='workspace.png')
# display expanded workspace
if 'expanded workspace' in generate:
sim.display(expanded=True, save='eworkspace.png')
# display solution for off-line problem
if 'global solution' in generate:
ts = sim.offline.ts
sim.offline.ts = Ts.load(ts_filename)
sim.display(expanded=True,
solution=prefix + suffix[1:],
save='global_solution.png')
sim.offline.ts = ts
# restore original fontsize
for r in sim.workspace.regions:
r.textStyle['fontsize'] = r.fontsize_orig
del r.fontsize_orig
# display both workspaces
if 'both workspaces' in generate:
sim.display(expanded='both')
# show construction of rrg
sim.offline.ts.init[initConf] = 1
if 'RRG construction' in generate:
sim.config['global-ts-color'] = {
'node_color': 'blue',
'edge_color': 'black'
}
sim.config['video-interval'] = 500
sim.config['video-file'] = 'rrg_construction.mp4'
sim.save_rrg_process(rrg_data)
# reset to default colors
sim.defaultConfiguration(reset=['global-ts-color'])
if 'RRG iterations' in generate:
sim.config['global-ts-color'] = {
'node_color': 'blue',
'edge_color': 'black'
}
rrg_iterations = [
i + (i == -1) * (len(rrg_data) + 1) for i in rrg_iterations
]
sim.save_rrg_iterations(rrg_data, rrg_iterations)
# reset to default colors
sim.defaultConfiguration(reset=['global-ts-color'])
# set to global and to save animation
sim.offline.ts = Ts.load(ts_filename)
print 'TS size', sim.offline.ts.size()
if 'offline plan' in generate:
sim.config['video-interval'] = 30
sim.config['sim-step'] = 0.02
sim.config['video-file'] = 'global_plan.mp4'
sim.simulate(loops=1, offline=True)
sim.play(output='video', show=False)
sim.save()
# get online trajectory
sim.offline.ts = Ts.load(ts_filename)
trajectory = [d['new configuration'] for d in rrt_data]
local_plans = [d['local plan'] for d in rrt_data] + [[]]
potential = [d['potential'] for d in rrt_data] + [0]
requests = [d['requests'] for d in rrt_data] + [[]]
print len(trajectory), len(local_plans)
if 'trajectory' in generate:
# set larger font for saving figures
for r in sim.workspace.regions | sim.expandedWorkspace.regions:
r.fontsize_orig = r.textStyle.get('fontsize', 12)
r.textStyle['fontsize'] = 24
sim.config['trajectory-min-transparency'] = 0.2 # fading
sim.config['trajectory-history-length'] = len(
trajectory) # full history
sim.config['global-policy-color'] = 'orange'
sim.online = LocalPlanner(None, sim.offline.ts, robot, localSpec)
sim.online.trajectory = trajectory
sim.online.robot.sensor.requests = []
sim.display(expanded=True,
solution=prefix + suffix[1:],
show_robot=False,
localinfo=('trajectory', ),
save='trajectory.png')
sim.defaultConfiguration(reset=[
'trajectory-min-transparency', 'trajectory-history-length',
'global-policy-color'
])
# restore original fontsize
for r in sim.workspace.regions:
r.textStyle['fontsize'] = r.fontsize_orig
del r.fontsize_orig
# local plan visualization
sim.online = LocalPlanner(None, sim.offline.ts, robot, localSpec)
sim.online.trajectory = trajectory
if 'online plan' in generate:
sim.config['video-interval'] = 30
sim.config['sim-step'] = 0.01
sim.config['video-file'] = 'local_plan.mp4'
sim.simulate(offline=False)
sim.play(output='video',
show=False,
localinfo={
'trajectory': trajectory,
'plans': local_plans,
'potential': potential,
'requests': requests
})
sim.save()
if 'online trajectory iterations' in generate:
sim.config['sim-step'] = 0.05
sim.config['trajectory-min-transparency'] = 1.0 # no fading
sim.config['trajectory-history-length'] = 100000 # entire history
sim.config['image-file-template'] = 'local_trajectory_{frame:04d}.png'
sim.config['global-policy-color'] = 'orange'
# simulate and save
sim.simulate(offline=False)
sim.play(output='plots',
show=False,
localinfo={
'trajectory': trajectory,
'plans': local_plans,
'potential': potential,
'requests': requests
})
sim.save(output='plots', frames=local_traj_iterations)
# set initial values
sim.defaultConfiguration(reset=[
'trajectory-min-transparency', 'trajectory-history-length',
'image-file-template', 'global-policy-color'
])
msize = np.mean([d['tree size'] for d in rrt_data if d['tree size'] > 0])
print 'Mean size:', msize
for k, d in enumerate(rrt_data):
if d['tree size'] > 0:
print(k, d['tree size'])
if any(option in generate
for option in ('LTS iterations', 'LTS construction')):
idx = lts_index
print rrt_data[idx]['tree size']
print 'current state:', rrt_data[idx - 1]['new configuration']
lts_data = sorted([
v for k, v in rrt_data[idx].items()
if str(k).startswith('lts iteration')
],
key=lambda x: x[0])
# print lts_data
sim.online.lts = Ts(directed=True, multi=False)
sim.online.lts.init[rrt_data[idx - 1]['new configuration']] = 1
# reset and fast-forward requests' locations
for r in sim.robot.sensor.all_requests:
aux = it.cycle(r.region.original_path)
for _ in range(idx - 1):
r.region.translate(next(aux))
sim.robot.sensor.requests = [
r for r in sim.robot.sensor.all_requests
if r in rrt_data[idx]['requests']
]
if 'LTS construction' in generate:
sim.config['video-interval'] = 500
sim.config['video-file'] = 'lts_construction.mp4'
sim.save_lts_process(lts_data, endframe_hold=20)
if 'LTS iterations' in generate:
lts_iterations = [
i + (i == -1) * len(lts_data) for i in lts_iterations
]
sim.save_lts_iterations(lts_data, lts_iterations)
if 'LTS statistics' in generate:
metrics = [('tree size', True), ('local planning runtime', False),
('local planning execution', False)]
rrt_data = rrt_data[1:]
# NOTE: This is for compatibility with the Cozmo logs
if 'local planning execution' not in rrt_data[0]:
for d in rrt_data:
duration = (d['plan stop time'] - d['plan start time']) * 1000
d['local planning execution'] = duration
data = [len(d['requests']) for d in rrt_data]
serviced = sum(n1 - n2 for n1, n2 in it.izip(data, data[1:])
if n1 > n2)
with open(os.path.join(outdir, 'local_performance_stats.txt'),
'w') as f:
print >> f, 'no trials:', len(rrt_data)
print >> f, 'no requests serviced', serviced
ops = [np.mean, np.min, np.max, np.std]
for key, positive in metrics:
if positive:
print >> f, key, [
op([
trial[key] for trial in rrt_data if trial[key] > 0
]) for op in ops
]
else:
print >> f, key, [
op([trial[key] for trial in rrt_data]) for op in ops
]
def define_problem(outputdir='.'):
'''Define case study setup (robot parameters, workspace, etc.).'''
# define robot diameter (used to compute the expanded workspace)
robotDiameter = 0.5
# define boundary
boundary = BoxBoundary2D([[0, 30], [0, 30]])
# define boundary style
boundary.style = {'color': 'black'}
# create expanded region
eboundary = BoxBoundary2D(boundary.ranges +
np.array([[1, -1], [1, -1]]) * robotDiameter / 2)
eboundary.style = {'color': 'black'}
# create robot's workspace and expanded workspace
wspace = Workspace(boundary=boundary)
ewspace = Workspace(boundary=eboundary)
# create robot object
robot = FullyActuatedRobot('Drone',
init=Point2D((2, 2)),
wspace=ewspace,
stepsize=5.999)
robot.diameter = robotDiameter
robot.localObst = 'local_obstacle'
logging.info('"Workspace": (%s, %s)', wspace, boundary.style)
logging.info('"Expanded workspace": (%s, %s)', ewspace, eboundary.style)
logging.info('"Robot name": "%s"', robot.name)
logging.info('"Robot initial configuration": %s', robot.initConf)
logging.info('"Robot step size": %f', robot.controlspace)
logging.info('"Robot diameter": %f', robot.diameter)
logging.info(
'"Robot constructor": "%s"',
'FullyActuatedRobot(robot_name, init=initConf, wspace=ewspace, '
'stepsize=stepsize)')
logging.info('"Local obstacle label": "%s"', robot.localObst)
# create simulation object
sim = Simulate2D(wspace, robot, ewspace)
sim.config['output-dir'] = outputdir
regions = []
#### add trucks ####
L = 8.0
W = 2.0
box = np.array([[-L / 2, L / 2], [-W / 2, W / 2]])
ntrucks = 4
for i in range(ntrucks):
# left array of trucks
lc = np.array([[7, 8.75 + i * 4.]]).T
regions.append((BoxRegion2D(lc + box, ['tl{}'.format(i)]), 'brown'))
# right array of trucks
rc = np.array([[23, 8.75 + i * 4.]]).T
regions.append((BoxRegion2D(rc + box, ['tr{}'.format(i)]), 'brown'))
#### add missiles ####
mc = np.array([[15.0, 15.0]]).T
mL = 2.5
mW = 16
box = np.array([[-mL / 2, mL / 2], [-mW / 2, mW / 2]])
regions.append((BoxRegion2D(mc + box, ['missile']), 'red'))
#### add charging stations ####
cs_size = 1.0
box = np.array([[-cs_size / 2, cs_size / 2], [-cs_size / 2, cs_size / 2]])
cs_center = np.array([[10, 2]]).T
regions.append((BoxRegion2D(cs_center + box, ['cs1']), 'blue'))
cs_center = np.array([[20, 2]]).T
regions.append((BoxRegion2D(cs_center + box, ['cs2']), 'blue'))
# add regions to workspace
for k, (r, c) in enumerate(regions):
# add styles to region
addStyle(r, style={'facecolor': c})
# add region to workspace
sim.workspace.addRegion(r)
# create expanded region
er = expandRegion(r, robot.diameter / 2)
# add style to the expanded region
addStyle(er, style={'facecolor': c})
# add expanded region to the expanded workspace
sim.expandedWorkspace.addRegion(er)
logging.info('("Global region", %d): (%s, %s)', k, r, r.style)
# # local requests
# F1 = (BallRegion2D([3.24, 1.98], 0.3, ['fire']),
# ([[3.24, 1.98], [2.54, 2.28], [3.5, 3], [4.02, 2.28]], 0.05),
# ('orange', 0.5))
# F2 = (BallRegion2D([1.26, 0.48], 0.18, ['fire']),
# ([[1.26, 0.48], [1.1, 1.1], [1.74, 0.92], [0.6, 0.6]], 0.05),
# ('orange', 0.5))
# S2 = (BallRegion2D([4.32, 1.48], 0.27, ['survivor']),
# ([[4.32, 1.48], [3.6, 1.2], [4, 2]], 0.05),
# ('yellow', 0.5))
# requests = [F1, F2, S2]
requests = []
# define local specification as a priority function
localSpec = {'survivor': 0, 'fire': 1}
logging.info('"Local specification": %s', localSpec)
# local obstacles #FIXME: defined in expanded workspace not workspace
obstacles = [
(BoxRegion2D([[6.5, 7.5], [8, 9]],
['LO']), ([[7.0, 8.5], [23.0, 8.5], [23.0, 20.5],
[7.0, 20.5]], 0.03), ('darkgray', 0.8)),
(PolygonRegion2D([[15.0 + np.cos(phi), 5.0 + np.sin(phi)]
for phi in np.arange(0, 2 * np.pi, np.pi / 3)],
['LO']), ([[15.0, 5.0],
[15.0, 25.0]], 0.03), ('gray', 0.8)),
(BallRegion2D([25, 22], 0.5,
['LO']), ([[25.0, 22.0], [25.0, 8.0], [15.0, 5.0],
[5.0, 8.0], [5.0, 22.0],
[15.0, 25.0]], 0.03), ('darkgray', 0.8))
]
# add style to local requests and obstacles
for k, (r, path, c) in enumerate(requests + obstacles):
# add styles to region
addStyle(r, style={'facecolor': to_rgba(*c)}) #FIXME: HACK
# create path
r_path = path
if path:
wps, step = path
wps = wps + [wps[0]]
r.path = []
for a, b in it.izip(wps[:-1], wps[1:]):
r.path += line_translate(a, b, step)
r_path = map(list, r.path)
r.path = it.cycle(r.path)
logging.info('("Local region", %d): (%s, %s, %s, %d)', k, r, r.style,
r_path, k < len(requests))
# create request objects
reqs = []
for r, _, _ in requests:
name = next(iter(r.symbols))
reqs.append(Request(r, name, localSpec[name]))
requests = reqs
obstacles = [o for o, _, _ in obstacles]
# set the robot's sensor
sensingShape = BallBoundary2D([0, 0], robot.diameter * 2.5)
robot.sensor = SimulatedSensor(robot, sensingShape, requests, obstacles)
logging.info(
'"Robot sensor constructor": "%s"',
'SimulatedSensor(robot, BallBoundary2D([0, 0], robot.diameter*2.5),'
'requests, obstacles)')
# display workspace
sim.display()
# display expanded workspace
sim.display(expanded=True)
# globalSpec = ('[] ( (<> r1) && (<> r2) && (<> r3) && (<> r4)'
# + ' && !(o1 || o2 || o3 || o4 ))')
# globalSpec = ('[] ( (<> tr0) && (<> tr1) && (<> tr2) && (<> tr3) && (<> tr4)'
# ' && (<> tl0) && (<> tl1) && (<> tl2) && (<> tl3) && (<> tl4)'
# ' && (<> missle) )')
# spec_tr = ' && '.join(['(<> tr{})'.format(i) for i in range(ntrucks)])
# spec_tl = ' && '.join(['(<> tl{})'.format(i) for i in range(ntrucks)])
# globalSpec = '[] ( {} && {} && {})'.format(spec_tr, spec_tl, '(<> missile)')
#globalSpec = '[] (<> (tl0 && <> (tl1 && <> (tl2 && <> ( tl3 && <> (tl4 && <> ( tr4 && <> ( tr4 && <> ( tr3 && <> (tr2 && <> ( tr1 && <> ( t0))))))))))))'
#### this is the LTL global spec ####
# globalSpec = '[] (<> (tl0 && <> (tl1 && <> (tl2 && <> ( tl3 && <> ( tr3 && <> (tr2 && <> ( tr1 && <> ( tr0)))))))))'
#### this is the TWTL global spec ####
globalSpec = '[H^2 tl0]^[0, 500] | [H^2 tl1]^[0, 500]'
logging.info('"Global specification": "%s"', globalSpec)
return robot, sim, globalSpec, localSpec
def define_problem(outputdir='.'):
'''Define case study setup (robot parameters, workspace, etc.).'''
# define robot diameter (used to compute the expanded workspace)
robotDiameter = 0.36
# define boundary
boundary = BoxBoundary2D([[0, 4.8], [0, 3.6]])
# define boundary style
boundary.style = {'color' : 'black'}
# create expanded region
eboundary = BoxBoundary2D(boundary.ranges +
np.array([[1, -1], [1, -1]]) * robotDiameter/2)
eboundary.style = {'color' : 'black'}
# create robot's workspace and expanded workspace
wspace = Workspace(boundary=boundary)
ewspace = Workspace(boundary=eboundary)
# create robot object
robot = FullyActuatedRobot('Cozmo', init=Point2D((2, 2)), wspace=ewspace,
stepsize=0.999)
robot.diameter = robotDiameter
robot.localObst = 'local_obstacle'
logging.info('"Workspace": (%s, %s)', wspace, boundary.style)
logging.info('"Expanded workspace": (%s, %s)', ewspace, eboundary.style)
logging.info('"Robot name": "%s"', robot.name)
logging.info('"Robot initial configuration": %s', robot.initConf)
logging.info('"Robot step size": %f', robot.controlspace)
logging.info('"Robot diameter": %f', robot.diameter)
logging.info('"Robot constructor": "%s"',
'FullyActuatedRobot(robot_name, init=initConf, wspace=ewspace, '
'stepsize=stepsize)')
logging.info('"Local obstacle label": "%s"', robot.localObst)
# create simulation object
sim = Simulate2D(wspace, robot, ewspace)
sim.config['output-dir'] = outputdir
# regions of interest
R1 = (BoxRegion2D([[0.45, 0.75], [0.45, 0.6]], ['r1']), 'brown')
R2 = (BallRegion2D([3.9, 1.2], 0.3, ['r2']), 'green')
R3 = (BoxRegion2D([[3.75, 4.05], [2.7, 3]], ['r3']), 'red')
R4 = (PolygonRegion2D([[1.2 , 2.85], [0.9 , 3.15], [0.6 , 2.85],
[0.9 , 2.55], [1.2 , 2.55]], ['r4']), 'magenta')
# global obstacles
O1 = (BallRegion2D([2.4, 1.5], 0.36, ['o1']), 'gray')
O2 = (PolygonRegion2D([[0.45, 1.2], [1.05, 1.5], [0.45, 1.8]], ['o2']),
'gray')
O3 = (PolygonRegion2D([[2.1, 2.7], [2.4, 3], [2.7, 2.7]], ['o3']), 'gray')
O4 = (BoxRegion2D([[1.65, 1.95], [0.45, 0.6]], ['o4']), 'gray')
# add all regions
regions = [R1, R2, R3, R4, O1, O2, O3, O4]
# add regions to workspace
for k, (r, c) in enumerate(regions):
# add styles to region
addStyle(r, style={'facecolor': c})
# add region to workspace
sim.workspace.addRegion(r)
# create expanded region
er = expandRegion(r, robot.diameter/2)
# add style to the expanded region
addStyle(er, style={'facecolor': c})
# add expanded region to the expanded workspace
sim.expandedWorkspace.addRegion(er)
logging.info('("Global region", %d): (%s, %s)', k, r, r.style)
# local requests
F1 = (BallRegion2D([3.24, 1.98], 0.3, ['fire']),
([[3.24, 1.98], [2.54, 2.28], [3.5, 3], [4.02, 2.28]], 0.05),
('orange', 0.5))
F2 = (BallRegion2D([1.26, 0.48], 0.18, ['fire']),
([[1.26, 0.48], [1.1, 1.1], [1.74, 0.92], [0.6, 0.6]], 0.05),
('orange', 0.5))
S2 = (BallRegion2D([4.32, 1.48], 0.27, ['survivor']),
([[4.32, 1.48], [3.6, 1.2], [4, 2]], 0.05),
('yellow', 0.5))
requests = [F1, F2, S2]
# define local specification as a priority function
localSpec = {'survivor': 0, 'fire': 1}
logging.info('"Local specification": %s', localSpec)
# local obstacles #FIXME: defined in expanded workspace not workspace
obstacles = [(BoxRegion2D([[3, 3.5], [2, 2.5]], ['LO']), None,
('gray', 0.8)),
(PolygonRegion2D([[3.2, 1.4], [3, 0.8], [3.4, 0.7]], ['LO']),
None, ('gray', 0.8)),
(BallRegion2D([1.6, 2.1], 0.15, ['LO']), None, ('gray', 0.8))]
# add style to local requests and obstacles
for k, (r, path, c) in enumerate(requests + obstacles):
# add styles to region
addStyle(r, style={'facecolor': to_rgba(*c)}) #FIXME: HACK
# create path
r_path = path
if path:
wps, step = path
wps = wps + [wps[0]]
r.path = []
for a, b in it.izip(wps[:-1], wps[1:]):
r.path += line_translate(a, b, step)
r_path = map(list, r.path)
r.path = it.cycle(r.path)
logging.info('("Local region", %d): (%s, %s, %s, %d)', k, r, r.style,
r_path, k < len(requests))
# create request objects
reqs = []
for r, _, _ in requests:
name = next(iter(r.symbols))
reqs.append(Request(r, name, localSpec[name]))
requests = reqs
obstacles = [o for o, _, _ in obstacles]
# set the robot's sensor
sensingShape = BallBoundary2D([0, 0], robot.diameter*2.5)
robot.sensor = SimulatedSensor(robot, sensingShape, requests, obstacles)
logging.info('"Robot sensor constructor": "%s"',
'SimulatedSensor(robot, BallBoundary2D([0, 0], robot.diameter*2.5),'
'requests, obstacles)')
# display workspace
sim.display()
# display expanded workspace
sim.display(expanded=True)
globalSpec = ('[] ( (<> r1) && (<> r2) && (<> r3) && (<> r4)'
+ ' && !(o1 || o2 || o3 || o4 ))')
logging.info('"Global specification": "%s"', globalSpec)
return robot, sim, globalSpec, localSpec
def caseStudy():
############################################################################
### Output and debug options ###############################################
############################################################################
outputdir = os.path.abspath('../data_ijrr/example2')
if not os.path.isdir(outputdir):
os.makedirs(outputdir)
# configure logging
fs = '%(asctime)s [%(levelname)s] -- { %(message)s }'
dfs = '%m/%d/%Y %I:%M:%S %p'
loglevel = logging.DEBUG
logfile = os.path.join(outputdir, 'ijrr_example_2.log')
verbose = True
logging.basicConfig(filename=logfile,
level=loglevel,
format=fs,
datefmt=dfs)
if verbose:
root = logging.getLogger()
ch = logging.StreamHandler(sys.stdout)
ch.setLevel(loglevel)
ch.setFormatter(logging.Formatter(fs, dfs))
root.addHandler(ch)
############################################################################
### Define case study setup (robot parameters, workspace, etc.) ############
############################################################################
# define robot diameter (used to compute the expanded workspace)
robotDiameter = 0.036
# define boundary
nrow, ncol = 5, 6
cell_size = 0.591
boundary = BoxBoundary2D([[0, ncol * cell_size], [0, nrow * cell_size]])
# define boundary style
boundary.style = {'color': 'black'}
# create expanded region
eboundary = BoxBoundary2D(boundary.ranges +
np.array([[1, -1], [1, -1]]) * robotDiameter / 2)
eboundary.style = {'color': 'black'}
# create robot's workspace and expanded workspace
wspace = Workspace(boundary=boundary)
ewspace = Workspace(boundary=eboundary)
# create robot object
robot = Cozmo('Cozmo',
init=Point2D((1.5 * cell_size, 2.5 * cell_size)),
wspace=ewspace,
stepsize=0.999)
robot.diameter = robotDiameter
robot.localObst = 'local_obstacle'
logging.info('"Workspace": (%s, %s)', wspace, boundary.style)
logging.info('"Expanded workspace": (%s, %s)', ewspace, eboundary.style)
logging.info('"Robot name": "%s"', robot.name)
logging.info('"Robot initial configuration": %s', robot.initConf)
logging.info('"Robot step size": %f', robot.controlspace)
logging.info('"Robot diameter": %f', robot.diameter)
logging.info(
'"Robot constructor": "%s"',
'Cozmo(robot_name, init=initConf, wspace=ewspace, stepsize=stepsize)')
logging.info('"Local obstacle label": "%s"', robot.localObst)
# create simulation object
sim = Simulate2D(wspace, robot, ewspace)
sim.config['output-dir'] = outputdir
# sim.config['background'] = os.path.abspath('../data_ijrr/imMap.png')
# regions of interest
R1 = (BoxRegion2D(np.array([[1., 2.], [4., 5.]]) * cell_size,
['r1']), 'brown')
R2 = (BoxRegion2D(np.array([[4., 5.], [1., 2.]]) * cell_size,
['r2']), 'green')
R3 = (BoxRegion2D(np.array([[0., 1.], [0., 1.]]) * cell_size,
['r3']), 'red')
R4 = (BoxRegion2D(np.array([[4., 6.], [4., 5.]]) * cell_size,
['r4']), 'blue')
# global obstacles
O1 = (BoxRegion2D(np.array([[1., 2.], [1., 2.]]) * cell_size,
['o1']), 'gray')
O2 = (BoxRegion2D(np.array([[4., 5.], [2., 3.]]) * cell_size,
['o2']), 'gray')
O3 = (PolygonRegion2D(
np.array([[1., 3.], [3., 3.], [3., 5.], [2., 5.], [2., 4.], [1., 4.]])
* cell_size, ['o3']), 'gray')
# add all regions
regions = [R1, R2, R3, R4, O1, O2, O3]
# add regions to workspace
for k, (r, c) in enumerate(regions):
# add styles to region
addStyle(r, style={'facecolor': c})
# add region to workspace
sim.workspace.addRegion(r)
# create expanded region
er = expandRegion(r, robot.diameter / 2)
# add style to the expanded region
addStyle(er, style={'facecolor': c})
# add expanded region to the expanded workspace
sim.expandedWorkspace.addRegion(er)
logging.info('("Global region", %d): (%s, %s)', k, r, r.style)
# local requests
F1 = (BallRegion2D([3.24, 1.98], 0.3, ['fire']), ('orange', 0.3))
F2 = (BallRegion2D([1.26, 0.48], 0.3, ['fire']), ('orange', 0.3))
S2 = (BallRegion2D([4.32, 1.48], 0.3, ['survivor']), ('yellow', 0.3))
requests = [F1, F2, S2]
# define local specification as a priority function
localSpec = {'survivor': 0, 'fire': 1}
logging.info('"Local specification": %s', localSpec)
localSpec_cube_color = {'survivor': 3, 'fire': 2}
# local obstacles
obstacles = []
# add style to local requests and obstacles
for k, (r, c) in enumerate(requests + obstacles):
# add styles to region
addStyle(r, style={'facecolor': to_rgba(*c)}) #FIMXE: HACK
r.cube_color = localSpec_cube_color[next(iter(r.symbols))]
logging.info('("Local region", %d): (%s, %s, %s, %d)', k, r, r.style,
None, k < len(requests))
# create request objects
reqs = []
for r, _ in requests:
name = next(iter(r.symbols))
reqs.append(Request(r, name, localSpec[name]))
requests = reqs
obstacles = [o for o, _, _ in obstacles]
# set the robot's sensor
sensingShape = BallBoundary2D([0, 0], 1.0)
robot.sensor = CozmoSensor(robot, sensingShape, requests, obstacles)
robot.sensor.reset()
logging.info(
'"Robot sensor constructor": "%s"',
'CozmoSensor(robot, BallBoundary2D([0, 0], 0.5), requests, obstacles)')
# display workspace
sim.display()
# display expanded workspace
sim.display(expanded=True)
############################################################################
### Generate global transition system and off-line control policy ##########
############################################################################
globalSpec = ('[] ( (<> r1) && (<> r2) && (<> r3) && (<> r4)'
' && !(o1 || o2 || o3))')
logging.info('"Global specification": "%s"', globalSpec)
# initialize incremental product automaton
checker = IncrementalProduct(
globalSpec) #, specFile='ijrr_globalSpec.txt')
logging.info('"Buchi size": (%d, %d)', checker.buchi.g.number_of_nodes(),
checker.buchi.g.number_of_edges())
# initialize global off-line RRG planner
sim.offline = RRGPlanner(robot, checker, iterations=1000)
sim.offline.eta = [0.5, 1.0] # good bounds for the planar case study
logging.info('"Start global planning": True')
with Timer(op_name='global planning', template='"%s runtime": %f'):
found = sim.offline.solve()
logging.info('"Found solution": %s', found)
if not found:
return
logging.info('"Iterations": %d', sim.offline.iteration)
logging.info('"Size of TS": %s', sim.offline.ts.size())
logging.info('"Size of PA": %s', sim.offline.checker.size())
# save global transition system and control policy
sim.offline.ts.save(os.path.join(outputdir, 'ts.yaml'))
############################################################################
### Display the global transition system and the off-line control policy ###
############################################################################
# display workspace and global transition system
prefix, suffix = sim.offline.checker.globalPolicy(sim.offline.ts)
sim.display(expanded=False, solution=prefix)
sim.display(expanded=False, solution=suffix)
sim.display(expanded=False, solution=prefix + suffix[1:])
logging.info('"global policy": (%s, %s)', prefix, suffix)
logging.info('"End global planning": True')
# move to start position
logging.debug('Moving to start configuration: %s', robot.initConf)
sim.robot.move([robot.initConf])
############################################################################
### Execute on-line path planning algorithm ################################
############################################################################
# compute potential for each state of PA
with Timer(op_name='Computing potential function',
template='"%s runtime": %f'):
if not compute_potentials(sim.offline.checker):
return
# set the step size for the local controller controlspace
robot.controlspace = 0.20
# initialize local on-line RRT planner
sim.online = LocalPlanner(sim.offline.checker,
sim.offline.ts,
robot,
localSpec,
eta=robot.controlspace)
sim.online.detailed_logging = True
# define number of surveillance cycles to run
cycles = 2
# execute controller
cycle = -1 # number of completed cycles, -1 accounts for the prefix
while cycle < cycles:
# update the locally sensed requests and obstacles
requests, obstacles = robot.sensor.sense()
# TODO: sense robot location
# TODO: update index in local plan
# TODO: add logging marker for start time for planning
logging.info('"plan start time": %f', time.time())
with Timer(op_name='local planning', template='"%s runtime": %f'):
# feed data to planner and get next control input
nextConf = sim.online.execute(requests, obstacles)
# TODO: add logging marker for stop time for planning
logging.info('"plan stop time": %f', time.time())
# sim.display(expanded=True, localinfo=('plan', 'trajectory'))
# enforce movement along plan
# FIXME: should pass only plan
robot.move([nextConf]) # + sim.online.local_plan)
# if completed cycle increment cycle
if sim.update():
cycle += 1
logging.info('"Local online planning finished": True')