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 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, 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)