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