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_command_fields(vehicle, T, reference_pose, vehicle_poses):
"""
vehicle_poses: list of lists of Poses
returns: list of list of command arrays
"""
reference_data = vehicle.compute_observations(reference_pose)
goal = reference_data.sensels
assert_reasonable_value(goal)
results = []
for row in vehicle_poses:
results_row = []
for pose_diff in row:
pose = reference_pose.oplus(pose_diff)
data = vehicle.compute_observations(pose)
y = data.sensels
assert_reasonable_value(y)
commands = dot(dot(T, y), (goal - y))
assert(len(commands) == vehicle.config.num_commands)
results_row.append(commands)
results.append(results_row)
return array(results)
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
