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