def create_custom_ir(robot, manip_base_iterator, n=IR_DATABASE_SAMPLES): if DEBUG: print 'Creating inverse reachability database' ir_database = [] for manip_trans, base_trans in take(manip_base_iterator, n): manip_trans2D = trans_from_base_values([manip_trans[0, 3], manip_trans[1, 3], get_manip_angle(robot, manip_trans)]) base_trans2D = trans2D_from_trans(base_trans) ir_database.append(base_values_from_trans(np.linalg.solve(manip_trans2D, base_trans2D))) # M * T = B return ir_database
def pap_ir_samples(env, max_failures=100, max_attempts=INF): # NOTE - max_failures should be large to prevent just easy placements from manipulation.inverse_reachability.inverse_reachability import openrave_base_iterator, create_custom_ir from manipulation.pick_and_place import PickAndPlace oracle = pap_ir_oracle(env) body_name = oracle.objects[0] table_name = oracle.tables[0] for pose in take(random_region_placements(oracle, body_name, [table_name]), max_attempts): grasp = choice(get_grasps(oracle, body_name)) pap = PickAndPlace(None, pose, grasp) if pap.sample(oracle, body_name, base_iterator_fn=openrave_base_iterator, max_failures=max_failures, check_base=False): yield pap.manip_trans, pap.base_trans
def grow(self, goal, iterations=50, store=ts.PATH, max_tree_size=500): if goal in self: self[goal].retrace() if self.collision(goal): return None nodes1, new_nodes1 = list(take(randomize(self.nodes.values()), max_tree_size)), [] nodes2, new_nodes2 = [], [TreeNode(goal)] for _ in irange(iterations): if len(nodes1) + len(new_nodes1) > len(nodes2) + len(new_nodes2): nodes1, nodes2 = nodes2, nodes1 new_nodes1, new_nodes2 = new_nodes2, new_nodes1 s = self.sample() last1 = argmin(lambda n: self.distance(n.config, s), nodes1 + new_nodes1) for q in self.extend(last1.config, s): if self.collision(q): break last1 = TreeNode(q, parent=last1) new_nodes1.append(last1) last2 = argmin(lambda n: self.distance(n.config, last1.config), nodes2 + new_nodes2) for q in self.extend(last2.config, last1.config): if self.collision(q): break last2 = TreeNode(q, parent=last2) new_nodes2.append(last2) else: if len(nodes1) == 0: nodes1, nodes2 = nodes2, nodes1 new_nodes1, new_nodes2 = new_nodes2, new_nodes1 last1, last2 = last2, last1 path1, path2 = last1.retrace(), last2.retrace()[:-1][::-1] for p, n in pairs(path2): n.parent = p if len(path2) == 0: # TODO - still some kind of circular error for n in new_nodes2: if n.parent == last2: n.parent = path1[-1] else: path2[0].parent = path1[-1] path = path1 + path2 if store in [ts.ALL, ts.SUCCESS]: self.add(*(new_nodes1 + new_nodes2[:-1])) elif store == ts.PATH: new_nodes_set = set(new_nodes1 + new_nodes2[:-1]) self.add(*[n for n in path if n in new_nodes_set]) return path if store == ts.ALL: self.add(*new_nodes1) return None
def pap_ir_statistics(env, trials=100): from manipulation.inverse_reachability.inverse_reachability import display_custom_ir from manipulation.pick_and_place import PickAndPlace oracle = pap_ir_oracle(env) body_name = oracle.objects[0] table_name = oracle.tables[0] successes = [] for pose in take(random_region_placements(oracle, body_name, [table_name]), trials): grasp = choice(get_grasps(oracle, body_name)) pap = PickAndPlace(None, pose, grasp) oracle.set_pose(body_name, pap.pose) #if pap.sample(oracle, body_name, max_failures=50, base_iterator_fn=openrave_base_iterator, check_base=False): if pap.sample(oracle, body_name, max_failures=50, check_base=False): oracle.set_robot_config(pap.grasp_config) successes.append(int(pap.iterations)) handles = display_custom_ir(oracle, pap.manip_trans) raw_input('Continue?') return float(len(successes))/trials, np.mean(successes), np.std(successes)
def srivastava_table(env, n=INF): env.Load(ENVIRONMENTS_DIR + '../srivastava/good_cluttered.dae') set_default_robot_config(env.GetRobots()[0]) body_names = [get_name(body) for body in env.GetBodies() if not body.IsRobot()] table_names = [body_name for body_name in body_names if 'table' in body_name] dx = .5 for body_name in body_names: body = env.GetKinBody(body_name) set_point(body, get_point(body) + np.array([dx, 0, 0])) objects = [env.GetKinBody(body_name) for body_name in body_names if body_name not in table_names] for obj in objects: env.Remove(obj) object_names = [] for obj in take(objects, n): randomly_place_body(env, obj, table_names) object_names.append(get_name(obj)) goal_holding = 'object1' goal_config = 'initial' # None return ManipulationProblem(None, object_names=object_names, table_names=table_names, goal_config=goal_config, goal_holding=goal_holding)
def grow(self, goal_sample, iterations=50, goal_probability=.2, store=ts.PATH, max_tree_size=500): if not callable(goal_sample): goal_sample = lambda: goal_sample nodes, new_nodes = list(take(randomize(self.nodes.values()), max_tree_size)), [] for i in irange(iterations): goal = random() < goal_probability or i == 0 s = goal_sample() if goal else self.sample() last = argmin(lambda n: self.distance(n.config, s), nodes + new_nodes) for q in self.extend(last.config, s): if self.collision(q): break last = TreeNode(q, parent=last) new_nodes.append(last) else: if goal: path = last.retrace() if store in [ts.ALL, ts.SUCCESS]: self.add(*new_nodes) elif store == ts.PATH: new_nodes_set = set(new_nodes) self.add(*[n for n in path if n in new_nodes_set]) return path if store == ts.ALL: self.add(*new_nodes) return None