def random_pose_simulation(
world_gen, vehicle,
random_pose_gen, num_iterations,
processing_class, previous_result=None):
"""
world
vehicle
num_iterations
random_pose_gen: XXX lambda -> RigidBodyState
processing_class
"""
# Use random seed
numpy.random.seed()
if previous_result is not None:
state = previous_result
# if we are called again, it means we need more iteration
if state.current_iteration == state.total_iterations:
state.total_iterations += num_iterations
else:
state = OpenStruct()
state.current_iteration = 0
state.total_iterations = num_iterations
state.result = processing_class(vehicle.config)
state.vehicle = vehicle
save_every = 50
generate_world_every = 50
world = None
while state.current_iteration < state.total_iterations:
# sample a random world
if world is None or \
state.current_iteration % generate_world_every == 0:
world = world_gen()
# give the map to the pose generator
random_pose_gen.set_map(world)
# sets the map for all sensors
vehicle.set_map(world)
state1 = random_pose_gen.generate_pose()
data = vehicle.compute_observations(state1)
assert_reasonable_value(data.sensels)
state.result.process_data(data)
if state.current_iteration % save_every == 0:
yield state, state.current_iteration, state.total_iterations
state.current_iteration += 1
yield state, state.current_iteration, state.total_iterations
def compute_observations(self, vehicle_state=None):
""" Computes the sensor observations at a certain state.
OK, you might think this function is kind of mysterious
because of all the introspection we use. However, a little
black magic here helps in making the client interface
clear and intuitive.
"""
if vehicle_state is None:
vehicle_state = self.state
data = OpenStruct()
data.sensels = []
data.all_sensors = []
for sensor_type in self.config.sensor_types.keys():
setattr(data, sensor_type, [])
for i, sensor in enumerate(self.config.sensors):
mountpoint = self.sensor2mountpoint[sensor]
sensor_pose = vehicle_state.oplus(mountpoint)
sensor_data = sensor.compute_observations(sensor_pose)
if not 'sensels' in sensor_data:
raise BVException('Sensor %s did not produce a "sensels" variable' %
sensor)
for k, v in sensor_data.items():
if isinstance(v, list):
sensor_data[k] = array(v)
sensor_data = OpenStruct(**sensor_data)
data_array = getattr(data, sensor.sensor_type_string())
data_array.append(sensor_data)
data.all_sensors.append(sensor_data)
assert_reasonable_value(sensor_data.sensels)
assert_1d_ndarray(sensor_data.sensels)
data.sensels.extend(list(sensor_data.sensels))
data.sensels = array(data.sensels)
return data
def compute_fields(firstorder_result, world_gen, spacing_xy=1, spacing_theta=90,
resolution=15, previous_result=None):
vehicle = firstorder_result.vehicle
T = firstorder_result.result.T
if previous_result is None:
# Create the lattice for sampling
lattice_x = linspace(-spacing_xy, spacing_xy, resolution)
lattice_y = linspace(-spacing_xy, spacing_xy, resolution)
lattice_theta = linspace(-deg2rad(spacing_theta),
deg2rad(spacing_theta),
resolution)
def make_grid(lattice_row, lattice_col, func):
rows = []
for y in lattice_row:
row = []
for x in lattice_col:
row.append(func(x, y))
rows.append(row)
return rows
result = OpenStruct()
result.lattice_x_y = make_grid(lattice_y, lattice_x,
lambda y, x: RigidBodyState(position=[x, y]))
result.lattice_x_theta = make_grid(lattice_theta, lattice_x,
lambda theta, x: RigidBodyState(position=[x, 0], attitude=theta))
result.lattice_theta_y = make_grid(lattice_y, lattice_theta,
lambda y, theta: RigidBodyState(position=[0, y], attitude=theta))
result.fields_x_y = []
result.fields_x_theta = []
result.fields_theta_y = []
result.worlds = []
result.ref_poses = []
else:
result = previous_result
# This is the number of completed iterations
number_completed = min([ len(x) \
for x in [result.fields_x_y, result.fields_x_theta, result.fields_theta_y]])
print "So far completed %s" % number_completed
# sample world if we don't have one
if len(result.worlds) <= number_completed:
result.worlds.append(world_gen())
world = result.worlds[-1]
# sample pose if we don't have one
if len(result.ref_poses) <= number_completed:
# Initalize raytracer for pose queries
raytracer = TexturedRaytracer()
raytracer.set_map(world)
result.ref_poses.append(
get_safe_pose(raytracer, world_radius=7, # TODO: bounding box
safe_zone=1, num_tries=1000))
del raytracer
ref_pose = result.ref_poses[-1]
vehicle.set_map(world)
num = number_completed * 3
total = (number_completed + 1) * 3
yield (result, num, total)
if len(result.fields_x_y) <= number_completed:
result.fields_x_y.append(
compute_command_fields(vehicle, T, ref_pose, result.lattice_x_y))
num += 1
yield (result, num, total)
if len(result.fields_x_theta) <= number_completed:
result.fields_x_theta.append(
compute_command_fields(vehicle, T, ref_pose, result.lattice_x_theta))
num += 1
yield (result, num, total)
if len(result.fields_theta_y) <= number_completed:
result.fields_theta_y.append(
compute_command_fields(vehicle, T, ref_pose, result.lattice_theta_y))
num += 1
yield (result, num, total)
def random_motion_simulation(
world_gen, vehicle,
random_pose_gen, num_iterations, random_commands_gen, dt,
processing_class, previous_result=None):
"""
world
vehicle
random_pose_gen: lambda iteration -> RigidBodyState
random_commands_gen: lambda iteration, vehicle -> castable to float list
processing_class
"""
# Use random seed
numpy.random.seed()
if previous_result is not None:
state = previous_result
# if we are called again, it means we need more iteration
if state.current_iteration == state.total_iterations:
state.total_iterations += num_iterations
print "Increasing to %d iterations" % state.total_iterations
else:
state = OpenStruct()
state.current_iteration = 0
state.total_iterations = num_iterations
state.result = processing_class(vehicle.config)
state.vehicle = vehicle
save_every = 50
generate_world_every = 5
world = None
while state.current_iteration < state.total_iterations:
# sample a random world
if world is None or \
state.current_iteration % generate_world_every == 0:
world = world_gen()
# give the map to the pose generator
random_pose_gen.set_map(world)
# sets the map for all sensors
vehicle.set_map(world)
state1 = random_pose_gen.generate_pose()
if state1 is None:
raise BVException('Could not generate a random pose.')
# generate random commands
commands = random_commands_gen(state.current_iteration, vehicle)
# get next state
state2 = vehicle.dynamics.evolve_state(state1, commands, dt)
data = vehicle.compute_observations_and_derivatives(state1, state2, dt)
data.commands = array(commands)
assert_reasonable_value(data.sensels)
assert_reasonable_value(data.sensels_dot)
#
# for i, v in enumerate(data.sensels):
# if v < 0.5:
# print "Found sensel %d: range %f surf %s valid %s" % \
# (i, v, data.rangefinder[0].surface[i], \
# data.rangefinder[0].valid[i], \
# ) # data.surface[i])
# #print data.rangefinder[0].__dict__.keys()
# print world
# break
#
state.result.process_data(data)
# housekeeping
if state.current_iteration % save_every == 0:
yield state, state.current_iteration, state.total_iterations
state.current_iteration += 1
yield state, state.current_iteration, state.total_iterations
