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)