def test_collisions():
L = 10
world = Box(L, L)
# id_vehicle = 'd_SE2_rb_v-rf360'
id_vehicle = 'd_SE2_rb_v-random_5'
get_vehicles_config().load('default')
vehicles = get_conftools_vehicles()
# vehicles.load('default')
vehicle = vehicles.instance(id_vehicle) # @UndefinedVariable
vehicle.set_world_primitives(world.get_primitives())
vehicle.set_pose(SE3.identity())
commands = np.array([1, 0, 0])
# go straight
simulation_length = 10
dt = 0.1
time = 0
steps = int(np.ceil(simulation_length / dt))
for _ in range(steps):
vehicle.simulate(commands, dt)
pose = SE2_from_SE3(vehicle.get_pose())
translation = translation_from_SE2(pose)
# print('t=%.3f %s' % (time, SE2.friendly(pose)))
time += dt
assert translation[0] <= 9.5
assert translation[0] >= 9.4
assert translation[1] == 0
def test_collisions():
L = 10
world = Box(L, L)
# id_vehicle = 'd_SE2_rb_v-rf360'
id_vehicle = 'd_SE2_rb_v-random_5'
get_vehicles_config().load('default')
vehicles = get_conftools_vehicles()
# vehicles.load('default')
vehicle = vehicles.instance(id_vehicle) # @UndefinedVariable
vehicle.set_world_primitives(world.get_primitives())
vehicle.set_pose(SE3.identity())
commands = np.array([1, 0, 0])
# go straight
simulation_length = 10
dt = 0.1
time = 0
steps = int(np.ceil(simulation_length / dt))
for _ in range(steps):
vehicle.simulate(commands, dt)
pose = SE2_from_SE3(vehicle.get_pose())
translation = translation_from_SE2(pose)
# print('t=%.3f %s' % (time, SE2.friendly(pose)))
time += dt
assert translation[0] <= 9.5
assert translation[0] >= 9.4
assert translation[1] == 0
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 __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
# if an exception is detected. '''
#
# try:
# function(*arguments)
# except:
# msg = ('Error detected when running test (%s); displaying debug info.'
# % function.__name__)
# if dynamics is not None:
# msg += '\ndynamics: %s' % dynamics
# # TODO: write other info
# logger.error(msg)
# raise
for_all_dynamics = comptests_for_all(get_conftools_dynamics())
for_all_vehicles = comptests_for_all(get_conftools_vehicles())
for_all_worlds = comptests_for_all(get_conftools_worlds())
for_all_sensors = comptests_for_all(get_conftools_sensors())
for_all_skins = comptests_for_all(get_conftools_skins())
for_all_world_vehicle_pairs = comptests_for_all_pairs(get_conftools_worlds(),
get_conftools_vehicles())
#
# for_all_dynamics = fancy_test_decorator(lister=all_dynamics,
# arguments=lambda x: (x, get_dynamics(x)),
# attributes=lambda x: dict(dynamics=x))
#
# for_all_worlds = fancy_test_decorator(lister=all_worlds,
# arguments=lambda x: (x, get_world(x)),
# attributes=lambda x: dict(world=x))
#
# ''' Calls function with arguments, and writes debug information
# if an exception is detected. '''
#
# try:
# function(*arguments)
# except:
# msg = ('Error detected when running test (%s); displaying debug info.'
# % function.__name__)
# if dynamics is not None:
# msg += '\ndynamics: %s' % dynamics
# # TODO: write other info
# logger.error(msg)
# raise
for_all_dynamics = comptests_for_all(get_conftools_dynamics())
for_all_vehicles = comptests_for_all(get_conftools_vehicles())
for_all_worlds = comptests_for_all(get_conftools_worlds())
for_all_sensors = comptests_for_all(get_conftools_sensors())
for_all_skins = comptests_for_all(get_conftools_skins())
for_all_world_vehicle_pairs = comptests_for_all_pairs(get_conftools_worlds(),
get_conftools_vehicles())
#
# for_all_dynamics = fancy_test_decorator(lister=all_dynamics,
# arguments=lambda x: (x, get_dynamics(x)),
# attributes=lambda x: dict(dynamics=x))
#
# for_all_worlds = fancy_test_decorator(lister=all_worlds,
# arguments=lambda x: (x, get_world(x)),
# attributes=lambda x: dict(world=x))
#
