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