def nonbatch(task, method, N, M):
simulation_object = create_env(task)
d = simulation_object.num_of_features
w_true = 2*np.random.rand(d)-1
w_true = w_true / np.linalg.norm(w_true)
print('If in automated mode: true w = {}'.format(w_true/np.linalg.norm(w_true)))
lower_input_bound = [x[0] for x in simulation_object.feed_bounds]
upper_input_bound = [x[1] for x in simulation_object.feed_bounds]
w_sampler = Sampler(d)
psi_set = []
s_set = []
for i in range(N):
w_sampler.A = psi_set
w_sampler.y = np.array(s_set).reshape(-1,1)
w_samples = w_sampler.sample(M)
mean_w_samples = np.mean(w_samples,axis=0)
print('Samples so far: ' + str(i))
print('w estimate = {}'.format(mean_w_samples/np.linalg.norm(mean_w_samples)))
print('Alignment = {}'.format(mean_w_samples.dot(w_true)/np.linalg.norm(mean_w_samples)))
input_A, input_B = run_algo(method, simulation_object, w_samples)
psi, s = get_feedback(simulation_object, input_A, input_B, w_true)
psi_set.append(psi)
s_set.append(s)
w_sampler.A = psi_set
w_sampler.y = np.array(s_set).reshape(-1,1)
w_samples = w_sampler.sample(M)
print('Samples so far: ' + str(N))
print('w estimate = {}'.format(mean_w_samples/np.linalg.norm(mean_w_samples)))
print('Alignment = {}'.format(mean_w_samples.dot(w_true)/np.linalg.norm(mean_w_samples)))
def nonbatch(task, method, N, M):
simulation_object = create_env(task)
d = simulation_object.num_of_features
lower_input_bound = [x[0] for x in simulation_object.feed_bounds]
upper_input_bound = [x[1] for x in simulation_object.feed_bounds]
w_sampler = Sampler(d)
psi_set = []
s_set = []
input_A = np.random.uniform(low=2 * lower_input_bound,
high=2 * upper_input_bound,
size=(2 * simulation_object.feed_size))
input_B = np.random.uniform(low=2 * lower_input_bound,
high=2 * upper_input_bound,
size=(2 * simulation_object.feed_size))
psi, s = get_feedback(simulation_object, input_A, input_B)
psi_set.append(psi)
s_set.append(s)
for i in range(1, N):
w_sampler.A = psi_set
w_sampler.y = np.array(s_set).reshape(-1, 1)
w_samples = w_sampler.sample(M)
mean_w_samples = np.mean(w_samples, axis=0)
print('w-estimate = {}'.format(mean_w_samples /
np.linalg.norm(mean_w_samples)))
input_A, input_B = run_algo(method, simulation_object, w_samples)
psi, s = get_feedback(simulation_object, input_A, input_B)
psi_set.append(psi)
s_set.append(s)
w_sampler.A = psi_set
w_sampler.y = np.array(s_set).reshape(-1, 1)
w_samples = w_sampler.sample(M)
print('w-estimate = {}'.format(mean_w_samples /
np.linalg.norm(mean_w_samples)))
def batch(task, method, N, M, b):
if N % b != 0:
print('N must be divisible to b')
exit(0)
B = 20 * b
simulation_object = create_env(task)
d = simulation_object.num_of_features
lower_input_bound = [x[0] for x in simulation_object.feed_bounds]
upper_input_bound = [x[1] for x in simulation_object.feed_bounds]
w_sampler = Sampler(d)
psi_set = []
s_set = []
inputA_set = np.random.uniform(low=2 * lower_input_bound,
high=2 * upper_input_bound,
size=(b, 2 * simulation_object.feed_size))
inputB_set = np.random.uniform(low=2 * lower_input_bound,
high=2 * upper_input_bound,
size=(b, 2 * simulation_object.feed_size))
for j in range(b):
input_A = inputA_set[j]
input_B = inputB_set[j]
psi, s = get_feedback(simulation_object, input_A, input_B)
psi_set.append(psi)
s_set.append(s)
i = b
while i < N:
w_sampler.A = psi_set
w_sampler.y = np.array(s_set).reshape(-1, 1)
w_samples = w_sampler.sample(M)
mean_w_samples = np.mean(w_samples, axis=0)
print('w-estimate = {}'.format(mean_w_samples /
np.linalg.norm(mean_w_samples)))
print('Samples so far: ' + str(i))
inputA_set, inputB_set = run_algo(method, simulation_object, w_samples,
b, B)
for j in range(b):
input_A = inputA_set[j]
input_B = inputB_set[j]
psi, s = get_feedback(simulation_object, input_B, input_A)
psi_set.append(psi)
s_set.append(s)
i += b
w_sampler.A = psi_set
w_sampler.y = np.array(s_set).reshape(-1, 1)
w_samples = w_sampler.sample(M)
mean_w_samples = np.mean(w_samples, axis=0)
print('w-estimate = {}'.format(mean_w_samples /
np.linalg.norm(mean_w_samples)))
def plan_path(self):
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
data = np.genfromtxt('colliders.csv', delimiter=',', dtype='str', max_rows=1)
_, lat0 = data[0].strip().split(' ')
_, lon0 = data[1].strip().split(' ')
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(float(lon0), float(lat0), 0)
# TODO: retrieve current global position
curr_glb_pos = [self._longitude, self._latitude, self._altitude]
# TODO: convert to current local position using global_to_local()
self._north, self._east, self._down = global_to_local(curr_glb_pos, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
sampler = Sampler(data, SAFETY_DISTANCE)
self.polygons = sampler._polygons
nodes = sampler.sample(3)
print('nodes_len: ', len(nodes))
xx = [[p[0], p[1], TARGET_ALTITUDE, 0] for p in nodes]
data = msgpack.dumps(xx)
print(data)
self.connection._master.write(data)
def create_graph_probabilistic(data,
drone_altitude,
safety_distance,
k=10,
sample_size=300):
sampler = Sampler(data, drone_altitude, safety_distance)
polygons = sampler._polygons
print("Polygons", len(polygons))
nodes = sampler.sample(sample_size)
print("Data", len(data))
print("Probabilistic node length", len(nodes))
g = nx.Graph()
tree = KDTree(nodes)
for n1 in nodes:
# for each node connect try to connect to k nearest nodes
idxs = tree.query([n1], k, return_distance=False)[0]
for idx in idxs:
n2 = nodes[idx]
if n2 == n1:
continue
if can_connect(n1, n2, polygons):
g.add_edge(n1, n2, weight=1)
return g
def create_graph_with_nodes(data, lat0, lon0, alt):
print('[start]create_grid_with_nodes')
sampler = Sampler(data)
global polygons
polygons = sampler._polygons
nodes = sampler.sample(300)
graph = create_graph(nodes, 10)
print("Number of edges", len(graph.edges))
grid = create_grid(data, sampler._zmax, 1)
nmin = np.min(data[:, 0])
emin = np.min(data[:, 1])
start = (lat0 + nmin, lon0 + emin, alt)
start_closest = closest_point(graph, start)
k = np.random.randint(len(graph.nodes))
print(k, len(graph.nodes))
goal = list(graph.nodes)[k]
print('start {0}'.format(start_closest))
print('goal {0}'.format(goal))
print('[end]create_grid_with_nodes')
return grid, graph, start_closest, goal
def get_path(start=(0, 0, 6), goal=(331, 116, 19)):
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
north_offset = -316
east_offset = -445
sampler = Sampler(data)
print("AAA")
polygons = sampler._polygons
print("BBB")
nodes = sampler.sample(3000)
# (339, 380) (441, 519)
# start = (0, 0, 6)
# goal = (331, 116, 19)
nodes += [start, goal]
print(nodes)
g = create_graph(nodes, k=10, polygons=polygons)
path, _ = a_star_graph(g, heuristic, start, goal)
waypoints = [[int(p[0]), int(p[1]), int(p[2]), 0] for p in path]
path_dict = {'path': waypoints}
print(waypoints)
with open('path.json', 'w') as json_file:
json.dump(path_dict, json_file)
with open('path.json', 'r') as json_file2:
dta = json.load(json_file2)
print(dta['path'])
return waypoints
def nonbatch(task, method, N, M, checkpoints=None):
if checkpoints is None:
checkpoints = []
checkpointed_weights = []
simulation_object = create_env(task)
d = simulation_object.num_of_features
lower_input_bound = [x[0] for x in simulation_object.feed_bounds]
upper_input_bound = [x[1] for x in simulation_object.feed_bounds]
w_sampler = Sampler(d)
psi_set = []
s_set = []
input_A = np.random.uniform(low=2 * lower_input_bound,
high=2 * upper_input_bound,
size=(2 * simulation_object.feed_size))
input_B = np.random.uniform(low=2 * lower_input_bound,
high=2 * upper_input_bound,
size=(2 * simulation_object.feed_size))
psi, s = get_feedback_auto(
simulation_object, input_A,
input_B) # psi is the difference, s is the 1 or -1 signal
psi_set.append(psi)
s_set.append(s)
for i in range(1, N):
w_sampler.A = psi_set
w_sampler.y = np.array(s_set).reshape(-1, 1)
w_samples = w_sampler.sample(M)
mean_w_samples = np.mean(w_samples, axis=0)
print('w-estimate = {}'.format(mean_w_samples /
np.linalg.norm(mean_w_samples)))
if i in checkpoints:
checkpointed_weights.append(mean_w_samples /
np.linalg.norm(mean_w_samples))
print("Weights saved at iteration {}".format(i))
input_A, input_B = run_algo(method, simulation_object, w_samples)
psi, s = get_feedback_auto(simulation_object, input_A, input_B)
psi_set.append(psi)
s_set.append(s)
w_sampler.A = psi_set
w_sampler.y = np.array(s_set).reshape(-1, 1)
w_samples = w_sampler.sample(M)
checkpointed_weights.append(mean_w_samples /
np.linalg.norm(mean_w_samples))
print('w-estimate = {}'.format(mean_w_samples /
np.linalg.norm(mean_w_samples)))
return checkpointed_weights
def batch(task, method, N, M, b):
if N % b != 0:
print('N must be divisible to b')
exit(0)
B = 20*b
simulation_object = create_env(task)
d = simulation_object.num_of_features
w_true = 2*np.random.rand(d)-1
w_true = w_true / np.linalg.norm(w_true)
print('If in automated mode: true w = {}'.format(w_true/np.linalg.norm(w_true)))
lower_input_bound = [x[0] for x in simulation_object.feed_bounds]
upper_input_bound = [x[1] for x in simulation_object.feed_bounds]
w_sampler = Sampler(d)
psi_set = []
s_set = []
i = 0
while i < N:
w_sampler.A = psi_set
w_sampler.y = np.array(s_set).reshape(-1,1)
w_samples = w_sampler.sample(M)
mean_w_samples = np.mean(w_samples,axis=0)
print('Samples so far: ' + str(i))
print('w estimate = {}'.format(mean_w_samples/np.linalg.norm(mean_w_samples)))
print('Alignment = {}'.format(mean_w_samples.dot(w_true)/np.linalg.norm(mean_w_samples)))
inputA_set, inputB_set = run_algo(method, simulation_object, w_samples, b, B)
for j in range(b):
input_A = inputA_set[j]
input_B = inputB_set[j]
psi, s = get_feedback(simulation_object, input_B, input_A, w_true)
psi_set.append(psi)
s_set.append(s)
i += b
w_sampler.A = psi_set
w_sampler.y = np.array(s_set).reshape(-1,1)
w_samples = w_sampler.sample(M)
mean_w_samples = np.mean(w_samples, axis=0)
print('Samples so far: ' + str(N))
print('w estimate = {}'.format(mean_w_samples/np.linalg.norm(mean_w_samples)))
print('Alignment = {}'.format(mean_w_samples.dot(w_true)/np.linalg.norm(mean_w_samples)))
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
data = np.genfromtxt('colliders.csv',
delimiter=',',
dtype='str',
max_rows=1)
_, lat0 = data[0].strip().split(' ')
_, lon0 = data[1].strip().split(' ')
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(float(lon0), float(lat0), 0)
# TODO: retrieve current global position
curr_glb_pos = [self._longitude, self._latitude, self._altitude]
# TODO: convert to current local position using global_to_local()
self._north, self._east, self._down = global_to_local(
curr_glb_pos, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
sampler = Sampler(data, SAFETY_DISTANCE)
self.polygons = sampler._polygons
nodes = sampler.sample(300)
print('nodes_len: ', len(nodes))
g = self.create_graph(nodes, 10)
print('graph_edgs: ', len(g.edges))
start = self.local_position
goal = global_to_local([-122.396428, 37.795128, TARGET_ALTITUDE],
self.global_home)
start = self.find_closest_node(g.nodes, start)
goal = self.find_closest_node(g.nodes, goal)
path, cost = a_star_for_graph(g, heuristic, start, goal)
print('a_star_path: ', path)
path = self.prune_path(path)
print('prune_path: ', path)
if len(path) > 0:
# Convert path to waypoints
waypoints = [[p[0], p[1], TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def sample_nodes(data, grid_start, grid_goal, debug):
start_time = time.time()
sampler = Sampler(data)
# Extract all the polygons
polygons = sampler._polygons
nodes = sampler.sample(grid_start, grid_goal, NUM_SAMPLES)
stop_time = time.time()
if debug:
print("Time taken to build Sampler is: {0:5.2f}s".format(stop_time -
start_time))
return nodes, polygons
def read_ply(self, file_name):
num_samples = self.num_samples // len(self.files_list)
if self.file_index == len(self.files_list) - 1:
num_samples = num_samples + (self.num_samples - (num_samples * len(self.files_list)))
root, ext = os.path.splitext(file_name)
if not os.path.isfile(root + ".npy"):
ply = PlyData.read(file_name)
vertex = ply['vertex']
(x, y, z) = (vertex[t] for t in ('x', 'y', 'z'))
points = zip(x.ravel(), y.ravel(), z.ravel())
np.save(root + ".npy", points)
else:
points = np.load(root + ".npy")
#load normals
if os.path.isfile(root + "_normals" + ".ply"):
if not os.path.isfile(root + "_normals" + ".npy"):
ply1 = PlyData.read(root + "_normals" + ".ply")
vertex = ply1['vertex']
(nx, ny, nz) = (vertex[t] for t in ('nx', 'ny', 'nz'))
self.normals = np.asarray(zip(nx.ravel(), ny.ravel(), nz.ravel()))
np.save(root + "_normals" + ".npy", self.normals)
else:
self.normals = np.load(root + "_normals" + ".npy")
if self.add_noise:
self.data = utils.add_noise_normal(points, std=self.nois_std)
else:
self.data = np.asarray(points)
self.pc_diameter = utils.get_pc_diameter(self.data)
self.l = self.relL*self.pc_diameter
rot = utils.angle_axis_to_rotation(self.rotation_angle, self.rotation_axis)
self.data = utils.transform_pc(self.data, rot)
#plotutils.show_pc(self.data)
#mlab.show()
#TODO: better sampling
print "sampling file: ", file_name
self.samples, self.sample_indices = Sampler.sample(self.data, -1, min_num_point=-1, file_name=file_name, sampling_algorithm=self.sampling_algorithm)
#self.samples, self.sample_indices = Sampler.sample(self.data, -1, num_samples, file_name=file_name, sampling_algorithm=self.sampling_algorithm)
#self.samples = self.samples[0:num_samples]
#self.sample_indices = self.sample_indices[0:num_samples]
self.tree = spatial.KDTree(self.data)
return self.data
def read_ply(self, file_name, num_samples=1000, sample_class_start=0, add_noise =False,
noise_prob=0.3, noise_factor=0.02, noise_std=0.1, sampling_algorithm=SampleAlgorithm.Uniform,
rotation_axis=[0, 0, 1], rotation_angle=0):
root, ext = os.path.splitext(file_name)
if not os.path.isfile(root + ".npy"):
ply = PlyData.read(file_name)
vertex = ply['vertex']
(x, y, z) = (vertex[t] for t in ('x', 'y', 'z'))
points = zip(x.ravel(), y.ravel(), z.ravel())
np.save(root + ".npy", points)
else:
points = np.load(root + ".npy")
#load normals
if os.path.isfile(root + "_normals" + ".ply"):
if not os.path.isfile(root + "_normals" + ".npy"):
ply1 = PlyData.read(root + "_normals" + ".ply")
vertex = ply1['vertex']
(nx, ny, nz) = (vertex[t] for t in ('nx', 'ny', 'nz'))
self.normals = np.asarray(zip(nx.ravel(), ny.ravel(), nz.ravel()))
np.save(root + "_normals" + ".npy", self.normals)
else:
self.normals = np.load(root + "_normals" + ".npy")
if add_noise:
print "adding noise to model.."
mr = utils.model_resolution(np.array(points))
#mr = 0.404
print "model resolution: ", mr
self.data = utils.add_noise_normal(np.array(points), mr, noise_std)
else:
self.data = np.asarray(points)
rot = utils.angle_axis_to_rotation(rotation_angle, rotation_axis)
self.data = utils.transform_pc(self.data, rot)
#plotutils.show_pc(self.data)
#mlab.show()
#TODO: better sampling
self.samples, self.sample_indices = Sampler.sample(self.data, -1, num_samples-1, file_name=file_name, pose=rot, sampling_algorithm=sampling_algorithm)
self.tree = spatial.KDTree(self.data)
self.sample_class_start = sample_class_start
self.sample_class_current = sample_class_start
self.num_samples = self.samples.shape[0]
print "num samples: ", self.num_samples
logging.basicConfig(filename='example.log',level=logging.DEBUG)
return self.data
# ## Step 2 - Sample Points # # # You may want to limit the z-axis values. # In[70]: from sampling import Sampler # TODO: sample points randomly # then use KDTree to find nearest neighbor polygon # and test for collision num_samp = 500 sampler = Sampler(data) polygons = sampler._polygons nodes = sampler.sample(num_samp) print(len(nodes)) # ## Step 3 - Connect Nodes # # Now we have to connect the nodes. There are many ways they might be done, it's completely up to you. The only restriction being no edge connecting two nodes may pass through an obstacle. # # NOTE: You can use `LineString()` from the `shapely` library to create a line. Additionally, `shapely` geometry objects have a method `.crosses` which return `True` if the geometries cross paths, for instance your `LineString()` with an obstacle `Polygon()`! # In[71]: # TODO: connect nodes # Suggested method # 1) cast nodes into a graph called "g" using networkx
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 7
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
with open('colliders.csv', newline='') as f:
reader = csv.reader(f)
row1 = next(reader) # gets the first line
lat0, lon0 = float(row1[0][5:]), float(row1[1][5:])
# TODO: set home position to (lat0, lon0, 0)
self.set_home_position(
lon0, lat0, 0) # set the current location to be the home position
# TODO: retrieve current global position
current_global_pos = (self._longitude, self._latitude, self._altitude)
# TODO: convert to current local position using global_to_local()
current_local_pos = global_to_local(current_global_pos,
self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Get the center of the grid
north_offset = int(np.floor(np.min(data[:, 0] - data[:, 3])))
east_offset = int(np.floor(np.min(data[:, 1] - data[:, 4])))
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
sampler = Sampler(data)
polygons = sampler._polygons
# Example: sampling 1200 points and removing
# ones conflicting with obstacles.
nodes = sampler.sample(1200)
print("Non collider nodes:", len(nodes))
t0 = time.time()
# Uncomment line 162 to generate the graph from the sampled points, and comment out line 163.
# Increase the Mavlink timer to avoid disconnecting from the simulator.
#G = create_graph(nodes, 10, polygons)
G = nx.read_gpickle(
"graph_1200_SD_nodes.gpickle"
) # Comment out this line if generating the graph instead of using the saved graph
print('graph took {0} seconds to build'.format(time.time() - t0))
print("Number of edges", len(G.edges))
# TODO: convert start position to current position rather than map center
grid_start = (int(current_local_pos[0] - north_offset),
int(current_local_pos[1] - east_offset), TARGET_ALTITUDE)
# TODO: adapt to set goal as latitude / longitude position and convert
goal_global_pos = (-122.394700, 37.789825, 13)
goal_local_pos = global_to_local(goal_global_pos, self.global_home)
grid_goal = (int(goal_local_pos[0] - north_offset),
int(goal_local_pos[1] - east_offset), goal_global_pos[2])
print("goal_local_N:", goal_local_pos[0], "goal_local_E:",
goal_local_pos[1], "goal_local_alt:", goal_local_pos[2])
#goal_ne = (455., 635., 20.)
start_ne_g = closest_point(G, grid_start)
goal_ne_g = closest_point(G, grid_goal)
print('Local Start and Goal: ', start_ne_g, goal_ne_g)
# Run A* to find a path from start to goal
path, cost = a_star_graph(G, heuristic_graph, start_ne_g, goal_ne_g)
print("Path Length:", len(path), "Path Cost:", cost)
int_path = [[int(p[0]), int(p[1]), int(p[2])] for p in path]
# TODO: prune path to minimize number of waypoints
pruned_path = prune_path(int_path)
print("Length Pruned Path:", len(pruned_path))
print("PRUNED PATH:", pruned_path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, p[2], 0]
for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
filename = 'colliders.csv'
data = np.loadtxt(filename, delimiter=';,', dtype='str')[0].split(", ")
lat0, lon0 = [float(d.split(" ")[1]) for d in data]
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
print("self.local_position ", self.local_position)
# TODO: retrieve current global position
current_glbl_pos = [self._longitude, self._latitude,self._altitude]
print(current_glbl_pos)
print(self.global_position)
# TODO: convert to current local position using global_to_local()
current_lcl_pos = global_to_local(current_glbl_pos, self.global_home)
print("current_lcl_pos ", len(current_lcl_pos))
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
_, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
# grid_start = (-north_offset, -east_offset)
# TODO: convert start position to current position rather than map center
grid_start = (int(current_lcl_pos[0]), int(current_lcl_pos[1]))
# Set goal as some arbitrary position on the grid
# grid_goal = (-north_offset + 10, -east_offset + 10)
# Example: sampling 300 points and removing
# ones conflicting with obstacles.
# print("Start of sampling...")
sampler = Sampler(data)
polygons = sampler.polygons
nodes = sampler.sample(300)
nodes.insert(0, (grid_start[0], grid_start[1], 5.0))
# Build a graph using the sampled nodes
# and connect each nodes to its 10th closest nodes
print("Start graph building...")
t0 = time.time()
graph = create_graph(polygons, nodes, 11)
print('graph took {0} seconds to build'.format(time.time() - t0))
start = list(graph.nodes)[0]
len_path = 0
while len_path == 0:
k = np.random.randint(len(graph.nodes))
goal = list(graph.nodes)[k]
path, _ = a_star(graph, heuristic, start, goal)
len_path = len(path)
pruned_path = prune_path(path, polygons)
waypoints = [[int(p[0]), int(p[1]), int(p[2]), 0] for p in pruned_path]
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
#Step 1 - Load Data
# This is the same obstacle data from the previous lesson.
filename = '../data/colliders.csv'
data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=2)
print(data)
#Step 2 - Sample Points in free space
from sampling import Sampler
sampler = Sampler(data)
polygons = sampler._polygons
# Example: sampling 100 points and removing
# ones conflicting with obstacles.
nodes = sampler.sample(300)
print(len(nodes))
#Step 3 - Connect Nodes
import numpy.linalg as LA
from sklearn.neighbors import KDTree
def can_connect(n1, n2):
l = LineString([n1, n2])
for p in polygons:
if p.crosses(l) and p.height >= min(n1[2], n2[2]):
return False
return True
def create_prm(data):
sampler = Sampler(data)
polygons = sampler._polygons
nodes = sampler.sample(300)
g = create_graph(nodes, 10,polygons)
return nodes,g
int(np.round(goal_local[1])) - east_offset, 0.0)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Sampling nodes...')
t0 = time.time()
print(data.shape)
sampler = Sampler(data)
polygons = sampler._polygons
print(len(polygons))
print('Sampler took {0} seconds to build'.format(time.time() - t0))
# 1000 is veeery slow
t0 = time.time()
nodes = sampler.sample(200)
print(nodes)
nodes.append((grid_start[0], grid_start[1], 5.0))
nodes.append(grid_goal)
print('Sampling took {0} seconds to build'.format(time.time() - t0))
print('Number of nodes: ', len(nodes))
print('Creating graph...')
t0 = time.time()
# k = 15 - https://arxiv.org/pdf/1105.1186.pdf
graph = create_graph(nodes, 15, sampler.polygons)
print('Graph took {0} seconds to build'.format(time.time() - t0))
print("Number of edges", len(graph.edges))
"""
fig = plt.figure()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# Read lat0, lon0 from colliders.csv into floating point values
# I just use `open()` method and `f.readline()` to read the first line,
# which line it read depend on the times it calls, I just call it once so it's the first line,
# then slice out the number
with open('colliders.csv', 'r') as f:
first_line_data = f.readline().rstrip().split(', ')
lat0 = float(first_line_data[0][5:])
lon0 = float(first_line_data[1][5:])
# Set home position
self.set_home_position(lon0, lat0, 0)
# Retrieve current global position
gp = self.global_position # (lon, lat, up)
# Convert to current local position
lp = global_to_local(gp, self.global_home) # (north, east, down)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
t0 = time.time()
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
print("Took {0} seconds to load csv file".format(time.time() - t0))
# Convert start position to current position
start = (lp[0], lp[1], TARGET_ALTITUDE)
t0 = time.time()
sampler = Sampler(data, self.global_home)
print(
"Took {0} seconds to initialize Sampler class".format(time.time() -
t0))
t0 = time.time()
nodes = sampler.sample(30)
# Set goal as some arbitrary position on the map
goal = sampler.sample_goal(nodes, 37.793, -122.4)
print('Local Start and Goal: ', start, goal)
nodes.append(start)
nodes.append(goal)
print("Took {0} seconds to create {1} sample nodes.".format(
time.time() - t0, len(nodes)))
t0 = time.time()
g = Graph(sampler.polygons, sampler.polygon_center_tree,
sampler.max_poly_xy)
g.create_graph(nodes, 10)
print("Took {0} seconds to build graph".format(time.time() - t0))
print("Number of edges:", len(g.edges))
t0 = time.time()
# Convert grid search to graph
path, _ = a_star_graph(g.graph, heuristic, start, goal)
print("Took {0} seconds to search path".format(time.time() - t0))
# Prune path to minimize number of waypoints
t0 = time.time()
path = prune_path(path)
print("Took {0} seconds to prune path".format(time.time() - t0))
# Convert path to waypoints
waypoints = [[int(p[0]), int(p[1]), TARGET_ALTITUDE, 0] for p in path]
self.waypoints = waypoints
# Send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
#Example
from sampling import Sampler
#elements is an array of pairs, the first member contains the element
#and the second the probability of that element being picked up
elements = [(i, 1 / 20) for i in range(10)] + [(10, 1 / 2)]
s = Sampler(elements)
#Testing correctness of my procedure, with the above elements we expect 10 to appear 50% of the time and
#the others 5% of the time each.
count = [0 for i in range(len(elements))]
for i in range(10000):
count[s.sample()] += 1
print(count)
def main_sample():
pc = utils.read_ply('E:/Fac/BMC Master/Thesis/Models/bunny/reconstruction/plytest/bun_zipper.ply')
sample_points = Sampler.sample(pc, -1, 1000, sampling_algorithm=SampleAlgorithm.Uniform)
mlab.points3d(sample_points[:, 0], sample_points[:, 1], sample_points[:, 2], color=(1, 1, 1), mode='point')
mlab.show()
print 'done'
def read_ply(self, file_name):
num_samples = self.num_samples // len(self.files_list)
if self.file_index == len(self.files_list) - 1:
num_samples = num_samples + (self.num_samples - (num_samples * len(self.files_list)))
root, ext = os.path.splitext(file_name)
if not os.path.isfile(root + ".npy"):
ply = PlyData.read(file_name)
vertex = ply['vertex']
(x, y, z) = (vertex[t] for t in ('x', 'y', 'z'))
points = zip(x.ravel(), y.ravel(), z.ravel())
np.save(root + ".npy", points)
else:
points = np.load(root + ".npy")
if self.add_noise:
self.data = utils.add_noise(points, prob=self.noise_prob, factor=self.noise_factor)
else:
self.data = np.asarray(points)
#if self.data.shape[0] > 2e5:
# self.data, _ = Sampler.sample(self.data, -1, 2e5, sampling_algorithm=self.sampling_algorithm)
pc_diameter = utils.get_pc_diameter(self.data)
self.l = self.relL*pc_diameter
rot = utils.angle_axis_to_rotation(self.rotation_angle, self.rotation_axis)
self.data = utils.transform_pc(self.data, rot)
#plotutils.show_pc(self.data)
#mlab.show()
#TODO: better sampling
print "sampling file: ", file_name
self.samples, self.sample_indices = Sampler.sample(self.data, -1, num_samples, file_name=file_name, sampling_algorithm=self.sampling_algorithm)
self.samples = self.samples[0:num_samples]
self.sample_indices = self.sample_indices[0:num_samples]
self.tree = spatial.KDTree(self.data)
#TODO:Intergrate with num_samples for consistency
if self.filter_bad_samples:
temp_file_samples = 'temp/' + os.path.basename(file_name) + '_' + str(num_samples) + '_filter' + str(self.filter_threshold) + '.npy'
print 'samples file: ', temp_file_samples
if os.path.isfile(temp_file_samples):
self.sample_indices = np.load(temp_file_samples)
self.samples = self.data[self.sample_indices]
else:
self.samples, self.sample_indices = Sampler.sample(self.data, -1, num_samples*2, sampling_algorithm=self.sampling_algorithm)
self.samples = self.samples[0:num_samples*2]
self.sample_indices = self.sample_indices[0:num_samples*2]
sample_indices_temp = []
for idx in self.sample_indices:
if self.is_good_sample(self.data[idx], self.filter_threshold):
sample_indices_temp.append(idx)
if len(sample_indices_temp) >= num_samples:
break
assert (len(sample_indices_temp) >= num_samples)
self.sample_indices = np.asarray(sample_indices_temp[0:num_samples])
self.samples = self.data[self.sample_indices]
np.save(temp_file_samples, self.sample_indices)
#plotutils.show_pc(self.samples)
#mlab.show()
logging.basicConfig(filename='example.log',level=logging.DEBUG)
return self.data
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 10
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
lat0, lon0 = read_home_location()
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
current_global_position = [
self._longitude, self._latitude, self._altitude
]
print("\nself.global_position: {0}".format(self.global_position))
print("current_global_position: {0}".format(current_global_position))
# TODO: convert to current local position using global_to_local()
current_local_position = global_to_local(current_global_position,
self.global_home)
print("current_local_position: {0}".format(current_local_position))
print('\nglobal home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
'''
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
#grid_start = (-north_offset, -east_offset) #hardcoded to middle of the grid, replacing it with below:
# TODO: convert start position to current position rather than map center
grid_start = ( int(np.ceil(current_local_position[0] - north_offset)), int(np.ceil(current_local_position[1] - east_offset)) )
print("grid_start: {0}".format(grid_start))
# Set goal as some arbitrary position on the grid
#grid_goal = (-north_offset + 10, -east_offset + 10) #hardcoded as some location 10 m north and 10 m east of map center as dfault, replacing it with below:
# TODO: adapt to set goal as latitude / longitude position and convert
#goal_global_position = new_global_position(self.global_home, 15, 25) #adding 20 x 10 meters to the global_home position.
goal_global_position = [-122.401055, 37.795461, 0]
goal_local_position = global_to_local(goal_global_position, self.global_home)
grid_goal = (int(goal_local_position[0] - north_offset), int(goal_local_position[1] - east_offset))
print("goal_local_position: {0}".format(goal_local_position))
print("grid_goal: {0}".format(grid_goal))
print("grid.shape: {0}".format(grid.shape))
if grid_goal[0] > grid.shape[0] or grid_goal[1] > grid.shape[1] or grid_goal[0] < 0 or grid_goal[1] < 0:
print("\n * NOTE: Destination is outside the grid map. setting it back to start ")
grid_goal = grid_start
self.landing_transition()
#if grid[grid_goal[0]][grid_goal[1]] > 0: # checking that the destination is not colliding with any obstacle
# print("\n * NOTE: Destination is inside an obstacle. Setting it to start position. ")
# grid_goal = grid_start
# self.landing_transition()
# REZA: Adding Skeleton median axis
skeleton = medial_axis(invert(grid))
skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_goal)
print('\nLocal Skelton Start and Goal: ', grid_start, grid_goal)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Grid Start and Goal: ', grid_start, grid_goal)
# Choose one of the following A*
#path, _ = a_star(grid, heuristic, grid_start, grid_goal) #Run A* on the grid
path, _ = a_star(invert(skeleton).astype(np.int), heuristic, tuple(skel_start), tuple(skel_goal))
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
print ("path: ", path)
path = prune_path(path)
print ("revised path: ", path)
'''
# Create the smaple points
print("\nStarting the Sampling ...")
sampler = Sampler(data)
polygons = sampler._polygons
nodes = sampler.sample(100)
print("Created probabilistic node samples: " + str(len(nodes)))
print("\nCurrent nx version: {0}, it must be 2.1".format(
nx.__version__)) # should be 2.1
print("\nStarting the path planning ...")
t0 = time.time()
graph = create_graph(nodes, polygons, 10)
print('graph took {0} seconds to build'.format(time.time() - t0))
print("Number of edges", str(graph.number_of_edges()))
#Setting my Goal position
goal_global_position = [-122.401055, 37.795461, 0]
goal_local_position = global_to_local(goal_global_position,
self.global_home)
graph_nodes_list = np.array(graph.nodes)
nearest_node_index = find_nearest_waypoint(current_local_position,
graph_nodes_list)
start_node = graph_nodes_list[graph_nodes_list]
nearest_node_index = find_nearest_waypoint(goal_local_position,
graph_nodes_list)
goal_node = graph_nodes_list[nearest_node_index]
print("Start Node: " + str(start_node))
print("End Node: " + str(goal_node))
self.waypoints, _ = a_star_waypoints(graph, heuristic, start_node,
goal_node)
#Adding my actual start and end positions to the begining and the end of the waypoints_path
self.waypoints.insert(
0, goal_local_position) #adding the start world position
###waypoints_path.appent()
# Set self.waypoints
#self.waypoints = waypoints_path
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
def can_connect(n1, n2, polygons):
l = LineString([n1, n2])
for p in polygons:
if p.crosses(l) and p.height >= min(n1[2], n2[2]):
return False
return True
def create_graph(nodes, k, polygons):
g = nx.Graph()
tree = KDTree(nodes)
for n1 in nodes:
# for each node connect try to connect to k nearest nodes
idxs = tree.query([n1], k, return_distance=False)[0]
for idx in idxs:
n2 = nodes[idx]
if n2 == n1:
continue
if can_connect(n1, n2, polygons):
g.add_edge(n1, n2, weight=1)
return g
# TODO: read lat0, lon0 from colliders into floating point values
data_pos = np.loadtxt('colliders.csv', dtype='str', max_rows=1)
(lat0, lon0) = [float(data_pos[1][:-1]), float(data_pos[3][:-1])]
# TODO: set home position to (lon0, lat0, 0)
(east_home, north_home, _, _) = utm.from_latlon(lat0, lon0)
# TODO: retrieve current global position
(east, north, _, _) = utm.from_latlon(self.global_position[1],
self.global_position[0])
# TODO: convert to current local position using global_to_local()
local_position = np.array([
north - north_home, east - east_home,
-(self.global_position[2] - self.global_home[2])
])
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Sample points
sampler = Sampler(data)
polygons = sampler._polygons
nodes = sampler.sample(150)
print(len(nodes))
# Connect nodes to graph
g = create_graph(nodes, 10, polygons)
#grid = create_grid(data, sampler._zmax, 1)
# define start as first point from graph, goal point is randomized
start = list(g.nodes)[0]
k = np.random.randint(len(g.nodes))
print("Amount of found nodes:{0} {1}".format(k, len(g.nodes)))
goal = list(g.nodes)[k]
# Run A* to find path
path, cost = a_star(g, heuristic, start, goal)
path_pairs = zip(path[:-1], path[1:])
for (n1, n2) in path_pairs:
print(n1, n2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
'''
# Define starting point on the grid (this is just grid center)
grid_start = (int(local_position[0]-north_offset), int(local_position[1]-east_offset))
# TODO: convert start position to current position rather than map center
# Set goal as some arbitrary position on the grid
grid_goal = (int(local_position[0]-north_offset), int(local_position[1]-east_offset))
# TODO: adapt to set goal as latitude / longitude position and convert
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path_, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
path = collinearity(path_)
# TODO (if you're feeling ambitious): Try a different approach altogether!
'''
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
print(waypoints)
print(grid_start, grid_goal)
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
import matplotlib.pyplot as plt
from shapely.geometry import Polygon, Point, LineString
from queue import PriorityQueue
filename = 'colliders.csv'
data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=2)
from sampling import Sampler
import random
random.seed(0)
sampler = Sampler(data)
polygons = sampler._polygons
NUM_SAMPLES = 1000
nodes = sampler.sample(NUM_SAMPLES)
print(len(nodes))
import numpy.linalg as LA
from sklearn.neighbors import KDTree
def can_connect(n1, n2):
l = LineString([n1, n2])
for p in polygons:
c = p._coords
if p.crosses(l) and p.height >= min(n1[2], n2[2]):
return False
return True
import networkx as nx
plt.rcParams['figure.figsize'] = 6, 6
filename = 'colliders.csv'
data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=3)
print(data)
from sampling import Sampler
sampler = Sampler(data)
polygons = sampler._polygons
# Example: sampling 100 points and removing
# ones conflicting with obstacles.
nodes = sampler.sample(100)
print(len(nodes))
import numpy.linalg as LA
from sklearn.neighbors import KDTree
def can_connect(n1, n2):
l = LineString([n1, n2])
for p in polygons:
if p.crosses(l) and p.height >= min(n1[2], n2[2]):
return False
return True
def create_graph(nodes, k):
# This is the same obstacle data from the previous lesson.
filename = 'colliders.csv'
data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=2)
print(data)
# In[35]:
# flight_altitude = 3
safety_distance = 3
sampler = Sampler(data)
polygons = sampler._polygons
# Example: sampling 100 points and removing
# ones conflicting with obstacles.
nodes = sampler.sample(500)
print(len(nodes))
# In[36]:
import numpy.linalg as LA
from sklearn.neighbors import KDTree
def can_connect(n1, n2):
l = LineString([n1, n2])
for p in polygons:
if p.crosses(l) and p.height >= min(n1[2], n2[2]):
return False
return True
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
home = pd.read_csv("colliders.csv",
header=None,
nrows=1,
sep=' |,',
engine='python')
lat0 = home[0][0]
lon0 = home[3][0]
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
current_global_pos = [self._longitude, self._latitude, self._altitude]
# TODO: convert to current local position using global_to_local()
current_local_pos = global_to_local(current_global_pos,
self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
#grid_start = (-north_offset, -east_offset)
# TODO: convert start position to current position rather than map center
grid_start = (int(current_local_pos[0]) - north_offset,
int(current_local_pos[1]) - east_offset)
# Set goal as some arbitrary position on the grid
#grid_goal = (-north_offset + 10, -east_offset + 10)
# TODO: adapt to set goal as latitude / longitude position and convert
lat_min = 37.789658
lat_max = 37.797905
lon_max = -122.391999
lon_min = -122.402527
grid_goal = goal_location(lat_min, lat_max, lon_min, lon_max)
while grid[grid_goal[0], grid_goal[1]] == 1:
grid_goal = goal_location(lat_min, lat_max, lon_min, lon_max)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
# Probabilistic roap map algorithm#
sampler = Sampler(data)
polygons = sampler._polygons
nodes = sampler.sample(300)
nodes.append(grid_start)
nodes.append(grid_goal)
print('Local Start and Goal: ', grid_start, grid_goal)
#Select one a_star algorithm depending on the type of the search algorithm
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
path, _ = a_star_graph(grid, heuristic_graph, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
optimal_path = prune_path(path)
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in optimal_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
