def post_process(problem, plan):
if plan is None:
return None
commands = []
for i, (name, args) in enumerate(plan):
print(i, name, args)
if name == 'move_base':
t = args[-1]
new_commands = [t]
elif name == 'pick':
a, b, p, g, _, t = args
attach = Attach(problem.robot, a, g, b)
new_commands = [t, attach, t.reverse()]
elif name == 'place':
a, b, p, g, _, t = args
detach = Detach(problem.robot, a, b)
new_commands = [t, detach, t.reverse()]
elif name == 'clean': # TODO: add text or change color?
body, sink = args
new_commands = [Clean(body)]
elif name == 'cook':
body, stove = args
new_commands = [Cook(body)]
elif name == 'press_clean':
body, sink, arm, button, bq, t = args
new_commands = [t, Clean(body), t.reverse()]
elif name == 'press_cook':
body, sink, arm, button, bq, t = args
new_commands = [t, Cook(body), t.reverse()]
else:
raise ValueError(name)
commands += new_commands
return commands
def post_process(problem, plan, teleport=False):
if plan is None:
return None
commands = []
for i, (name, args) in enumerate(plan):
if name == 'move_base':
c = args[-1]
new_commands = c.commands
elif name == 'pick':
a, b, p, g, _, c = args
[t] = c.commands
close_gripper = GripperCommand(problem.robot,
a,
g.grasp_width,
teleport=teleport)
attach = Attach(problem.robot, a, g, b)
new_commands = [t, close_gripper, attach, t.reverse()]
elif name == 'place':
a, b, p, g, _, c = args
[t] = c.commands
gripper_joint = get_gripper_joints(problem.robot, a)[0]
position = get_max_limit(problem.robot, gripper_joint)
open_gripper = GripperCommand(problem.robot,
a,
position,
teleport=teleport)
detach = Detach(problem.robot, a, b)
new_commands = [t, detach, open_gripper, t.reverse()]
elif name == 'clean': # TODO: add text or change color?
body, sink = args
new_commands = [Clean(body)]
elif name == 'cook':
body, stove = args
new_commands = [Cook(body)]
elif name == 'press_clean':
body, sink, arm, button, bq, c = args
[t] = c.commands
new_commands = [t, Clean(body), t.reverse()]
elif name == 'press_cook':
body, sink, arm, button, bq, c = args
[t] = c.commands
new_commands = [t, Cook(body), t.reverse()]
else:
raise ValueError(name)
print(i, name, args, new_commands)
commands += new_commands
return commands