def test_stability_tower_is_stable(): tp = TowerPlanner() obj_a = Object('a', Dimensions(0.1,0.1,0.1), 1, Position(0,0,0), Color(0,1,1)) obj_b = Object('b', Dimensions(0.3,0.1,0.1), 3, Position(0,0,0), Color(1,0,1)) obj_c = Object('c', Dimensions(0.1,0.1,0.2), 2, Position(0,0,0), Color(1,1,0)) # the single block is stable obj_a.pose = Pose(Position(0, 0, 0.05), ZERO_ROT) assert tp.tower_is_stable([obj_a]) # this is stable obj_b.pose = Pose(Position(0, 0, 0.15), ZERO_ROT) assert tp.tower_is_stable([obj_a, obj_b]) # this is unstable obj_b.pose = Pose(Position(0.06, 0, 0.15), ZERO_ROT) assert not tp.tower_is_stable([obj_a, obj_b]) # it becomes stable when we add another block obj_c.pose = Pose(Position(0.0, 0, 0.3), ZERO_ROT) assert tp.tower_is_stable([obj_a, obj_b, obj_c]) # this tower is constructible, but not stable obj_b.pose = Pose(Position(0, 0.04, 0.15), ZERO_ROT) obj_c.pose = Pose(Position(0, 0.08, 0.3), ZERO_ROT) assert not tp.tower_is_stable([obj_a, obj_b, obj_c])
def test_stability_tower_is_stable_with_sim(vis): tp = TowerPlanner() for _ in range(10): # sample three random blocks blocks = [Object.random(str(i)) for i in range(3)] # stack all the blocks on top of eachother (center of geometry, not COM) prev_z = 0 for block in blocks: pos = Position(0,0,block.dimensions.z/2+prev_z) block.pose = Pose(pos, ZERO_ROT) prev_z += block.dimensions.z assert tp.tower_is_stable(blocks) ==\ tower_is_stable_in_pybullet(blocks, vis=vis, T=50)
def active(strategy, vis=False): hypotheses = get_all_hypotheses() tp = TowerPlanner(stability_mode='contains') for nx in range(1, MAX_N): # Generate a random set of 5 blocks. blocks = [Object.random(f'obj_{ix}') for ix in range(NUM_BLOCKS)] # Choose a tower to build. if strategy == 'random': num_blocks = np.random.randint(2, NUM_BLOCKS + 1) tower = sample_random_tower(blocks[:num_blocks]) tower = [get_rotated_block(b) for b in tower] tower = [deepcopy(b) for b in tower] elif strategy == 'entropy': tower = find_entropy_tower(blocks, hypotheses) else: raise NotImplementedError() # Check for consistent models. valid_hypotheses = [] for h in hypotheses: true = tp.tower_is_stable(tower) pred = h(tower) if true == pred: valid_hypotheses.append(h) hypotheses = valid_hypotheses # Visualize the chosen tower and print the updated hypothesis list. if vis: TeleportAgent.simulate_tower(tower, vis=True, T=300) print(hypotheses) # Check if true model found. if len(hypotheses) == 1: break return nx
def evaluate_predictions(fname): with open(fname, 'rb') as handle: results = pickle.load(handle) tp = TowerPlanner(stability_mode='contains') # Index this as [stable][cog_stable][pw_stable] for ix, (towers, labels, preds) in enumerate(results): correct = [[[0, 0], [0, 0]], [[0, 0], [0, 0]]] total = [[[0, 0], [0, 0]], [[0, 0], [0, 0]]] # Check the tower stability type. for tower, label, pred in zip(towers, labels, preds): blocks = to_blocks(tower) cog_stable = tp.tower_is_cog_stable(blocks) pw_stable = tp.tower_is_constructible(blocks) stable = tp.tower_is_stable(blocks) if stable != label: print('WAT', stable, label) #assert stable == label total[stable][cog_stable][pw_stable] += 1 if (pred > 0.5) == label: correct[stable][cog_stable][pw_stable] += 1 print(total) print('%d Towers' % (ix + 2)) for stable in [0, 1]: for cog_stable in [0, 1]: for pw_stable in [0, 1]: if ix == 0 and pw_stable != stable: continue acc = correct[stable][cog_stable][pw_stable] / total[ stable][cog_stable][pw_stable] print( 'Stable: %d\tCOG_Stable: %d\tPW_Stable: %d\tAcc: %f' % (stable, cog_stable, pw_stable, acc))
def com_stable(tower): tp = TowerPlanner(stability_mode='contains') return tp.tower_is_stable(tower)