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)