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 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 __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 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 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 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)
