def take_out_cloth():
print "Taking out cloth"
RosUtils.call_service("move_torso",MoveTorso,height=0.01)
DryerNavigationUtils.goToPosition("enter_dryer")
GripUtils.go_to(x=-0.1,y=-0.33,z=-0.4,roll=0,pitch=0,yaw=0,grip=False,arm="l",frame="dryer")
DryerNavigationUtils.goToPosition("into_dryer")
GripUtils.go_to(x=0.15,y=-0.33,z=-0.65,roll=0,pitch=pi/2,yaw=0,grip=False,arm="l",frame="dryer")
GripUtils.close_gripper("l")
GripUtils.go_to(x=0.15,y=-0.33,z=-0.4,roll=0,pitch=0,yaw=0,grip=True,arm="l",frame="dryer")
DryerNavigationUtils.goToPosition("enter_dryer")
RosUtils.call_service("move_torso",MoveTorso,height=0.3)
return True
def take_out_cloth():
print "Taking out cloth"
RosUtils.call_service("move_torso", MoveTorso, height=0.01)
DryerNavigationUtils.goToPosition("enter_dryer")
GripUtils.go_to(x=-0.1,
y=-0.33,
z=-0.4,
roll=0,
pitch=0,
yaw=0,
grip=False,
arm="l",
frame="dryer")
DryerNavigationUtils.goToPosition("into_dryer")
GripUtils.go_to(x=0.15,
y=-0.33,
z=-0.65,
roll=0,
pitch=pi / 2,
yaw=0,
grip=False,
arm="l",
frame="dryer")
GripUtils.close_gripper("l")
GripUtils.go_to(x=0.15,
y=-0.33,
z=-0.4,
roll=0,
pitch=0,
yaw=0,
grip=True,
arm="l",
frame="dryer")
DryerNavigationUtils.goToPosition("enter_dryer")
RosUtils.call_service("move_torso", MoveTorso, height=0.3)
return True
