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}")