Ejemplo n.º 1
0
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):
Ejemplo n.º 2
0
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}