def putdowntray(self, unused1, tray_name, tray_loc): """Move the robot to the tray goal location and releases the tray """ tray = self.env.GetKinBody(tray_name) if tray is None: raise ValueError("Object %s does not exist" % tray_name) if tray_loc == "trayloc2": T = tray_world.tray_destination else: T = tray_world.tray_initial_loc self.pause("Moving to target position") tray_world.move_robot_base_infront_tray(self.robot, T) print "Releasing the tray" self.robot.Release(tray) for obj in self.tray_stack: self.robot.Release(obj) self.tray_stack = [] utils.pr2_tuck_arm(self.robot)
def picktray(self, unused1, tray_name, unused2): """Grabs the tray and all the items on it """ tray = self.env.GetKinBody(tray_name) if tray is None: raise ValueError("Object %s does not exist" % tray_name) if len(self.tray_stack) > 0 and (np.random.uniform() <= 0.0): e = ExecutingException("Tray is heavy!") e.robot = self.robot e.object_to_grasp = tray raise e self.pause("Moving in front of the tray") tray_world.move_robot_base_infront_tray(self.robot, tray) self.pause("Grasping the tray") tray_world.put_left_arm_over_tray(self.env, self.robot, tray) tray_world.put_right_arm_over_tray(self.env, self.robot, tray) self.robot.Grab(tray) for obj in self.tray_stack: self.robot.Grab(obj)