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 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 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.02
# define boundary
boundary = BoxBoundary2D([[0, 1], [0, 1]])
# 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 = Robot('vector',
init=Point2D((0.3, 0.3)),
wspace=ewspace,
stepsize=0.168)
robot.diameter = robotDiameter
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)
# create simulation object
sim = Simulate2D(wspace, robot, ewspace)
sim.config['output-dir'] = outputdir
# regions of interest
R1 = (BoxRegion2D([[0, 0.2], [0, 0.2]], ['r1']), 'green')
R2 = (BoxRegion2D([[0.8, 1], [0, 0.2]], ['r2']), 'green')
R3 = (BoxRegion2D([[0.8, 1], [0.8, 1]], ['r3']), 'green')
R4 = (BoxRegion2D([[0, 0.2], [0.8, 1]], ['r4']), 'green')
# global obstacles
O1 = (BoxRegion2D([[0.4, 0.6], [0, 0.1]], ['o1']), 'red')
O3 = (BoxRegion2D([[0, 0.1], [0.4, 0.6]], ['o1']), 'gray')
O2 = (BoxRegion2D([[0.9, 1], [0.4, 0.6]], ['o2']), 'gray')
O4 = (BoxRegion2D([[0.4, 0.6], [0.4, 0.6]], ['o4']), 'red')
# 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)
# display workspace
sim.display()
# display expanded workspace
sim.display(expanded=True)
ltlSpec = ('[] ( (<> r1) && (<> r2) && (<> r3) && (<> r4) && '
'!(o1 || o2 || o3 || o4))')
logging.info('"Global specification": "%s"', ltlSpec)
return robot, sim, ltlSpec
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')