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)