def sample_unlabeled_data(n_samples, block_set=None, range_n_blocks=(2, 5)): """ Generate n_samples random towers. For now each sample can also have random blocks. We should change this later so that the blocks are fixed (i.e., chosen elsewhere) and we only sample the configuration. :param n_samples: Number of random towers to consider. :param block_set (optional): blocks to use in towers. generate new blocks if None :return: Dict containining numpy arrays of the towers sorted by size. """ # initialize a dictionary of lists to store the generated data sampled_towers = {} for i in range(range_n_blocks[0], range_n_blocks[1] + 1): k = f'{i}block' sampled_towers[k] = {} sampled_towers[k]['towers'] = [] sampled_towers[k]['labels'] = [] if block_set is not None: sampled_towers[k]['block_ids'] = [] min_n_blocks = range_n_blocks[0] max_n_blocks = min(range_n_blocks[1], len(block_set)) # sample random towers and add them to the lists in the dictionary for ix in range(n_samples): n_blocks = np.random.randint(min_n_blocks, max_n_blocks + 1) # get n_blocks, either from scratch or from the block set if block_set is not None: blocks = np.random.choice(block_set, n_blocks, replace=False) else: blocks = [Object.random(f'obj_{ix}') for ix in range(n_blocks)] # sample a new tower tower = sample_random_tower(blocks) rotated_tower = [get_rotated_block(b) for b in tower] # and save that tower in the sampled_towers dict sampled_towers['%dblock' % n_blocks]['towers'].append( vectorize(rotated_tower)) if block_set is not None: block_ids = [block.get_id() for block in rotated_tower] sampled_towers['%dblock' % n_blocks]['block_ids'].append(block_ids) # convert all the sampled towers to numpy arrays for k in sampled_towers.keys(): sampled_towers[k]['towers'] = np.array(sampled_towers[k]['towers']) sampled_towers[k]['labels'] = np.zeros( (sampled_towers[k]['towers'].shape[0], )) if block_set is not None: sampled_towers[k]['block_ids'] = np.array( sampled_towers[k]['block_ids']) return sampled_towers
def check_regrasp_position(agent, blocks, base_xy): block = [blocks[0]] #np.random.choice(blocks, 1, replace=False) tower = sample_random_tower(block) # while tower[0].pose.pos.z < 0.04: # tower = sample_random_tower(block) # Simulate this tower at tower_pose. base_block = agent.pddl_block_lookup[tower[0].name] base_pos = (base_xy[0], base_xy[1], tower[0].pose.pos.z) base_pose = (base_pos, tower[0].rotation) base_pose = pb_robot.vobj.BodyPose(base_block, base_pose) agent.teleport_block(base_block, base_pose.pose) placed_tform = pb_robot.geometry.tform_from_pose(base_pose.pose) count_grasp_solutions(agent, base_block, base_pose) input('Wait')
def main(args): NOISE=0.00005 # get a bunch of random blocks # if args.use_vision: if True: blocks = load_blocks(fname=args.blocks_file, num_blocks=args.num_blocks) else: blocks = get_adversarial_blocks(num_blocks=args.num_blocks) agent = PandaAgent(blocks, NOISE, use_platform=False, teleport=False, use_action_server=args.use_action_server, use_vision=args.use_vision, real=args.real) if args.show_frames: agent.step_simulation(T=1, vis_frames=True, lifeTime=0.) input('Start building?') p.removeAllUserDebugItems() for tx in range(0, args.num_towers): # Build a random tower out of blocks. n_blocks = np.random.randint(2, args.num_blocks + 1) tower_blocks = np.random.choice(blocks, n_blocks, replace=False) tower = sample_random_tower(tower_blocks) #tower = build_tower(tower_blocks, constructable=True, max_attempts=50000) # and execute the resulting plan. print(f"Starting tower {tx}") if args.use_action_server: agent.simulate_tower_parallel(tower, real=args.real, base_xy=(0.5, -0.3), vis=True, T=2500) else: success, stable = agent.simulate_tower(tower, real=args.real, base_xy=(0.5, -0.3), vis=True, T=2500, save_tower=args.save_tower) if not success: print('Planner failed.') print(f"Finished tower {tx}")
def generate_candidate_towers(self, blocks, args, num_blocks=None, n_tower=None): if num_blocks is None: num_blocks = len(blocks) #tower_vectors, tower_block_ids = self.get_cached_towers(args, num_blocks, blocks, n_tower) tower_vectors = None if tower_vectors is None: tower_vectors = [] tower_block_ids = [] for _ in range(0, self.n_samples[num_blocks]): tower, rotated_tower = sample_random_tower(blocks, num_blocks=num_blocks, \ ret_rotated=True, discrete=args.discrete) tower_vectors.append([b.vectorize() for b in rotated_tower]) tower_block_ids.append([b.get_id() for b in rotated_tower]) #self.cache_towers(args, tower_vectors, tower_block_ids, n_tower) return tower_vectors, tower_block_ids
def check_tower_position(agent, blocks, base_xy): # Build a random tower of blocks. n_blocks = 5 tower_blocks = np.random.choice(blocks, n_blocks, replace=False) tower = sample_random_tower(tower_blocks) # Simulate this tower at tower_pose. base_block = agent.pddl_block_lookup[tower[0].name] base_pos = (base_xy[0], base_xy[1], tower[0].pose.pos.z) base_pose = (base_pos, tower[0].rotation) base_pose = pb_robot.vobj.BodyPose(base_block, base_pose) agent.teleport_block(base_block, base_pose.pose) placed_tform = pb_robot.geometry.tform_from_pose(base_pose.pose) count_grasp_solutions(agent, base_block, base_pose) input('Wait') # Now loop through the other tower blocks for b_ix in range(1, len(tower)): bottom_block = tower[b_ix - 1] bottom_pose = (bottom_block.pose.pos, bottom_block.rotation) bottom_tform = pb_robot.geometry.tform_from_pose(bottom_pose) top_block = tower[b_ix] top_pose = (top_block.pose.pos, top_block.rotation) top_tform = pb_robot.geometry.tform_from_pose(top_pose) rel_tform = np.linalg.inv(bottom_tform) @ top_tform top_pddl = agent.pddl_block_lookup[top_block.name] bottom_pddl = agent.pddl_block_lookup[bottom_block.name] placed_tform = placed_tform @ rel_tform placed_pose = pb_robot.geometry.pose_from_tform(placed_tform) agent.teleport_block(top_pddl, placed_pose) pose = pb_robot.vobj.BodyPose(top_pddl, placed_pose) count_grasp_solutions(agent, top_pddl, pose) input('Wait')
from block_utils import Object from learning.domains.towers.generate_tower_training_data import sample_random_tower from learning.domains.towers.tower_data import TowerDataset, TowerSampler from tower_planner import TowerPlanner import pickle import copy if __name__ == '__main__': with open('learning/domains/towers/eval_block_set_12.pkl', 'rb') as handle: blocks = pickle.load(handle) tp = TowerPlanner(stability_mode='contains') towers = [] n_stable = 0 for _ in range(0, 10000): copy_blocks = copy.deepcopy(blocks) tower, rotated_tower = sample_random_tower(copy_blocks, num_blocks=5, \ ret_rotated=True, discrete=False) stable = tp.tower_is_constructable(rotated_tower) n_stable += stable print(f"n_stable: {n_stable}")