def test_stability_pair_is_stable(): tp = TowerPlanner() a = Object.random('a') b = Object.random('b') # center the COM of the top object over the bottom object stable_a_pos = Position(-a.com.x, -a.com.y, a.dimensions.z/2 + b.dimensions.z) a.pose = Pose(stable_a_pos, ZERO_ROT) assert tp.pair_is_stable(b, a) # center the COM of the top object on the positive x edge of the bottom # object, and then move it a tiny bit farther unstable_a_pos = Position(b.dimensions.x/2 - a.com.x + 1e-5, -a.com.y, a.dimensions.z/2 + b.dimensions.z) a.pose = Pose(unstable_a_pos, ZERO_ROT) assert not tp.pair_is_stable(b, a)
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))