def execute(self, userdata):
pr2_say(talk_pickupclump)
process_mono = rospy.ServiceProxy("clump_center_node/process_mono",
ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
z_offset = 0.06
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.close_gripper("l")
while not GripUtils.has_object("l") and not rospy.is_shutdown():
print "Z_offset = %f" % z_offset
z_offset -= 0.02
if (z_offset < -0.02):
return FAILURE
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.close_gripper("l")
GripUtils.recall_arm("l", grip=True)
return SUCCESS
def execute(self, userdata):
process_mono = rospy.ServiceProxy("triangle_fitter_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt_l = resp.pts3d[0]
pt_r = resp.pts3d[1]
pt_tl = resp.pts3d[2]
pt_tr = resp.pts3d[3]
#Compute approximate widths
towel_width = sqrt( (pt_l.point.x - pt_tr.point.x)**2 + (pt_l.point.y - pt_tr.point.y)**2 )
towel_height = sqrt( (pt_l.point.x - pt_tl.point.x)**2 + (pt_l.point.y - pt_tl.point.y)**2 )
userdata.cloth_width = towel_width
userdata.cloth_height = towel_height
if resp.params[resp.param_names.index("left")] == 0:
left = False
else:
left = True
print "Correct?"
tf = raw_input()
if len(tf) > 0 and (tf[0] == 'n' or tf[0] == 'f'):
left = not left
if left:
GripUtils.recall_arm("l")
if GripUtils.grab_point(pt_l,roll=pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=0.005):
return SUCCESS
else:
return FAILURE
else:
GripUtils.recall_arm("r")
if GripUtils.grab_point(pt_r,roll=-pi/2,yaw=pi/3,pitch=pi/4,arm="r",x_offset=0.005):
return SUCCESS
else:
return FAILURE
def execute(self, userdata):
process_mono = rospy.ServiceProxy("triangle_fitter_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt_l = resp.pts3d[0]
pt_r = resp.pts3d[1]
pt_tl = resp.pts3d[2]
pt_tr = resp.pts3d[3]
#Compute approximate widths
towel_width = sqrt( (pt_l.point.x - pt_tr.point.x)**2 + (pt_l.point.y - pt_tr.point.y)**2 )
towel_height = sqrt( (pt_l.point.x - pt_tl.point.x)**2 + (pt_l.point.y - pt_tl.point.y)**2 )
userdata.cloth_width = towel_width
userdata.cloth_height = towel_height
if resp.params[resp.param_names.index("left")] == 0:
left = False
else:
left = True
print "Correct?"
tf = raw_input()
if len(tf) > 0 and (tf[0] == 'n' or tf[0] == 'f'):
left = not left
if left:
GripUtils.recall_arm("l")
if GripUtils.grab_point(pt_l,roll=pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=0.005):
return SUCCESS
else:
return FAILURE
else:
GripUtils.recall_arm("r")
if GripUtils.grab_point(pt_r,roll=-pi/2,yaw=pi/3,pitch=pi/4,arm="r",x_offset=0.005):
return SUCCESS
else:
return FAILURE
def execute(self, userdata):
initial_separation = 0.11
print 'Smooth distance: %d' % self.distance
if self.arm=="b":
rospy.loginfo('Self.arm is b')
outcome = SUCCESS
#Put arms together, with a gap of initial_separation between grippers
if not GripUtils.go_to_pts(point_l=self.location,grip_r=True, grip_l=True, point_r=self.location,
roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,y_offset_l=initial_separation/2.0,z_offset_l=0.05
,link_frame_l="l_wrist_back_frame",
roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,y_offset_r=-1*initial_separation/2.0,z_offset_r=0.05
,link_frame_r="r_wrist_back_frame",dur=4.0):
outcome = FAILURE
rospy.loginfo('Smooth on table goto 1: Success is %s', outcome==SUCCESS)
if not GripUtils.go_to_pts(point_l=self.location,grip_r=True, grip_l=True, point_r=self.location,
roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,y_offset_l=initial_separation/2.0,z_offset_l=0.00,
link_frame_l="l_wrist_back_frame",
roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,y_offset_r=-1*initial_separation/2.0,z_offset_r=0.00,
link_frame_r="r_wrist_back_frame",dur=2.0):
outcome = FAILURE
rospy.loginfo('Smooth on table goto 2: Success is %s', outcome==SUCCESS)
if not GripUtils.go_to_pts(point_l=self.location,grip_r=True, grip_l=True, point_r=self.location,
roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,
y_offset_l=(self.distance+initial_separation)/2.0, z_offset_l=0.00,
link_frame_l="l_wrist_back_frame",
roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,
y_offset_r=-1*(self.distance+initial_separation)/2.0, z_offset_r=0.00,
link_frame_r="r_wrist_back_frame",dur=2.0):
outcome = FAILURE
rospy.loginfo('Smooth on table goto 3: Success is %s', outcome==SUCCESS)
GripUtils.recall_arm("b")
return outcome
else:
#Right is negative in the y axis
if self.arm=="r":
y_multiplier = -1
else:
y_multiplier = 1
rospy.loginfo('arm is %s, y multiplier is %s' % (self.arm, y_multiplier))
print 'Location: %s' % str(self.location)
if not GripUtils.go_to_pt(point=self.location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2,
z_offset=0.05,arm=self.arm,
link_frame="%s_wrist_back_frame"%self.arm,dur=4.0, verbose=True):
return FAILURE
rospy.loginfo('Step 2')
if not GripUtils.go_to_pt(point=self.location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2,
z_offset=-0.01,arm=self.arm,
link_frame="%s_wrist_back_frame"%self.arm,dur=2.0, verbose=True):
#return FAILURE
pass
rospy.loginfo('Step 3')
print 'Offset: %f' % (y_multiplier*self.distance)
if not GripUtils.go_to_pt(point=self.location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2,
y_offset=y_multiplier*self.distance,z_offset=-0.01,arm=self.arm,
link_frame="%s_wrist_back_frame"%self.arm,dur=2.0, verbose=True):
return FAILURE
rospy.loginfo('Step 4. 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):
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):
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 smooth(arm='b'):
location = PointStamped()
location.point.x = 0.5
location.header.frame_id = "table_height"
distance = TABLE_WIDTH/3
initial_separation = 0.11
GripUtils.recall_arm(arm)
if arm == 'b':
#Put arms together, with a gap of initial_separation between grippers
if not GripUtils.go_to_pts(point_l=location,grip_r=True, grip_l=True, point_r=location,
roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,y_offset_l=initial_separation/2.0,z_offset_l=0.05
,link_frame_l="l_wrist_back_frame",
roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,y_offset_r=-1*initial_separation/2.0,z_offset_r=0.05
,link_frame_r="r_wrist_back_frame",dur=4.0):
success = False
if not GripUtils.go_to_pts(point_l=location,grip_r=True, grip_l=True, point_r=location,
roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,y_offset_l=initial_separation/2.0,z_offset_l=-0.03,
link_frame_l="l_wrist_back_frame",
roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,y_offset_r=-1*initial_separation/2.0,z_offset_r=-0.03,
link_frame_r="r_wrist_back_frame",dur=2.0):
success = False
if not GripUtils.go_to_pts(point_l=location,grip_r=True, grip_l=True, point_r=location,
roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,
y_offset_l=(distance+initial_separation)/2.0, z_offset_l=-0.03,
link_frame_l="l_wrist_back_frame",
roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,
y_offset_r=-1*(distance+initial_separation)/2.0, z_offset_r=-0.03,
link_frame_r="r_wrist_back_frame",dur=2.0):
success = False
else:
#Right is negative in the y axis
if arm=="r":
y_multiplier = -1
else:
y_multiplier = 1
location.point.y -= y_multiplier*distance/2
if not GripUtils.go_to_pt(point=location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2,
z_offset=0.05,arm=arm,
link_frame="%s_wrist_back_frame"%arm,dur=4.0):
success = False
if not GripUtils.go_to_pt(point=location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2,
z_offset=-0.01,arm=arm,
link_frame="%s_wrist_back_frame"%arm,dur=2.0):
success = False
if not GripUtils.go_to_pt(point=location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2,
y_offset=y_multiplier*distance,z_offset=-0.01,arm=arm,
link_frame="%s_wrist_back_frame"%arm,dur=2.0):
success = False
GripUtils.recall_arm(arm)
return True
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):
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):
pr2_say(talk_pickupclump)
process_mono = rospy.ServiceProxy("clump_center_node/process_mono", ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
z_offset = 0.06
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.close_gripper("l")
while not GripUtils.has_object("l") and not rospy.is_shutdown():
print "Z_offset = %f" % z_offset
z_offset -= 0.02
if z_offset < -0.02:
return FAILURE
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.close_gripper("l")
GripUtils.recall_arm("l", grip=True)
return SUCCESS
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 execute(self,userdata):
pt_bl = userdata.bl
pt_tl = userdata.tl
pt_br = userdata.br
pt_tr = userdata.tr
(bl_x,bl_y,bl_z) = (pt_bl.point.x,pt_bl.point.y,pt_bl.point.z)
(tl_x,tl_y,tl_z) = (pt_tl.point.x,pt_tl.point.y,pt_tl.point.z)
(br_x,br_y,br_z) = (pt_br.point.x,pt_br.point.y,pt_br.point.z)
(tr_x,tr_y,tr_z) = (pt_tr.point.x,pt_tr.point.y,pt_tr.point.z)
ctr_l_x = .75*bl_x + .25*tl_x
#Make more centered
ctr_l_x -= 0.01
ctr_l_y = .75*bl_y + .25*tl_y
z = bl_z
yaw = -pi/2
roll = -pi/2
pitch = pi/4
frame = pt_bl.header.frame_id
if not GripUtils.grab(x=ctr_l_x,y=ctr_l_y,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame):
return FAILURE
ctr_r_x = .75*br_x + .25*tr_x
#Make more centered
ctr_r_x -= 0.01
ctr_r_y = .75*br_y + .25*tr_y
alpha = 0.333
ctr_ml_x = (1-alpha)*ctr_l_x + alpha*ctr_r_x
ctr_ml_y = (1-alpha)*ctr_l_y + alpha*ctr_r_y
ctr_mr_x = (1-alpha)*ctr_r_x + alpha*ctr_l_x
ctr_mr_y = (1-alpha)*ctr_r_y + alpha*ctr_l_y
up_z = z+sqrt( (ctr_l_x - ctr_r_x)**2 + (ctr_l_y - ctr_r_y)**2) / 3.0
pitch = pi/2
if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y,z=up_z,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame,grip=True,dur=5.0):
return FAILURE
if not GripUtils.go_to(x=(ctr_ml_x+ctr_mr_x)/2.0,y=(ctr_ml_y+ctr_mr_y+0.02)/2.0,z=(up_z+bl_z)/2.0,roll=roll,yaw=yaw,pitch=(pitch+3*pi/4)/2.0,arm="l",frame=frame,grip=True,dur=5.0):
return FAILURE
z = bl_z
pitch = 3*pi/4
if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y+0.02,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame,grip=True,dur=5.0):
return FAILURE
yaw = pi/2
roll = pi/2
pitch = pi/4
GripUtils.open_gripper("l")
if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y-0.05,z=z+0.02,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame,grip=False,dur=1.0):
return FAILURE
GripUtils.recall_arm("l")
if not GripUtils.grab(x=ctr_r_x,y=ctr_r_y-0.01,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame):
return FAILURE
pitch = pi/2
if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y-0.02,z=up_z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0):
return FAILURE
if not GripUtils.go_to(x=(ctr_mr_x+ctr_ml_x)/2.0,y=(ctr_mr_y-0.02+ctr_ml_y-0.02)/2.0,z=(up_z+bl_z+0.01)/2.0,roll=roll,yaw=yaw,pitch=(pitch+3*pi/4)/2.0,arm="r",frame=frame,grip=True,dur=5.0):
return FAILURE
z = bl_z+0.01
pitch = 3*pi/4
if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y-0.02,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0):
return FAILURE
GripUtils.open_gripper("r")
if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y+0.05,z=z+0.02,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=False,dur=1.0):
return FAILURE
GripUtils.recall_arm("r")
"""
if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y+0.06,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame
,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y-0.03,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame
,dur=3.0):
return FAILURE
if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame
,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame
,dur=3.0):
return FAILURE
GripUtils.close_gripper("b")
if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.1,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,frame_l=frame
,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.1,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=True,frame_r=frame
,dur=3.0):
return FAILURE
"""
return SUCCESS
def execute(self, userdata):
GripUtils.recall_arm('b')
return SUCCESS
def smooth(arm='b'):
location = PointStamped()
location.point.x = 0.5
location.header.frame_id = "table_height"
distance = TABLE_WIDTH / 3
initial_separation = 0.11
GripUtils.recall_arm(arm)
if arm == 'b':
#Put arms together, with a gap of initial_separation between grippers
if not GripUtils.go_to_pts(point_l=location,
grip_r=True,
grip_l=True,
point_r=location,
roll_l=pi / 2,
yaw_l=0,
pitch_l=-pi / 2,
y_offset_l=initial_separation / 2.0,
z_offset_l=0.05,
link_frame_l="l_wrist_back_frame",
roll_r=pi / 2,
yaw_r=0,
pitch_r=-pi / 2,
y_offset_r=-1 * initial_separation / 2.0,
z_offset_r=0.05,
link_frame_r="r_wrist_back_frame",
dur=4.0):
success = False
if not GripUtils.go_to_pts(point_l=location,
grip_r=True,
grip_l=True,
point_r=location,
roll_l=pi / 2,
yaw_l=0,
pitch_l=-pi / 2,
y_offset_l=initial_separation / 2.0,
z_offset_l=-0.03,
link_frame_l="l_wrist_back_frame",
roll_r=pi / 2,
yaw_r=0,
pitch_r=-pi / 2,
y_offset_r=-1 * initial_separation / 2.0,
z_offset_r=-0.03,
link_frame_r="r_wrist_back_frame",
dur=2.0):
success = False
if not GripUtils.go_to_pts(
point_l=location,
grip_r=True,
grip_l=True,
point_r=location,
roll_l=pi / 2,
yaw_l=0,
pitch_l=-pi / 2,
y_offset_l=(distance + initial_separation) / 2.0,
z_offset_l=-0.03,
link_frame_l="l_wrist_back_frame",
roll_r=pi / 2,
yaw_r=0,
pitch_r=-pi / 2,
y_offset_r=-1 * (distance + initial_separation) / 2.0,
z_offset_r=-0.03,
link_frame_r="r_wrist_back_frame",
dur=2.0):
success = False
else:
#Right is negative in the y axis
if arm == "r":
y_multiplier = -1
else:
y_multiplier = 1
location.point.y -= y_multiplier * distance / 2
if not GripUtils.go_to_pt(point=location,
grip=True,
roll=pi / 2,
yaw=0,
pitch=-pi / 2,
z_offset=0.05,
arm=arm,
link_frame="%s_wrist_back_frame" % arm,
dur=4.0):
success = False
if not GripUtils.go_to_pt(point=location,
grip=True,
roll=pi / 2,
yaw=0,
pitch=-pi / 2,
z_offset=-0.01,
arm=arm,
link_frame="%s_wrist_back_frame" % arm,
dur=2.0):
success = False
if not GripUtils.go_to_pt(point=location,
grip=True,
roll=pi / 2,
yaw=0,
pitch=-pi / 2,
y_offset=y_multiplier * distance,
z_offset=-0.01,
arm=arm,
link_frame="%s_wrist_back_frame" % arm,
dur=2.0):
success = False
GripUtils.recall_arm(arm)
return True
def fold_cloth(dir='l'):
print "Folding cloth"
_cloth_tracker.scoot(-0.1)
GripUtils.recall_arm("b")
(waist_l_base, waist_r_base) = get_waist_points()
now = rospy.Time.now()
waist_l_base.header.stamp = now
waist_r_base.header.stamp = now
_listener.waitForTransform("table_height", waist_l_base.header.frame_id,
now, rospy.Duration(5.0))
waist_l = _listener.transformPoint("table_height", waist_l_base)
waist_r = _listener.transformPoint("table_height", waist_r_base)
waist_l.point.z = 0
waist_r.point.z = 0
#Grab the waist point
smooth()
GripUtils.grab_point(waist_l, arm="l", yaw=-pi / 2, x_offset=0.02)
#Fold over
ctr_x = (waist_l.point.x + waist_r.point.x) / 2 + 0.02
ctr_y = (waist_l.point.y + waist_r.point.y) / 2
ctr_z = waist_l.point.y - ctr_y
GripUtils.go_to(x=ctr_x,
y=ctr_y,
z=ctr_z,
roll=pi / 2,
pitch=pi / 2,
yaw=-pi / 2,
arm="l",
grip=True,
frame=waist_l.header.frame_id)
GripUtils.go_to_pt(waist_r,
arm="l",
roll=pi / 2,
pitch=3 * pi / 4,
yaw=-pi / 2,
grip=True,
y_offset=0.01,
x_offset=0.02)
GripUtils.go_to_pt(waist_r,
arm="l",
roll=pi / 2,
pitch=3 * pi / 4,
yaw=-pi / 2,
grip=False,
y_offset=-0.05,
x_offset=0.02,
dur=2.5)
GripUtils.go_to_pt(waist_r,
arm="l",
roll=pi / 2,
pitch=3 * pi / 4,
yaw=-pi / 2,
grip=False,
y_offset=-0.05,
x_offset=0.02,
z_offset=0.05,
dur=2.5)
GripUtils.recall_arm("b")
#Grab waist
scoot_amt = 0.2
_cloth_tracker.scoot(-scoot_amt)
ctr_x = 0.25 * waist_l.point.x + 0.75 * waist_r.point.x + scoot_amt
ctr_y = 0.25 * waist_l.point.y + 0.75 * waist_r.point.y
ctr_z = waist_l.point.z
GripUtils.grab(x=ctr_x,
y=ctr_y,
z=ctr_z,
roll=pi / 2,
pitch=pi / 4,
yaw=0,
arm="r",
frame=waist_l.header.frame_id)
sweep_drag_amount = 0.95 * TABLE_WIDTH
sweep_lift_amount = 0.6
(x, y, z, r, p, yw) = sweep_cloth_with_scoot("r",
sweep_drag_amount,
TABLE_WIDTH,
sweep_lift_amount,
roll=pi / 2)
print "y is %f" % y
print "(%f,%f,%f,%f,%f,%f)" % (x, y, z, r, p, y)
smooth("l")
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH / 4 - 0.03,
z=PANTS_LENGTH / 4,
roll=r,
pitch=3 * pi / 8,
yaw=yw,
arm="r",
grip=True,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH / 2 - 0.03,
z=PANTS_LENGTH / 2,
roll=r,
pitch=pi / 2,
yaw=yw,
arm="r",
grip=True,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + 3 * PANTS_LENGTH / 4 - 0.03,
z=PANTS_LENGTH / 4,
roll=r,
pitch=5 * pi / 8,
yaw=yw,
arm="r",
grip=True,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH - 0.03,
z=0.01,
roll=r,
pitch=pi - p,
yaw=yw,
arm="r",
grip=True,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH + 0.05,
z=0.01,
roll=r,
pitch=pi - p,
yaw=yw,
arm="r",
grip=False,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH + 0.05,
z=0.1,
roll=r,
pitch=pi - p,
yaw=yw,
arm="r",
grip=False,
frame="table_height",
dur=2.5)
GripUtils.recall_arm("r")
smooth()
_cloth_tracker.scoot(scoot_amt + 0.08)
return True
def 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):
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 = 0.75 * bl_x + 0.25 * tl_x
ctr_l_y = 0.75 * bl_y + 0.25 * tl_y
z = bl_z
yaw = -pi / 2
roll = -pi / 2
pitch = pi / 4
frame = pt_bl.header.frame_id
ctr_r_x = 0.75 * br_x + 0.25 * tr_x
ctr_r_y = 0.75 * br_y + 0.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 - 0.01, z=z, 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):
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):
#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):
initial_separation = 0.11
print 'Smooth distance: %d' % self.distance
if self.arm == "b":
rospy.loginfo('Self.arm is b')
outcome = SUCCESS
#Put arms together, with a gap of initial_separation between grippers
if not GripUtils.go_to_pts(
point_l=self.location,
grip_r=True,
grip_l=True,
point_r=self.location,
roll_l=pi / 2,
yaw_l=0,
pitch_l=-pi / 2,
y_offset_l=initial_separation / 2.0,
z_offset_l=0.05,
link_frame_l="l_wrist_back_frame",
roll_r=pi / 2,
yaw_r=0,
pitch_r=-pi / 2,
y_offset_r=-1 * initial_separation / 2.0,
z_offset_r=0.05,
link_frame_r="r_wrist_back_frame",
dur=4.0):
outcome = FAILURE
rospy.loginfo('Smooth on table goto 1: Success is %s',
outcome == SUCCESS)
if not GripUtils.go_to_pts(
point_l=self.location,
grip_r=True,
grip_l=True,
point_r=self.location,
roll_l=pi / 2,
yaw_l=0,
pitch_l=-pi / 2,
y_offset_l=initial_separation / 2.0,
z_offset_l=0.00,
link_frame_l="l_wrist_back_frame",
roll_r=pi / 2,
yaw_r=0,
pitch_r=-pi / 2,
y_offset_r=-1 * initial_separation / 2.0,
z_offset_r=0.00,
link_frame_r="r_wrist_back_frame",
dur=2.0):
outcome = FAILURE
rospy.loginfo('Smooth on table goto 2: Success is %s',
outcome == SUCCESS)
if not GripUtils.go_to_pts(
point_l=self.location,
grip_r=True,
grip_l=True,
point_r=self.location,
roll_l=pi / 2,
yaw_l=0,
pitch_l=-pi / 2,
y_offset_l=(self.distance + initial_separation) / 2.0,
z_offset_l=0.00,
link_frame_l="l_wrist_back_frame",
roll_r=pi / 2,
yaw_r=0,
pitch_r=-pi / 2,
y_offset_r=-1 * (self.distance + initial_separation) / 2.0,
z_offset_r=0.00,
link_frame_r="r_wrist_back_frame",
dur=2.0):
outcome = FAILURE
rospy.loginfo('Smooth on table goto 3: Success is %s',
outcome == SUCCESS)
GripUtils.recall_arm("b")
return outcome
else:
#Right is negative in the y axis
if self.arm == "r":
y_multiplier = -1
else:
y_multiplier = 1
rospy.loginfo('arm is %s, y multiplier is %s' %
(self.arm, y_multiplier))
print 'Location: %s' % str(self.location)
if not GripUtils.go_to_pt(
point=self.location,
grip=True,
roll=pi / 2,
yaw=0,
pitch=-pi / 2,
z_offset=0.05,
arm=self.arm,
link_frame="%s_wrist_back_frame" % self.arm,
dur=4.0,
verbose=True):
return FAILURE
rospy.loginfo('Step 2')
if not GripUtils.go_to_pt(
point=self.location,
grip=True,
roll=pi / 2,
yaw=0,
pitch=-pi / 2,
z_offset=-0.01,
arm=self.arm,
link_frame="%s_wrist_back_frame" % self.arm,
dur=2.0,
verbose=True):
#return FAILURE
pass
rospy.loginfo('Step 3')
print 'Offset: %f' % (y_multiplier * self.distance)
if not GripUtils.go_to_pt(
point=self.location,
grip=True,
roll=pi / 2,
yaw=0,
pitch=-pi / 2,
y_offset=y_multiplier * self.distance,
z_offset=-0.01,
arm=self.arm,
link_frame="%s_wrist_back_frame" % self.arm,
dur=2.0,
verbose=True):
return FAILURE
rospy.loginfo('Step 4. Done!')
return SUCCESS
def execute(self,userdata):
GripUtils.recall_arm('b')
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 execute(self, userdata):
if GripUtils.recall_arm(self.arm):
return SUCCESS
else:
return FAILURE
def fold_cloth(dir='l'):
print "Folding cloth"
_cloth_tracker.scoot(-0.1)
GripUtils.recall_arm("b")
(waist_l_base,waist_r_base) = get_waist_points()
now = rospy.Time.now()
waist_l_base.header.stamp = now
waist_r_base.header.stamp = now
_listener.waitForTransform("table_height",waist_l_base.header.frame_id,now,rospy.Duration(5.0))
waist_l = _listener.transformPoint("table_height",waist_l_base)
waist_r= _listener.transformPoint("table_height",waist_r_base)
waist_l.point.z = 0
waist_r.point.z = 0
#Grab the waist point
smooth()
GripUtils.grab_point(waist_l,arm="l",yaw=-pi/2,x_offset=0.02)
#Fold over
ctr_x = (waist_l.point.x + waist_r.point.x)/2+0.02
ctr_y = (waist_l.point.y + waist_r.point.y)/2
ctr_z = waist_l.point.y - ctr_y
GripUtils.go_to( x=ctr_x, y=ctr_y, z=ctr_z,
roll=pi/2, pitch=pi/2, yaw=-pi/2,
arm="l", grip=True, frame=waist_l.header.frame_id)
GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=True,y_offset=0.01,x_offset=0.02)
GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=False,y_offset=-0.05,x_offset=0.02,dur=2.5)
GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=False,y_offset=-0.05,x_offset=0.02,z_offset=0.05,dur=2.5)
GripUtils.recall_arm("b")
#Grab waist
scoot_amt = 0.2
_cloth_tracker.scoot(-scoot_amt)
ctr_x = 0.25 * waist_l.point.x + 0.75 * waist_r.point.x + scoot_amt
ctr_y = 0.25 * waist_l.point.y + 0.75 * waist_r.point.y
ctr_z = waist_l.point.z
GripUtils.grab( x=ctr_x, y=ctr_y, z=ctr_z,
roll=pi/2, pitch=pi/4, yaw=0,
arm="r", frame=waist_l.header.frame_id)
sweep_drag_amount = 0.95*TABLE_WIDTH
sweep_lift_amount = 0.6
(x,y,z,r,p,yw) = sweep_cloth_with_scoot("r", sweep_drag_amount, TABLE_WIDTH, sweep_lift_amount,roll=pi/2)
print "y is %f"%y
print "(%f,%f,%f,%f,%f,%f)"%(x,y,z,r,p,y)
smooth("l")
GripUtils.go_to( x=x, y=y+PANTS_LENGTH/4-0.03, z=PANTS_LENGTH/4,
roll=r, pitch=3*pi/8, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH/2-0.03, z=PANTS_LENGTH/2,
roll=r, pitch=pi/2, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+3*PANTS_LENGTH/4-0.03, z=PANTS_LENGTH/4,
roll=r, pitch=5*pi/8, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH-0.03, z=0.01,
roll=r, pitch=pi-p, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH+0.05, z=0.01,
roll=r, pitch=pi-p, yaw=yw,
arm="r", grip=False, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH+0.05, z=0.1,
roll=r, pitch=pi-p, yaw=yw,
arm="r", grip=False, frame="table_height", dur=2.5)
GripUtils.recall_arm("r")
smooth()
_cloth_tracker.scoot(scoot_amt+0.08)
return True
def execute(self, userdata):
if GripUtils.recall_arm(self.arm):
return SUCCESS
else:
return FAILURE
