def test_nearest(empty_environment_info):
space = empty_environment_info["space"]
root = space.create_state(np.array([10, 10]))
seeds = [np.random.rand() for _ in range(50)]
nodes = [
space.create_state(space.bounds[:, 0] * seed + space.bounds[:, 1] *
(1 - seed)) for seed in seeds
]
tree = Tree(root, space)
for node in nodes:
tree.add_node(node)
search = space.create_state(np.array([15, 15]))
assert np.linalg.norm(search.pos - tree.nearest(search).pos) == min(
[np.linalg.norm(search.pos - node.pos) for node in nodes])
class RTSamplingPlanner:
"""
Real time sampling planner, parent class for RTRRT and AMRRT
"""
def __init__(self,
space,
agent_pos,
assisting_metric=None,
t_exp=0.15,
t_root=0.003,
t_steer=0.002):
"""
:param space: State space planner operates in
:param agent_pos: Initial position of agent
:param assisting_metric: Assisting metric for the planner, default None for Euclidean
:param t_exp: Time spent expanding and rewiring
:param t_root: Time spent rewiring at the root
:param t_steer: Time spent steering
"""
self.space = space
self.euclidean_metric = EuclideanMetric()
self.assisting_metric = assisting_metric if assisting_metric is not None else self.euclidean_metric
self.euclidean_distance = lambda a, b: self.euclidean_metric.distance(
a, b)
self.assisting_distance = lambda a, b: self.assisting_metric.distance(
a, b)
self.tree = Tree(self.space.create_state(agent_pos),
self.space,
assisting_dist_fn=self.assisting_distance)
self.goal = None
self.root_queue = []
self.rewired_root = set()
self.t_exp, self.t_root, self.t_steer = t_exp, t_root, t_steer
def _nearest_node(self, node):
"""
Find nearest node, first by Euclidean metric if the path is free or fallback on the assisting metric
"""
euclidean = self.tree.euclidean_nearest(node)
if self.space.free_path(
node,
euclidean) or self.euclidean_metric == self.assisting_metric:
return euclidean
return self.tree.nearest(node)
def set_root(self, root):
"""
Set the root node for the planner tree, and reset rewiring queue
"""
self.tree.set_root(root)
self.root_queue = [self.tree.root]
self.rewired_root = {self.tree.root}
def set_goal(self, pos):
"""
Set new goal from position, try to add goal to tree if within range & path free
"""
self.goal = self.space.create_state(pos)
xnearest = self._nearest_node(self.goal)
if self.euclidean_distance(
xnearest, self.goal) < self.max_step and self.space.free_path(
xnearest, self.goal):
self.tree.add_node(self.goal, xnearest)
def _add_node(self, xnew, xnearest, nearby):
"""
Add new node to the tree, connecting it to the node in nearby that minimises cost
Also attempt to add the goal to the tree via xnew if within range & free path
:param xnew: New state to be added
:param xnearest: Nearest node to xnew
:param nearby: Set of nodes in the neighbourhood of xnew
"""
xmin = xnearest
cmin = self.cost(xnearest) + self.euclidean_distance(xnearest, xnew)
for xnear in nearby:
cnew = self.cost(xnear) + self.euclidean_distance(xnear, xnew)
if cnew < cmin and self.space.free_path(xnear, xnew):
xmin = xnear
cmin = cnew
self.tree.add_node(xnew, xmin)
if self.goal is not None and self.euclidean_distance(
xnew, self.goal) < self.max_step and self.space.free_path(
xnew, self.goal):
self.tree.add_node(self.goal, xnew)
def goal_path(self):
"""
Return path to the goal if one exists, if not a path to the nearest node
"""
if self.goal is None:
return []
if self.goal in self.tree.nodes:
return self.tree.path(self.goal)
return self.tree.path(self._nearest_node(self.goal))
def cost(self, node):
"""
Returns path cost to given node (as Cost object)
"""
if node == self.tree.root:
return Cost(0, False)
path = self.tree.path(node)
running_cost = 0
blocked = False
for i in range(1, len(path)):
if not self.space.dynamically_free_path(path[i - 1], path[i]):
blocked = True
running_cost += self.euclidean_distance(path[i - 1], path[i])
return Cost(running_cost, blocked)
def add_dynamic_obstacle(self, pos, radius):
"""
Add circular dynamic obstacle, and reset goal queues
:param pos: Position of dynamic obstacle
:param radius: Radius of dynamic obstacle
"""
self.space.add_dynamic_obstacle(pos, radius)
if self.goal is not None: self.set_goal(self.goal.pos)