def test_rewire(self):
node1 = Node(np.random.rand(2))
node2 = Node(np.random.rand(2))
node3 = Node(np.random.rand(2))
node4 = Node(np.random.rand(2))
node1.cost = 1
node2.cost = 10
node3.cost = 50
self.planner.add_newnode(node1)
self.planner.add_newnode(node2)
self.planner.add_newnode(node3)
self.planner.add_newnode(node4)
poses = np.array([node1.pos, node2.pos, node3.pos, node4.pos])
newnode, nn = self.planner.choose_least_cost_parent(
node4,
node1,
nodes=self.planner.nodes,
poses=poses,
)
# newnode, nn, nodes=self.nodes, skip_optimality=True)
self.planner.add_newnode(node1)
# rewire to see what the newly added node can do for us
self.planner.rewire(node1, self.planner.nodes, poses=poses)
def test_choose_least_cost_parent(self):
node1 = Node(np.random.rand(2))
node2 = Node(np.random.rand(2))
node3 = Node(np.random.rand(2))
node4 = Node(np.random.rand(2))
node1.cost = 1
node2.cost = 10
node3.cost = 50
# ensure that the nn returned is as expected
newnode, nn = self.planner.choose_least_cost_parent(
newnode=node4, nodes=[node1, node2, node3])
self.assertEqual(nn, node1)
# ensure that the nn returned is as expected
newnode, nn = self.planner.choose_least_cost_parent(
newnode=node2, nodes=[node1, node4, node3])
self.assertEqual(nn, node1)
def choose_least_cost_parent(
self,
newnode: Node,
nn: Node = None,
nodes: List[Node] = None,
skip_optimality: bool = False,
use_rtree: bool = True,
poses: List[np.ndarray] = None,
) -> Node:
"""
Given a new node, a node from root, return a node from root that
has the least cost (toward the newly added node)
:param newnode: the newly created node that we want to search for a new parent
:param nn: the current closest node, optional.
:param nodes: the list of node to search against
:param skip_optimality: skip searching for optimality (i.e. non-asymptomatic)
:param use_rtree: whether use rtree to store tree or not
:param poses: list of configurations, with the same length as ``nodes``
:return: the node with the lowest cost
"""
skip_optimality = False
use_rtree = False
if skip_optimality:
if nn is None:
raise RuntimeError("Not enough information")
newnode.parent = nn
newnode.cost = nn.cost + self.args.env.dist(nn.pos, newnode.pos)
return newnode, nn
if use_rtree or poses is not None:
nn2 = None
if use_rtree:
canidates = list(self.tree.nearby(newnode.pos, n=20))
else:
distances = np.linalg.norm(poses - newnode.pos, axis=1)
canidates = [nodes[idx] for idx in np.argsort(distances)[:20]]
canidates.sort(key=operator.attrgetter("cost"))
for n in canidates:
if self.args.env.dist(
newnode.pos, n.pos
) <= self.args.radius and self.args.env.cc.visible(newnode.pos, n.pos):
nn2 = n
break
if nn2 is None:
if nn is None:
raise RuntimeError("Unable to find nn that is connectable")
nn2 = nn
newnode.cost = nn2.cost + self.args.env.dist(nn2.pos, newnode.pos)
newnode.parent = nn2
return newnode, nn2
if nn is not None:
_newnode_to_nn_cost = self.args.env.dist(newnode.pos, nn.pos)
self._new_node_dist_to_all_others = {}
for p in nodes:
_newnode_to_p_cost = self.args.env.dist(newnode.pos, p.pos)
self._new_node_dist_to_all_others[(newnode, p)] = _newnode_to_p_cost
if _newnode_to_p_cost <= self.args.radius and self.args.env.cc.visible(
newnode.pos, p.pos
):
# This is another valid parent. Check if it's better than our current one.
if nn is None or (
p.cost + _newnode_to_p_cost < nn.cost + _newnode_to_nn_cost
):
nn = p
_newnode_to_nn_cost = _newnode_to_p_cost
if nn is None:
raise LookupError(
"ERROR: Provided nn=None, and cannot find any valid nn by this function. This newnode is not close to the root tree...?"
)
newnode.cost = nn.cost + self.args.env.dist(nn.pos, newnode.pos)
assert newnode is not nn
newnode.parent = nn
return newnode, nn