def draw(self, env, color=(1, 0, 0, .5)): self._handle = draw_node(env, self.q, color=color)
def test5(oracle):
t0 = time()
robot = oracle.robot
cspace = CSpace.robot_arm_and_base(robot.GetActiveManipulator())
cspace.set_active()
start = Config(robot.GetActiveDOFValues())
# Cache
convert_full_config = lambda q: Config(np.concatenate([q.value[get_arm_indices(oracle)], base_values_from_full_config(q.value)]))
# Start and goal placements
NUM_GENERAL_POSES = 5
NUM_GRAPS_PER_POSE = 4
for geom_hash in set(oracle.get_geom_hash(object_name) for object_name in oracle.objects):
object_name = oracle.get_body_name(geom_hash)
grasps = get_grasps(oracle, object_name)
if len(grasps) == 0: continue
with oracle.state_saver():
oracle.set_all_object_poses({object_name: oracle.get_pose(object_name)})
num_poses = count(0)
for pose in random_region_placements(oracle, object_name, oracle.tables, region_weights=True):
num_grasps = count(0)
for grasp in randomize(grasps):
pap = PickAndPlace(geom_hash, pose, grasp)
if pap.sample(oracle, object_name, max_failures=PAP_MAX_FAILURES, sample_vector=True, sample_arm=False):
oracle.add_pap(object_name, pap)
if next(num_grasps) >= NUM_GRAPS_PER_POSE: break
else: continue
if next(num_poses) >= NUM_GENERAL_POSES: break
table_paps = defaultdict(list)
for pap in flatten(oracle.pick_and_places.values()):
for table in oracle.tables:
if oracle.region_contains(table, oracle.get_body_name(pap.geom_hash), pap.pose):
table_paps[table].append(pap)
break
distance = q_distance_fn(robot) # TODO - use workspace distance to connect
#sample = q_sample_fn(robot)
extend = q_extend_fn(robot)
collision = q_collision_fn(oracle.env, robot)
target_degree = 4
roadmaps = []
with oracle.state_saver():
oracle.env.Lock()
oracle.set_all_object_poses()
for table in oracle.tables:
#print pap.trajs[-1].end()
grasp_configs = [pap.grasp_config for pap in table_paps[table]]
approach_configs = [pap.approach_config for pap in table_paps[table]]
vector_configs = []
for pap in table_paps[table]:
vector_config = pap.grasp_config.value.copy()
vector_config[get_arm_indices(oracle)] = pap.trajs[-1].end()
vector_configs.append(Config(vector_config))
samples = map(convert_full_config, grasp_configs + approach_configs + vector_configs)
#samples = map(convert_full_config, list(flatten([pap.grasp_config, pap.approach_config] for pap in table_paps[table])))
roadmaps.append(DegreePRM(distance, extend, collision, samples=samples, target_degree=target_degree))
oracle.env.Unlock()
roadmap = roadmaps[0]
#roadmap.merge(*roadmaps[1:])
print time() - t0
#roadmap.draw(oracle.env)
path = roadmap(list(roadmap.vertices)[0], list(roadmap.vertices)[-1])
for config in path:
config._handle = draw_node(oracle.env, config)
for config in path:
robot.SetActiveDOFValues(config.value)
raw_input('Next')
def draw(self, env, color=(1, 0, 0, .5)):
self.node_handle = draw_node(env, self.config, color=color)
if self.parent is not None:
self.edge_handle = draw_edge(env, self.config, self.parent.config, color=color)
