class InformedPlanner(GenericGraphPlanner):
"""
This one knows that some plans are redundant
"""
def __init__(self,
diffeo_structure_params,
pre_expand=[],
*args,
**kwargs):
super(InformedPlanner, self).__init__(*args, **kwargs)
self.diffeo_structure_params = diffeo_structure_params
self.pre_expand = pre_expand
def __str__(self):
return 'InformedPlanner(%s)' % (self.__strparams__())
def __strparams__(self):
# TODO: use classes
p = GenericGraphPlanner.__strparams__(self)
return p
@contract(dds=DiffeoSystem)
def init(self, id_dds, dds):
self.ds = DiffeoStructure(dds=dds, **self.diffeo_structure_params)
self.original_dds = dds
dds_expanded = diffeosystem_expand(dds,
pr=self.ds.get_plan_reducer(),
heuristics=self.pre_expand)
# TODO: recompute structure
self.info('pre_expand: %s' % str(self.pre_expand))
self.info('Expanded to %s from %s actions' %
(len(dds_expanded.actions), len(dds.actions)))
super(InformedPlanner, self).init(id_dds, dds_expanded)
def plan(self, y0, y1, precision, min_visibility):
# we need to translate back the result according to the original plan
result = super(InformedPlanner, self).plan(y0, y1, precision,
min_visibility)
if result.success:
plan_extended = result.plan
self.info('Solution found in extended space: %s' %
str(plan_extended))
plan_simple = self.get_dds().plan_with_simple_actions(
plan_extended)
self.info('With simple actions: %s' % str(plan_simple))
result.plan = plan_simple
return result
@contract(report=Report)
def init_report(self, report):
""" Creates a report for the initialization phase. """
super(InformedPlanner, self).init_report(report)
self.ds.display(report.section('diffeo_structure'))
def get_plan_reducer(self):
return self.ds.get_plan_reducer()
class InformedPlanner(GenericGraphPlanner):
"""
This one knows that some plans are redundant
"""
def __init__(self, diffeo_structure_params, pre_expand=[],
*args, **kwargs):
super(InformedPlanner, self).__init__(*args, **kwargs)
self.diffeo_structure_params = diffeo_structure_params
self.pre_expand = pre_expand
def __str__(self):
return 'InformedPlanner(%s)' % (self.__strparams__())
def __strparams__(self):
# TODO: use classes
p = GenericGraphPlanner.__strparams__(self)
return p
@contract(dds=DiffeoSystem)
def init(self, id_dds, dds):
self.ds = DiffeoStructure(dds=dds, **self.diffeo_structure_params)
self.original_dds = dds
dds_expanded = diffeosystem_expand(dds, pr=self.ds.get_plan_reducer(),
heuristics=self.pre_expand)
# TODO: recompute structure
self.info('pre_expand: %s' % str(self.pre_expand))
self.info('Expanded to %s from %s actions' %
(len(dds_expanded.actions), len(dds.actions)))
super(InformedPlanner, self).init(id_dds, dds_expanded)
def plan(self, y0, y1, precision, min_visibility):
# we need to translate back the result according to the original plan
result = super(InformedPlanner, self).plan(y0, y1, precision, min_visibility)
if result.success:
plan_extended = result.plan
self.info('Solution found in extended space: %s' % str(plan_extended))
plan_simple = self.get_dds().plan_with_simple_actions(plan_extended)
self.info('With simple actions: %s' % str(plan_simple))
result.plan = plan_simple
return result
@contract(report=Report)
def init_report(self, report):
""" Creates a report for the initialization phase. """
super(InformedPlanner, self).init_report(report)
self.ds.display(report.section('diffeo_structure'))
def get_plan_reducer(self):
return self.ds.get_plan_reducer()
def set_discdds(self, discdds):
self.discdds = discdds
plans = self.plans_generator.get_plans(discdds)
reducer = DiffeoStructure(discdds, tolerance=self.reduce_tolerance)
red_plans, _ = reducer.get_minimal_equiv_set(plans)
# add back the empty plan
red_plans.append(())
print('reduced %d plans to %d' % (len(plans), len(red_plans)))
self.plans = red_plans
self.actions = [discdds.plan2action(p) for p in self.plans]
self.actions_i = [a.inverse() for a in self.actions]
print self.plans
def set_discdds(self, discdds):
self.discdds = discdds
plans = self.plans_generator.get_plans(discdds)
reducer = DiffeoStructure(discdds, tolerance=self.reduce_tolerance)
red_plans, _ = reducer.get_minimal_equiv_set(plans)
# add back the empty plan
red_plans.append(())
print('reduced %d plans to %d' % (len(plans), len(red_plans)))
self.plans = red_plans
self.actions = [discdds.plan2action(p) for p in self.plans]
self.actions_i = [a.inverse() for a in self.actions]
print self.plans
def init(self, id_dds, dds):
self.ds = DiffeoStructure(dds=dds, **self.diffeo_structure_params)
self.original_dds = dds
dds_expanded = diffeosystem_expand(dds,
pr=self.ds.get_plan_reducer(),
heuristics=self.pre_expand)
# TODO: recompute structure
self.info('pre_expand: %s' % str(self.pre_expand))
self.info('Expanded to %s from %s actions' %
(len(dds_expanded.actions), len(dds.actions)))
super(InformedPlanner, self).init(id_dds, dds_expanded)
def __init__(self, id_discdds, diffeo_structure_threshold, id_algo,
plan_length, num_tests, get_planning_thresholds, plans):
config = get_current_config()
self.config = config
# Load objects from configuration manager
self.discdds = config.discdds.instance(id_discdds)
self.algo = init_algorithm(self.config, id_algo, id_discdds,
self.discdds)
self.cmdlist = [
self.discdds.actions[i].original_cmd
for i in range(len(self.discdds.actions))
]
# pdb.set_trace()
self.metric_goal = self.algo.metric_goal
# Save input arguments
self.id_discdds = id_discdds
self.id_algo = id_algo
self.diffeo_structure_threshold = diffeo_structure_threshold
self.plan_length = plan_length
self.num_tests = num_tests
# Load orbit camera module
self.orbit_module = OrbitModule(self.discdds.get_shape())
# Set get_planning_thresholds function
if get_planning_thresholds.__class__ == 'str':
try:
self.get_planning_thresholds = eval(get_planning_thresholds)
except:
logger.info()
else:
self.get_planning_thresholds = get_planning_thresholds
self.get_planning_thresholds_name = str(get_planning_thresholds)
# Initiate diffeo_structure
self.diffeo_struct = DiffeoStructure(self.discdds,
diffeo_structure_threshold)
if plans == 'random':
self.plans = self.gennerate_random_plans()
elif plans.__class__ in [list, tuple]:
self.plans = tuple(plans)
else:
self.plans = eval(plans)
logger.info('Initialized with plans: ' + str(self.plans))
def show_diffeo_structure(dds, report, tolerance):
ds = DiffeoStructure(dds, tolerance=tolerance)
with report.subsection('display') as r:
ds.display(r)
with report.subsection('show_reduction_steps') as r:
ds.show_reduction_steps(r, max_nsteps=5)
with report.subsection('show_reduction') as r:
ds.show_reduction(r)
def __init__(self, id_discdds, plan_length):
'''
'''
self.plan_time = 1
self.metric = DistanceNorm(2)
# config_path = 'default:/home/adam/diffeo-data/'
# config = DiffeoplanConfigMaster()
# config.load(config_path)
# set_current_config(config)
config = get_current_config()
self.discdds = config.discdds.instance(id_discdds)
# Initiate diffeo_structure
diffeo_structure_threshold = 0.2
self.diffeo_struct = DiffeoStructure(self.discdds,
diffeo_structure_threshold)
# pdb.set_trace()
n = len(self.discdds.actions)
self.all_plans = []
for i in range(1, plan_length + 1):
self.all_plans += list(itertools.product(range(n), repeat=i))
self.plan_reduced = []
for plan in self.all_plans:
canon_plan = self.diffeo_struct.get_canonical(plan)
if self.plan_reduced.count(
canon_plan) == 0 and len(canon_plan) >= 1:
self.plan_reduced.append(canon_plan)
logger.info('Total number of plans initially: %g ' %
len(self.all_plans))
logger.info('Number of plans after reduction: %g ' %
len(self.plan_reduced))
print('Total number of plans initially: %g ' % len(self.all_plans))
print('Number of plans after reduction: %g ' % len(self.plan_reduced))
self.generate_diffeo(self.plan_reduced)
self.D = np.matrix(self.composed_discdds.actions_distance_L2())
logger.info('Action Graph Composed')
def __init__(self, id_dds, dds, tolerance, collapse_threshold,
min_visibility, debug_it, max_it):
self.dds = dds
self.ds = DiffeoStructure(dds, tolerance)
collapse_metric = DiffeoActionL2iwNormalized(self.ds)
self.cover = DiffeoCoverExp(
id_dds=id_dds,
dds=self.dds,
plan_reducer=self.ds.get_plan_reducer(),
collapse_metric=collapse_metric,
# collapse_threshold=collapse_threshold,
collapse_threshold=0.001,
max_depth=3,
debug_iterations=debug_it,
max_iterations=max_it)
self.cover.set_min_visibility(min_visibility)
self.cover.go()
def show_diffeo_structure(dds, report, tolerance):
ds = DiffeoStructure(dds, tolerance=tolerance)
with report.subsection('display') as r:
ds.display(r)
with report.subsection('show_reduction_steps') as r:
ds.show_reduction_steps(r, max_nsteps=5)
with report.subsection('show_reduction') as r:
ds.show_reduction(r)
def report_dds_geometry(id_discdds, tolerance):
dds = get_conftools_discdds().instance(id_discdds)
r = Report('dds_geometry-%s-%s' % (id_discdds, tolerance))
ds = DiffeoStructure(dds, tolerance=tolerance)
with r.subsection('display') as r:
ds.display(r)
with r.subsection('show_reduction_steps') as r:
ds.show_reduction_steps(r, max_nsteps=5)
with r.subsection('show_reduction') as r:
ds.show_reduction(r)
return r
class ComposeGraph():
def __init__(self, id_discdds, plan_length):
'''
'''
self.plan_time = 1
self.metric = DistanceNorm(2)
# config_path = 'default:/home/adam/diffeo-data/'
# config = DiffeoplanConfigMaster()
# config.load(config_path)
# set_current_config(config)
config = get_current_config()
self.discdds = config.discdds.instance(id_discdds)
# Initiate diffeo_structure
diffeo_structure_threshold = 0.2
self.diffeo_struct = DiffeoStructure(self.discdds, diffeo_structure_threshold)
# pdb.set_trace()
n = len(self.discdds.actions)
self.all_plans = []
for i in range(1, plan_length + 1):
self.all_plans += list(itertools.product(range(n), repeat=i))
self.plan_reduced = []
for plan in self.all_plans:
canon_plan = self.diffeo_struct.get_canonical(plan)
if self.plan_reduced.count(canon_plan) == 0 and len(canon_plan) >= 1:
self.plan_reduced.append(canon_plan)
logger.info('Total number of plans initially: %g ' % len(self.all_plans))
logger.info('Number of plans after reduction: %g ' % len(self.plan_reduced))
print('Total number of plans initially: %g ' % len(self.all_plans))
print('Number of plans after reduction: %g ' % len(self.plan_reduced))
self.generate_diffeo(self.plan_reduced)
self.D = np.matrix(self.composed_discdds.actions_distance_L2())
logger.info('Action Graph Composed')
def generate_diffeo(self, plan_reduced):
self.composed_actions = []
for plan in plan_reduced:
logger.info('Diffeomorphism generated for plan: ' + str(plan))
self.composed_actions.append(self.discdds.plan2action(plan))
self.composed_discdds = DiffeoSystem('Composed diffeo system', self.composed_actions)
def init(self, id_dds, dds):
self.ds = DiffeoStructure(dds=dds, **self.diffeo_structure_params)
self.original_dds = dds
dds_expanded = diffeosystem_expand(dds, pr=self.ds.get_plan_reducer(),
heuristics=self.pre_expand)
# TODO: recompute structure
self.info('pre_expand: %s' % str(self.pre_expand))
self.info('Expanded to %s from %s actions' %
(len(dds_expanded.actions), len(dds.actions)))
super(InformedPlanner, self).init(id_dds, dds_expanded)
def __init__(self, id_dds, dds, tolerance, collapse_threshold,
min_visibility, debug_it, max_it):
self.dds = dds
self.ds = DiffeoStructure(dds, tolerance)
self.cover = DiffeoCover(id_dds,
self.dds,
self.ds,
collapse_threshold=collapse_threshold,
min_visibility=min_visibility,
debug_it=debug_it,
max_it=max_it)
self.cover.go()
def __init__(self, id_discdds,
diffeo_structure_threshold,
id_algo,
plan_length,
num_tests,
get_planning_thresholds,
plans):
config = get_current_config()
self.config = config
# Load objects from configuration manager
self.discdds = config.discdds.instance(id_discdds)
self.algo = init_algorithm(self.config, id_algo, id_discdds, self.discdds)
self.cmdlist = [self.discdds.actions[i].original_cmd for i in range(len(self.discdds.actions))]
# pdb.set_trace()
self.metric_goal = self.algo.metric_goal
# Save input arguments
self.id_discdds = id_discdds
self.id_algo = id_algo
self.diffeo_structure_threshold = diffeo_structure_threshold
self.plan_length = plan_length
self.num_tests = num_tests
# Load orbit camera module
self.orbit_module = OrbitModule(self.discdds.get_shape())
# Set get_planning_thresholds function
if get_planning_thresholds.__class__ == 'str':
try:
self.get_planning_thresholds = eval(get_planning_thresholds)
except:
logger.info()
else:
self.get_planning_thresholds = get_planning_thresholds
self.get_planning_thresholds_name = str(get_planning_thresholds)
# Initiate diffeo_structure
self.diffeo_struct = DiffeoStructure(self.discdds, diffeo_structure_threshold)
if plans == 'random':
self.plans = self.gennerate_random_plans()
elif plans.__class__ in [list, tuple]:
self.plans = tuple(plans)
else:
self.plans = eval(plans)
logger.info('Initialized with plans: ' + str(self.plans))
def report_dds_geometry(id_discdds, tolerance):
dds = get_conftools_discdds().instance(id_discdds)
r = Report('dds_geometry-%s-%s' % (id_discdds, tolerance))
ds = DiffeoStructure(dds, tolerance=tolerance)
with r.subsection('display') as r:
ds.display(r)
with r.subsection('show_reduction_steps') as r:
ds.show_reduction_steps(r, max_nsteps=5)
with r.subsection('show_reduction') as r:
ds.show_reduction(r)
return r
def __init__(self, id_dds, dds, tolerance,
collapse_threshold,
min_visibility, debug_it, max_it):
self.dds = dds
self.ds = DiffeoStructure(dds, tolerance)
collapse_metric = DiffeoActionL2iwNormalized(self.ds)
self.cover = DiffeoCoverExp(id_dds=id_dds,
dds=self.dds,
plan_reducer=self.ds.get_plan_reducer(),
collapse_metric=collapse_metric,
# collapse_threshold=collapse_threshold,
collapse_threshold=0.001,
max_depth=3,
debug_iterations=debug_it,
max_iterations=max_it)
self.cover.set_min_visibility(min_visibility)
self.cover.go()
class DiffeoSystemBounds2(object):
def __init__(self, id_dds, dds, tolerance, collapse_threshold,
min_visibility, debug_it, max_it):
self.dds = dds
self.ds = DiffeoStructure(dds, tolerance)
collapse_metric = DiffeoActionL2iwNormalized(self.ds)
self.cover = DiffeoCoverExp(
id_dds=id_dds,
dds=self.dds,
plan_reducer=self.ds.get_plan_reducer(),
collapse_metric=collapse_metric,
# collapse_threshold=collapse_threshold,
collapse_threshold=0.001,
max_depth=3,
debug_iterations=debug_it,
max_iterations=max_it)
self.cover.set_min_visibility(min_visibility)
self.cover.go()
#
# self.make_bases(self.dds, self.ds)
def make_bases(self, dds, ds):
n = len(dds.actions)
# # Create all combinations of n of all elements
# print('Creating all combinations of len %s' % n)
# bplans = plans_of_max_length(n, n)
# print('Created %d ' % len(bplans))
# minimal, mmap = ds.get_minimal_equiv_set(bplans)
# print('Minimal %d ' % len(minimal))
# # print minimal
# self.find_non_red_plans(nactions=n, length=3, threshold=0.05)
self.make_closure(nactions=n, length=3, threshold=0.05)
@dp_memoize_instance
def plan_distance_norm(self, plan1, plan2):
dn = self.cover.plan_distance(
plan1, plan2, diffeoaction_distance_L2_infow) / self.ds.scalew
return dn
def minimum_dist_to_set(self, plan, plans):
d = [self.plan_distance_norm(plan, p) for p in plans]
i = np.argmin(d)
return plans[i], min(d)
def plan_inverse(self, plan):
l = [
self.ds.plan_reducer.action_get_inverse(a) for a in reversed(plan)
]
return tuple(l)
def make_commutator(self, plan1, plan2):
com = plan1 + plan2 + self.plan_inverse(plan1) + self.plan_inverse(
plan2)
com = self.ds.plan_reducer.get_canonical(com)
return com
def make_closure(self, nactions, length, threshold):
if length == 1:
return [(a, ) for a in range(nactions)] + [()]
prev = self.make_closure(nactions, length - 1, threshold)
generated = []
for prev1, prev2 in itertools.product(prev, prev):
if prev1 == prev2:
continue
com = self.make_commutator(prev1, prev2)
# print('[%s, %s] -> %s ' % (prev1, prev2, com))
_, md = self.minimum_dist_to_set(com, prev + generated)
# print('%s md %s to %s' % (com, md, closest))
if md < threshold:
# print('%s matches %s' % (com, closest))
continue
else:
generated.append(com)
print('Length %d generated %d:' % (length, len(generated)))
for _, g in enumerate(generated):
print(' - %s' % str(g))
return generated + prev
def find_non_red_plans(self, nactions, length, threshold):
if length == 0:
return [()]
prev = self.find_non_red_plans(nactions, length - 1, threshold)
cur = []
cur.extend(prev)
for p0 in prev:
if len(p0) != length - 1:
# only build in those from the previous level
continue
for action in range(nactions):
if action in p0:
# no repeated actions
continue
p1 = p0 + (action, )
closest, md = self.minimum_dist_to_set(p1, cur)
print('%s md %s to %s' % (p1, md, closest))
if md < threshold:
print('%s matches %s' % (p1, closest))
continue
else:
cur.append(p1)
print('Of length %d, found: %s' % (length, len(cur)))
return cur
def display(self, report): # @UnusedVariable
with report.subsection('draw_graph') as r:
self.cover.draw_graph(r)
def display_products(self, report, nsteps):
for a in self.dds_hard.actions:
f = report.figure(a.label, cols=nsteps)
A = a
for k in range(nsteps):
A = DiffeoAction.compose(A, a)
rgb = A.get_diffeo2d_forward().get_rgb_info()
f.data_rgb('%s_%s' % (a.label, k), rgb)
class DiffeoSystemBounds2(object):
def __init__(self, id_dds, dds, tolerance,
collapse_threshold,
min_visibility, debug_it, max_it):
self.dds = dds
self.ds = DiffeoStructure(dds, tolerance)
collapse_metric = DiffeoActionL2iwNormalized(self.ds)
self.cover = DiffeoCoverExp(id_dds=id_dds,
dds=self.dds,
plan_reducer=self.ds.get_plan_reducer(),
collapse_metric=collapse_metric,
# collapse_threshold=collapse_threshold,
collapse_threshold=0.001,
max_depth=3,
debug_iterations=debug_it,
max_iterations=max_it)
self.cover.set_min_visibility(min_visibility)
self.cover.go()
#
# self.make_bases(self.dds, self.ds)
def make_bases(self, dds, ds):
n = len(dds.actions)
# # Create all combinations of n of all elements
# print('Creating all combinations of len %s' % n)
# bplans = plans_of_max_length(n, n)
# print('Created %d ' % len(bplans))
# minimal, mmap = ds.get_minimal_equiv_set(bplans)
# print('Minimal %d ' % len(minimal))
# # print minimal
# self.find_non_red_plans(nactions=n, length=3, threshold=0.05)
self.make_closure(nactions=n, length=3, threshold=0.05)
@dp_memoize_instance
def plan_distance_norm(self, plan1, plan2):
dn = self.cover.plan_distance(plan1, plan2, diffeoaction_distance_L2_infow) / self.ds.scalew
return dn
def minimum_dist_to_set(self, plan, plans):
d = [self.plan_distance_norm(plan, p) for p in plans]
i = np.argmin(d)
return plans[i], min(d)
def plan_inverse(self, plan):
l = [self.ds.plan_reducer.action_get_inverse(a) for a in reversed(plan)]
return tuple(l)
def make_commutator(self, plan1, plan2):
com = plan1 + plan2 + self.plan_inverse(plan1) + self.plan_inverse(plan2)
com = self.ds.plan_reducer.get_canonical(com)
return com
def make_closure(self, nactions, length, threshold):
if length == 1:
return [(a,) for a in range(nactions)] + [()]
prev = self.make_closure(nactions, length - 1, threshold)
generated = []
for prev1, prev2 in itertools.product(prev, prev):
if prev1 == prev2:
continue
com = self.make_commutator(prev1, prev2)
# print('[%s, %s] -> %s ' % (prev1, prev2, com))
_, md = self.minimum_dist_to_set(com, prev + generated)
# print('%s md %s to %s' % (com, md, closest))
if md < threshold:
# print('%s matches %s' % (com, closest))
continue
else:
generated.append(com)
print('Length %d generated %d:' % (length, len(generated)))
for _, g in enumerate(generated):
print(' - %s' % str(g))
return generated + prev
def find_non_red_plans(self, nactions, length, threshold):
if length == 0:
return [()]
prev = self.find_non_red_plans(nactions, length - 1, threshold)
cur = []
cur.extend(prev)
for p0 in prev:
if len(p0) != length - 1:
# only build in those from the previous level
continue
for action in range(nactions):
if action in p0:
# no repeated actions
continue
p1 = p0 + (action,)
closest, md = self.minimum_dist_to_set(p1, cur)
print('%s md %s to %s' % (p1, md, closest))
if md < threshold:
print('%s matches %s' % (p1, closest))
continue
else:
cur.append(p1)
print('Of length %d, found: %s' % (length, len(cur)))
return cur
def display(self, report): # @UnusedVariable
with report.subsection('draw_graph') as r:
self.cover.draw_graph(r)
def display_products(self, report, nsteps):
for a in self.dds_hard.actions:
f = report.figure(a.label, cols=nsteps)
A = a
for k in range(nsteps):
A = DiffeoAction.compose(A, a)
rgb = A.get_diffeo2d_forward().get_rgb_info()
f.data_rgb('%s_%s' % (a.label, k), rgb)
class OnlinePlanning():
def __init__(self, id_discdds,
diffeo_structure_threshold,
id_algo,
plan_length,
num_tests,
get_planning_thresholds,
plans):
config = get_current_config()
self.config = config
# Load objects from configuration manager
self.discdds = config.discdds.instance(id_discdds)
self.algo = init_algorithm(self.config, id_algo, id_discdds, self.discdds)
self.cmdlist = [self.discdds.actions[i].original_cmd for i in range(len(self.discdds.actions))]
# pdb.set_trace()
self.metric_goal = self.algo.metric_goal
# Save input arguments
self.id_discdds = id_discdds
self.id_algo = id_algo
self.diffeo_structure_threshold = diffeo_structure_threshold
self.plan_length = plan_length
self.num_tests = num_tests
# Load orbit camera module
self.orbit_module = OrbitModule(self.discdds.get_shape())
# Set get_planning_thresholds function
if get_planning_thresholds.__class__ == 'str':
try:
self.get_planning_thresholds = eval(get_planning_thresholds)
except:
logger.info()
else:
self.get_planning_thresholds = get_planning_thresholds
self.get_planning_thresholds_name = str(get_planning_thresholds)
# Initiate diffeo_structure
self.diffeo_struct = DiffeoStructure(self.discdds, diffeo_structure_threshold)
if plans == 'random':
self.plans = self.gennerate_random_plans()
elif plans.__class__ in [list, tuple]:
self.plans = tuple(plans)
else:
self.plans = eval(plans)
logger.info('Initialized with plans: ' + str(self.plans))
def gennerate_random_plans(self):
n_cmd = len(self.discdds.actions)
plans = []
for _ in range(self.num_tests):
plan = ()
ie = 0
while len(plan) < int(self.plan_length):
# Add new command
plan += (np.random.randint(0, n_cmd),)
# remove redundant command
plan = self.diffeo_struct.get_canonical(plan)
ie += 1
if ie > 50:
print('WARNING, a lot of iterations to generate a non redundant plan.')
plans.append(plan)
break
plans.append(plan)
return plans
def run_all_tests(self, env='default'):
all_stats = []
for plan in self.plans:
stat = self.run_test(plan, env)
all_stats.append(stat)
return all_stats
def run_test(self, plan, env='default'):
# Initiate stats object
labels = {}
labels['id_discdds'] = self.id_discdds
labels['diffeo_structure_threshold'] = self.diffeo_structure_threshold
labels['id_algo'] = self.id_algo
labels['plan_length'] = self.plan_length
labels['get_planning_thresholds'] = self.get_planning_thresholds_name
labels['env'] = env
active_instance = OnlineStats(labels, self.metric_goal)
# Setup the planning problem
self.create_testcase(plan_true=plan,
active_instance=active_instance)
# Run the planning problem
self.run_planning(active_instance=active_instance)
self.prediction_images(active_instance)
# return info about test
return active_instance
def create_testcase(self, plan_true, active_instance):
# The goal image is where we start the demo
# self.y_goal = self.orbit_module.get_image()
active_instance.y_goal = self.orbit_module.get_image()
active_instance.labels['plan_true'] = plan_true
active_instance.labels['plan_true_reduced'] = self.diffeo_struct.get_canonical(plan_true)
active_instance.plan_true = plan_true
active_instance.plan_true_reduced = self.diffeo_struct.get_canonical(plan_true)
# Move the camera to the start position
plan_inverse = self.orbit_module.inverse_plan(plan_true)
self.orbit_module.execute_plan(plan_inverse)
# self.y_start = self.orbit_module.get_image()
def run_planning(self, active_instance):
# Capture the initial image
active_instance.y_start = self.orbit_module.get_image()
# pdb.set_trace()
if not self.get_planning_thresholds.__class__ in [list, tuple]:
precision, min_visibility = self.get_planning_thresholds(algo=self.algo,
discdds=self.discdds,
active_instance=active_instance)
else:
precision, min_visibility = self.get_planning_thresholds
active_instance.labels['precision'] = precision
active_instance.labels['min_visibility'] = min_visibility
active_instance.precision = precision
active_instance.min_visibility = min_visibility
planning_result = self.algo.plan(active_instance.y_start, active_instance.y_goal, precision=precision,
min_visibility=min_visibility)
if not planning_result.success:
return None
logger.info('Plan found: ' + str(planning_result.plan))
plan_found = self.orbit_module.inverse_plan(planning_result.plan)
self.orbit_module.execute_plan(plan_found)
# Update active_instance with results
active_instance.plan_found = plan_found
active_instance.plan_found_reduced = self.diffeo_struct.get_canonical(plan_found)
active_instance.y_result = self.orbit_module.get_image()
active_instance.labels['plan_found'] = plan_found
active_instance.labels['plan_found_reduced'] = self.diffeo_struct.get_canonical(plan_found)
def prediction_images(self, active_instance):
'''
predicts the resulting images from active_instance.y_start and plan
:param active_instance:
'''
if active_instance.plan_found is not None:
inv_plan = self.orbit_module.inverse_plan(active_instance.plan_found)
active_instance.y_found_pred = self.discdds.predict(active_instance.y_start, inv_plan)
if active_instance.plan_true is not None:
inv_plan = self.orbit_module.inverse_plan(active_instance.plan_true)
active_instance.y_goal_pred = self.discdds.predict(active_instance.y_start, inv_plan)
class OnlinePlanning():
def __init__(self, id_discdds, diffeo_structure_threshold, id_algo,
plan_length, num_tests, get_planning_thresholds, plans):
config = get_current_config()
self.config = config
# Load objects from configuration manager
self.discdds = config.discdds.instance(id_discdds)
self.algo = init_algorithm(self.config, id_algo, id_discdds,
self.discdds)
self.cmdlist = [
self.discdds.actions[i].original_cmd
for i in range(len(self.discdds.actions))
]
# pdb.set_trace()
self.metric_goal = self.algo.metric_goal
# Save input arguments
self.id_discdds = id_discdds
self.id_algo = id_algo
self.diffeo_structure_threshold = diffeo_structure_threshold
self.plan_length = plan_length
self.num_tests = num_tests
# Load orbit camera module
self.orbit_module = OrbitModule(self.discdds.get_shape())
# Set get_planning_thresholds function
if get_planning_thresholds.__class__ == 'str':
try:
self.get_planning_thresholds = eval(get_planning_thresholds)
except:
logger.info()
else:
self.get_planning_thresholds = get_planning_thresholds
self.get_planning_thresholds_name = str(get_planning_thresholds)
# Initiate diffeo_structure
self.diffeo_struct = DiffeoStructure(self.discdds,
diffeo_structure_threshold)
if plans == 'random':
self.plans = self.gennerate_random_plans()
elif plans.__class__ in [list, tuple]:
self.plans = tuple(plans)
else:
self.plans = eval(plans)
logger.info('Initialized with plans: ' + str(self.plans))
def gennerate_random_plans(self):
n_cmd = len(self.discdds.actions)
plans = []
for _ in range(self.num_tests):
plan = ()
ie = 0
while len(plan) < int(self.plan_length):
# Add new command
plan += (np.random.randint(0, n_cmd), )
# remove redundant command
plan = self.diffeo_struct.get_canonical(plan)
ie += 1
if ie > 50:
print(
'WARNING, a lot of iterations to generate a non redundant plan.'
)
plans.append(plan)
break
plans.append(plan)
return plans
def run_all_tests(self, env='default'):
all_stats = []
for plan in self.plans:
stat = self.run_test(plan, env)
all_stats.append(stat)
return all_stats
def run_test(self, plan, env='default'):
# Initiate stats object
labels = {}
labels['id_discdds'] = self.id_discdds
labels['diffeo_structure_threshold'] = self.diffeo_structure_threshold
labels['id_algo'] = self.id_algo
labels['plan_length'] = self.plan_length
labels['get_planning_thresholds'] = self.get_planning_thresholds_name
labels['env'] = env
active_instance = OnlineStats(labels, self.metric_goal)
# Setup the planning problem
self.create_testcase(plan_true=plan, active_instance=active_instance)
# Run the planning problem
self.run_planning(active_instance=active_instance)
self.prediction_images(active_instance)
# return info about test
return active_instance
def create_testcase(self, plan_true, active_instance):
# The goal image is where we start the demo
# self.y_goal = self.orbit_module.get_image()
active_instance.y_goal = self.orbit_module.get_image()
active_instance.labels['plan_true'] = plan_true
active_instance.labels[
'plan_true_reduced'] = self.diffeo_struct.get_canonical(plan_true)
active_instance.plan_true = plan_true
active_instance.plan_true_reduced = self.diffeo_struct.get_canonical(
plan_true)
# Move the camera to the start position
plan_inverse = self.orbit_module.inverse_plan(plan_true)
self.orbit_module.execute_plan(plan_inverse)
# self.y_start = self.orbit_module.get_image()
def run_planning(self, active_instance):
# Capture the initial image
active_instance.y_start = self.orbit_module.get_image()
# pdb.set_trace()
if not self.get_planning_thresholds.__class__ in [list, tuple]:
precision, min_visibility = self.get_planning_thresholds(
algo=self.algo,
discdds=self.discdds,
active_instance=active_instance)
else:
precision, min_visibility = self.get_planning_thresholds
active_instance.labels['precision'] = precision
active_instance.labels['min_visibility'] = min_visibility
active_instance.precision = precision
active_instance.min_visibility = min_visibility
planning_result = self.algo.plan(active_instance.y_start,
active_instance.y_goal,
precision=precision,
min_visibility=min_visibility)
if not planning_result.success:
return None
logger.info('Plan found: ' + str(planning_result.plan))
plan_found = self.orbit_module.inverse_plan(planning_result.plan)
self.orbit_module.execute_plan(plan_found)
# Update active_instance with results
active_instance.plan_found = plan_found
active_instance.plan_found_reduced = self.diffeo_struct.get_canonical(
plan_found)
active_instance.y_result = self.orbit_module.get_image()
active_instance.labels['plan_found'] = plan_found
active_instance.labels[
'plan_found_reduced'] = self.diffeo_struct.get_canonical(
plan_found)
def prediction_images(self, active_instance):
'''
predicts the resulting images from active_instance.y_start and plan
:param active_instance:
'''
if active_instance.plan_found is not None:
inv_plan = self.orbit_module.inverse_plan(
active_instance.plan_found)
active_instance.y_found_pred = self.discdds.predict(
active_instance.y_start, inv_plan)
if active_instance.plan_true is not None:
inv_plan = self.orbit_module.inverse_plan(
active_instance.plan_true)
active_instance.y_goal_pred = self.discdds.predict(
active_instance.y_start, inv_plan)
