from collections import Mapping
from random import random
from manipulation.motion.rrt import TreeNode, configs
from tools.functions import irange, argmin, pairs, randomize
from tools.objects import enum
from tools.numerical import INF
from tools.generators import take
ts = enum('ALL', 'SUCCESS', 'PATH', 'NONE')
# TODO - resample and use nearest neighbors when the tree is large
# TODO - possible weirdness if a node is already in the tree
class MultiTree(Mapping, object):
def __init__(self, start, distance, sample, extend, collision):
self.nodes = {}
self.distance = distance
self.sample = sample
self.extend = extend
self.collision = collision
self.add(TreeNode(start))
def add(self, *nodes):
for n in nodes: self.nodes[n.config] = n
def __getitem__(self, q):
return self.nodes[q]
# return first(lambda v: self.distance(v.config, q) < 1e-6, self.nodes)
def __len__(self):
return len(self.nodes)
def __iter__(self):
for n in self.nodes.values(): yield n
def __call__(self, q1, q2=None, iterations=50):
from tools.objects import enum
# TODO
# - Use cost information
# - Only compute expensive actions when you are sure the are reachable
# - Don't actually compute the grasps until you need them
# - If at a config, pick/place using the current thing
# - Add more edges if things aren't reachable or something
# - One reachable, assign relative frequencies to generate new samples
PRINT = False
CONFIGURATIONS = enum('FIXED_BRANCHING', 'DYNAMIC')
USE_CONFIGURATION = CONFIGURATIONS.FIXED_BRANCHING
if USE_CONFIGURATION == CONFIGURATIONS.DYNAMIC:
REACHABLE_TERMINATE = True
elif USE_CONFIGURATION == CONFIGURATIONS.FIXED_BRANCHING:
REACHABLE_TERMINATE = False
def state_obstacles(obj_name, state, oracle):
return {obst_name: state[obst_name] for obst_name in oracle.objects \
if obj_name != obst_name and state[obst_name] is not False}