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 setUp(self) -> None: self.tree = Tree(dimension=2) pos = np.array([1, 2]) self.a = Node(pos) self.tree.add_vertex(self.a, pos) pos = np.array([4, 2]) self.b = Node(pos) self.tree.add_vertex(self.b, pos) pos = np.array([4, 8]) self.c = Node(pos) self.tree.add_vertex(self.c, pos)
def test_find_nearest_neighbour_idx(self): node1 = Node(np.array([0, 0])) node2 = Node(np.array([1, 1])) node3 = Node(np.array([2, 2])) node4 = Node(np.array([3, 3])) nodes = [node1, node2, node3, node4] poses = np.array([n.pos for n in nodes]) for i, n in enumerate(nodes): self.assertEqual( self.planner.find_nearest_neighbour_idx(n.pos + 1e-2, poses), i)
def set_start_goal_points( self, start: Optional[np.ndarray] = None, goal: Optional[np.ndarray] = None, **kwargs, ): """A function that query user for start/goal and set them accordingly. :param start: the start configuration of the motion planning problem :param goal: the goal configuration of the motion planning problem """ from utils.common import Node ################################################## # Get starting and ending point LOGGER.info("Select Starting Point and then Goal Point") if start is not None and goal is not None: return start, goal self.env_instance.start_pt = self.env_instance.goal_pt = None self.update_screen(update_all=True) while start is None or goal is None: mouse_pos = None for e in pygame.event.get(): if e.type == MOUSEBUTTONDOWN: mouse_pos = np.array(e.pos) / self.args.scaling from collisionChecker import RobotArm4dCollisionChecker if type(self.cc) == RobotArm4dCollisionChecker: mouse_pos = np.array( [*mouse_pos, *np.random.uniform(-np.pi, np.pi, 2)] ) if not self.cc.feasible(mouse_pos): # failed to pass collision check mouse_pos = None elif e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): LOGGER.info("Leaving.") return # convert mouse pos to Node if mouse_pos is not None: if start is None: start = Node(mouse_pos) self.env_instance.start_pt = start LOGGER.info(f"starting point set: {mouse_pos}") elif goal is None: goal = Node(mouse_pos) self.env_instance.goal_pt = goal LOGGER.info(f"goal point set: {mouse_pos}") self.update_screen(update_all=True) return start, goal
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 test_add_newnode(self): # repeat the testing multiple times for i in range(10): node = Node(np.random.rand(2)) self.planner.add_newnode(node) # assert that the position is correct self.assertTrue( np.isclose(self.planner.poses[len(self.planner.nodes) - 1], node.pos).all()) # assert that the added node is correct self.assertEqual(self.planner.nodes[-1], node)
def test_get_next_pos(self): np.random.seed(0) self.sampler.args.goalBias = 0 # test all results are different results = set() for i in range(100): results.add(Node(self.sampler.get_next_pos()[0])) self.assertEqual(len(results), 100) self.sampler.args.goalBias = 1 # test all results, because of goalBias, are same results = set() for i in range(100): results.add(Node(self.sampler.get_next_pos()[0])) self.assertEqual(len(results), 1) # test that the result is the exact same point as the goal point self.assertEqual(results.pop(), self.sampler.args.goal_pt)
def test_likelihood_increases(self): pt, report_success, report_fail = self.sampler.get_next_pos() target_pt = np.array([10, 10]) nn = Node(np.array([20, 20])) rand_pos = np.array([5, 5]) prev_val_1 = None prev_val_2 = None prev_val_3 = None for i in range(20): report_success( pos=target_pt, nn=nn, rand_pos=rand_pos, ) # val_1 comes from the target point (probability should increases) val_1 = self.sampler.prob_vector[ tuple((target_pt / self.sampler.PROB_BLOCK_SIZE).astype(int)) ] if prev_val_1 is not None: # the likelihood should increases self.assertTrue(val_1 > prev_val_1) prev_val_1 = val_1 # val_2 comes from the nearest neighbour point (probability should # increases) val_2 = self.sampler.prob_vector[ tuple((nn.pos / self.sampler.PROB_BLOCK_SIZE).astype(int)) ] if prev_val_2 is not None: # the likelihood should increases self.assertTrue(val_2 > prev_val_2) prev_val_2 = val_2 # val_3 comes from the desire rand pos (but hasn't reached yet, # should this should not be affected) val_3 = self.sampler.prob_vector[ tuple((rand_pos / self.sampler.PROB_BLOCK_SIZE).astype(int)) ] if prev_val_3 is not None: # the likelihood should remains the same self.assertTrue(val_3 == prev_val_3) prev_val_3 = val_3
def run_once(self): """Execute the main planning procedure once (mostly of the time this corresponds to adding one new node to the tree) This where the main bulk of actions happens, e.g., creating nodes or edges. """ # Get an sample that is free (not in blocked space) rand_pos, report_success, report_fail = self.args.sampler.get_valid_next_pos() # Found a node that is not in X_obs idx = self.find_nearest_neighbour_idx(rand_pos, self.poses[: len(self.nodes)]) nn = self.nodes[idx] # get an intermediate node according to step-size newpos = self.args.env.step_from_to(nn.pos, rand_pos) # check if it has a free path to nn or not if not self.args.env.cc.visible(nn.pos, newpos): self.args.env.stats.add_invalid(obs=False) report_fail(pos=rand_pos, free=False) else: newnode = Node(newpos) self.args.env.stats.add_free() self.args.sampler.add_tree_node(pos=newnode.pos) report_success(pos=newnode.pos, nn=nn, rand_pos=rand_pos) ###################### newnode, nn = self.choose_least_cost_parent(newnode, nn, nodes=self.nodes) self.add_newnode(newnode) # rewire to see what the newly added node can do for us self.rewire(newnode, self.nodes) if self.args.env.cc.visible(newnode.pos, self.goal_pt.pos): if ( self.args.env.dist(newnode.pos, self.goal_pt.pos) < self.args.goal_radius ): if newnode.cost < self.c_max: self.c_max = newnode.cost self.goal_pt.parent = newnode self.visualiser.draw_solution_path()
def test_index_op(self): n = Node(np.array([5.2, 2.5])) self.assertAlmostEqual(n[0], 5.2) self.assertAlmostEqual(n[1], 2.5)
def test_equality(self): n = Node(np.array([5.2, 2.5])) self.assertEqual(n, Node(np.array([5.2, 2.5]))) n = Node(np.array([2.4124, 53.623, 23.423])) self.assertEqual(n, Node(np.array([2.4124, 53.623, 23.423])))
def __init__(self, args: MagicDict, fixed_seed: int = None): """ :param args: the dictionary of arguments to config the planning problem :param fixed_seed: if given, fix the random seed """ self.started = False if fixed_seed is not None: np.random.seed(fixed_seed) random.seed(fixed_seed) print(f"Fixed random seed: {fixed_seed}") # initialize and prepare screen self.args = args self.stats = Stats(showSampledPoint=self.args.showSampledPoint) cc_type, self.dist = { "image": (collisionChecker.ImgCollisionChecker, self.euclidean_dist), "4d": (collisionChecker.RobotArm4dCollisionChecker, self.euclidean_dist), "klampt": (collisionChecker.KlamptCollisionChecker, self.radian_dist), }[self.args.engine] self.cc = cc_type(self.args.image, stats=self.stats, args=self.args) # setup visualiser if self.args.no_display: # use pass-through visualiser VisualiserSwitcher.choose_visualiser("base") else: if self.args.engine in ("image", "4d"): # use pygame visualiser VisualiserSwitcher.choose_visualiser("pygame") elif self.args.engine == "klampt": VisualiserSwitcher.choose_visualiser("klampt") self.args["num_dim"] = self.cc.get_dimension() self.args["image_shape"] = self.cc.get_image_shape() self.dim = self.args["image_shape"] self.args["cc"] = self.cc def parse_input_pt(pt_as_str): if pt_as_str is None: return None pt = pt_as_str.split(",") if len(pt) != self.args["num_dim"]: raise RuntimeError( f"Expected to have number of dimension = {self.args['num_dim']}, " f"but " f"was n={len(pt)} from input '{pt_as_str}'" ) return tuple(map(float, pt)) if type(self.args["start_pt"]) is str: self.args["start_pt"] = parse_input_pt(self.args["start_pt"]) if type(self.args["goal_pt"]) is str: self.args["goal_pt"] = parse_input_pt(self.args["goal_pt"]) self.args.planner = self.args.planner_data_pack.planner_class(**self.args) self.args["planner"] = self.args.planner self.planner = self.args.planner self.planner.args.env = self self.visualiser = VisualiserSwitcher.env_clname(env_instance=self) self.visualiser.visualiser_init(no_display=self.args["no_display"]) start_pt, goal_pt = self.visualiser.set_start_goal_points( start=self.args["start_pt"], goal=self.args["goal_pt"] ) if not self.cc.feasible(start_pt): raise ValueError(f"The given start conf. is not feasible {start_pt}.") if not self.cc.feasible(goal_pt): raise ValueError(f"The given goal conf. is not feasible {goal_pt}.") self.start_pt = self.goal_pt = None if start_pt is not None: self.start_pt = Node(start_pt) if goal_pt is not None: self.goal_pt = Node(goal_pt) self.start_pt.is_start = True self.goal_pt.is_goal = True self.planner.add_newnode(self.start_pt) self.visualiser.update_screen(update_all=True) # update the string pt to object self.args["start_pt"] = self.start_pt self.args["goal_pt"] = self.goal_pt self.planner.init(env=self, **self.args) if self.args["engine"] == "klampt": self.args.sampler.set_use_radian(True) if self.args["epsilon"] > 1: import warnings warnings.warn( f"Epsilon value is very high at {self.args['epsilon']} (" f">than 1.0). It might not work well as klampt uses " f"radian for joints value" ) if self.args["radius"] > 2: import warnings warnings.warn( f"Radius value is very high at {self.args['radius']} (" f">than 2.0). It might not work well as klampt uses " f"radian for joints value" )
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