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