def restack_blocks(n=10, height=5):
height = max(height, 0)
n = max(height, n)
blocks = ['Block' + str(i) for i in range(1, n + 1)]
operators = [Pick(item) for item in blocks] + \
[Place(item) for item in blocks] + \
[Unstack(*item) for item in permutations(blocks, 2)] + \
[Stack(*item) for item in permutations(blocks, 2)]
initial = State(
{OnTable(blocks[height - 1]),
ArmEmpty(), Clear(blocks[0])} | {
On(obj, under_obj)
for under_obj, obj in pairs(blocks[:height][::-1])
} | {OnTable(block)
for block in blocks[height:]}
| {Clear(block)
for block in blocks[height:]})
goal = PartialState(
{OnTable(blocks[0]), ArmEmpty()}
| {On(obj, under_obj)
for under_obj, obj in pairs(blocks[:height])})
return initial, goal, operators
def sparsify_path(robot, path):
diff = [robot.SubtractActiveDOFValues(q2, q1) for q1, q2 in pairs(path)]
sparse_path = []
config = path[0]
i = 0
while i < len(diff) - 1:
sparse_path.append(config.copy())
config += diff[i]
for j in range(i+1, len(diff)):
if not np.allclose(diff[i], diff[j]):
break
config += diff[j]
i = j
return sparse_path + [config.copy()]