def execute(self, userdata):
bl = userdata.bl
br = userdata.br
tl = userdata.tl
tr = userdata.tr
userdata.cloth_width = sqrt((bl.point.x - br.point.x) ** 2 + (bl.point.y - br.point.y) ** 2) * 1.075
userdata.cloth_height = max([abs(tl.point.x - bl.point.x), abs(tr.point.x - br.point.x)])
# if not GripUtils.grab_point(bl,roll=pi/2,yaw=-pi/2,pitch=pi/4,arm="l",x_offset=0.01,INIT_SCOOT_AMT = 0.01):
# return FAILURE
# if not GripUtils.grab_point(br,roll=-pi/2,yaw=pi/2,pitch=pi/4,arm="r",x_offset=0.01,INIT_SCOOT_AMT = 0.01):
# return FAILURE
if not GripUtils.grab_points(
point_l=bl,
roll_l=pi / 2,
yaw_l=-pi / 2,
pitch_l=pi / 4,
x_offset_l=0.01,
point_r=br,
roll_r=-pi / 2,
yaw_r=pi / 2,
pitch_r=pi / 4,
x_offset_r=0.01,
y_offset_l=-0.01,
y_offset_r=0.02,
INIT_SCOOT_AMT=0.01,
):
return FAILURE
return SUCCESS
def execute(self, userdata):
bl = userdata.bl
br = userdata.br
tl = userdata.tl
tr = userdata.tr
userdata.cloth_width = sqrt((bl.point.x - br.point.x)**2 +
(bl.point.y - br.point.y)**2) * 1.075
userdata.cloth_height = max(
[abs(tl.point.x - bl.point.x),
abs(tr.point.x - br.point.x)])
#if not GripUtils.grab_point(bl,roll=pi/2,yaw=-pi/2,pitch=pi/4,arm="l",x_offset=0.01,INIT_SCOOT_AMT = 0.01):
# return FAILURE
#if not GripUtils.grab_point(br,roll=-pi/2,yaw=pi/2,pitch=pi/4,arm="r",x_offset=0.01,INIT_SCOOT_AMT = 0.01):
# return FAILURE
if not GripUtils.grab_points(point_l=bl,
roll_l=pi / 2,
yaw_l=-pi / 2,
pitch_l=pi / 4,
x_offset_l=0.01,
point_r=br,
roll_r=-pi / 2,
yaw_r=pi / 2,
pitch_r=pi / 4,
x_offset_r=0.01,
y_offset_l=-0.01,
y_offset_r=0.02,
INIT_SCOOT_AMT=0.01):
return FAILURE
return SUCCESS
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 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 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
