def apply(self, state, **kwargs):
with LockRenderer():
self.cone = get_viewcone(color=(1, 0, 0, 0.5))
state.poses[self.cone] = None
cone_pose = Pose(self.cone, unit_pose())
attach = Attach(self.robot, self.group, cone_pose, self.cone)
attach.assign()
wait_for_duration(1e-2)
for _ in attach.apply(state, **kwargs):
yield
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):
if plan is None:
return None
commands = []
#attachments = {}
for i, (name, args) in enumerate(plan):
if name == 'vaporize':
if len(args) == 2:
b, p = args
else:
r, q, b, p, g = args
new_commands = [Vaporize(b)]
elif name == 'pick':
r, q, b, p, g = args
#attachments[b] = r
new_commands = [Attach(r, arm=BASE_LINK, grasp=g, body=b)]
elif name == 'place':
r, q, b, p, g = args
#del attachments[b]
new_commands = [Detach(r, arm=BASE_LINK, body=b)]
elif name == 'move':
r, q1, q2, t = args
new_commands = [t]
# elif name == 'cook':
# b, s = args
# new_commands = []
else:
raise ValueError(name)
print(i, name, args, new_commands)
commands += new_commands
return commands
def post_process(problem, plan, teleport=False):
if plan is None:
return None
commands = []
attachments = {}
for i, (name, args) in enumerate(plan):
if name == 'vaporize':
if len(args) == 2:
b, p = args
else:
r, q, b, p, g = args
new_commands = [Vaporize(b)]
elif name == 'pick':
r, q, b, p, g = args
#attachments[r] = r
new_commands = [Attach(r, arm=BASE_LINK, grasp=g, body=b)]
elif name == 'place':
# TODO: make a drop all rocks
r, s = args
new_commands = [Detach(r, arm=BASE_LINK, body=attachments[r])]
elif name == 'move':
r, q1, q2, t = args
new_commands = [t]
else:
raise ValueError(name)
print(i, name, args, new_commands)
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
def post_process(problem, plan):
if plan is None:
return None
commands = []
attachments = {}
for i, (name, args) in enumerate(plan):
if name == 'send_image':
v, q, y, l, o, m = args
new_commands = [y]
elif name == 'send_analysis':
v, q, y, l, r = args
new_commands = [y]
elif name == 'sample_rock':
v, q, r, s = args
attachments[v] = r
new_commands = [
Attach(v, arm=BASE_LINK, grasp=None, body=attachments[v])
]
elif name == 'drop_rock':
# TODO: make a drop all rocks
v, s = args
new_commands = [Detach(v, arm=BASE_LINK, body=attachments[v])]
elif name == 'calibrate':
v, bq, y, o, c = args
new_commands = [
Register(v, o, camera_frame=KINECT_FRAME, max_depth=VIS_RANGE)
]
elif name == 'take_image':
v, bq, y, o, c, m = args
new_commands = [
Scan(v, o, detect=False, camera_frame=KINECT_FRAME)
]
elif name == 'move':
v, q1, t, q2 = args
new_commands = [t]
else:
raise ValueError(name)
print(i, name, args, new_commands)
commands += new_commands
return commands
def post_process(state,
plan,
replan_obs=True,
replan_base=False,
look_move=True):
problem = state.task
if plan is None:
return None
# TODO: refine actions
robot = problem.robot
commands = []
uncertain_base = False
expecting_obs = False
for i, (name, args) in enumerate(plan):
if replan_obs and expecting_obs:
break
saved_world = WorldSaver() # StateSaver()
if name == 'move_base':
t = args[-1]
# TODO: look at the trajectory (endpoint or path) to ensure fine
# TODO: I should probably move base and keep looking at the path
# TODO: I could keep updating the head goal as the base moves along the path
if look_move:
new_commands = [move_look_trajectory(t)]
#new_commands = [inspect_trajectory(t), t]
else:
new_commands = [t]
if replan_base:
uncertain_base = True
elif name == 'pick':
if uncertain_base:
break
a, b, p, g, _, t = args
attach = Attach(robot, a, g, b)
new_commands = [t, attach, t.reverse()]
elif name == 'place':
if uncertain_base:
break
a, b, p, g, _, t = args
detach = Detach(robot, a, b)
new_commands = [t, detach, t.reverse()]
elif name == 'scan':
o, p, bq, hq, ht = args
ht0 = plan_head_traj(robot, hq.values)
new_commands = [ht0]
if o in problem.rooms:
attach, detach = get_cone_commands(robot)
new_commands += [attach, ht, ScanRoom(robot, o), detach]
else:
new_commands += [ht, Scan(robot, o)]
#with BodySaver(robot):
# for hq2 in ht.path:
# st = plan_head_traj(robot, hq2.values)
# new_commands += [st, Scan(robot, o)]
# hq2.step()
# TODO: return to start conf?
elif name == 'localize':
r, _, o, _ = args
new_commands = [Detect(robot, r, o)]
expecting_obs = True
elif name == 'register':
o, p, bq, hq, ht = args
ht0 = plan_head_traj(robot, hq.values)
register = Register(robot, o)
new_commands = [ht0, register]
expecting_obs = True
else:
raise ValueError(name)
saved_world.restore()
for command in new_commands:
if isinstance(command, Trajectory) and command.path:
command.path[-1].step()
commands += new_commands
return commands
def apply(self, state, **kwargs):
self.cone = get_viewcone(color=(1, 0, 0, 0.5))
state.poses[self.cone] = None
attach = Attach(self.robot, self.group, Pose(self.cone, unit_pose()),
self.cone)
attach.apply(state, **kwargs)