def create_surface_attachment(world, obj_name, surface_name): body = world.get_body(obj_name) if surface_name in ENV_SURFACES: surface_body = world.environment_bodies[surface_name] surface_link = BASE_LINK else: surface = surface_from_name(surface_name) surface_body = world.kitchen surface_link = link_from_name(surface_body, surface.link) return create_attachment(surface_body, surface_link, body)
def approach(self): grasp = np.array([0, 0, 0.2]) target_point_1 = np.array(get_pose(self.pw.pipe))[0] + grasp grasp = np.array([0, 0, 0.13]) target_point_2 = np.array(get_pose(self.pw.pipe))[0] + grasp target_quat = (1, 0.5, 0, 0) #get whatever it is by default target_pose = (target_point_1, target_quat) obstacles = [self.pw.pipe, self.pw.hollow] self.go_to_pose(target_pose, obstacles=obstacles) target_pose = (target_point_2, target_quat) self.go_to_pose(target_pose, obstacles=obstacles) self.pipe_attach = create_attachment(self.pw.robot, self.ee_link, self.pw.pipe) self.squeeze(0, width=self.default_width)
def skip_to_end(sim_world, planning_world, plan): if plan is None: return plan bead_attachments = [ create_attachment(cup, -1, bead) for bead, cup in sim_world.initial_beads.items() ] action_index = -1 action, args = plan[action_index] arm = args[0] control = args[-1] context = control['context'] command = control['commands'][0] context.apply_mapping(planning_world.body_mapping) for arm, name in list(sim_world.controller.holding.items()): sim_world.controller.detach(arm, name) attachments = context.assign() # TODO: need to move the beads as well next(command.iterate(sim_world, attachments)) for attachment in list(attachments.values()) + bead_attachments: attachment.assign() for name in attachments: sim_world.controller.attach(get_arm_prefix(arm), name) return plan[action_index:]
def attach(self): return create_attachment(self.robot, self.link, self.body)
def create_world_pose(world, name, **kwargs): attachment = create_attachment(world.kitchen, BASE_LINK, world.get_body(name)) return pose_from_attachment(attachment, support=None, **kwargs)