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.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')