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 execute(self,userdata):
pt_bl = userdata.bl
pt_tl = userdata.tl
pt_br = userdata.br
pt_tr = userdata.tr
#if not GripUtils.go_to_pts(pt_tl, -pi/2, -pi/3, pi/4, pt_tr, pi/2, pi/3, pi/4, x_offset_l=-0.04,\
# x_offset_r=-0.04):
# return FAILURE
#FIXME Hard-coded X offsets to compensate for poor calibration. Re-calibrate and REMOVE
'''
if not GripUtils.grab_point(pt_tl,roll=-pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=-0.04):
return FAILURE
if not GripUtils.grab_point(pt_tr,roll=pi/2,yaw= pi/3,pitch=pi/4,arm="r",x_offset=-0.04):
return FAILURE
'''
#FIXME: For some reason large offsets required, #DEBUG
if not GripUtils.grab_points(pt_tl,roll_l=-pi/2,yaw_l=-pi/3,pitch_l=pi/4,x_offset_l=-0.05, z_offset_l=-0.0025
,point_r=pt_tr,roll_r=pi/2,yaw_r= pi/3,pitch_r=pi/4,x_offset_r=-0.06,
y_offset_r=0.00, y_offset_l=-0.00, z_offset_r=0.01):
return FAILURE
(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)
x_l = (tl_x+bl_x)/2.0
y_l = (tl_y+bl_y)/2.0
z_l = bl_z + sqrt( (bl_x-tl_x)**2 + (bl_y-tl_y)**2 )/2.0
x_r = (tr_x+br_x)/2.0
y_r = (tr_y+br_y)/2.0
z_r = br_z + sqrt( (br_x-tr_x)**2 + (br_y-tr_y)**2 )/2.0
pitch_l = pitch_r = pi/4
roll_l = 0
roll_r = 0
yaw_l=-pi/2
yaw_r= pi/2
grip_l=grip_r=True
frame_l=frame_r = pt_bl.header.frame_id
if not GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_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,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
,dur=4.0):
return_val = FAILURE
print "Folding down!"
x_l = bl_x + 0.02 # overshoots fold down
x_r = br_x + 0.02
y_l = bl_y-0.01
y_r = br_y+0.01
z_l = z_r = bl_z + 0.02
yaw_l = -3*pi/4
yaw_r = 3*pi/4
pitch_l=pitch_r = pi/4
roll_l = pi/2
roll_r = -pi/2
GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_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,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
,dur=7.5)
GripUtils.recall_arm("b")
pr2_say(talk_pose2)
return SUCCESS
def execute(self, userdata):
cloth_width = userdata.cloth_width
if cloth_width < 0.35:
rospy.logwarn("Cannot shake at less than 0.35m apart; tried to do %s" % str(cloth_width))
return FAILURE
if cloth_width > 0.65:
cloth_width = 0.65
forward_amount = 0.45
height = 0.625
drop_amount = 0.3
lateral_amount = 0.1
for i in range(self.num_shakes):
if i == 0:
duration = 4.0
if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height,
roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True,
x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height,
roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=duration):
return FAILURE
duration = .45
return_val = SUCCESS
if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0-lateral_amount,z_l=height-drop_amount,
roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,
x_r=forward_amount,y_r=-cloth_width/2.0+lateral_amount,z_r=height-drop_amount,
roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=duration):
return_val = FAILURE
if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height,
roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True,
x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height,
roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=duration):
return_val = FAILURE
cloth_width = cloth_width * self.clothWidthScaleFactor
if not GripUtils.go_to_multi(x_l=forward_amount+.2,y_l=cloth_width/2.0,z_l=height,
roll_l=0,pitch_l=0,yaw_l=-pi/4,grip_l=True,
x_r=forward_amount+.2,y_r=-cloth_width/2.0,z_r=height,
roll_r=0,pitch_r=0,yaw_r=pi/4,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=4.0):
return FAILURE
return SUCCESS
def execute(self, userdata):
height = 0.35
lateral_amount = 0.65
forward_amount = 0.3
if not GripUtils.go_to_multi(x_l=forward_amount,
y_l=lateral_amount,
z_l=height,
roll_l=0,
pitch_l=0,
yaw_l=0,
grip_l=True,
x_r=forward_amount,
y_r=-lateral_amount,
z_r=height,
roll_r=0,
pitch_r=0,
yaw_r=0,
grip_r=True,
frame_l="torso_lift_link",
frame_r="torso_lift_link",
dur=4.0):
return FAILURE
else:
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):
pt_bl = userdata.bl
pt_tl = userdata.tl
pt_br = userdata.br
pt_tr = userdata.tr
#FIXME Hard-coded X offsets to compensate for poor calibration. Re-calibrate and REMOVE
if not GripUtils.grab_point(pt_tl,roll=-pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=-0.04):
return FAILURE
if not GripUtils.grab_point(pt_tr,roll=pi/2,yaw= pi/3,pitch=pi/4,arm="r",x_offset=-0.04):
return FAILURE
(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)
x_l = (tl_x+bl_x)/2.0
y_l = (tl_y+bl_y)/2.0
z_l = bl_z + sqrt( (bl_x-tl_x)**2 + (bl_y-tl_y)**2 )/2.0
x_r = (tr_x+br_x)/2.0
y_r = (tr_y+br_y)/2.0
z_r = br_z + sqrt( (br_x-tr_x)**2 + (br_y-tr_y)**2 )/2.0
pitch_l = pitch_r = pi/4
roll_l = 0
roll_r = 0
yaw_l=-pi/2
yaw_r= pi/2
grip_l=grip_r=True
frame_l=frame_r = pt_bl.header.frame_id
if not GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_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,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
,dur=7.5):
return_val = FAILURE
print "Folding down!"
x_l = bl_x-0.015
y_l = bl_y+0.025
z_l = z_r = bl_z + 0.02
x_r = br_x-0.015
y_r = br_y-0.025
yaw_l = -3*pi/4
yaw_r = 3*pi/4
pitch_l=pitch_r = pi/4
roll_l = pi/2
roll_r = -pi/2
GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_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,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
,dur=7.5)
GripUtils.recall_arm("b")
return SUCCESS
def execute(self,userdata):
pt_bl = userdata.bl
pt_tl = userdata.tl
pt_br = userdata.br
pt_tr = userdata.tr
#FIXME Hard-coded X offsets to compensate for poor calibration. Re-calibrate and REMOVE
if not GripUtils.grab_point(pt_tl,roll=-pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=-0.08):
return FAILURE
if not GripUtils.grab_point(pt_tr,roll=pi/2,yaw= pi/3,pitch=pi/4,arm="r",x_offset=-0.08):
return FAILURE
(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)
x_l = (tl_x+bl_x)/2.0
y_l = (tl_y+bl_y)/2.0
z_l = bl_z + sqrt( (bl_x-tl_x)**2 + (bl_y-tl_y)**2 )/2.0
x_r = (tr_x+br_x)/2.0
y_r = (tr_y+br_y)/2.0
z_r = br_z + sqrt( (br_x-tr_x)**2 + (br_y-tr_y)**2 )/2.0
pitch_l = pitch_r = pi/4
roll_l = 0
roll_r = 0
yaw_l=-pi/2
yaw_r= pi/2
grip_l=grip_r=True
frame_l=frame_r = pt_bl.header.frame_id
if not GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_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,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
,dur=7.5):
return_val = FAILURE
print "Folding down!"
x_l = bl_x-0.015
y_l = bl_y+0.025
z_l = z_r = bl_z + 0.02
x_r = br_x-0.015
y_r = br_y-0.025
yaw_l = -3*pi/4
yaw_r = 3*pi/4
pitch_l=pitch_r = pi/4
roll_l = pi/2
roll_r = -pi/2
GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_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,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
,dur=7.5)
GripUtils.recall_arm("b")
return SUCCESS
def execute(self, userdata):
cloth_width = userdata.cloth_width*.90
if cloth_width < MINIMUM_SHAKE_WIDTH:
rospy.logwarn("Cannot shake at less than %s m apart; tried to do %d" % (MINIMUM_SHAKE_WIDTH, cloth_width))
return FAILURE
forward_amount = 0.45
height = 0.625
if self.violent:
drop_amount = 0.3
else:
drop_amount = 0.1
lateral_amount = 0.1
for i in range(self.num_shakes):
if i == 0:
duration = 4.0
if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height,
roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True,
x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height,
roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=duration):
return_val = FAILURE
duration = .6
return_val = SUCCESS
if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0-lateral_amount,z_l=height-drop_amount,
roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,
x_r=forward_amount,y_r=-cloth_width/2.0+lateral_amount,z_r=height-drop_amount,
roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=duration):
return_val = FAILURE
if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height,
roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True,
x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height,
roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=duration):
return_val = FAILURE
return SUCCESS
def execute(self, userdata):
height = 0.35
lateral_amount = 0.65
forward_amount = 0.3
if not GripUtils.go_to_multi( x_l=forward_amount, y_l=lateral_amount, z_l=height,
roll_l=0, pitch_l=0, yaw_l=0, grip_l=self.grip,
x_r=forward_amount, y_r=-lateral_amount, z_r=height,
roll_r=0, pitch_r=0, yaw_r=0, grip_r=self.grip,
frame_l="torso_lift_link", frame_r="torso_lift_link", dur=4.0):
return FAILURE
else:
return SUCCESS
def smooth(pt,side):
y_offset = -0.08
x_offset = 0.02
x = pt.point.x + x_offset
y = pt.point.y + y_offset
z = pt.point.z
frame = "base_footprint"
x_l = x
x_r = x
y_l = y
y_r = y
z_l = z_r = z
roll_l = roll_r = 0
yaw_l = -pi/2
yaw_r = pi/2
pitch_l = pitch_r=pi/6
grip_l = grip_r = True
GripUtils.go_to_multi (x_l=x,y_l=y_l,z_l=z+0.04,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame
,x_r=x,y_r=y_r,z_r=z+0.04,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame
,dur=5.0)
GripUtils.go_to_multi (x_l=x,y_l=y_l,z_l=z,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame
,x_r=x,y_r=y_r,z_r=z,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame
,dur=3.0)
y_l += 0.06
y_r -= 0.06
pitch_l = pi/4
pitch_r = pi/4
GripUtils.go_to_multi (x_l=x,y_l=y_l,z_l=z+0.01,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame
,x_r=x,y_r=y_r,z_r=z+0.01,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame
,dur=5.0)
def put_on_dopple(depth=0.28):
open_amt = 0.02
x = DOPPLE_X+0.0075
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
GripUtils.go_to_multi (x_l=x_l-0.1,y_l=y_l,z_l=z_l,roll_l=roll_l+pi/2,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r-0.1,y_r=y_r,z_r=z_r,roll_r=roll_r-pi/2,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.05,y_l=y_l,z_l=z_l,roll_l=roll_l+pi/4,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r-0.05,y_r=y_r,z_r=z_r,roll_r=roll_r-pi/4,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,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=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)
z_l = z_r = TABLE_HEIGHT + DOPPLE_HEIGHT-depth
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=5.0)
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 initRobot():
# Look Down
if not StanceUtils.call_stance('look_down',5.0):
print "Look Down: Failure"
return False
# ARMS UP
height = 0.35
lateral_amount = 0.65
forward_amount = 0.3
if not GripUtils.go_to_multi( x_l=forward_amount, y_l=lateral_amount, z_l=height,
roll_l=0, pitch_l=0, yaw_l=0, grip_l=False,
x_r=forward_amount, y_r=-lateral_amount, z_r=height,
roll_r=0, pitch_r=0, yaw_r=0, grip_r=False,
frame_l="torso_lift_link", frame_r="torso_lift_link", dur=4.0):
return False
else:
return True
def executeDrag(self,gripPts,endPts,color='blue'):
"""
drag from gripPts to endPts
"""
startpoints = []
for vertex in gripPts:
pt_world = self.convert_to_world_frame(vertex)
now = rospy.Time.now()
self.listener.waitForTransform("base_footprint",pt_world.header.frame_id,now,rospy.Duration(10.0))
pt_transformed = self.listener.transformPoint("base_footprint",pt_world)
startpoints.append(pt_transformed)
#print "grabbed (x,y,z)",startpoints[0].point.x,startpoints[0].point.y,startpoints[0].point.z
if not GripUtils.grab_points(point_l=startpoints[0],roll_l=-pi/2,yaw_l=-pi/3,pitch_l=pi/4,x_offset_l=-0.01, z_offset_l=0.005,approach= True,
point_r=startpoints[1],roll_r=pi/2,yaw_r= pi/3,pitch_r=pi/4,x_offset_r=-0.01, z_offset_r=0.005):
print "failure"
return False
else:
print "grabbed (x,y,z)",startpoints[0].point.x,startpoints[0].point.y,startpoints[0].point.z
print "done"
midpoints = []
for vertex in endPts:
pt_world = self.dupl_PointStamped(vertex)#self.convert_to_world_frame(vertex)
midpoints.append(pt_world)
frame_l = frame_r = midpoints[0].header.frame_id
if not GripUtils.go_to_multi (x_l=midpoints[0].point.x,y_l=midpoints[0].point.y,z_l=midpoints[0].point.z,roll_l=-pi/2,yaw_l=-pi/2,pitch_l=pi/4,grip_l=True,frame_l=frame_l
,x_r=midpoints[1].point.x, y_r= midpoints[1].point.y ,z_r= midpoints[1].point.z ,roll_r=pi/2,yaw_r=pi/2,pitch_r=pi/4,grip_r=True,frame_r=frame_r,dur=5):
print "failure"
#return False
else:
print "dragging done"
if (color == 'blue'):
GripUtils.open_grippers()
return True
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):
cloth_width = userdata.cloth_width * .90
if cloth_width < MINIMUM_SHAKE_WIDTH:
rospy.logwarn(
"Cannot shake at less than %s m apart; tried to do %d" %
(MINIMUM_SHAKE_WIDTH, cloth_width))
return FAILURE
forward_amount = 0.45
height = 0.625
if self.violent:
drop_amount = 0.3
else:
drop_amount = 0.1
lateral_amount = 0.1
for i in range(self.num_shakes):
if i == 0:
duration = 4.0
if not GripUtils.go_to_multi(x_l=forward_amount,
y_l=cloth_width / 2.0,
z_l=height,
roll_l=0,
pitch_l=0,
yaw_l=-pi / 2,
grip_l=True,
x_r=forward_amount,
y_r=-cloth_width / 2.0,
z_r=height,
roll_r=0,
pitch_r=0,
yaw_r=pi / 2,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=duration):
return_val = FAILURE
duration = .6
return_val = SUCCESS
if not GripUtils.go_to_multi(
x_l=forward_amount,
y_l=cloth_width / 2.0 - lateral_amount,
z_l=height - drop_amount,
roll_l=0,
pitch_l=pi / 4,
yaw_l=-pi / 2,
grip_l=True,
x_r=forward_amount,
y_r=-cloth_width / 2.0 + lateral_amount,
z_r=height - drop_amount,
roll_r=0,
pitch_r=pi / 4,
yaw_r=pi / 2,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=duration):
return_val = FAILURE
if not GripUtils.go_to_multi(x_l=forward_amount,
y_l=cloth_width / 2.0,
z_l=height,
roll_l=0,
pitch_l=0,
yaw_l=-pi / 2,
grip_l=True,
x_r=forward_amount,
y_r=-cloth_width / 2.0,
z_r=height,
roll_r=0,
pitch_r=0,
yaw_r=pi / 2,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=duration):
return_val = FAILURE
return SUCCESS
def executeFold(self,gripPts,endPts,color_current='blue',color_next='blue'):
"""
execute a fold
"""
#--- TODO--- convert gripPts,endPts to current frame of robot
gripPts_new = []
for pt in gripPts:
if pt == None:
continue
pt_world = self.convert_to_world_frame(pt)
now = rospy.Time.now()
self.listener.waitForTransform("base_footprint",pt_world.header.frame_id,now,rospy.Duration(10.0))
pt_transformed = self.listener.transformPoint("base_footprint",pt_world)
gripPts_new.append(pt_transformed)
endPts_new = []
for pt in endPts:
if pt == None:
continue
pt_world = self.convert_to_world_frame(pt)
now = rospy.Time.now()
self.listener.waitForTransform("base_footprint",pt_world.header.frame_id,now,rospy.Duration(10.0))
pt_transformed = self.listener.transformPoint("base_footprint",pt_world)
endPts_new.append(pt_transformed)
gripPts = gripPts_new
endPts = endPts_new
if(gripPts[1] == None):
self.executeFold_left(gripPt=gripPts[0],endPt=endPts[0],color_current=color_current,color_next=color_next)
return
elif(gripPts[0] == None):
self.executeFold_right(gripPt=gripPts[1],endPt=endPts[1],color_current=color_current,color_next=color_next)
return
# if blue fold, move arms to gripping position. (if red fold, arms already holding cloth at gripping position)
if (color_current == 'blue'):
if (len(gripPts) > 2) or (len(endPts) > 2):
rospy.loginfo("Requires too many grippers")
return False
startpoints = []
for pt in gripPts:
#pt_world = self.convert_to_world_frame(pt)
pt_world = self.dupl_PointStamped(pt)
startpoints.append(pt_world)
# determine approach yaws
#yaw_l = -pi/3 if gripPts[0].point.x < endPts[0].point.x else -4*pi/3 # left gripper
#yaw_r = pi/3 if gripPts[1].point.x > endPts[1].point.x else 4*pi/3 # right gripper
# move both arms
if not GripUtils.grab_points(point_l=startpoints[0],roll_l=-pi/2,yaw_l=-pi/3,pitch_l=pi/4,x_offset_l=-0.01, z_offset_l=0.005,approach= True,
point_r=startpoints[1],roll_r=pi/2,yaw_r=pi/3,pitch_r=pi/4,x_offset_r=-0.01, z_offset_r=0.005):
print "failure"
return False
midpoints1 = []
midpoints = []
for pt in gripPts:
pt_world = pt# self.convert_to_world_frame(pt)
midpoints1.append(pt_world)
i = 0
for pt in endPts:
pt_world = self.dupl_PointStamped(pt) #self.convert_to_world_frame(pt)
pt_world.point.x = (pt_world.point.x + midpoints1[i].point.x)/2.0
pt_world.point.y = (pt_world.point.y + midpoints1[i].point.y)/2.0
pt_world.point.z = pt_world.point.z + 0.1
midpoints.append(pt_world)
i +=1
pitch_l = pitch_r = pi/4
roll_l = -pi/2
roll_r = pi/2
yaw_l=-pi/2
yaw_r= pi/2
grip_l=grip_r=True
frame_l=frame_r = midpoints[0].header.frame_id
if not GripUtils.go_to_multi (x_l=midpoints[0].point.x,y_l=midpoints[0].point.y,z_l=midpoints[0].point.z,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l,
x_r=midpoints[1].point.x,y_r=midpoints[1].point.y,z_r=midpoints[1].point.z,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r,dur=7.5):
print "failure"
#return False
endpoints = []
for pt in endPts:
pt_world = self.dupl_PointStamped(pt) #self.convert_to_world_frame(pt)
endpoints.append(pt_world)
frame_l=frame_r = endpoints[0].header.frame_id
if not GripUtils.go_to_multi (x_l=endpoints[0].point.x,y_l=endpoints[0].point.y,z_l=endpoints[0].point.z,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l
,x_r=endpoints[1].point.x,y_r=endpoints[1].point.y,z_r=endpoints[1].point.z,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
,dur=7.5):
print "failure"
#return False
if(color_next == 'blue'):
initRobot()
return True
def execute(self, userdata):
cloth_width = userdata.cloth_width
if cloth_width < 0.35:
rospy.logwarn(
"Cannot shake at less than 0.35m apart; tried to do %s" %
str(cloth_width))
return FAILURE
if cloth_width > 0.65:
cloth_width = 0.65
forward_amount = 0.45
height = 0.625
drop_amount = 0.3
lateral_amount = 0.1
for i in range(self.num_shakes):
if i == 0:
duration = 4.0
if not GripUtils.go_to_multi(x_l=forward_amount,
y_l=cloth_width / 2.0,
z_l=height,
roll_l=0,
pitch_l=0,
yaw_l=-pi / 2,
grip_l=True,
x_r=forward_amount,
y_r=-cloth_width / 2.0,
z_r=height,
roll_r=0,
pitch_r=0,
yaw_r=pi / 2,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=duration):
return FAILURE
duration = .45
return_val = SUCCESS
if not GripUtils.go_to_multi(
x_l=forward_amount,
y_l=cloth_width / 2.0 - lateral_amount,
z_l=height - drop_amount,
roll_l=0,
pitch_l=pi / 4,
yaw_l=-pi / 2,
grip_l=True,
x_r=forward_amount,
y_r=-cloth_width / 2.0 + lateral_amount,
z_r=height - drop_amount,
roll_r=0,
pitch_r=pi / 4,
yaw_r=pi / 2,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=duration):
return_val = FAILURE
if not GripUtils.go_to_multi(x_l=forward_amount,
y_l=cloth_width / 2.0,
z_l=height,
roll_l=0,
pitch_l=0,
yaw_l=-pi / 2,
grip_l=True,
x_r=forward_amount,
y_r=-cloth_width / 2.0,
z_r=height,
roll_r=0,
pitch_r=0,
yaw_r=pi / 2,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=duration):
return_val = FAILURE
cloth_width = cloth_width * self.clothWidthScaleFactor
if not GripUtils.go_to_multi(x_l=forward_amount + .2,
y_l=cloth_width / 2.0,
z_l=height,
roll_l=0,
pitch_l=0,
yaw_l=-pi / 4,
grip_l=True,
x_r=forward_amount + .2,
y_r=-cloth_width / 2.0,
z_r=height,
roll_r=0,
pitch_r=0,
yaw_r=pi / 4,
grip_r=True,
frame_l="table_height",
frame_r="table_height",
dur=4.0):
return FAILURE
return SUCCESS
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 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 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 put_on_dopple(depth=0.28):
open_amt = 0.02
x = DOPPLE_X + 0.0075
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
GripUtils.go_to_multi(x_l=x_l - 0.1,
y_l=y_l,
z_l=z_l,
roll_l=roll_l + pi / 2,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r - 0.1,
y_r=y_r,
z_r=z_r,
roll_r=roll_r - pi / 2,
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.05,
y_l=y_l,
z_l=z_l,
roll_l=roll_l + pi / 4,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame_l,
x_r=x_r - 0.05,
y_r=y_r,
z_r=z_r,
roll_r=roll_r - pi / 4,
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,
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=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)
z_l = z_r = TABLE_HEIGHT + DOPPLE_HEIGHT - depth
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=5.0)
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 smooth(pt, side):
y_offset = -0.08
x_offset = 0.02
x = pt.point.x + x_offset
y = pt.point.y + y_offset
z = pt.point.z
frame = "base_footprint"
x_l = x
x_r = x
y_l = y
y_r = y
z_l = z_r = z
roll_l = roll_r = 0
yaw_l = -pi / 2
yaw_r = pi / 2
pitch_l = pitch_r = pi / 6
grip_l = grip_r = True
GripUtils.go_to_multi(x_l=x,
y_l=y_l,
z_l=z + 0.04,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame,
x_r=x,
y_r=y_r,
z_r=z + 0.04,
roll_r=roll_l,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame,
dur=5.0)
GripUtils.go_to_multi(x_l=x,
y_l=y_l,
z_l=z,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame,
x_r=x,
y_r=y_r,
z_r=z,
roll_r=roll_l,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame,
dur=3.0)
y_l += 0.06
y_r -= 0.06
pitch_l = pi / 4
pitch_r = pi / 4
GripUtils.go_to_multi(x_l=x,
y_l=y_l,
z_l=z + 0.01,
roll_l=roll_l,
yaw_l=yaw_l,
pitch_l=pitch_l,
grip_l=grip_l,
frame_l=frame,
x_r=x,
y_r=y_r,
z_r=z + 0.01,
roll_r=roll_l,
yaw_r=yaw_r,
pitch_r=pitch_r,
grip_r=grip_r,
frame_r=frame,
dur=5.0)
