def get_sample(self): if random.randint(0, 100) > self.end_sample_percent: sample = Node([random.uniform(self.space[0], self.space[1]), random.uniform(self.space[0], self.space[1]), random.uniform(-math.pi, math.pi)]) else: # end point sampling sample = Node(self.end.pose) return sample
def steer_2(self, source_node, dest_node):
# take source_node and steer towards destination node
dtheta = np.array([-self.max_curvature, 0, self.max_curvature])
dx = np.cos(source_node.pose[2] + dtheta/2)
dy = np.sin(source_node.pose[2] + dtheta/2)
vec = np.array([dx, dy, dtheta])
new_node_list = [Node(source_node.pose + self.growth * vec[:, 0]), Node(source_node.pose + self.growth * vec[:, 1]), Node(source_node.pose + self.growth * vec[:, 2])]
d_list = [dist(new_node, dest_node) for new_node in new_node_list]
mindist = min(d_list)
new_node = new_node_list[d_list.index(mindist)]
new_node.cost = float("inf")
new_node.parent = None
return new_node
def get_sample(self):
sample = Node([
random.uniform(self.space[0], self.space[1]),
random.uniform(self.space[2], self.space[3]),
random.uniform(-math.pi, math.pi)
])
return sample
def steer(self, source_node, dest_node): # take source_node and steer towards destination node dtheta = random.uniform(-self.max_curvature, self.max_curvature) dx = np.cos(source_node.pose[2] + dtheta/2) dy = np.sin(source_node.pose[2] + dtheta/2) vec = np.array([dx, dy, dtheta]) new_node = Node(source_node.pose + self.growth * vec) return new_node
def __init__(self, start, end, space, obstacles, growth=1.0, max_iter=500, end_sample_percent=15): """ :param start: [x,y] starting location :param end: [x,y[ ending location :param space: [min,max] bounds on square space :param obstacles: list of square obstacles :param growth: size of growth each new sample :param max_iter: max number of iterations for algorithm :param end_sample_percent: percent chance to get sample from goal location """ self.start = Node(start) self.end = Node(end) self.space = space self.growth = growth self.end_sample_percent = end_sample_percent self.max_iter = max_iter self.obstacles = obstacles self.kd_tree = KD_Tree.create([self.start])
def __init__(self, start, goal, space, obstacles, growth=3.0, max_iter=300, max_dist=40, max_curvature=1.0, end_sample_percent=15): """ :param start: [x,y] starting location :param end: [x,y[ ending location :param space: [min,max] bounds on square space :param obstacles: list of square obstacles :param growth: size of growth each new sample :param max_iter: max number of iterations for algorithm :param end_sample_percent: percent chance to get sample from goal location """ self.start = Node(start) self.end = Node(goal) self.space = space self.growth = growth self.max_iter = max_iter self.max_dist = max_dist self.obstacles = obstacles self.node_list = [self.start] self.max_curvature = max_curvature self.local_planner_time = 0.0 self.end_sample_percent = end_sample_percent
def steer(self, sample, nearest_node):
# take nearest_node and expand in direction of sample
angle = math.atan2(sample.pose[1] - nearest_node.pose[1], sample.pose[0] - nearest_node.pose[0])
new_node = Node([sample.pose[0], sample.pose[1]])
currentDistance = dist(sample, nearest_node)
# find a point within growth of nearest_node, and closest to sample
if currentDistance <= self.growth:
pass
else:
new_node.pose[0] = nearest_node.pose[0] + self.growth * math.cos(angle)
new_node.pose[1] = nearest_node.pose[1] + self.growth * math.sin(angle)
new_node.cost = float("inf")
new_node.parent = None
return new_node
def steer(self, source_node, dest_node): # take source_node and steer towards destination node dtheta = random.uniform(-self.max_curvature/2, self.max_curvature/2) dx = np.cos(source_node.pose[2] + dtheta/2) dy = np.sin(source_node.pose[2] + dtheta/2) vec = np.array([dx, dy, dtheta]) new_node = Node(source_node.pose + self.growth * vec) if new_node.pose[0] < self.space[0]: new_node.pose[0] = self.space[0] - random.uniform(0, 1) if new_node.pose[0] > self.space[1]: new_node.pose[0] = self.space[1] - random.uniform(0, 1) if new_node.pose[1] < self.space[2]: new_node.pose[1] = self.space[2] - random.uniform(0, 1) if new_node.pose[1] > self.space[3]: new_node.pose[1] = self.space[3] - random.uniform(0, 1) return new_node
def steer(self, sample, nearest_node):
# take nearest_node and expand in direction of sample
vec = np.array((sample.pose[0] - nearest_node.pose[0],
sample.pose[1] - nearest_node.pose[1]))
unit_vec = vec / np.linalg.norm(vec)
new_node = Node([sample.pose[0], sample.pose[1]])
currentDistance = dist(sample, nearest_node)
# find a point within growth of nearest_node, and closest to sample
if currentDistance <= self.growth:
pass
else:
new_node.pose[0] = nearest_node.pose[0] + self.growth * unit_vec[0]
new_node.pose[1] = nearest_node.pose[1] + self.growth * unit_vec[1]
new_node.cost = float("inf")
new_node.parent = None
return new_node
def __init__(self, start, RRT_params, gmrf_params, var_x, max_dist, plot): """ :param start: initial location of agent :param RRT_params: specified in config file :param gmrf_params: specified in config file :param var_x: variance of field as a 1D vector of variance of each node in GMRF :param max_dist: maximum distance that the algorithm solution will return :param plot: only used for plotting in the middle of running algorithm good for debugging """ self.start = Node(start) self.node_list = [self.start] self.max_dist = max(max_dist, 10) # can't just take the max_dist in case at the end of the simulation this will allow no possible paths self.var_x = var_x self.gmrf_params = gmrf_params (self.space, self.max_time, self.max_curvature, self.growth, self.min_dist, self.obstacles) = RRT_params self.local_planner_time = 0.0 self.method_time = 0.0 self.plot = plot