def execute(self, userdata):
forward_amount = 0.375
height = 0.65
drop_amount = 0.25
lateral_amount = 0.05
if self.arm=="r":
yaw_multiplier = 1
y_multiplier = -1
else:
yaw_multiplier = -1
y_multiplier = 1
for i in range(self.num_shakes):
if i == 0:
duration = 4.0
if not GripUtils.go_to(x=forward_amount,y=0,z=height,
roll=0,pitch=0,yaw=yaw_multiplier*pi/2,grip=True,
frame="table_height",arm=self.arm,dur=duration):
return FAILURE
duration = .5
if not GripUtils.go_to(x=forward_amount,y=lateral_amount*y_multiplier,z=height-drop_amount,
roll=0,pitch=pi/4,yaw=yaw_multiplier*pi/2,grip=True,
frame="table_height",arm=self.arm,dur=duration):
return FAILURE
if not GripUtils.go_to(x=forward_amount,y=0,z=height,
roll=0,pitch=0,yaw=yaw_multiplier*pi/2,grip=True,
frame="table_height",arm=self.arm,dur=duration):
return FAILURE
return SUCCESS
def executeFold_right(self,gripPt,endPt,color_current='blue',color_next='blue'):
"""
execute fold with right arm
"""
print "points",gripPt,endPt
if (color_current == 'blue'):
startpoint = self.convert_to_world_frame(gripPt)
if not GripUtils.grab(x = startpoint.point.x,y=startpoint.point.y,z=startpoint.point.z,arm='r',
roll=pi/2,yaw=pi/3,pitch=pi/4,approach= True,frame=startpoint.header.frame_id):
print "failure"
return False
midpoint = self.dupl_PointStamped(endPt)#self.convert_to_world_frame(endPt)
midpoint.point.x = (midpoint.point.x + startpoint.point.x)/2
midpoint.point.y = (midpoint.point.y + startpoint.point.y)/2
midpoint.point.z = (midpoint.point.z + 0.1)
pitch = pi/4
roll = pi/2
yaw=pi/2
grip=True
frame= midpoint.header.frame_id
if not GripUtils.go_to(x=midpoint.point.x,y=midpoint.point.y,z=midpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='r',dur=7.5):
print "failure to go to ",midpoint.point.x,midpoint.point.y,midpoint.point.z
#return False
endpoint = self.dupl_PointStamped(endPt)#self.convert_to_world_frame(endPt)
frame=endpoint.header.frame_id
if not GripUtils.go_to(x=endpoint.point.x,y=endpoint.point.y,z=endpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='r',dur=7.5):
print "failure"
#return False
if(color_next == 'blue'):
initRobot()
return True
def execute(self, userdata):
forward_amount = 0.5
hover_amount = 0.06
if self.direction == "r":
drag_arm = "r"
edge_y = -1.0 * self.table_width / 2.0
drag_yaw = pi/2
final_y = edge_y + self.drag_amount
else:
drag_arm = "l"
edge_y = 1.0 * self.table_width / 2.0
drag_yaw = -pi/2
final_y = edge_y - self.drag_amount
if not GripUtils.go_to(x=forward_amount*.8,y=edge_y*1.25,z=self.lift_amount,
roll=0,pitch=0,yaw=drag_yaw/4,grip=True,
frame="table_heig@staticmethodht",arm=drag_arm):
print 'f1'
return FAILURE
if not GripUtils.go_to(x=forward_amount,y=edge_y,z=hover_amount,
roll=0,pitch=pi/4,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm):
print 'f2'
return FAILURE
if not GripUtils.go_to(x=forward_amount,y=final_y,z=hover_amount,
roll=0,pitch=pi/2,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm):
print 'f3'
return FAILURE
return SUCCESS
def execute(self,userdata):
pt_bl = userdata.bl
pt_tl = userdata.tl
pt_br = userdata.br
pt_tr = userdata.tr
(bl_x,bl_y,bl_z) = (pt_bl.point.x,pt_bl.point.y,pt_bl.point.z)
(tl_x,tl_y,tl_z) = (pt_tl.point.x,pt_tl.point.y,pt_tl.point.z)
(br_x,br_y,br_z) = (pt_br.point.x,pt_br.point.y,pt_br.point.z)
(tr_x,tr_y,tr_z) = (pt_tr.point.x,pt_tr.point.y,pt_tr.point.z)
ctr_l_x = .75*bl_x + .25*tl_x
ctr_l_y = .75*bl_y + .25*tl_y
z = bl_z
yaw = -pi/2
roll = -pi/2
pitch = pi/4
frame = pt_bl.header.frame_id
ctr_r_x = .75*br_x + .25*tr_x
ctr_r_y = .75*br_y + .25*tr_y + 0.01 # bit too far right
alpha = 0.333
ctr_ml_x = (1-alpha)*ctr_l_x + alpha*ctr_r_x
ctr_ml_y = (1-alpha)*ctr_l_y + alpha*ctr_r_y
ctr_mr_x = (1-alpha)*ctr_r_x + alpha*ctr_l_x
ctr_mr_y = (1-alpha)*ctr_r_y + alpha*ctr_l_y
up_z = z+sqrt( (ctr_l_x - ctr_r_x)**2 + (ctr_l_y - ctr_r_y)**2) / 3.0
yaw = pi/2
roll = pi/2
pitch = pi/4
if not GripUtils.grab(x=ctr_r_x,y=ctr_r_y,z=z+0.0025,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame):
GripUtils.open_gripper("r")
GripUtils.recall_arm("r")
return FAILURE
pitch = pi/2
if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y-0.02,z=up_z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0):
GripUtils.open_gripper("r")
GripUtils.recall_arm("r")
return FAILURE
if not GripUtils.go_to(x=(ctr_mr_x+ctr_ml_x)/2.0,y=(ctr_mr_y-0.02+ctr_ml_y-0.02)/2.0,z=(up_z+bl_z+0.01)/2.0,roll=roll,yaw=yaw,pitch=(pitch+3*pi/4)/2.0,arm="r",frame=frame,grip=True,dur=5.0):
GripUtils.open_gripper("r")
GripUtils.recall_arm("r")
return FAILURE
z = bl_z+0.01
pitch = 3*pi/4
if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y-0.02,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0):
GripUtils.open_gripper("r")
GripUtils.recall_arm("r")
return FAILURE
GripUtils.open_gripper("r")
if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y+0.06,z=z+0.02,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=False,dur=1.0):
GripUtils.recall_arm("r")
return FAILURE
GripUtils.recall_arm("r")
pr2_say(talk_done)
return SUCCESS
def execute(self, userdata):
forward_amount = 0.375
height = 0.65
drop_amount = 0.25
lateral_amount = 0.05
if self.arm == "r":
yaw_multiplier = 1
y_multiplier = -1
else:
yaw_multiplier = -1
y_multiplier = 1
for i in range(self.num_shakes):
if i == 0:
duration = 4.0
if not GripUtils.go_to(x=forward_amount,
y=0,
z=height,
roll=0,
pitch=0,
yaw=yaw_multiplier * pi / 2,
grip=True,
frame="table_height",
arm=self.arm,
dur=duration):
return FAILURE
duration = .5
if not GripUtils.go_to(x=forward_amount,
y=lateral_amount * y_multiplier,
z=height - drop_amount,
roll=0,
pitch=pi / 4,
yaw=yaw_multiplier * pi / 2,
grip=True,
frame="table_height",
arm=self.arm,
dur=duration):
return FAILURE
if not GripUtils.go_to(x=forward_amount,
y=0,
z=height,
roll=0,
pitch=0,
yaw=yaw_multiplier * pi / 2,
grip=True,
frame="table_height",
arm=self.arm,
dur=duration):
return FAILURE
return SUCCESS
def go_to_folding_station():
print "Going to folding station"
DryerNavigationUtils.dryerToTable1()
GripUtils.go_to(arm="l",
x=0.5,
y=0,
z=0.35,
roll=0,
yaw=0,
pitch=pi / 2,
grip=True,
frame="table_height")
GripUtils.open_gripper("l")
return True
def open_sock():
x=0.5
y=0.0
z=TABLE_HEIGHT+0.4
frame="base_footprint"
#old strategy
GripUtils.go_to(x=x,y=y+0.007,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/8,grip=True,frame=frame,arm="r",dur=5.0)
GripUtils.go_to(x=x,y=y+0.05,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=False,frame=frame,arm="l",dur=5.0)
GripUtils.go_to(x=x,y=y-0.004,z=z+0.01,roll=pi/2,yaw=-pi/2,pitch=pi/5,grip=False,frame=frame,arm="l",dur=5.0)
GripUtils.go_to(x=x,y=y-0.0035,z=z+0.001,roll=pi/2,yaw=-pi/2,pitch=pi/5,grip=True,frame=frame,arm="l",dur=2.0) #changed from -0.007 to -0.0065
#GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=0,grip=True,frame=frame,arm="r",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.04,z=z+0.05,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.04,z=z+0.005,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
#GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/16,grip=True,frame=frame,arm="r",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.06,z=z-0.025,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.025,z=z-0.225,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=True,frame=frame,arm="l",dur=1.0)
#GripUtils.go_to(x=x,y=y+0.025,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=True,frame=frame,arm="l",dur=5.0)
GripUtils.close_gripper("l",extra_tight=True)
open_amt = 0.025
GripUtils.go_to_multi (x_l=x,y_l=y+open_amt,z_l=z+0.01,roll_l=pi/2,yaw_l=-pi/2,pitch_l=0,grip_l=True,frame_l=frame
,x_r=x,y_r=y-open_amt,z_r=z+0.01,roll_r=pi/2,yaw_r=pi/2,pitch_r=0,grip_r=True,frame_r=frame
,dur=5.0)
def take_off_dopple(pt,arm):
x = DOPPLE_X
y = DOPPLE_Y
if arm == "l":
yaw = -pi/2
y -= 0.005
else:
yaw = pi/2
y += 0.005
z = DOPPLE_HEIGHT+TABLE_HEIGHT - 0.045
frame = "base_footprint"
GripUtils.go_to(x=x,y=y,z=z,roll=0,yaw=yaw,pitch=pi/4,arm=arm,grip=False,frame=frame,dur=5.0)
GripUtils.close_gripper(arm,extra_tight=False)
GripUtils.go_to(x=x,y=y,z=z+0.1,roll=0,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0)
GripUtils.go_to(x=x,y=y,z=z+0.2,roll=0,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0)
roll = 0
if arm=="l":
yaw = -pi/2
else:
yaw = pi/2
GripUtils.go_to(x=x-0.15,y=y,z=z+0.2,roll=roll,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0)
#Spreads out
GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=0,arm=arm,grip=True,z_offset = 0.2,y_offset=0.2,dur=5.0) # previously z_o= 0.08 y_o 0.1
GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=pi/2,arm=arm,grip=True,dur=7.5,y_offset = -0.2,z_offset=0.015)
GripUtils.open_gripper(arm)
GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=pi/4,arm=arm,grip=False,dur=2.5,y_offset = -0.2,z_offset=0.025)
StanceUtils.call_stance("arms_up",5.0)
def execute(self, userdata):
forward_amount = 0.5
hover_amount = 0.06
if self.direction == "r":
drag_arm = "r"
edge_y = -1.0 * self.table_width / 2.0
drag_yaw = pi / 2
final_y = edge_y + self.drag_amount
else:
drag_arm = "l"
edge_y = 1.0 * self.table_width / 2.0
drag_yaw = -pi / 2
final_y = edge_y - self.drag_amount
if not GripUtils.go_to(x=forward_amount * .8,
y=edge_y * 1.25,
z=self.lift_amount,
roll=0,
pitch=0,
yaw=drag_yaw / 4,
grip=True,
frame="table_height",
arm=drag_arm):
print 'f1'
return FAILURE
if not GripUtils.go_to(x=forward_amount,
y=edge_y,
z=hover_amount,
roll=0,
pitch=pi / 4,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm):
print 'f2'
return FAILURE
if not GripUtils.go_to(x=forward_amount,
y=final_y,
z=hover_amount,
roll=0,
pitch=pi / 2,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm):
print 'f3'
return FAILURE
return SUCCESS
def execute(self, userdata):
height = 0.35
lateral_amount = 0.65
forward_amount = 0.3
if self.arm == 'b':
lateral_amount_r = lateral_amount * -1
lateral_amount_l = lateral_amount
if not GripUtils.go_to_multi( x_l=forward_amount, y_l=lateral_amount_l, z_l=height,
roll_l=0, pitch_l=0, yaw_l=0,
frame_l="torso_lift_link", grip_l=False,
x_r=forward_amount, y_r=lateral_amount_r, z_r=height,
roll_r=0, pitch_r=0, yaw_r=0,
frame_r="torso_lift_link", grip_r=False, dur=4.0):
return FAILURE
else:
return SUCCESS
else:
if self.arm == 'r':
lateral_amount = lateral_amount * -1
if not GripUtils.go_to( x=forward_amount, y=lateral_amount, z=height,
roll=0, pitch=0, yaw=0, grip=False,
frame="torso_lift_link", dur=4.0, arm=self.arm):
return FAILURE
else:
return SUCCESS
def execute(self,userdata):
arm = userdata.arm
multiplier = 1 if arm == 'r' else -1
if not GripUtils.go_to(x=0.4,y=0,z=0.35,roll=0,yaw=pi/2*multiplier,pitch=pi/4,arm=arm,grip=True,frame="table_height",dur=3.0):
return FAILURE
GripUtils.open_gripper(arm)
GripUtils.recall_arm("b")
return SUCCESS
def execute(self,userdata):
arm = userdata.arm
multiplier = 1 if arm == 'r' else -1
if not GripUtils.go_to(x=0.4,y=0,z=0.35,roll=0,yaw=pi/2*multiplier,pitch=pi/4,arm=arm,grip=True,frame="table_height",dur=3.0):
return FAILURE
GripUtils.open_gripper(arm)
GripUtils.recall_arm("b")
return SUCCESS
def take_off_dopple_pair():
x = DOPPLE_X
y = DOPPLE_Y
yaw = pi/2
y += 0.005
z = DOPPLE_HEIGHT+TABLE_HEIGHT - 0.045
frame = "base_footprint"
GripUtils.go_to(x=x,y=y,z=z,roll=0,yaw=yaw,pitch=pi/4,arm="r",grip=False,frame=frame,dur=5.0)
GripUtils.close_gripper("r",extra_tight=False)
GripUtils.go_to(x=x,y=y,z=z+0.1,roll=pi/2,yaw=yaw,pitch=0,arm="r",grip=True,frame=frame,dur=5.0)
GripUtils.go_to(x=x,y=y,z=z+0.2,roll=pi/2,yaw=yaw,pitch=0,arm="r",grip=True,frame=frame,dur=5.0)
GripUtils.go_to(x=x-0.2,y=y,z=z+0.2,roll=pi/2,yaw=yaw,pitch=0,arm="r",grip=True,frame=frame,dur=5.0)
def execute(self, userdata):
forward_amount = self.forward_amount
hover_amount = 0.06
if self.direction == "r":
pr2_say(talk_drag2)
drag_arm = "r"
edge_y = self.table_width / 2.0
drag_yaw = pi/2
final_y = edge_y - self.drag_amount
else:
pr2_say(talk_drag1)
drag_arm = "l"
edge_y = -1 * self.table_width / 2.0
drag_yaw = -pi/2
final_y = edge_y + self.drag_amount
#roll = 0
roll = pi/2
GripUtils.go_to(x=forward_amount,y=edge_y,z=self.lift_amount,
roll=roll,pitch=0,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm,dur=2.0)
rospy.sleep(1.0) #Give time to stabilize
if not GripUtils.go_to(x=forward_amount,y=edge_y,z=hover_amount,
roll=roll,pitch=pi/4,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm,dur=2.0):
return FAILURE
if not GripUtils.go_to(x=forward_amount,y=(edge_y+final_y)/2.0,z=hover_amount,
roll=roll,pitch=pi/4,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm,dur=2.5):
return FAILURE
if not GripUtils.go_to(x=forward_amount,y=final_y,z=hover_amount,
roll=roll,pitch=pi/4,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm,dur=2.5):
return FAILURE
return SUCCESS
def take_off_dopple_pair():
x = DOPPLE_X
y = DOPPLE_Y
yaw = pi / 2
y += 0.005
z = DOPPLE_HEIGHT + TABLE_HEIGHT - 0.045
frame = "base_footprint"
GripUtils.go_to(x=x,
y=y,
z=z,
roll=0,
yaw=yaw,
pitch=pi / 4,
arm="r",
grip=False,
frame=frame,
dur=5.0)
GripUtils.close_gripper("r", extra_tight=False)
GripUtils.go_to(x=x,
y=y,
z=z + 0.1,
roll=pi / 2,
yaw=yaw,
pitch=0,
arm="r",
grip=True,
frame=frame,
dur=5.0)
GripUtils.go_to(x=x,
y=y,
z=z + 0.2,
roll=pi / 2,
yaw=yaw,
pitch=0,
arm="r",
grip=True,
frame=frame,
dur=5.0)
GripUtils.go_to(x=x - 0.2,
y=y,
z=z + 0.2,
roll=pi / 2,
yaw=yaw,
pitch=0,
arm="r",
grip=True,
frame=frame,
dur=5.0)
def sweep_cloth_with_scoot(direction, drag_amount, table_width, lift_amount,roll=0,scoot=0):
forward_amount = 0.5
hover_amount = 0.06
edge_distance = (table_width/2.0)+.04
edge_distance_low = (table_width/2.0)
if direction == "r":
drag_arm = "r"
edge_y_high = edge_distance
edge_y_low = edge_distance_low
drag_yaw = pi/2
waypoint_y = edge_y_low - (drag_amount/2.0)
final_y = edge_y_low - drag_amount
else:
drag_arm = "l"
edge_y_high = -1 * edge_distance
edge_y_low = -1 * edge_distance_low
drag_yaw = -pi/2
waypoint_y = edge_y_low + (drag_amount/2.0)
final_y = edge_y_low + drag_amount
# Take it to table edge and hold it high
if not GripUtils.go_to(x=forward_amount-.21,y=edge_y_high,z=lift_amount+0.01,
roll=roll,pitch=0,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm,dur=3):
return False
for i in range(2):
GripUtils.go_to(x=forward_amount-.21,y=edge_y_high-.02,z=lift_amount-.2,
roll=roll,pitch=0,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm,dur=1.0/(0.7*i+1))
# Lower it at table edge
if not GripUtils.go_to(x=forward_amount,y=edge_y_low,z=hover_amount,
roll=roll,pitch=pi/4,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm,dur=1):
return False
# Sweep it back halfway
if not GripUtils.go_to(x=forward_amount,y=waypoint_y,z=hover_amount,
roll=roll,pitch=pi/4,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm,dur=2.5):
return False
# Sweep it back the rest of the way
if not GripUtils.go_to(x=forward_amount,y=final_y,z=hover_amount,
roll=roll,pitch=pi/4,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm,dur=2.5):
return False
print "Final_y is: %f"%final_y
return (forward_amount,final_y,hover_amount,
roll,pi/4,drag_yaw)
def executeFold_left(self,gripPt,endPt,color_current='blue',color_next='blue'):
"""
execute fold with left arm
"""
#tmp = self.convert_to_world_frame(endPt)
#print "EndPts",tmp.point.x,tmp.point.y,tmp.point.z
# if blue fold, move arm to gripping position. (if red fold, arm is already holding it at gripping position)
if (color_current == 'blue'):
startpoint = self.dupl_PointStamped(gripPt) #self.convert_to_world_frame(gripPt)
print "grabbing (x,y,z)",startpoint.point.x,startpoint.point.y,startpoint.point.z
if not GripUtils.grab(x = startpoint.point.x,y=startpoint.point.y,z=startpoint.point.z,arm='l',
roll=-pi/2,yaw=-pi/3,pitch=pi/4,approach= True,frame=startpoint.header.frame_id):
return False
midpoint = self.dupl_PointStamped(endPt) #self.convert_to_world_frame(endPt)
midpoint.point.x = (midpoint.point.x + startpoint.point.x)/2
midpoint.point.y = (midpoint.point.y + startpoint.point.y)/2
midpoint.point.z = (midpoint.point.z + 0.1)
pitch = pi/4
roll = -pi/2
yaw=-pi/2
grip=True
frame= midpoint.header.frame_id
#print " going to ",midpoint.point.x,midpoint.point.y,midpoint.point.z
if not GripUtils.go_to(x=midpoint.point.x,y=midpoint.point.y,z=midpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='l',dur=7.5):
print "failure to go to ",midpoint.point.x,midpoint.point.y,midpoint.point.z
#return False
endpoint = self.dupl_PointStamped(endPt) #self.convert_to_world_frame(endPt)
frame=endpoint.header.frame_id
if not GripUtils.go_to(x=endpoint.point.x,y=endpoint.point.y,z=endpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='l',dur=7.5):
print "failure"
#return False
if(color_next == 'blue'):
initRobot()
return True
def execute(self, userdata):
height = 0.35
lateral_amount = 0.65
forward_amount = 0.3
if self.arm == 'b':
lateral_amount_r = lateral_amount * -1
lateral_amount_l = lateral_amount
if not GripUtils.go_to_multi(x_l=forward_amount,
y_l=lateral_amount_l,
z_l=height,
roll_l=0,
pitch_l=0,
yaw_l=0,
frame_l="torso_lift_link",
grip_l=False,
x_r=forward_amount,
y_r=lateral_amount_r,
z_r=height,
roll_r=0,
pitch_r=0,
yaw_r=0,
frame_r="torso_lift_link",
grip_r=False,
dur=4.0):
return FAILURE
else:
return SUCCESS
else:
if self.arm == 'r':
lateral_amount = lateral_amount * -1
if not GripUtils.go_to(x=forward_amount,
y=lateral_amount,
z=height,
roll=0,
pitch=0,
yaw=0,
grip=False,
frame="torso_lift_link",
dur=4.0,
arm=self.arm):
return FAILURE
else:
return SUCCESS
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
def fold_cloth(dir='l'):
print "Folding cloth"
_cloth_tracker.scoot(-0.1)
GripUtils.recall_arm("b")
(waist_l_base, waist_r_base) = get_waist_points()
now = rospy.Time.now()
waist_l_base.header.stamp = now
waist_r_base.header.stamp = now
_listener.waitForTransform("table_height", waist_l_base.header.frame_id,
now, rospy.Duration(5.0))
waist_l = _listener.transformPoint("table_height", waist_l_base)
waist_r = _listener.transformPoint("table_height", waist_r_base)
waist_l.point.z = 0
waist_r.point.z = 0
#Grab the waist point
smooth()
GripUtils.grab_point(waist_l, arm="l", yaw=-pi / 2, x_offset=0.02)
#Fold over
ctr_x = (waist_l.point.x + waist_r.point.x) / 2 + 0.02
ctr_y = (waist_l.point.y + waist_r.point.y) / 2
ctr_z = waist_l.point.y - ctr_y
GripUtils.go_to(x=ctr_x,
y=ctr_y,
z=ctr_z,
roll=pi / 2,
pitch=pi / 2,
yaw=-pi / 2,
arm="l",
grip=True,
frame=waist_l.header.frame_id)
GripUtils.go_to_pt(waist_r,
arm="l",
roll=pi / 2,
pitch=3 * pi / 4,
yaw=-pi / 2,
grip=True,
y_offset=0.01,
x_offset=0.02)
GripUtils.go_to_pt(waist_r,
arm="l",
roll=pi / 2,
pitch=3 * pi / 4,
yaw=-pi / 2,
grip=False,
y_offset=-0.05,
x_offset=0.02,
dur=2.5)
GripUtils.go_to_pt(waist_r,
arm="l",
roll=pi / 2,
pitch=3 * pi / 4,
yaw=-pi / 2,
grip=False,
y_offset=-0.05,
x_offset=0.02,
z_offset=0.05,
dur=2.5)
GripUtils.recall_arm("b")
#Grab waist
scoot_amt = 0.2
_cloth_tracker.scoot(-scoot_amt)
ctr_x = 0.25 * waist_l.point.x + 0.75 * waist_r.point.x + scoot_amt
ctr_y = 0.25 * waist_l.point.y + 0.75 * waist_r.point.y
ctr_z = waist_l.point.z
GripUtils.grab(x=ctr_x,
y=ctr_y,
z=ctr_z,
roll=pi / 2,
pitch=pi / 4,
yaw=0,
arm="r",
frame=waist_l.header.frame_id)
sweep_drag_amount = 0.95 * TABLE_WIDTH
sweep_lift_amount = 0.6
(x, y, z, r, p, yw) = sweep_cloth_with_scoot("r",
sweep_drag_amount,
TABLE_WIDTH,
sweep_lift_amount,
roll=pi / 2)
print "y is %f" % y
print "(%f,%f,%f,%f,%f,%f)" % (x, y, z, r, p, y)
smooth("l")
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH / 4 - 0.03,
z=PANTS_LENGTH / 4,
roll=r,
pitch=3 * pi / 8,
yaw=yw,
arm="r",
grip=True,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH / 2 - 0.03,
z=PANTS_LENGTH / 2,
roll=r,
pitch=pi / 2,
yaw=yw,
arm="r",
grip=True,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + 3 * PANTS_LENGTH / 4 - 0.03,
z=PANTS_LENGTH / 4,
roll=r,
pitch=5 * pi / 8,
yaw=yw,
arm="r",
grip=True,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH - 0.03,
z=0.01,
roll=r,
pitch=pi - p,
yaw=yw,
arm="r",
grip=True,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH + 0.05,
z=0.01,
roll=r,
pitch=pi - p,
yaw=yw,
arm="r",
grip=False,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH + 0.05,
z=0.1,
roll=r,
pitch=pi - p,
yaw=yw,
arm="r",
grip=False,
frame="table_height",
dur=2.5)
GripUtils.recall_arm("r")
smooth()
_cloth_tracker.scoot(scoot_amt + 0.08)
return True
def fold_cloth(dir='l'):
print "Folding cloth"
_cloth_tracker.scoot(-0.1)
GripUtils.recall_arm("b")
(waist_l_base,waist_r_base) = get_waist_points()
now = rospy.Time.now()
waist_l_base.header.stamp = now
waist_r_base.header.stamp = now
_listener.waitForTransform("table_height",waist_l_base.header.frame_id,now,rospy.Duration(5.0))
waist_l = _listener.transformPoint("table_height",waist_l_base)
waist_r= _listener.transformPoint("table_height",waist_r_base)
waist_l.point.z = 0
waist_r.point.z = 0
#Grab the waist point
smooth()
GripUtils.grab_point(waist_l,arm="l",yaw=-pi/2,x_offset=0.02)
#Fold over
ctr_x = (waist_l.point.x + waist_r.point.x)/2+0.02
ctr_y = (waist_l.point.y + waist_r.point.y)/2
ctr_z = waist_l.point.y - ctr_y
GripUtils.go_to( x=ctr_x, y=ctr_y, z=ctr_z,
roll=pi/2, pitch=pi/2, yaw=-pi/2,
arm="l", grip=True, frame=waist_l.header.frame_id)
GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=True,y_offset=0.01,x_offset=0.02)
GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=False,y_offset=-0.05,x_offset=0.02,dur=2.5)
GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=False,y_offset=-0.05,x_offset=0.02,z_offset=0.05,dur=2.5)
GripUtils.recall_arm("b")
#Grab waist
scoot_amt = 0.2
_cloth_tracker.scoot(-scoot_amt)
ctr_x = 0.25 * waist_l.point.x + 0.75 * waist_r.point.x + scoot_amt
ctr_y = 0.25 * waist_l.point.y + 0.75 * waist_r.point.y
ctr_z = waist_l.point.z
GripUtils.grab( x=ctr_x, y=ctr_y, z=ctr_z,
roll=pi/2, pitch=pi/4, yaw=0,
arm="r", frame=waist_l.header.frame_id)
sweep_drag_amount = 0.95*TABLE_WIDTH
sweep_lift_amount = 0.6
(x,y,z,r,p,yw) = sweep_cloth_with_scoot("r", sweep_drag_amount, TABLE_WIDTH, sweep_lift_amount,roll=pi/2)
print "y is %f"%y
print "(%f,%f,%f,%f,%f,%f)"%(x,y,z,r,p,y)
smooth("l")
GripUtils.go_to( x=x, y=y+PANTS_LENGTH/4-0.03, z=PANTS_LENGTH/4,
roll=r, pitch=3*pi/8, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH/2-0.03, z=PANTS_LENGTH/2,
roll=r, pitch=pi/2, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+3*PANTS_LENGTH/4-0.03, z=PANTS_LENGTH/4,
roll=r, pitch=5*pi/8, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH-0.03, z=0.01,
roll=r, pitch=pi-p, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH+0.05, z=0.01,
roll=r, pitch=pi-p, yaw=yw,
arm="r", grip=False, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH+0.05, z=0.1,
roll=r, pitch=pi-p, yaw=yw,
arm="r", grip=False, frame="table_height", dur=2.5)
GripUtils.recall_arm("r")
smooth()
_cloth_tracker.scoot(scoot_amt+0.08)
return True
def go_to_folding_station():
print "Going to folding station"
DryerNavigationUtils.dryerToTable1()
GripUtils.go_to(arm="l",x=0.5,y=0,z=0.35,roll=0,yaw=0,pitch=pi/2,grip=True,frame="table_height")
GripUtils.open_gripper("l")
return True
def open_sock():
x = 0.5
y = 0.0
z = TABLE_HEIGHT + 0.4
frame = "base_footprint"
#old strategy
GripUtils.go_to(x=x,
y=y + 0.007,
z=z,
roll=pi / 2,
yaw=pi / 2,
pitch=-pi / 8,
grip=True,
frame=frame,
arm="r",
dur=5.0)
GripUtils.go_to(x=x,
y=y + 0.05,
z=z,
roll=pi / 2,
yaw=-pi / 2,
pitch=0,
grip=False,
frame=frame,
arm="l",
dur=5.0)
GripUtils.go_to(x=x,
y=y - 0.004,
z=z + 0.01,
roll=pi / 2,
yaw=-pi / 2,
pitch=pi / 5,
grip=False,
frame=frame,
arm="l",
dur=5.0)
GripUtils.go_to(x=x,
y=y - 0.0035,
z=z + 0.001,
roll=pi / 2,
yaw=-pi / 2,
pitch=pi / 5,
grip=True,
frame=frame,
arm="l",
dur=2.0) #changed from -0.007 to -0.0065
#GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=0,grip=True,frame=frame,arm="r",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.04,z=z+0.05,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.04,z=z+0.005,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
#GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/16,grip=True,frame=frame,arm="r",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.06,z=z-0.025,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.025,z=z-0.225,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=True,frame=frame,arm="l",dur=1.0)
#GripUtils.go_to(x=x,y=y+0.025,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=True,frame=frame,arm="l",dur=5.0)
GripUtils.close_gripper("l", extra_tight=True)
open_amt = 0.025
GripUtils.go_to_multi(x_l=x,
y_l=y + open_amt,
z_l=z + 0.01,
roll_l=pi / 2,
yaw_l=-pi / 2,
pitch_l=0,
grip_l=True,
frame_l=frame,
x_r=x,
y_r=y - open_amt,
z_r=z + 0.01,
roll_r=pi / 2,
yaw_r=pi / 2,
pitch_r=0,
grip_r=True,
frame_r=frame,
dur=5.0)
def take_off_dopple(pt, arm):
x = DOPPLE_X
y = DOPPLE_Y
if arm == "l":
yaw = -pi / 2
y -= 0.005
else:
yaw = pi / 2
y += 0.005
z = DOPPLE_HEIGHT + TABLE_HEIGHT - 0.045
frame = "base_footprint"
GripUtils.go_to(x=x,
y=y,
z=z,
roll=0,
yaw=yaw,
pitch=pi / 4,
arm=arm,
grip=False,
frame=frame,
dur=5.0)
GripUtils.close_gripper(arm, extra_tight=False)
GripUtils.go_to(x=x,
y=y,
z=z + 0.1,
roll=0,
yaw=yaw,
pitch=0,
arm=arm,
grip=True,
frame=frame,
dur=5.0)
GripUtils.go_to(x=x,
y=y,
z=z + 0.2,
roll=0,
yaw=yaw,
pitch=0,
arm=arm,
grip=True,
frame=frame,
dur=5.0)
roll = 0
if arm == "l":
yaw = -pi / 2
else:
yaw = pi / 2
GripUtils.go_to(x=x - 0.15,
y=y,
z=z + 0.2,
roll=roll,
yaw=yaw,
pitch=0,
arm=arm,
grip=True,
frame=frame,
dur=5.0)
#Spreads out
GripUtils.go_to_pt(pt,
roll=roll,
yaw=yaw,
pitch=0,
arm=arm,
grip=True,
z_offset=0.2,
y_offset=0.2,
dur=5.0) # previously z_o= 0.08 y_o 0.1
GripUtils.go_to_pt(pt,
roll=roll,
yaw=yaw,
pitch=pi / 2,
arm=arm,
grip=True,
dur=7.5,
y_offset=-0.2,
z_offset=0.015)
GripUtils.open_gripper(arm)
GripUtils.go_to_pt(pt,
roll=roll,
yaw=yaw,
pitch=pi / 4,
arm=arm,
grip=False,
dur=2.5,
y_offset=-0.2,
z_offset=0.025)
StanceUtils.call_stance("arms_up", 5.0)
def execute(self, userdata):
GripUtils.go_to(0.58, 0, 0.15, 0, pi / 4, 0, True, "torso_lift_link",
"l")
GripUtils.open_gripper("l")
GripUtils.go_to(0.3, 0.65, 0.35, 0, 0, 0, True, "torso_lift_link", "l")
return SUCCESS
def execute(self, userdata):
GripUtils.go_to(0.3, 0.65, 0.35, 0, 0, 0, True, "torso_lift_link", "l")
return SUCCESS
def execute(self, userdata):
pt_bl = userdata.bl
pt_tl = userdata.tl
pt_br = userdata.br
pt_tr = userdata.tr
(bl_x, bl_y, bl_z) = (pt_bl.point.x, pt_bl.point.y, pt_bl.point.z)
(tl_x, tl_y, tl_z) = (pt_tl.point.x, pt_tl.point.y, pt_tl.point.z)
(br_x, br_y, br_z) = (pt_br.point.x, pt_br.point.y, pt_br.point.z)
(tr_x, tr_y, tr_z) = (pt_tr.point.x, pt_tr.point.y, pt_tr.point.z)
ctr_l_x = .75 * bl_x + .25 * tl_x
#Make more centered
ctr_l_x -= 0.01
ctr_l_y = .75 * bl_y + .25 * tl_y
z = bl_z
yaw = -pi / 2
roll = -pi / 2
pitch = pi / 4
frame = pt_bl.header.frame_id
if not GripUtils.grab(x=ctr_l_x,
y=ctr_l_y,
z=z,
roll=roll,
yaw=yaw,
pitch=pitch,
arm="l",
frame=frame):
return FAILURE
ctr_r_x = .75 * br_x + .25 * tr_x
#Make more centered
ctr_r_x -= 0.01
ctr_r_y = .75 * br_y + .25 * tr_y
alpha = 0.333
ctr_ml_x = (1 - alpha) * ctr_l_x + alpha * ctr_r_x
ctr_ml_y = (1 - alpha) * ctr_l_y + alpha * ctr_r_y
ctr_mr_x = (1 - alpha) * ctr_r_x + alpha * ctr_l_x
ctr_mr_y = (1 - alpha) * ctr_r_y + alpha * ctr_l_y
up_z = z + sqrt((ctr_l_x - ctr_r_x)**2 + (ctr_l_y - ctr_r_y)**2) / 3.0
pitch = pi / 2
if not GripUtils.go_to(x=ctr_ml_x,
y=ctr_ml_y,
z=up_z,
roll=roll,
yaw=yaw,
pitch=pitch,
arm="l",
frame=frame,
grip=True,
dur=5.0):
return FAILURE
if not GripUtils.go_to(x=(ctr_ml_x + ctr_mr_x) / 2.0,
y=(ctr_ml_y + ctr_mr_y + 0.02) / 2.0,
z=(up_z + bl_z) / 2.0,
roll=roll,
yaw=yaw,
pitch=(pitch + 3 * pi / 4) / 2.0,
arm="l",
frame=frame,
grip=True,
dur=5.0):
return FAILURE
z = bl_z
pitch = 3 * pi / 4
if not GripUtils.go_to(x=ctr_mr_x,
y=ctr_mr_y + 0.02,
z=z,
roll=roll,
yaw=yaw,
pitch=pitch,
arm="l",
frame=frame,
grip=True,
dur=5.0):
return FAILURE
yaw = pi / 2
roll = pi / 2
pitch = pi / 4
GripUtils.open_gripper("l")
if not GripUtils.go_to(x=ctr_mr_x,
y=ctr_mr_y - 0.05,
z=z + 0.02,
roll=roll,
yaw=yaw,
pitch=pitch,
arm="l",
frame=frame,
grip=False,
dur=1.0):
return FAILURE
GripUtils.recall_arm("l")
if not GripUtils.grab(x=ctr_r_x,
y=ctr_r_y - 0.01,
z=z,
roll=roll,
yaw=yaw,
pitch=pitch,
arm="r",
frame=frame):
return FAILURE
pitch = pi / 2
if not GripUtils.go_to(x=ctr_mr_x,
y=ctr_mr_y - 0.02,
z=up_z,
roll=roll,
yaw=yaw,
pitch=pitch,
arm="r",
frame=frame,
grip=True,
dur=5.0):
return FAILURE
if not GripUtils.go_to(x=(ctr_mr_x + ctr_ml_x) / 2.0,
y=(ctr_mr_y - 0.02 + ctr_ml_y - 0.02) / 2.0,
z=(up_z + bl_z + 0.01) / 2.0,
roll=roll,
yaw=yaw,
pitch=(pitch + 3 * pi / 4) / 2.0,
arm="r",
frame=frame,
grip=True,
dur=5.0):
return FAILURE
z = bl_z + 0.01
pitch = 3 * pi / 4
if not GripUtils.go_to(x=ctr_ml_x,
y=ctr_ml_y - 0.02,
z=z,
roll=roll,
yaw=yaw,
pitch=pitch,
arm="r",
frame=frame,
grip=True,
dur=5.0):
return FAILURE
GripUtils.open_gripper("r")
if not GripUtils.go_to(x=ctr_ml_x,
y=ctr_ml_y + 0.05,
z=z + 0.02,
roll=roll,
yaw=yaw,
pitch=pitch,
arm="r",
frame=frame,
grip=False,
dur=1.0):
return FAILURE
GripUtils.recall_arm("r")
"""
if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y+0.06,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame
,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y-0.03,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame
,dur=3.0):
return FAILURE
if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame
,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame
,dur=3.0):
return FAILURE
GripUtils.close_gripper("b")
if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.1,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,frame_l=frame
,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.1,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=True,frame_r=frame
,dur=3.0):
return FAILURE
"""
return SUCCESS
def execute(self, userdata):
GripUtils.go_to(0.6,0,0.15,0,0,0,True,"torso_lift_link","l")
GripUtils.open_gripper("l")
return SUCCESS
def execute(self, userdata):
GripUtils.go_to(0.3,0.65,0.35,0,0,0,True,"torso_lift_link","l")
return SUCCESS
def execute(self,userdata):
pt_bl = userdata.bl
pt_tl = userdata.tl
pt_br = userdata.br
pt_tr = userdata.tr
(bl_x,bl_y,bl_z) = (pt_bl.point.x,pt_bl.point.y,pt_bl.point.z)
(tl_x,tl_y,tl_z) = (pt_tl.point.x,pt_tl.point.y,pt_tl.point.z)
(br_x,br_y,br_z) = (pt_br.point.x,pt_br.point.y,pt_br.point.z)
(tr_x,tr_y,tr_z) = (pt_tr.point.x,pt_tr.point.y,pt_tr.point.z)
ctr_l_x = .75*bl_x + .25*tl_x
#Make more centered
ctr_l_x -= 0.01
ctr_l_y = .75*bl_y + .25*tl_y
z = bl_z
yaw = -pi/2
roll = -pi/2
pitch = pi/4
frame = pt_bl.header.frame_id
if not GripUtils.grab(x=ctr_l_x,y=ctr_l_y,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame):
return FAILURE
ctr_r_x = .75*br_x + .25*tr_x
#Make more centered
ctr_r_x -= 0.01
ctr_r_y = .75*br_y + .25*tr_y
alpha = 0.333
ctr_ml_x = (1-alpha)*ctr_l_x + alpha*ctr_r_x
ctr_ml_y = (1-alpha)*ctr_l_y + alpha*ctr_r_y
ctr_mr_x = (1-alpha)*ctr_r_x + alpha*ctr_l_x
ctr_mr_y = (1-alpha)*ctr_r_y + alpha*ctr_l_y
up_z = z+sqrt( (ctr_l_x - ctr_r_x)**2 + (ctr_l_y - ctr_r_y)**2) / 3.0
pitch = pi/2
if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y,z=up_z,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame,grip=True,dur=5.0):
return FAILURE
if not GripUtils.go_to(x=(ctr_ml_x+ctr_mr_x)/2.0,y=(ctr_ml_y+ctr_mr_y+0.02)/2.0,z=(up_z+bl_z)/2.0,roll=roll,yaw=yaw,pitch=(pitch+3*pi/4)/2.0,arm="l",frame=frame,grip=True,dur=5.0):
return FAILURE
z = bl_z
pitch = 3*pi/4
if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y+0.02,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame,grip=True,dur=5.0):
return FAILURE
yaw = pi/2
roll = pi/2
pitch = pi/4
GripUtils.open_gripper("l")
if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y-0.05,z=z+0.02,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame,grip=False,dur=1.0):
return FAILURE
GripUtils.recall_arm("l")
if not GripUtils.grab(x=ctr_r_x,y=ctr_r_y-0.01,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame):
return FAILURE
pitch = pi/2
if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y-0.02,z=up_z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0):
return FAILURE
if not GripUtils.go_to(x=(ctr_mr_x+ctr_ml_x)/2.0,y=(ctr_mr_y-0.02+ctr_ml_y-0.02)/2.0,z=(up_z+bl_z+0.01)/2.0,roll=roll,yaw=yaw,pitch=(pitch+3*pi/4)/2.0,arm="r",frame=frame,grip=True,dur=5.0):
return FAILURE
z = bl_z+0.01
pitch = 3*pi/4
if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y-0.02,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0):
return FAILURE
GripUtils.open_gripper("r")
if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y+0.05,z=z+0.02,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=False,dur=1.0):
return FAILURE
GripUtils.recall_arm("r")
"""
if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y+0.06,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame
,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y-0.03,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame
,dur=3.0):
return FAILURE
if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame
,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame
,dur=3.0):
return FAILURE
GripUtils.close_gripper("b")
if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.1,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,frame_l=frame
,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.1,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=True,frame_r=frame
,dur=3.0):
return FAILURE
"""
return SUCCESS
def execute(self, userdata):
forward_amount = self.forward_amount
hover_amount = 0.06
if self.direction == "r":
pr2_say(talk_drag2)
drag_arm = "r"
edge_y = self.table_width / 2.0
drag_yaw = pi / 2
final_y = edge_y - self.drag_amount
else:
pr2_say(talk_drag1)
drag_arm = "l"
edge_y = -1 * self.table_width / 2.0
drag_yaw = -pi / 2
final_y = edge_y + self.drag_amount
#roll = 0
roll = pi / 2
GripUtils.go_to(x=forward_amount,
y=edge_y,
z=self.lift_amount,
roll=roll,
pitch=0,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=2.0)
rospy.sleep(1.0) #Give time to stabilize
if not GripUtils.go_to(x=forward_amount,
y=edge_y,
z=hover_amount,
roll=roll,
pitch=pi / 4,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=2.0):
return FAILURE
if not GripUtils.go_to(x=forward_amount,
y=(edge_y + final_y) / 2.0,
z=hover_amount,
roll=roll,
pitch=pi / 4,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=2.5):
return FAILURE
if not GripUtils.go_to(x=forward_amount,
y=final_y,
z=hover_amount,
roll=roll,
pitch=pi / 4,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=2.5):
return FAILURE
return SUCCESS
def sweep_cloth_with_scoot(direction,
drag_amount,
table_width,
lift_amount,
roll=0,
scoot=0):
forward_amount = 0.5
hover_amount = 0.06
edge_distance = (table_width / 2.0) + .04
edge_distance_low = (table_width / 2.0)
if direction == "r":
drag_arm = "r"
edge_y_high = edge_distance
edge_y_low = edge_distance_low
drag_yaw = pi / 2
waypoint_y = edge_y_low - (drag_amount / 2.0)
final_y = edge_y_low - drag_amount
else:
drag_arm = "l"
edge_y_high = -1 * edge_distance
edge_y_low = -1 * edge_distance_low
drag_yaw = -pi / 2
waypoint_y = edge_y_low + (drag_amount / 2.0)
final_y = edge_y_low + drag_amount
# Take it to table edge and hold it high
if not GripUtils.go_to(x=forward_amount - .21,
y=edge_y_high,
z=lift_amount + 0.01,
roll=roll,
pitch=0,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=3):
return False
for i in range(2):
GripUtils.go_to(x=forward_amount - .21,
y=edge_y_high - .02,
z=lift_amount - .2,
roll=roll,
pitch=0,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=1.0 / (0.7 * i + 1))
# Lower it at table edge
if not GripUtils.go_to(x=forward_amount,
y=edge_y_low,
z=hover_amount,
roll=roll,
pitch=pi / 4,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=1):
return False
# Sweep it back halfway
if not GripUtils.go_to(x=forward_amount,
y=waypoint_y,
z=hover_amount,
roll=roll,
pitch=pi / 4,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=2.5):
return False
# Sweep it back the rest of the way
if not GripUtils.go_to(x=forward_amount,
y=final_y,
z=hover_amount,
roll=roll,
pitch=pi / 4,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=2.5):
return False
print "Final_y is: %f" % final_y
return (forward_amount, final_y, hover_amount, roll, pi / 4, drag_yaw)
