def __init__(self, world=None, id_world=None, vehicle=None, id_vehicle=None, dt=VehiclesConstants.DEFAULT_SIMULATION_DT, **kwargs): self.dt = dt if not ((world is not None) ^ (id_world is not None)): raise ValueError('Specify exactly one of "world" and "id_world".') if not ((vehicle is not None) ^ (id_vehicle is not None)): raise ValueError('Specify exactly one of "vehicle" ' 'and "id_vehicle".') vehicles = get_conftools_vehicles() worlds = get_conftools_worlds() # TODO: user shortcuts if vehicle is not None: id_vehicle = vehicle['id'] vehicle_spec = vehicle # TODO: check well formed vehicle = vehicles.instance_spec(vehicle) else: vehicle_spec = vehicles[id_vehicle] vehicle = vehicles.instance(id_vehicle) if world is not None: id_world = world['id'] # TODO: check well formed world_spec = world world = worlds.instance_spec(world) else: world_spec = worlds[id_world] world = worlds.instance(id_world) VehicleSimulation.__init__(self, vehicle, world, **kwargs) cmd_spec = StreamSpec.from_yaml( self.vehicle.dynamics.get_commands_spec()) self.last_commands = cmd_spec.get_default_value() if len(self.vehicle.sensors) == 0: raise Exception('Vehicle %r has no sensors defined.' % id_vehicle) obs_spec = create_obs_spec(self.vehicle) self._boot_spec = BootSpec(obs_spec=obs_spec, cmd_spec=cmd_spec, id_robot=id_vehicle) # XXX: id, desc, extra? self.commands_source = BootOlympicsConstants.CMD_SOURCE_REST self.boot_episode_started = False # Save for later self.id_world = id_world self.id_vehicle = id_vehicle self.world_spec = world_spec self.vehicle_spec = vehicle_spec
def check_simulation_one_step_conf(id_world, world, id_vehicle, vehicle): sim = VehicleSimulation(vehicle, world) dt = 1 try: sim.new_episode() cmds = random_commands(sim.vehicle.dynamics.get_commands_spec()) sim.simulate(cmds, dt) sim.compute_observations() except: logger.error('Error for vehicle=%s world=%s' % (id_vehicle, id_world)) raise
def get_observations(self): if not self.boot_episode_started: raise Exception('get_observations() called before new_episode().') observations = VehicleSimulation.compute_observations(self) episode_end = True if self.vehicle_collided else False if episode_end: self.info("Ending boot episode due to collision.") self.boot_episode_started = False return RobotObservations(timestamp=self.timestamp, observations=observations, commands=self.last_commands, commands_source=self.commands_source, robot_pose=self.vehicle.get_pose(), episode_end=episode_end)
def check_blind_robot(id_dynamics, dynamics): vehicle = Vehicle() vehicle.add_dynamics(id_dynamics, dynamics) world = Empty([[-10, 10], [-10, 10], [-10, 10]]) sim = VehicleSimulation(vehicle, world) check_simulation(sim, num_episodes=3, num_instants=10, dt=0.1)
def set_commands(self, commands, commands_source): if not self.boot_episode_started: raise Exception('set_commands() called before new_episode().') self.commands_source = commands_source VehicleSimulation.simulate(self, commands, self.dt)
def new_episode(self): self.boot_episode_started = True e = VehicleSimulation.new_episode(self) self.info("New episode started.") return EpisodeDesc(e.id_episode, self.id_world)
def fps_main(): if True: import contracts contracts.disable_all() # import contracts # contracts.disable_all() usage = """ # vehicles = ['d_SE2_rb_v-rf180', 'd_SE2_rb_v-cam180'] # vehicles = ['d_SE2_rb_v-rf180'] # vehicles += ['d_SE2_rb_v-cam180'] # vehicles += ['d_SE2_rb_v-fs_05_12x12'] id_world = 'box10' id_world = 'StocSources_w10_n20_s1' d_SE2_rb_v-cam_f360_n180_s """ parser = OptionParser(usage=usage) parser.add_option("-w", "--world", default='StocSources_w10_n20_s1', help="World") parser.add_option("-v", "--vehicle", default=['d_SE2_rb_v-fs_05_12x12'], action='append', help="Vehicles to simulate") (options, _) = parser.parse_args() get_vehicles_config().load() id_world = options.world world = VehiclesConfig.worlds.instance(id_world) # @UndefinedVariable stats = [] Stat = namedtuple('Stat', 'id_vehicle id_world fps') def stat2str(s): return "v: %-25s w: %-25s %5dfps" % (s.id_vehicle, s.id_world, s.fps) # vehicles = list(VehiclesConfig.vehicles.keys()) # print vehicles vehicles = options.vehicle T = 200 # T = 100000 dt = 0.05 for id_vehicle in vehicles: instance = VehiclesConfig.vehicles.instance # @UndefinedVariable vehicle = instance(id_vehicle) print('vehicle: %s' % id_vehicle) sim = VehicleSimulation(vehicle, world) fps = check_simulation(sim, num_instants=T, dt=dt) stats.append(Stat(id_vehicle=id_vehicle, id_world=id_world, fps=fps)) print(stat2str(stats[-1])) print('---- Sorted:') stats.sort(key=lambda x: (-x.fps)) for s in stats: print(stat2str(s))
def plan_analysis(global_options, data, args): # @UnusedVariable from vehicles import VehicleSimulation, VehiclesConfig # XXX np.random.seed(12345226) from matplotlib import rc rc('font', **{'family': 'serif', 'serif': ['Times', 'Times New Roman', 'Palatino'], 'size': 9.0}) contracts.disable_all() usage = "" parser = OptionParser(usage=usage) parser.disable_interspersed_args() parser.add_option("--world", dest='id_world', default="stochastic_box_10", help="Vehicles world to use") (options, args) = parser.parse_args(args) if args: raise Exception('Extra args') id_robot = data['id_robot'] # id_agent = data['id_agent'] pub = data['publisher'] vehicle = VehiclesConfig.vehicles.instance(id_robot) # @UndefinedVariable world = VehiclesConfig.worlds.instance(options.id_world) # @UndefinedVariable sim = VehicleSimulation(vehicle, world) FORWARD = [1, 1] BACKWARD = [-1, -1] LEFT = [-1, +1] RIGHT = [+1, -1] FORWARD = np.array([0, +1]) BACKWARD = np.array([0, -1]) LEFT = np.array([+0.3, 0]) RIGHT = np.array([-0.3, 0]) FWD_L = np.array([1, +1]) FWD_R = np.array([1, -1]) BWD_R = np.array([-1, -1]) BWD_L = np.array([+1, -1]) T1 = 1 T2 = 2 T1 = 1 dt = 0.02 def commutator(cmd1, cmd2, T): return [(cmd1, T), (cmd2, T), (-cmd1, T), (-cmd2, T)] examples = { 'forwd1': {'trajectory': [(FORWARD, T1)]}, # # 'forwd2': {'trajectory': [(FORWARD, T2)]}, # 'left1': {'trajectory': [(LEFT, T1)]}, # 'right1': {'trajectory': [(RIGHT, T1)]}, # 'back1': {'trajectory': [(BACKWARD, T1)]}, # 'turnl2': {'trajectory': [(LEFT, T2)]}, # 'l3': {'trajectory': [([0.3, 0], T1)]}, # 'l4': {'trajectory': [([0.4, 0], T1)]}, # 'l5': {'trajectory': [([0.5, 0], T1)]}, # 'l6': {'trajectory': [([0.6, 0], T1)]}, # 'l7': {'trajectory': [([0.7, 0], T1)]}, # 'l8': {'trajectory': [([0.8, 0], T1)]}, # 'l9': {'trajectory': [([0.9, 0], T1)]}, # 'l10': {'trajectory': [([1.0, 0], T1)]}, # 'l1': {'trajectory': [([1, 0], T1)]}, # # 'sidel1': {'trajectory': [(LEFT, T1), # (FORWARD, T1), # (RIGHT, T1 * 2), # (BACKWARD, T1), # (LEFT, T1) ]}, # 'sidel2': {'trajectory': commutator(FWD_L, LEFT, T1) } # 'sidel2': {'trajectory': commutator(FORWARD, LEFT, T1) }, # 'sidel3': {'trajectory': commutator(LEFT, FORWARD, T1) } # } # examples = {} for name, scenario in examples.items(): while True: try: scenario_compute_inputs(scenario, sim, dt=dt) break except ValueError as e: print(e) actions = data['actions'] for name, scenario in examples.items(): scenario_solve(scenario, actions) for name, scenario in examples.items(): S = pub.section(name) scenario_display(scenario, S, sim)
def main(): from vehicles_cairo import vehicles_has_cairo if not vehicles_has_cairo: logger.error('This program cannot be run if Cairo is not installed.') return from vehicles_cairo import (vehicles_cairo_display_pdf, vehicles_cairo_display_png, vehicles_cairo_display_svg, cairo_plot_circle2) parser = OptionParser(usage=usage) parser.disable_interspersed_args() parser.add_option("--vehicles", default='*', help="ID vehicle [%default].") parser.add_option("--world", default='SBox2_10a', # default='empty_fixed', help="ID world [%default].") parser.add_option("--outdir", "-o", default='vehicles_demo_display_vehicles', help="output directory [%default]") parser.add_option("--figsize", default=10, type='float', help="figsize (inches) [%default]") parser.add_option("-g", "--grid", default=1, type='float', help="grid size in meters; 0 for no grid [%default]") parser.add_option("-d", dest="config", default=".", help="Config directory") parser.add_option("--scale", default=False, action='store_true', help="If given, displays the scale with a red circle") (options, args) = parser.parse_args() if args: raise Exception() # XXX id_world = options.world logger.info(' id_world: %s' % id_world) from reprep import Report, MIME_PDF basename = 'vehicles_demo' r = Report(basename) logger.info('Loading configuration from %s' % options.config) VehiclesConfig.load(options.config) # TODO: selection all_vehicles = VehiclesConfig.vehicles.keys() if options.vehicles is None: vehicles = all_vehicles else: vehicles = expand_string(options.vehicles, all_vehicles) print('Plotting vehicles: %s' % vehicles) f0 = r.figure(cols=6) f0_data = r.figure(cols=6) for id_vehicle in sorted(vehicles): sec = r.node(id_vehicle) f = sec.figure(cols=6) world = VehiclesConfig.specs['worlds'].instance(id_world) vehicle = VehiclesConfig.specs['vehicles'].instance(id_vehicle) simulation = VehicleSimulation(vehicle, world) simulation.new_episode() simulation.compute_observations() sim_state = simulation.to_yaml() def draw_scale(cr): if options.scale: cairo_plot_circle2(cr, 0, 0, vehicle.radius, fill_color=(1, .7, .7)) plot_params = dict(grid=options.grid, zoom=1.5, zoom_scale_radius=True, width=500, height=500, show_sensor_data=True, show_sensor_data_compact=True, extra_draw_world=draw_scale, bgcolor=None, show_world=False) with f.data_file('png_with', MIME_PNG) as filename: vehicles_cairo_display_png(filename, sim_state=sim_state, **plot_params) f0_data.sub(f.last(), caption=id_vehicle) plot_params['show_sensor_data'] = False with f.data_file('png_without', MIME_PNG) as filename: vehicles_cairo_display_png(filename, sim_state=sim_state, **plot_params) f0.sub(f.last(), caption=id_vehicle) with f.data_file('svg', MIME_SVG) as filename: vehicles_cairo_display_svg(filename, sim_state=sim_state, **plot_params) plot_params['grid'] = 0 plot_params['extra_draw_world'] = None with sec.data_file('pdf', MIME_PDF) as filename: vehicles_cairo_display_pdf(filename, sim_state=sim_state, **plot_params) plot_params['show_robot_body'] = True plot_params['show_robot_sensors'] = False with f.data_file('png_only_body', MIME_PNG) as filename: vehicles_cairo_display_png(filename, sim_state=sim_state, **plot_params) filename = os.path.join(options.outdir, 'index.html') logger.info('Writing to %r.' % filename) r.to_html(filename)
def run_simulation(task, vehicle, agent, log, dt, maxT): world = task.get_world() simulation = VehicleSimulation(world=world, vehicle=vehicle) directions = simulation.vehicle.sensors[0].sensor.directions vehicle_spec = VehicleSpec(directions=directions) agent.init(vehicle_spec) simulation.new_episode() tmp_log = log + '.active' ldir = os.path.dirname(log) #last = os.path.join(ldir, 'last.yaml') logger.info('Writing on log %r.' % log) #logger.info(' (also accessible as %r)' % last) if not os.path.exists(ldir): os.makedirs(ldir) #if os.path.exists(last): # os.unlink(last) logfile = open(tmp_log, 'w') #assert not os.path.exists(last) assert os.path.exists(tmp_log) #logger.info('Link %s, %s' % (tmp_log, last)) #os.symlink(tmp_log, last) logger.info('Simulation dt=%.3f max T: %.3f' % (dt, maxT)) while True: simulation.compute_observations() # TODO: perhaps this needs to be gerealized luminance = simulation.vehicle.sensors[0].current_observations['luminance'] observations = VehicleObservations(time=simulation.timestamp, dt=dt, luminance=luminance) agent.process_observations(observations) commands = agent.choose_commands() # TODO: check format if logfile is not None: y = simulation.to_yaml() y['commands'] = commands.tolist() logfile.write('---\n') yaml.dump(y, logfile, Dumper=Dumper) logger.info('t=%.3f pose: %s' % (simulation.timestamp, SE2.friendly(SE2_from_SE3(simulation.vehicle.get_pose())))) if task.end_condition(simulation): break if simulation.timestamp > maxT: # TODO: add why we finished break simulation.simulate(commands, dt) logfile.close() if os.path.exists(log): os.unlink(log) assert not os.path.exists(log) os.rename(tmp_log, log) assert not os.path.exists(tmp_log) assert os.path.exists(log)