def update(self, observation):
# observation is a tuple (action, rot, timesteps, pose)
# resample the distribution
self.particles = sample_and_wiggle(self.particles, self.experience[-5:],
self.OBS_MODEL_COV, self.block, self.com_ranges)
self.experience.append(observation)
action, T, end_pose = observation
particle_blocks = [deepcopy(self.block) for particle in self.particles.particles]
for (com, particle_block) in zip(self.particles.particles, particle_blocks):
particle_block.com = com
particle_worlds = [make_platform_world(pb, action) for pb in particle_blocks]
env = Environment(particle_worlds, vis_sim=self.vis_sim)
for _ in range(T):
env.step(action=action)
# update all particle weights
new_weights = []
for pi, (particle_world, old_weight) in enumerate(zip(particle_worlds, self.particles.weights)):
particle_end_pose = particle_world.get_pose(particle_world.objects[1])
obs_model = multivariate_normal.pdf(end_pose.pos,
mean=particle_end_pose.pos,
cov=self.OBS_MODEL_COV)
new_weight = old_weight * obs_model
new_weights.append(new_weight)
# normalize particle weights
new_weights = np.array(new_weights)/np.sum(new_weights)
# and update the particle distribution with the new weights
self.particles = ParticleDistribution(self.particles.particles, new_weights)
if self.plot:
# visualize particles (it's very slow)
self.setup_ax(self.ax, self.block)
self.plot_particles(self.ax, self.particles.particles, new_weights)
mean = np.array(self.particles.particles)[email protected](self.particles.weights)
self.estimated_coms.append(mean)
env.disconnect()
env.cleanup()
return self.particles, self.estimated_coms
def simulate_tower(tower, vis=True, T=250, copy_blocks=True, save_tower=False):
if copy_blocks:
tower = [copy(block) for block in tower]
world = World(tower)
env = Environment([world],
vis_sim=vis,
use_hand=False,
save_tower=save_tower)
for tx in range(T):
env.step(vis_frames=vis)
env.disconnect()
env.cleanup()
return world.get_poses()
def simulate(particles, action, T, true_block):
particle_blocks = [copy.deepcopy(true_block) for particle in particles]
for (com, particle_block) in zip(particles, particle_blocks):
particle_block.com = com
worlds = [make_platform_world(pb, action) for pb in particle_blocks]
env = Environment(worlds, vis_sim=False)
for _ in range(T):
env.step(action=action)
poses = []
for particle_world in worlds:
pose = particle_world.get_pose(particle_world.objects[1])
poses.append(pose.pos)
env.disconnect()
env.cleanup()
return np.array(poses)
def simulate_action(self, action, block_ix, T=50, vis_sim=False):
real_block = self.blocks[block_ix]
# set up the environment with the real block
true_world = make_platform_world(real_block, action)
env = Environment([true_world], vis_sim=vis_sim)
# configure the duration of the action
action.timesteps = T
# run the simulator
for _ in range(T):
env.step(action=action)
# get ground truth object_b pose (observation)
end_pose = true_world.get_pose(true_world.objects[1])
end_pose = add_noise(end_pose, self.noise*np.eye(3))
observation = (action, T, end_pose)
# turn off the sim
env.disconnect()
env.cleanup()
# and return the observed trajectory
return observation
def save_collected_tower_images(logger):
tower_keys = ['2block', '3block', '4block', '5block']
for tx in range(0, 20):
towers, _ = logger.load_acquisition_data(tx)
ix = np.random.randint(0, 10)
print(towers['2block']['towers'].shape)
start = 0
for k in tower_keys:
end = start + towers[k]['towers'].shape[0]
if ix < end:
tower = towers[k]['towers'][ix - start, :, :]
block_tower = [Object.from_vector(tower[bx, :]) for bx in range(tower.shape[0])]
label = towers[k]['labels'][ix-start]
break
start = end
w = World(block_tower)
env = Environment([w], vis_sim=False, vis_frames=True)
env.step(vis_frames=True)
view_matrix = p.computeViewMatrixFromYawPitchRoll(distance=0.3,
yaw=45,
pitch=-10,
roll=0,
upAxisIndex=2,
cameraTargetPosition=(0., 0., 0.25))
aspect = 100. / 190.
nearPlane = 0.01
farPlane = 10
fov = 90
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
image_data = p.getCameraImage(200, 380, shadow=1, viewMatrix=view_matrix, projectionMatrix=projection_matrix)
w, h, im = image_data[:3]
np_im = np.array(im, dtype=np.uint8).reshape(h, w, 4)[:, :, 0:3]
#plt.imshow(np.array(np_im))
plt.imsave(logger.get_figure_path('tower_%d_%d.png' % (tx, label)), np_im)
env.disconnect()
def test_observations(blocks, block_ix):
"""
Test method to try placing the given blocks on the platform.
"""
agent = PandaAgent(blocks, NOISE)
for r in list(rotation_group())[0:]:
action = PlaceAction(pos=None, rot=r, block=blocks[block_ix])
obs = agent.simulate_action(action, block_ix, T=50)
# TODO: Check that the simulated observation agrees with the true observation.
particle_block = deepcopy(blocks[block_ix])
world = make_platform_world(particle_block, action)
env = Environment([world], vis_sim=False)
if False:
env.step(action=action)
length, lifeTime = 0.2, 0.0
pos = end_pose = world.get_pose(world.objects[1])[0]
quat = action.rot.as_quat()
new_x = transformation([length, 0.0, 0.0], pos, quat)
new_y = transformation([0.0, length, 0.0], pos, quat)
new_z = transformation([0.0, 0.0, length], pos, quat)
p.addUserDebugLine(pos, new_x, [1, 0, 0], lifeTime=lifeTime)
p.addUserDebugLine(pos, new_y, [0, 1, 0], lifeTime=lifeTime)
p.addUserDebugLine(pos, new_z, [0, 0, 1], lifeTime=lifeTime)
input('Continue')
for _ in range(50):
env.step(action=action)
end_pose = world.get_pose(world.objects[1])
print('Simulated Pose:', end_pose)
print('TAMP Pose:', obs[2])
input('Continue?')
def filter_block(p_true_block, exp_type, args):
if args.plot:
plt.ion()
fig = plt.figure()
ax = Axes3D(fig)
com_particle_dist = None
experience = []
estimated_coms = []
for i in range(I):
print('Explore action %d/%d' % (i, I))
true_block = copy.deepcopy(p_true_block)
if args.plot: setup_ax(ax, true_world.objects[1])
# create particle worlds for obj_b's COM
com_ranges = get_com_ranges(true_block)
if com_particle_dist is None:
com_particle_dist = create_uniform_particles(N, D, com_ranges)
else:
# update the distribution with the new weights
print(weights)
com_particle_dist = ParticleDistribution(
com_particle_dist.particles, weights)
# and resample the distribution
com_particle_dist = sample_and_wiggle(com_particle_dist,
experience, OBS_MODEL_COV,
true_block, com_ranges)
weights = com_particle_dist.weights
particle_blocks = [
copy.deepcopy(true_block)
for particle in com_particle_dist.particles
]
for (com, particle_block) in zip(com_particle_dist.particles,
particle_blocks):
particle_block.com = com
# Choose action to maximize variance of particles.
if exp_type == 'reduce_var':
rot, direc = plan_action(particle_blocks)
elif exp_type == 'random':
rot = random.choice(list(rotation_group()))
direc = PushAction.get_random_dir()
true_world = make_platform_world(true_block, rot)
particle_worlds = [
make_platform_world(pb, rot) for pb in particle_blocks
]
env = Environment([true_world] + particle_worlds, vis_sim=args.vis)
# action to apply to all worlds
action = PushAction(block_pos=true_world.get_pose(
true_world.objects[1]).pos,
direction=direc,
timesteps=T)
for t in range(T):
env.step(action=action)
# get ground truth object_b pose (observation)
objb_pose = true_world.get_pose(true_world.objects[1])
objb_pose = add_noise(objb_pose)
experience.append((action, rot, T, objb_pose))
# update all particle weights
new_weights = []
for pi, (particle_world,
old_weight) in enumerate(zip(particle_worlds, weights)):
particle_objb_pose = particle_world.get_pose(
particle_world.objects[1])
obs_model = multivariate_normal.pdf(objb_pose.pos,
mean=particle_objb_pose.pos,
cov=OBS_MODEL_COV)
new_weight = old_weight * obs_model
new_weights.append(new_weight)
# normalize particle weights
weights_sum = sum(new_weights)
weights = np.divide(new_weights, weights_sum)
# print('max particle weight: ', max(weights))
if args.plot and not t % 5:
# visualize particles (it's very slow)
plot_particles(ax, com_particle_dist.particles, weights, t=t)
com = np.array(com_particle_dist.particles).T
print(
'Mean COM', com @ weights,
np.diag(
np.cov(com_particle_dist.particles,
rowvar=False,
aweights=weights + 1e-3)))
print('True COM', true_block.com)
print('Error COM', np.linalg.norm(true_block.com - com @ weights))
estimated_coms.append(com @ weights)
env.disconnect()
env.cleanup()
return ParticleDistribution(com_particle_dist.particles,
weights), estimated_coms
def sample_sequential_data(block_set, dataset, n_samples):
""" Generate n_samples random towers. Each tower has the property that its
base (the tower until the last block) is stable. To ensure this, we start
with all the stable towers plus base cases of single block towers.
:param block_set: List of blocks that can be used in the towers.
:param dataset: List of current towers that have been built.
:param n_samples: Number of random towers to consider.
:param max_blocks
:return: Dict containining numpy arrays of the towers sorted by size.
"""
print('Generating data sequentially...')
keys = ['2block', '3block', '4block', '5block']
# initialize a dictionary of lists to store the generated data
sampled_towers = {k: {} for k in keys}
for k in keys:
sampled_towers[k]['towers'] = []
sampled_towers[k]['labels'] = []
if block_set is not None:
sampled_towers[k]['block_ids'] = []
# Gather list of all stable towers. Towers should be of blocks that are rotated in "Block" format.
stable_towers = []
# Get all single block stable towers.
for block in block_set:
for orn in QUATERNIONS:
new_block = deepcopy(block)
new_block.pose = Pose(ZERO_POS, orn)
rot_block = get_rotated_block(new_block)
rot_block.pose = Pose((0., 0., rot_block.dimensions.z / 2.),
(0, 0, 0, 1))
stable_towers.append([rot_block])
# Get all stable towers from the dataset.
for k in keys[:3]:
if dataset is None:
break
tower_tensors = unprocess(
dataset.tower_tensors[k].cpu().numpy().copy())
tower_labels = dataset.tower_labels[k]
for ix, (tower_vec,
tower_label) in enumerate(zip(tower_tensors, tower_labels)):
if tower_label == 1:
block_tower = []
for bx in range(tower_vec.shape[0]):
block = Object.from_vector(tower_vec[bx, :])
if block_set is not None:
block.name = 'obj_' + str(
int(dataset.tower_block_ids[k][ix, bx]))
block_tower.append(block)
stable_towers.append(block_tower)
# maintain block info my block name
# TODO: this will NOT work if using random blocks
block_lookup = {}
for block in block_set:
block_lookup[block.name] = block
# Sample random towers by randomly choosing a stable base then trying to add a block.
for ix in range(n_samples):
# Choose a stable base.
tower_ix = np.random.choice(np.arange(0, len(stable_towers)))
base_tower = stable_towers[tower_ix]
# Choose a block that's not already in the tower.
remaining_blocks = {}
for k in block_lookup:
used = False
for block in base_tower:
if k == block.name:
used = True
if not used:
remaining_blocks[k] = block_lookup[k]
# if we switch block sets during training then the remaining_blocks list
# will be longer than block_set - len(base_tower)
#assert(len(remaining_blocks) == len(block_set) - len(base_tower))
new_block = deepcopy(np.random.choice(list(remaining_blocks.values())))
# Choose an orientation.
orn = QUATERNIONS[np.random.choice(np.arange(0, len(QUATERNIONS)))]
new_block.pose = Pose(ZERO_POS, orn)
rot_block = get_rotated_block(new_block)
# Sample a displacement.
base_dims = np.array(base_tower[-1].dimensions)[:2]
new_dims = np.array(rot_block.dimensions)[:2]
max_displacements_xy = (base_dims + new_dims) / 2.
noise_xy = np.clip(0.5 * np.random.randn(2), -0.95, 0.95)
rel_xy = max_displacements_xy * noise_xy
# Calculate the new pose.
base_pos = np.array(base_tower[-1].pose.pos)[:2]
pos_xy = base_pos + rel_xy
pos_z = np.sum([b.dimensions.z
for b in base_tower]) + rot_block.dimensions.z / 2.
rot_block.pose = Pose((pos_xy[0], pos_xy[1], pos_z), (0, 0, 0, 1))
# Add block to tower.
new_tower = base_tower + [rot_block]
if False:
w = World(new_tower)
env = Environment([w], vis_sim=True, vis_frames=True)
for tx in range(240):
env.step(vis_frames=True)
time.sleep(1 / 240.)
env.disconnect()
# Save that tower in the sampled_towers dict
n_blocks = len(new_tower)
sampled_towers['%dblock' % n_blocks]['towers'].append(
vectorize(new_tower))
# save block id
if block_set is not None:
block_ids = [block.get_id() for block in new_tower]
sampled_towers['%dblock' % n_blocks]['block_ids'].append(block_ids)
# convert all the sampled towers to numpy arrays
for k in keys:
sampled_towers[k]['towers'] = np.array(sampled_towers[k]['towers'])
if sampled_towers[k]['towers'].shape[0] == 0:
sampled_towers[k]['towers'] = sampled_towers[k]['towers'].reshape(
(0, int(k[0]), 21))
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 evaluate_planner(logger, blocks, reward_fn, fname, args, save_imgs=False, img_prefix=''):
tower_keys = [str(ts)+'block' for ts in args.tower_sizes]
tp = TowerPlanner(stability_mode='contains')
ep = EnsemblePlanner(logger)
# Store regret for towers of each size.
regrets = {k: [] for k in tower_keys}
rewards = {k: [] for k in tower_keys}
if args.max_acquisitions is not None:
eval_range = range(0, args.max_acquisitions, 10)
elif args.acquisition_step is not None:
eval_range = [args.acquisition_step]
for tx in eval_range:
print('Acquisition step:', tx)
ensemble = logger.get_ensemble(tx)
if torch.cuda.is_available():
ensemble = ensemble.cuda()
for k, size in zip(tower_keys, args.tower_sizes):
print('Tower size', k)
num_failures, num_pw_failures = 0, 0
curr_regrets = []
curr_rewards = []
for t in range(0, args.n_towers):
print('Tower number', t)
if blocks is not None:
plan_blocks = np.random.choice(blocks, size, replace=False)
plan_blocks = copy.deepcopy(plan_blocks)
else:
plan_blocks = [Object.random() for _ in range(size)]
tower, reward, max_reward, tower_block_ids = ep.plan(plan_blocks,
ensemble,
reward_fn,
args,
num_blocks=size,
n_tower=t)
block_tower = []
for vec_block, block_id in zip(tower, tower_block_ids):
block = Object.from_vector(vec_block)
block.name = 'obj_%d' % block_id
block_tower.append(block)
# save tower info to /evaluation_towers
if args.exec_mode is None:
if args.planning_model == 'noisy-model':
logger.save_evaluation_tower(block_tower, reward, max_reward, tx, args.planning_model, args.problem, noise=args.plan_xy_noise)
else:
logger.save_evaluation_tower(block_tower, reward, max_reward, tx, args.planning_model, args.problem)
# perturb tower if evaluating with noisy model
if args.exec_mode == 'noisy-model':
block_tower = []
for vec_block, block_id in zip(tower, tower_block_ids):
vec_block[7:9] += np.random.randn(2)*args.exec_xy_noise
block = Object.from_vector(vec_block)
block.name = 'obj_%d' % block_id
block_tower.append(block)
# build found tower
if args.exec_mode == 'noisy-model' or args.exec_mode == 'simple-model':
if not tp.tower_is_constructable(block_tower):
reward = 0
num_failures += 1
if tp.tower_is_pairwise_stable(block_tower):
num_pw_failures += 1
else:
pairs = []
dists = []
for i in range(len(tower) - 1):
# check that each pair of blocks is stably individually
top = block_tower[i+1]
bottom = block_tower[i]
if not tp.pair_is_stable(bottom, top):
pairs.append(False)
else:
pairs.append(True)
top_rel_pos = np.array(top.pose.pos) - np.array(bottom.pose.pos)
top_rel_com = top_rel_pos + top.com
dists.append((np.abs(top_rel_com)*2 - bottom.dimensions)[:2])
#print('Pairs:', pairs, dists)
#print('PW Stable:', tp.tower_is_pairwise_stable(block_tower))
#print('Global Stable:', tp.tower_is_stable(block_tower))
if False and reward != 0:
print(reward, max_reward)
w = World(block_tower)
env = Environment([w], vis_sim=True, vis_frames=True)
input()
for tx in range(240):
env.step(vis_frames=True)
time.sleep(1/240.)
env.disconnect()
# Note that in general max reward may not be the best possible due to sampling.
#ground_truth = np.sum([np.max(b.dimensions) for b in blocks])
#print(max_reward, ground_truth)
# Compare heights and calculate regret.
regret = (max_reward - reward)/max_reward
#print(reward, max_reward)
#print(regret)
curr_regrets.append(regret)
curr_rewards.append(reward)
if args.exec_mode == 'noisy-model' or args.exec_mode == 'simple-model':
regrets[k].append(curr_regrets)
rewards[k].append(curr_rewards)
if args.max_acquisitions is not None:
if args.exec_mode == 'noisy-model' or args.exec_mode == 'simple-model':
with open(logger.get_figure_path(fname+'_regrets.pkl'), 'wb') as handle:
pickle.dump(regrets, handle)
with open(logger.get_figure_path(fname+'_rewards.pkl'), 'wb') as handle:
pickle.dump(rewards, handle)
# if just ran for one acquisition step, output final regret and reward
if args.acquisition_step is not None:
if args.exec_mode == 'noisy-model' or args.exec_mode == 'simple-model':
final_median_regret = np.median(regrets[k][0])
final_upper75_regret = np.quantile(regrets[k][0], 0.75)
final_lower25_regret = np.quantile(regrets[k][0][0], 0.25)
final_median_reward = np.median(rewards[k][0])
final_upper75_reward = np.quantile(rewards[k][0], 0.75)
final_lower25_reward = np.quantile(rewards[k][0], 0.25)
final_average_regret = np.average(regrets[k][0])
final_std_regret = np.std(regrets[k][0])
final_average_reward = np.average(rewards[k][0])
final_std_reward = np.std(rewards[k][0])
print('Final Median Regret: (%f) %f (%f)' % (final_lower25_regret, final_median_regret, final_upper75_regret))
print('Final Median Reward: (%f) %f (%f)' % (final_lower25_reward, final_median_reward, final_upper75_reward))
print('Final Average Regret: %f +/- %f' % (final_average_regret, final_std_regret))
print('Final Average Reward: %f +/- %f' % (final_average_reward, final_std_reward))
def augment(all_data, K_skip, translate=False, mirror=False, vis_tower=False):
datasets = {}
for k_block in all_data.keys():
num_blocks = int(k_block.strip('block'))
#print('Augmenting %d block towers...' % num_blocks)
data = all_data[f'{num_blocks}block']
# load the tower data
towers = data['towers'][::K_skip, :]
labels = data['labels'][::K_skip]
if 'block_ids' in data.keys() and data['block_ids'].shape != (0,):
block_ids = data['block_ids'][::K_skip, :]
N, K, D = towers.shape
# calculate the number of augmented towers that will be created
N_angles = 4
N_shift = 4 if translate else 1
N_mirror = 3 if mirror else 1
tower_multiplier = N_angles * N_mirror * N_shift
N_towers_to_add = N * tower_multiplier
# and create new arrays to store those towers
augmented_towers = np.zeros((N_towers_to_add, K, D))
augmented_labels = np.zeros(N_towers_to_add)
augmented_block_ids = np.zeros((N_towers_to_add, K))
for ix in range(N):
#if ix % 1000 == 0:
#print(ix)
original_tower = [Object.from_vector(towers[ix, jx, :]) for jx in range(num_blocks)]
rot_towers = []
for kx, z_rot in enumerate([0., np.pi/2., np.pi, 3*np.pi/2]):
rot = R.from_rotvec([0., 0., z_rot])
# rotate each block in the tower and add the new tower to the dataset
rot_poses = rot.apply(np.array([b.pose.pos for b in original_tower]))
rot_tower = []
for bx in range(num_blocks):
rot_block = deepcopy(original_tower[bx])
new_pose = Pose(Position(*rot_poses[bx,:].tolist()),
Quaternion(*rot.as_quat().tolist()))
# new_pose = Pose(Position(*rot.apply(block.pose.pos)),
# Quaternion(*rot.as_quat().tolist()))
rot_block.set_pose(new_pose)
orig_rot = R.from_quat(rot_block.rotation)
rot_block = get_rotated_block(rot_block)
rot_block.rotation = (rot*orig_rot).as_quat().tolist()
rot_tower.append(rot_block)
augmented_towers[tower_multiplier*ix + kx, bx, :] = rot_tower[bx].vectorize()
rot_towers.append(rot_tower)
augmented_labels[tower_multiplier*ix + kx] = labels[ix]
if 'block_ids' in data.keys():
augmented_block_ids[tower_multiplier*ix + kx, :] = block_ids[ix, :]
# translate the base block in the tower and add after the rotated blocks
# if translate:
# dx, dy = np.random.uniform(-0.2, 0.2, 2)
# shifted_tower = augmented_towers[(4+N_shift)*ix + kx, :].copy()
# # Indices 7,8 correspond to the pose.
# # The CoM doesn't need to be shifted because it is relative.
# shifted_tower[:, 7] += dx
# shifted_tower[:, 8] += dy
# augmented_towers[tower_multiplier*ix + N_angles + kx, :, :] = shifted_tower
# augmented_labels[tower_multiplier*ix + N_angles + kx] = labels[ix]
# worlds = [World(original_tower)] + [World(tower) for tower in rot_towers]
# env = Environment(worlds, vis_sim=True, vis_frames=True)
# env.step(vis_frames=True)
# input('Next?')
# env.disconnect()
# flip the mirror the COM about the COG and negate the relative position in x
# and y for each block. Creates a new tower that is the mirror of the original
# tower about the x and y axes
if mirror:
start_index = tower_multiplier*ix
# pull out a vector of all the towers we just added. This will be of shape
# [N_angles x num_blocks x D]
rot_towers = augmented_towers[start_index: start_index+N_angles, ...]
# create a vector that will mirror a tower when we multiply it
# indices 1 and 7 correspond to the x coordinates of COM and relative position
# indices 2 and 8 correspond to the y coordinates of COM and relative position
mirror_in_x = np.ones([1,1,D])
mirror_in_y = np.ones([1,1,D])
mirror_in_x[..., [1,7]] *= -1
mirror_in_y[..., [2,8]] *= -1
# add the mirrored towers to the augmented towers dataset
augmented_towers[start_index+N_angles*1 : start_index+N_angles*2, ...] = rot_towers * mirror_in_x
augmented_towers[start_index+N_angles*2 : start_index+N_angles*3, ...] = rot_towers * mirror_in_y
augmented_labels[start_index:start_index+N_angles*N_mirror] = labels[ix]
if 'block_ids' in data.keys():
augmented_block_ids[start_index:start_index+N_angles*N_mirror, :] = block_ids[ix, :]
if vis_tower:
for i in range(tower_multiplier):
print('VISUALIZE', ix*tower_multiplier+i, N_towers_to_add)
new_tower = [Object.from_vector(augmented_towers[ix*tower_multiplier+i, jx, :]) for jx in range(num_blocks)]
w = World(new_tower)
env = Environment([w], vis_sim=True, vis_frames=True)
for tx in range(60):
env.step(vis_frames=False)
time.sleep(1/240.)
env.disconnect()
datasets[f'{num_blocks}block'] = {'towers': augmented_towers,
'labels': augmented_labels}
if 'block_ids' in data.keys():
datasets[f'{num_blocks}block']['block_ids'] = augmented_block_ids
return datasets
def plan_action(belief, k=1, exp_type='reduce_var', action_type='push'):
""" Given a set of particles, choose the action that maximizes the observed variance.
:param particle_block: A list of the current set of particles instantiated as blocks.
:param k: Number of pushes to do for each orientation.
"""
if action_type == 'push':
if exp_type == 'reduce_var':
print('Finding variance reducing push action')
results = []
for rot in rotation_group():
for _ in range(k):
action = PushAction(rot=rot, timesteps=50, block=belief.block)
# create a bunch of blocks with the same geometry where each COM
# for each block is set to one of the particles
particle_blocks = [copy.deepcopy(belief.block) for particle in belief.particles.particles]
for (com, particle_block) in zip(belief.particles.particles, particle_blocks):
particle_block.com = com
particle_worlds = [make_platform_world(pb, action) for pb in particle_blocks]
env = Environment(particle_worlds, vis_sim=False)
# get the the position of the block to set the start position of the push action
# action.set_push_start_pos(particle_worlds[0].get_pose(particle_worlds[0].objects[1]).pos)
for _ in range(50):
env.step(action=action)
# Get end pose of all particle blocks.
poses = np.array([w.get_pose(w.objects[1]).pos for w in particle_worlds])
var = np.var(poses, axis=0)
score = np.mean(var)
print(var, score)
results.append((action, score))
env.disconnect()
env.cleanup()
return max(results, key=itemgetter(1))[0]
else:
print('Finding random push action')
rs = [r for r in rotation_group()]
ix = np.random.choice(np.arange(len(rs)))
rot = rs[ix]
push_action = PushAction(rot=rot, block=belief.block)
return push_action
else:
if exp_type == 'reduce_var':
print('Finding variance reducing place action')
results = []
for rot in rotation_group():
for _ in range(k):
action = PlaceAction(rot=rot, pos=None, block=belief.block)
# create a bunch of blocks with the same geometry where each COM
# for each block is set to one of the particles
particle_blocks = [copy.deepcopy(belief.block) for particle in belief.particles.particles]
for (com, particle_block) in zip(belief.particles.particles, particle_blocks):
particle_block.com = com
particle_worlds = [make_platform_world(pb, action) for pb in particle_blocks]
env = Environment(particle_worlds, vis_sim=False)
# get the the position of the block to set the start position of the push action
# action.set_push_start_pos(particle_worlds[0].get_pose(particle_worlds[0].objects[1]).pos)
for _ in range(50):
env.step(action=action)
# Get end pose of all particle blocks.
poses = np.array([w.get_pose(w.objects[1]).pos for w in particle_worlds])
var = np.var(poses, axis=0)
score = np.max(var)
cov = np.cov(poses, rowvar=False)
w, v = np.linalg.eig(cov)
#print(w)
score = np.max(w)
#print(var, score)
results.append((action, score))
env.disconnect()
env.cleanup()
return max(results, key=itemgetter(1))[0]
else:
print('Finding random place action')
# pick a random rotation
rs = [r for r in rotation_group()]
ix = np.random.choice(np.arange(len(rs)))
rot = rs[ix]
# construct the corresponding place action
place_action = PlaceAction(rot=rot, pos=None, block=belief.block)
return place_action
# Create the action.
if args.action_type == 'push':
action = PushAction(direction=None,
timesteps=50,
rot=r,
block=block)
elif args.action_type == 'place':
action = PlaceAction(pos=None,
rot=r)
else:
raise NotImplementedError()
# Make worlds for each block and test using different CoMs.
true_world = make_platform_world(block, action)
com_ranges = get_com_ranges(true_world.objects[1])
com_particles, _ = create_uniform_particles(10, 3, com_ranges)
particle_blocks = []
for com in com_particles:
particle_block = copy.deepcopy(block)
particle_block.com = com
particle_blocks.append(particle_block)
particle_worlds = [make_platform_world(pb, action) for pb in particle_blocks]
env = Environment([true_world]+particle_worlds, vis_sim=True)
for ix in range(100):
env.step(action=action)
p.disconnect()