def test_register_duplicate_id(self):
register_sampler(
"my_duplicate_sampler_id",
sampler_class=MySampler,
)
# cannot have duplicate id
with self.assertRaises(ValueError):
register_sampler(
"my_duplicate_sampler_id",
sampler_class=MySampler,
)
elif self.args.engine == "4d":
self.low, self.high = [
[0, 0, -np.pi, -np.pi],
[self.args.env.dim[0], self.args.env.dim[1], np.pi, np.pi],
]
else:
self.use_original_method = True
@overrides
def get_next_pos(self) -> Sampler.GetNextPosReturnType:
# Random path
if random.random() < self.args.goalBias:
# goal bias
p = self.goal_pos
else:
if self.use_original_method:
p = self.random.get_random(self.random_method)
p *= self.args.env.dim
else:
p = np.random.uniform(self.low, self.high)
return p, self.report_success, self.report_fail
# start register
sampler_id = "random"
planner_registry.register_sampler(sampler_id,
sampler_class=RandomPolicySampler)
# finish register
if self.sampleCount % 20 == 0:
pass
self.prob_vector_normalized = np.copy(self.tree_vector)
self.prob_vector_normalized = sp.ndimage.filters.gaussian_filter(
self.prob_vector_normalized, (8.0, 8.0), mode="reflect"
)
tree_vector_normalized = np.copy(self.tree_vector)
tree_vector_normalized = sp.ndimage.filters.gaussian_filter(
self.tree_vector, (1.0, 1.0), mode="reflect"
)
self.prob_vector_normalized -= tree_vector_normalized
self.prob_vector_normalized = np.clip(
self.prob_vector_normalized, 0, None
)
self.prob_vector_normalized /= self.prob_vector_normalized.sum()
self.sampleCount += 1
# start register
sampler_id = "nearby_sampler"
planner_registry.register_sampler(
sampler_id,
sampler_class=likelihoodPolicySampler.LikelihoodPolicySampler,
visualise_pygame_paint=likelihoodPolicySampler.pygame_likelihood_sampler_paint,
visualise_pygame_paint_init=likelihoodPolicySampler.pygame_likelihood_sampler_paint_init,
)
# finish register
internal :class:`samplers.randomPolicySampler.RandomPolicySampler`
:param value: whether to use radian
"""
self.use_radian = value
self.randomSampler.use_radian = value
@overrides
def get_next_pos(self) -> Sampler.GetNextPosReturnType:
"""Get next sampled position"""
# Random path
while True:
if random.random() < self.args.goalBias:
# init/goal bias
if self.args.planner.goal_tree_turn:
p = self.start_pos
else:
p = self.goal_pos
else:
p = self.randomSampler.get_next_pos()[0]
return p, self.report_success, self.report_fail
# start register
planner_registry.register_sampler(
"birrt_sampler",
sampler_class=BiRRTSampler,
)
# finish register
from overrides import overrides
from samplers.randomPolicySampler import RandomPolicySampler
from utils import planner_registry
class PRMSampler(RandomPolicySampler):
r"""This sampler is basically the same as
:class:`samplers.randomPolicySampler.RandomPolicySampler`, excepts we
always overrides the goal bias epsilon :math:`\epsilon=0` as PRM could not
utilise the benefit of goal bias.
"""
def __init__(self, **kwargs):
super().__init__(**kwargs)
@overrides
def init(self, **kwargs):
"""The delayed **initialisation** method"""
kwargs["goalBias"] = 0
super().init(**kwargs)
# start register
sampler_id = "prm_sampler"
planner_registry.register_sampler(
sampler_id,
sampler_class=PRMSampler,
)
# finish register
# rotate via surface
ellipse_surface = pygame.Surface((b, a), pygame.SRCALPHA, 32).convert_alpha()
try:
pygame.draw.ellipse(ellipse_surface, (255, 0, 0, 80), r)
pygame.draw.ellipse(
ellipse_surface, Colour.black, r, int(2 * sampler.args.scaling)
)
except ValueError:
# sometime it will fail to draw due to ellipse being too narrow
pass
# rotate
ellipse_surface = pygame.transform.rotate(
ellipse_surface, -angle * 180 / math.pi
)
# we need to offset the blitz based on the surface ceenter
rcx, rcy = ellipse_surface.get_rect().center
ellipse_x = sampler.xCenter[0] * sampler.args.scaling - rcx
ellipse_y = sampler.xCenter[1] * sampler.args.scaling - rcy
sampler.args.env.window.blit(ellipse_surface, (ellipse_x, ellipse_y))
# start register
sampler_id = "informed_sampler"
planner_registry.register_sampler(
sampler_id,
sampler_class=InformedSampler,
visualise_pygame_paint=pygame_informed_sampler_paint,
)
# 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
self.report_success,
self.report_fail,
)
@staticmethod
def get_mouse_click_position(scaling):
"""Helper function to return the current position of the mouse pointer
:param scaling: scaling factor ot be applied to the mouse position
"""
import pygame
while True:
time.sleep(0.05)
pygame.event.wait()
if pygame.mouse.get_pressed()[0] == 1: # Left mouse pressed
pos = pygame.mouse.get_pos()
pos = (int(pos[0] / scaling), int(pos[1] / scaling))
return pos
# start register
sampler_id = "mouse"
planner_registry.register_sampler(
sampler_id,
sampler_class=MouseSampler,
)
# finish register
def test_register_not_derived(self):
with self.assertRaises(TypeError):
register_sampler(
"my_non_derived_sampler_id",
sampler_class=DummyClass,
)
def test_register(self):
register_sampler(
"my_sampler_id",
sampler_class=MySampler,
)