From 947fcfbc43d142d2c2d7189ae9fc6cffcbdf3306 Mon Sep 17 00:00:00 2001 From: Nataliya <23035717+nataliya-dev@users.noreply.github.com> Date: Mon, 18 Sep 2023 21:37:42 -0600 Subject: [PATCH] fixing the missing output from euclidean tree example --- examples/euclidean_tree/euclidean_tree.py | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/examples/euclidean_tree/euclidean_tree.py b/examples/euclidean_tree/euclidean_tree.py index 6c9b611..4f46f7c 100644 --- a/examples/euclidean_tree/euclidean_tree.py +++ b/examples/euclidean_tree/euclidean_tree.py @@ -30,14 +30,22 @@ def compute_edge_cost(self, parent_node: Node, child_node: Node): x_child = child_node.q return np.linalg.norm(x_parent - x_child) - def extend_towards_q(self, node: Node, q: np.array): - if np.linalg.norm(q - node.q) < self.rrt_params.radius: + def extend_towards_q(self, parent_node: Node, q: np.array): + if np.linalg.norm(q - parent_node.q) < self.rrt_params.radius: child_q = q else: - child_q = node.q + self.rrt_params.radius * ( - q - node.q - ) / np.linalg.norm(q - node.q) - return Node(child_q) + child_q = parent_node.q + self.rrt_params.radius * ( + q - parent_node.q + ) / np.linalg.norm(q - parent_node.q) + + child_node = Node(child_q) + + edge = Edge() + edge.parent = parent_node + edge.child = child_node + edge.cost = self.compute_edge_cost(parent_node, child_node) + + return child_node, edge def calc_distance_batch(self, q_query: np.array): q_batch = self.get_q_matrix_up_to()