def execute(self, userdata):
if self.side == 'l':
arm = 'l'
processor = 'far_left_finder_node'
yaw = -pi/2
else:
arm = 'r'
processor = 'far_right_finder_node'
yaw = pi/2
process_mono = rospy.ServiceProxy("%s/process_mono"%processor,ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
pt_opp = resp.pts3d[1]
userdata.arm = 'l' if arm == 'r' else 'r' #Arm returns the arm which is currently gripping
homogeneity = resp.params[resp.param_names.index("homogeneity")]
if homogeneity > 0.13:
print "Too homogeneous: %f"%homogeneity
return FAILURE
userdata.cloth_width = sqrt( (pt_opp.point.x - pt.point.x)**2 + (pt_opp.point.y - pt.point.y)**2 )
#userdata.cloth_width = abs(pt_opp.point.y - pt.point.y)
userdata.cloth_height = None
if arm == "l":
x_offset = -0.02 #was -0.01
else:
x_offset = -0.01
if not GripUtils.grab_point(pt,roll=-pi/2,yaw=yaw,arm=arm,x_offset=x_offset):
return FAILURE
else:
if self.let_go:
GripUtils.open_gripper(opp_arm(arm))
return SUCCESS
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):
if self.side == 'l':
arm = 'l'
processor = 'far_left_finder_node'
yaw = -pi / 2
else:
arm = 'r'
processor = 'far_right_finder_node'
yaw = pi / 2
process_mono = rospy.ServiceProxy("%s/process_mono" % processor,
ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
pt_opp = resp.pts3d[1]
userdata.arm = 'l' if arm == 'r' else 'r' #Arm returns the arm which is currently gripping
homogeneity = resp.params[resp.param_names.index("homogeneity")]
if homogeneity > 0.13:
print "Too homogeneous: %f" % homogeneity
return FAILURE
userdata.cloth_width = sqrt((pt_opp.point.x - pt.point.x)**2 +
(pt_opp.point.y - pt.point.y)**2)
#userdata.cloth_width = abs(pt_opp.point.y - pt.point.y)
userdata.cloth_height = None
if arm == "l":
x_offset = -0.02 #was -0.01
else:
x_offset = -0.01
if not GripUtils.grab_point(
pt, roll=-pi / 2, yaw=yaw, arm=arm, x_offset=x_offset):
return FAILURE
else:
if self.let_go:
GripUtils.open_gripper(opp_arm(arm))
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):
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 execute(self, userdata):
process_mono = rospy.ServiceProxy("clump_center_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
z_offset = 0.16
GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",z_offset=z_offset,grip=False,dur=5.0)
GripUtils.open_gripper("l")
return SUCCESS
def add_default_stances(self):
rospy.Service("stances/pause", ExecuteStance, self.pause)
rospy.Service("stances/open_left", ExecuteStance, lambda req: GripUtils.open_gripper("l"))
rospy.Service("stances/open_right", ExecuteStance, lambda req: GripUtils.open_gripper("r"))
rospy.Service("stances/open_both", ExecuteStance, lambda req: GripUtils.open_gripper("b"))
rospy.Service("stances/close_left", ExecuteStance, lambda req: GripUtils.close_gripper("l"))
rospy.Service("stances/close_right", ExecuteStance, lambda req: GripUtils.close_gripper("r"))
rospy.Service("stances/close_both", ExecuteStance, lambda req: GripUtils.close_gripper("b"))
self.default_stances = ()
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 execute(self, userdata):
if self.arm == "l":
arm = "l"
processor = "far_left_finder_node"
yaw = -pi / 2
roll = -pi / 2
else:
arm = "r"
processor = "far_right_finder_node"
yaw = pi / 2
roll = pi / 2
process_mono = rospy.ServiceProxy("%s/process_mono" % processor, ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
pt_opp = resp.pts3d[1]
userdata.arm = "l" if arm == "r" else "r" # Arm returns the arm which is currently gripping
homogeneity = resp.params[resp.param_names.index("homogeneity")]
if homogeneity > 0.13:
print "Too homogeneous: %f" % homogeneity
return FAILURE
userdata.cloth_width = sqrt((pt_opp.point.x - pt.point.x) ** 2 + (pt_opp.point.y - pt.point.y) ** 2)
# userdata.cloth_width = abs(pt_opp.point.y - pt.point.y)
userdata.cloth_height = None
# Offsets for safety
if arm == "l":
y_offset = -0.02
z_offset = 0.01
else:
y_offset = 0.02
x_offset = 0.02
if self.side == "t":
x_offset = -0.01
elif self.side == "b":
x_offset = 0.01
else:
x_offset = 0
if not GripUtils.grab_point(pt, roll=roll, yaw=yaw, arm=arm, x_offset=x_offset, y_offset=y_offset):
return FAILURE
else:
if self.let_go:
GripUtils.open_gripper(opp_arm(arm))
if self.recall:
GripUtils.recall_arm(arm, grip=True)
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 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):
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 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.6,0,0.15,0,0,0,True,"torso_lift_link","l")
GripUtils.open_gripper("l")
return SUCCESS
def execute(self,userdata):
#cloth_width = userdata.cloth_width*.85
cloth_width = userdata.cloth_width * 0.885
cloth_height = userdata.cloth_height
start_x = 0.20
end_x = start_x + self.forward_amount
down_height = 0.03
up_height = 0.625
return_val = SUCCESS
if not GripUtils.go_to_multi(x_l=start_x,y_l=cloth_width/2.0,z_l=up_height,
roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True,
x_r=start_x,y_r=-cloth_width/2.0,z_r=up_height,
roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=4.0):
return_val = FAILURE
if not cloth_height:
if not GripUtils.go_to_multi(x_l=start_x,y_l=cloth_width/2.0,z_l=(0.75*up_height + 0.25*down_height),
roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,
x_r=start_x,y_r=-cloth_width/2.0,z_r=(0.75*up_height + 0.25*down_height),
roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=3.0):
return_val = FAILURE
if not GripUtils.go_to_multi(x_l=(start_x + end_x)/2.5,y_l=cloth_width/2.0,z_l=0.25*up_height + 0.75*down_height,
roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,
x_r=(start_x+end_x)/2.5,y_r=-cloth_width/2.0,z_r=0.25*up_height + 0.75*down_height,
roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=3.0):
return_val = FAILURE
if not GripUtils.go_to_multi(x_l=end_x-0.1,y_l=cloth_width/2.0,z_l=0.25*up_height + 0.75*down_height,
roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,
x_r=end_x-0.1,y_r=-cloth_width/2.0,z_r=0.25*up_height + 0.75*down_height,
roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=3.0):
return_val = FAILURE
if not GripUtils.go_to_multi(x_l=end_x+0.1,y_l=cloth_width/2.0,z_l=down_height+0.03,
roll_l=0,pitch_l=pi/4,yaw_l=-pi/5,grip_l=True,
x_r=end_x+0.1,y_r=-cloth_width/2.0,z_r=down_height+0.03,
roll_r=0,pitch_r=pi/4,yaw_r=pi/5,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=3.0):
return_val = FAILURE
else:
if not GripUtils.go_to_multi(x_l=start_x,y_l=cloth_width/2.0,z_l=cloth_height+down_height,
roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,
x_r=start_x,y_r=-cloth_width/2.0,z_r=cloth_height+down_height,
roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=3.0):
return_val = FAILURE
if not GripUtils.go_to_multi(x_l=end_x - 0.1 - cloth_height/1.5,y_l=cloth_width/2.0,z_l=cloth_height/1.5+down_height+0.03,
roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,
x_r=end_x - 0.1 - cloth_height/1.5,y_r=-cloth_width/2.0,z_r=cloth_height/1.5+down_height+0.03,
roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=3.0):
return_val = FAILURE
if not GripUtils.go_to_multi(x_l=end_x-0.1,y_l=cloth_width/2.0,z_l=down_height+0.1,
roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,
x_r=end_x-0.1,y_r=-cloth_width/2.0,z_r=down_height+0.1,
roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=3.0):
return_val = FAILURE
cloth_width *= 1.05
if not GripUtils.go_to_multi(x_l=end_x+0.1,y_l=cloth_width/2.0,z_l=down_height+0.03,
roll_l=0,pitch_l=pi/4,yaw_l=-pi/5,grip_l=True,
x_r=end_x+0.1,y_r=-cloth_width/2.0,z_r=down_height+0.03,
roll_r=0,pitch_r=pi/4,yaw_r=pi/5,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=3.0):
return_val = FAILURE
if self.recall:
GripUtils.open_gripper("b")
GripUtils.recall_arm("b")
return SUCCESS #was return_val
def wiggle_down_dopple(back_up=True):
open_amt = 0.02 #original 0.025
x = DOPPLE_X
y = DOPPLE_Y
z = DOPPLE_HEIGHT + TABLE_HEIGHT
x_l = x
x_r = x
y_l = y + open_amt
y_r = y - open_amt
z_l = z
z_r = z
roll_l = pi / 2
roll_r = pi / 2
yaw_l = -pi / 2
yaw_r = pi / 2
pitch_l = 0
pitch_r = 0
frame_l = "base_footprint"
frame_r = "base_footprint"
grip_l = grip_r = True
offsetToLeft = 0.01
GripUtils.go_to_multi(x_l=x_l + 0.005,
y_l=y_l - offsetToLeft,
z_l=z_l,
roll_l=roll_l + 1.0 / 5,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r + 0.005,
y_r=y_r - offsetToLeft,
z_r=z_r,
roll_r=roll_r - 1.0 / 5,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=5.0)
GripUtils.go_to_multi(x_l=x_l + 0.005,
y_l=y_l,
z_l=z_l,
roll_l=roll_l + 1.0 / 5,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r + 0.005,
y_r=y_r,
z_r=z_r,
roll_r=roll_r - 1.0 / 5,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=5.0)
z_l -= 0.04
z_r -= 0.04
GripUtils.go_to_multi(x_l=x_l,
y_l=y_l,
z_l=z_l,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r,
y_r=y_r,
z_r=z_r,
roll_r=roll_r,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=2.0)
x_step = 0.015
#z_step = 0.0175
z_step = 0.02
#max_iters = (DOPPLE_HEIGHT - 0.15) / z_step * 2
max_iters = 27
new_z_l = z_l
new_z_r = z_r
print "Going for %d wiggle iterations" % max_iters
for i in range(3):
new_z_l -= 2.0 * z_step
new_z_r -= 2.0 * z_step
GripUtils.go_to_multi(x_l=x_l,
y_l=y_l,
z_l=new_z_l,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r,
y_r=y_r,
z_r=new_z_r,
roll_r=roll_r,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=1)
new_z_l += 1 * z_step
new_z_r += 1 * z_step
GripUtils.go_to_multi(x_l=x_l,
y_l=y_l,
z_l=new_z_l,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r,
y_r=y_r,
z_r=new_z_r,
roll_r=roll_r,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=1)
xWiggleList = [0, 0.02, 0, -0.02]
yWiggleList = [0.02, 0, -0.02, 0]
new_z_l += 2.5 * z_step
new_z_r += 2.5 * z_step
for i in range(max_iters):
currentXoffset = xWiggleList[i % 4]
currentYoffset = yWiggleList[i % 4]
new_z_l -= 5.0 * z_step
new_z_r -= 5.0 * z_step
GripUtils.go_to_multi(x_l=x_l + currentXoffset,
y_l=y_l + currentYoffset,
z_l=new_z_l,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r + currentXoffset,
y_r=y_r + currentYoffset,
z_r=new_z_r,
roll_r=roll_r,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=2)
new_z_l += 5.0 * z_step
new_z_r += 5.0 * z_step
GripUtils.go_to_multi(x_l=x_l + currentXoffset,
y_l=y_l + currentYoffset,
z_l=new_z_l,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r + currentXoffset,
y_r=y_r + currentYoffset,
z_r=new_z_r,
roll_r=roll_r,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=2)
new_z_l -= 5.0 * z_step
new_z_r -= 5.0 * z_step
GripUtils.go_to_multi(x_l=x_l + currentXoffset,
y_l=y_l + currentYoffset,
z_l=new_z_l,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r + currentXoffset,
y_r=y_r + currentYoffset,
z_r=new_z_r,
roll_r=roll_r,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=2)
new_z_l += 4.75 * z_step
new_z_r += 4.75 * z_step
GripUtils.go_to_multi(x_l=x_l + currentXoffset,
y_l=y_l + currentYoffset,
z_l=new_z_l,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r + currentXoffset,
y_r=y_r + currentYoffset,
z_r=new_z_r,
roll_r=roll_r,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=2)
new_z_l = TABLE_HEIGHT + 0.2
new_z_r = TABLE_HEIGHT + 0.2
GripUtils.go_to_multi(x_l=x_l + currentXoffset,
y_l=y_l + currentYoffset,
z_l=new_z_l,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r + currentXoffset,
y_r=y_r + currentYoffset,
z_r=new_z_r,
roll_r=roll_r,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=3)
new_z_l = TABLE_HEIGHT + 0.3
new_z_r = TABLE_HEIGHT + 0.3
GripUtils.go_to_multi(x_l=x_l + currentXoffset,
y_l=y_l + currentYoffset,
z_l=new_z_l,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r + currentXoffset,
y_r=y_r + currentYoffset,
z_r=new_z_r,
roll_r=roll_r,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=3)
#new_z_l = TABLE_HEIGHT + 0.05
#new_z_r = TABLE_HEIGHT + 0.05
#
#GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=5)
new_z_l = TABLE_HEIGHT + 0.1
new_z_r = TABLE_HEIGHT + 0.1
GripUtils.go_to_multi(x_l=x_l + currentXoffset,
y_l=y_l + currentYoffset,
z_l=new_z_l,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r + currentXoffset,
y_r=y_r + currentYoffset,
z_r=new_z_r,
roll_r=roll_r,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=3)
new_z_l = TABLE_HEIGHT + 0.2
new_z_r = TABLE_HEIGHT + 0.2
GripUtils.go_to_multi(x_l=x_l + currentXoffset,
y_l=y_l + currentYoffset,
z_l=new_z_l,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r + currentXoffset,
y_r=y_r + currentYoffset,
z_r=new_z_r,
roll_r=roll_r,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=3)
new_z_l = TABLE_HEIGHT + 0.05
new_z_r = TABLE_HEIGHT + 0.05
GripUtils.go_to_multi(x_l=x_l + currentXoffset,
y_l=y_l + currentYoffset,
z_l=new_z_l,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r + currentXoffset,
y_r=y_r + currentYoffset,
z_r=new_z_r,
roll_r=roll_r,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame_r,
dur=3)
new_z_l = TABLE_HEIGHT + 0.15
new_z_r = TABLE_HEIGHT + 0.15
GripUtils.go_to_multi(x_l=x_l + currentXoffset,
y_l=y_l + currentYoffset,
z_l=new_z_l,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l - pi / 8,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r + currentXoffset,
y_r=y_r + currentYoffset,
z_r=new_z_r,
roll_r=roll_r,
yaw_r=yaw_r,
pitch_r=pitch_r - pi / 8,
grip_r=grip_r,
frame_r=frame_r,
dur=3)
#amplitude= 2.0
#new_z_l -= 0.5*amplitude*z_step;
#new_z_r += 0.5*amplitude*z_step;
# new wiggle algorithm
#for i in range(max_iters):
# if (i>3):
# new_z_l += 0.5*amplitude*z_step;
# new_z_r -= 0.5*amplitude*z_step;
# amplitude= 5.0
# new_z_l -= 0.5*amplitude*z_step;
# new_z_r += 0.5*amplitude*z_step;
# new_z_l += amplitude*z_step
# new_z_r -= amplitude*z_step
# new_z_l -= 0.25*z_step
# new_z_r -= 0.25*z_step
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=2)
#
# new_z_l -= amplitude*z_step
# new_z_r += amplitude*z_step
# new_z_l -= 0.25*z_step
# new_z_r -= 0.25*z_step
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=2)
# for i in range(max_iters):
# roll_l = roll_r = pi/2
# new_z_l -= z_step
# new_z_r -= z_step
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=0.75*(i/3 + 1))
#
# x_l += x_step
# x_r -= x_step
# roll_l = 2*pi/5
# roll_r = 3 *pi/5
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=0.75)
# x_l -= 2*x_step
# x_r += 2*x_step
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=0.75)
# roll_l = roll_r = pi/2
# x_l += x_step
# x_r -= x_step
# if back_up:
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=0.75*(i/3 + 1))
#
# #End down
# new_z_l -= z_step
# new_z_r -= z_step
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=5.0)
GripUtils.open_gripper("b")
def wiggle_down_dopple(back_up = True):
open_amt = 0.02 #original 0.025
x = DOPPLE_X
y = DOPPLE_Y
z = DOPPLE_HEIGHT+TABLE_HEIGHT
x_l = x
x_r = x
y_l = y+open_amt
y_r = y-open_amt
z_l = z
z_r = z
roll_l = pi/2
roll_r = pi/2
yaw_l = -pi/2
yaw_r = pi/2
pitch_l = 0
pitch_r = 0
frame_l = "base_footprint"
frame_r = "base_footprint"
grip_l = grip_r = True
offsetToLeft= 0.01
GripUtils.go_to_multi (x_l=x_l+0.005,y_l=y_l-offsetToLeft,z_l=z_l,roll_l=roll_l+1.0/5,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r+0.005,y_r=y_r-offsetToLeft,z_r=z_r,roll_r=roll_r-1.0/5,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=5.0)
GripUtils.go_to_multi (x_l=x_l+0.005,y_l=y_l,z_l=z_l,roll_l=roll_l+1.0/5,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r+0.005,y_r=y_r,z_r=z_r,roll_r=roll_r-1.0/5,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=5.0)
z_l -= 0.04
z_r -= 0.04
GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=2.0)
x_step = 0.015
#z_step = 0.0175
z_step = 0.02
#max_iters = (DOPPLE_HEIGHT - 0.15) / z_step * 2
max_iters = 27;
new_z_l = z_l
new_z_r = z_r
print "Going for %d wiggle iterations"%max_iters
for i in range(3):
new_z_l -= 2.0*z_step;
new_z_r -= 2.0*z_step;
GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=1)
new_z_l += 1*z_step;
new_z_r += 1*z_step;
GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=1)
xWiggleList = [0, 0.02, 0, -0.02]
yWiggleList = [0.02, 0, -0.02, 0]
new_z_l += 2.5*z_step;
new_z_r += 2.5*z_step;
for i in range(max_iters):
currentXoffset= xWiggleList[i%4];
currentYoffset= yWiggleList[i%4];
new_z_l -= 5.0*z_step;
new_z_r -= 5.0*z_step;
GripUtils.go_to_multi (x_l=x_l + currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r + currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=2)
new_z_l += 5.0*z_step;
new_z_r += 5.0*z_step;
GripUtils.go_to_multi (x_l=x_l + currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r + currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=2)
new_z_l -= 5.0*z_step;
new_z_r -= 5.0*z_step;
GripUtils.go_to_multi (x_l=x_l + currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r + currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=2)
new_z_l += 4.75*z_step;
new_z_r += 4.75*z_step;
GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=2)
new_z_l = TABLE_HEIGHT + 0.2
new_z_r = TABLE_HEIGHT + 0.2
GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=3)
new_z_l = TABLE_HEIGHT + 0.3
new_z_r = TABLE_HEIGHT + 0.3
GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=3)
#new_z_l = TABLE_HEIGHT + 0.05
#new_z_r = TABLE_HEIGHT + 0.05
#
#GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=5)
new_z_l = TABLE_HEIGHT + 0.1
new_z_r = TABLE_HEIGHT + 0.1
GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=3)
new_z_l = TABLE_HEIGHT + 0.2
new_z_r = TABLE_HEIGHT + 0.2
GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=3)
new_z_l = TABLE_HEIGHT + 0.05
new_z_r = TABLE_HEIGHT + 0.05
GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
,dur=3)
new_z_l = TABLE_HEIGHT + 0.15
new_z_r = TABLE_HEIGHT + 0.15
GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l-pi/8,grip_l=grip_l,frame_l=frame_l
,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r-pi/8,grip_r=grip_r,frame_r=frame_r
,dur=3)
#amplitude= 2.0
#new_z_l -= 0.5*amplitude*z_step;
#new_z_r += 0.5*amplitude*z_step;
# new wiggle algorithm
#for i in range(max_iters):
# if (i>3):
# new_z_l += 0.5*amplitude*z_step;
# new_z_r -= 0.5*amplitude*z_step;
# amplitude= 5.0
# new_z_l -= 0.5*amplitude*z_step;
# new_z_r += 0.5*amplitude*z_step;
# new_z_l += amplitude*z_step
# new_z_r -= amplitude*z_step
# new_z_l -= 0.25*z_step
# new_z_r -= 0.25*z_step
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=2)
#
# new_z_l -= amplitude*z_step
# new_z_r += amplitude*z_step
# new_z_l -= 0.25*z_step
# new_z_r -= 0.25*z_step
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=2)
# for i in range(max_iters):
# roll_l = roll_r = pi/2
# new_z_l -= z_step
# new_z_r -= z_step
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=0.75*(i/3 + 1))
#
# x_l += x_step
# x_r -= x_step
# roll_l = 2*pi/5
# roll_r = 3 *pi/5
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=0.75)
# x_l -= 2*x_step
# x_r += 2*x_step
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=0.75)
# roll_l = roll_r = pi/2
# x_l += x_step
# x_r -= x_step
# if back_up:
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=0.75*(i/3 + 1))
#
# #End down
# new_z_l -= z_step
# new_z_r -= z_step
# GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
# ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
# ,dur=5.0)
GripUtils.open_gripper("b")
def execute(self, userdata):
if not GripUtils.open_gripper(self.arm):
return FAILURE
else:
return SUCCESS
def execute(self, userdata):
if not GripUtils.open_gripper(self.arm):
return FAILURE
else:
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):
#cloth_width = userdata.cloth_width*.85
cloth_width = userdata.cloth_width * 0.885
cloth_height = userdata.cloth_height
start_x = 0.20
end_x = start_x + self.forward_amount
down_height = 0.03
up_height = 0.625
return_val = SUCCESS
if not GripUtils.go_to_multi(x_l=start_x,
y_l=cloth_width / 2.0,
z_l=up_height,
roll_l=0,
pitch_l=0,
yaw_l=-pi / 2,
grip_l=True,
x_r=start_x,
y_r=-cloth_width / 2.0,
z_r=up_height,
roll_r=0,
pitch_r=0,
yaw_r=pi / 2,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=4.0):
return_val = FAILURE
if not cloth_height:
if not GripUtils.go_to_multi(
x_l=start_x,
y_l=cloth_width / 2.0,
z_l=(0.75 * up_height + 0.25 * down_height),
roll_l=0,
pitch_l=pi / 4,
yaw_l=-pi / 2,
grip_l=True,
x_r=start_x,
y_r=-cloth_width / 2.0,
z_r=(0.75 * up_height + 0.25 * down_height),
roll_r=0,
pitch_r=pi / 4,
yaw_r=pi / 2,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=3.0):
return_val = FAILURE
if not GripUtils.go_to_multi(
x_l=(start_x + end_x) / 2.5,
y_l=cloth_width / 2.0,
z_l=0.25 * up_height + 0.75 * down_height,
roll_l=0,
pitch_l=pi / 4,
yaw_l=-pi / 2,
grip_l=True,
x_r=(start_x + end_x) / 2.5,
y_r=-cloth_width / 2.0,
z_r=0.25 * up_height + 0.75 * down_height,
roll_r=0,
pitch_r=pi / 4,
yaw_r=pi / 2,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=3.0):
return_val = FAILURE
if not GripUtils.go_to_multi(
x_l=end_x - 0.1,
y_l=cloth_width / 2.0,
z_l=0.25 * up_height + 0.75 * down_height,
roll_l=0,
pitch_l=pi / 4,
yaw_l=-pi / 2,
grip_l=True,
x_r=end_x - 0.1,
y_r=-cloth_width / 2.0,
z_r=0.25 * up_height + 0.75 * down_height,
roll_r=0,
pitch_r=pi / 4,
yaw_r=pi / 2,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=3.0):
return_val = FAILURE
if not GripUtils.go_to_multi(x_l=end_x + 0.1,
y_l=cloth_width / 2.0,
z_l=down_height + 0.03,
roll_l=0,
pitch_l=pi / 4,
yaw_l=-pi / 5,
grip_l=True,
x_r=end_x + 0.1,
y_r=-cloth_width / 2.0,
z_r=down_height + 0.03,
roll_r=0,
pitch_r=pi / 4,
yaw_r=pi / 5,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=3.0):
return_val = FAILURE
else:
if not GripUtils.go_to_multi(x_l=start_x,
y_l=cloth_width / 2.0,
z_l=cloth_height + down_height,
roll_l=0,
pitch_l=pi / 4,
yaw_l=-pi / 2,
grip_l=True,
x_r=start_x,
y_r=-cloth_width / 2.0,
z_r=cloth_height + down_height,
roll_r=0,
pitch_r=pi / 4,
yaw_r=pi / 2,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=3.0):
return_val = FAILURE
if not GripUtils.go_to_multi(
x_l=end_x - 0.1 - cloth_height / 1.5,
y_l=cloth_width / 2.0,
z_l=cloth_height / 1.5 + down_height + 0.03,
roll_l=0,
pitch_l=pi / 4,
yaw_l=-pi / 2,
grip_l=True,
x_r=end_x - 0.1 - cloth_height / 1.5,
y_r=-cloth_width / 2.0,
z_r=cloth_height / 1.5 + down_height + 0.03,
roll_r=0,
pitch_r=pi / 4,
yaw_r=pi / 2,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=3.0):
return_val = FAILURE
if not GripUtils.go_to_multi(x_l=end_x - 0.1,
y_l=cloth_width / 2.0,
z_l=down_height + 0.1,
roll_l=0,
pitch_l=pi / 4,
yaw_l=-pi / 2,
grip_l=True,
x_r=end_x - 0.1,
y_r=-cloth_width / 2.0,
z_r=down_height + 0.1,
roll_r=0,
pitch_r=pi / 4,
yaw_r=pi / 2,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=3.0):
return_val = FAILURE
cloth_width *= 1.05
if not GripUtils.go_to_multi(x_l=end_x + 0.1,
y_l=cloth_width / 2.0,
z_l=down_height + 0.03,
roll_l=0,
pitch_l=pi / 4,
yaw_l=-pi / 5,
grip_l=True,
x_r=end_x + 0.1,
y_r=-cloth_width / 2.0,
z_r=down_height + 0.03,
roll_r=0,
pitch_r=pi / 4,
yaw_r=pi / 5,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=3.0):
return_val = FAILURE
if self.recall:
GripUtils.open_gripper("b")
GripUtils.recall_arm("b")
return SUCCESS #was return_val
