def test_register_not_derived(self):
with self.assertRaises(TypeError):
register_planner(
"my_non_derived_id",
planner_class=DummyClass,
sampler_id="my_sampler_id",
)
def test_register_duplicate_id(self):
register_planner(
"my_duplicate_id",
planner_class=MyPlanner,
sampler_id="my_sampler_id",
)
# cannot have duplicate id
with self.assertRaises(ValueError):
register_planner(
"my_duplicate_id",
planner_class=MyPlanner,
sampler_id="my_sampler_id",
)
def test_missing_argunment(self):
with self.assertRaises(TypeError):
generate_args()
with self.assertRaises(TypeError):
generate_args(planner_id="rrt")
with self.assertRaises(TypeError):
generate_args(map_fname="maps/4d.png")
# should not raise error
generate_args(planner_id="rrt", map_fname="maps/4d.png")
# test error if the planner id has not been registered yet
with self.assertRaises(ValueError):
generate_args(planner_id="my_planner_id", map_fname="maps/4d.png")
# test that after the planner id is registered, it will work.
planner_registry.register_planner(
planner_id="my_planner_id",
planner_class=DummyPlannerClass,
sampler_id="random",
)
generate_args(planner_id="my_planner_id", map_fname="maps/4d.png")
def klampt_prm_planner_paint_when_terminate(planner):
"""Visualisation function to paint for planner when termiante
:param planner: the planner to visualise
"""
from utils.common import Colour
planner.build_graph()
# draw all edges
for n1, n2 in planner.tree.edges():
planner.args.env.draw_path(n1, n2, Colour.path_blue)
planner.get_solution()
planner.args.env.update_screen()
input("\nPress Enter to quit...")
# start register
planner_registry.register_planner(
"prm",
planner_class=PRMPlanner,
visualise_pygame_paint=pygame_prm_planner_paint,
visualise_pygame_paint_terminate=pygame_prm_planner_paint_when_terminate,
visualise_klampt_paint=klampt_prm_paint,
visualise_klampt_paint_terminate=klampt_prm_planner_paint_when_terminate,
sampler_id=prmSampler.sampler_id,
)
# finish register
from planners import rrtPlanner
from samplers import nearbyPolicySampler
from utils import planner_registry
# start register
planner_registry.register_planner(
"nearby",
planner_class=rrtPlanner.RRTPlanner,
visualise_pygame_paint=rrtPlanner.pygame_rrt_paint,
sampler_id=nearbyPolicySampler.sampler_id,
)
# finish register
"""Represent a planner."""
from planners import rrtPlanner
from samplers import informedSampler
from utils import planner_registry
# start register
planner_registry.register_planner(
"informedrrt",
planner_class=rrtPlanner.RRTPlanner,
visualise_pygame_paint=rrtPlanner.pygame_rrt_paint,
sampler_id=informedSampler.sampler_id,
)
# finish register
for n in planner.root.nodes:
if n.parent is not None:
new_set = frozenset({n, n.parent})
if new_set not in drawn_nodes_pairs:
drawn_nodes_pairs.add(new_set)
planner.args.env.draw_path(
planner.args.env.cc.get_eef_world_pos(n.pos),
planner.args.env.cc.get_eef_world_pos(n.parent.pos),
colour=c,
)
# start register
sampler_id = "rrdt_sampler"
planner_registry.register_sampler(
sampler_id,
sampler_class=RRdTSampler,
visualise_pygame_paint=pygame_rrdt_sampler_paint,
visualise_pygame_paint_init=pygame_rrdt_sampler_paint_init,
)
planner_registry.register_planner(
"rrdt",
planner_class=RRdTPlanner,
visualise_pygame_paint=pygame_rrdt_planner_paint,
visualise_klampt_paint=klampt_rrdt_planner_paint,
sampler_id=sampler_id,
)
# finish register
from planners import rrtPlanner
from samplers import likelihoodPolicySampler
from utils import planner_registry
# start register
planner_registry.register_planner(
"likelihood",
planner_class=rrtPlanner.RRTPlanner,
visualise_pygame_paint=rrtPlanner.pygame_rrt_paint,
sampler_id=likelihoodPolicySampler.sampler_id,
)
# finish register
planner.args.env.draw_path(n, n.parent)
if planner.goal_pt.parent is not None:
planner.visualiser.draw_solution_path()
from planners.rrtPlanner import klampt_draw_nodes_paint_func
def klampt_birrt_paint(planner):
"""Visualiser paint function for BiRRT
:param planner: the planner to be visualised
"""
for c, nodes in (
((1, 0, 0, 1), planner.nodes),
((0, 0, 1, 1), planner.goal_tree_nodes),
):
klampt_draw_nodes_paint_func(planner, nodes, c)
# start register
planner_registry.register_planner(
"birrt",
planner_class=BiRRTPlanner,
visualise_pygame_paint=pygame_birrt_planner_paint,
visualise_klampt_paint=klampt_birrt_paint,
sampler_id="birrt_sampler",
)
# finish register
new_set = frozenset({n, n.parent})
if new_set not in drawn_nodes_pairs:
drawn_nodes_pairs.add(new_set)
planner.args.env.draw_path(
planner.args.env.cc.get_eef_world_pos(n.pos),
planner.args.env.cc.get_eef_world_pos(n.parent.pos),
colour=colour,
)
def klampt_rrt_paint(planner: Planner) -> None:
"""Visualiser paint function for RRT
:param planner: the planner to be visualised
"""
colour = (1, 0, 0, 1)
klampt_draw_nodes_paint_func(planner, planner.nodes, colour)
# start register
planner_registry.register_planner(
"rrt",
planner_class=RRTPlanner,
visualise_pygame_paint=pygame_rrt_paint,
visualise_klampt_paint=klampt_rrt_paint,
sampler_id="random",
)
# finish register
def test_register(self):
register_planner(
"my_id",
planner_class=MyPlanner,
sampler_id="my_sampler_id",
)
from planners import rrtPlanner
from samplers import mouseSampler
from utils import planner_registry
# start register
planner_registry.register_planner(
"mouse",
planner_class=rrtPlanner.RRTPlanner,
visualise_pygame_paint=rrtPlanner.pygame_rrt_paint,
sampler_id=mouseSampler.sampler_id,
)
# finish register