def generate_pose(self):
pose = get_safe_pose(
raytracer=self.raytracer,
world_radius=9,
safe_zone=1, num_tries=100)
#print "Found pose %s" % pose
return pose
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)
