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):
bl = userdata.bl
br = userdata.br
tl = userdata.tl
tr = userdata.tr
midleft = bl
midleft.point.x = (bl.point.x + tl.point.x) / 2
midleft.point.y = (bl.point.y + tl.point.y) / 2
midleft.point.z = (bl.point.z + tl.point.z) / 2
midright = br
midright.point.x = (br.point.x + tr.point.x) / 2
midright.point.y = (br.point.y + tr.point.y) / 2
midright.point.z = (br.point.z + tr.point.z) / 2
if not GripUtils.grab_point(
midleft, roll=pi / 2, yaw=-pi / 2, pitch=pi / 4, arm="l", x_offset=-0.01, z_offset=0.01, INIT_SCOOT_AMT=0.01
):
return FAILURE
if not GripUtils.grab_point(
midright,
roll=-pi / 2,
yaw=pi / 2,
pitch=pi / 4,
arm="r",
x_offset=-0.01,
z_offset=0.01,
INIT_SCOOT_AMT=0.01,
):
return FAILURE
if not GripUtils.go_to_relative_multi(0, 0, 0.4, True, 0, 0, 0.4, True, "base_footprint"):
return FAILURE
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):
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
return SUCCESS
def grab_far_right(arm):
StanceUtils.call_stance("look_down",5.0)
rospy.sleep(2.5)
process_mono = rospy.ServiceProxy("far_right_finder_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
GripUtils.grab_point(pt,roll=-pi/2,yaw=pi/2,arm=arm,z_offset = 0.02,x_offset = -0.01)
return pt
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
return SUCCESS
def grab_far_right(arm):
StanceUtils.call_stance("look_down", 5.0)
rospy.sleep(2.5)
process_mono = rospy.ServiceProxy("far_right_finder_node/process_mono",
ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
GripUtils.grab_point(pt,
roll=-pi / 2,
yaw=pi / 2,
arm=arm,
z_offset=0.02,
x_offset=-0.01)
return pt
def execute(self, userdata):
bl = userdata.bl
br = userdata.br
tl = userdata.tl
tr = userdata.tr
midleft = bl
midleft.point.x = (bl.point.x + tl.point.x) / 2
midleft.point.y = (bl.point.y + tl.point.y) / 2
midleft.point.z = (bl.point.z + tl.point.z) / 2
midright = br
midright.point.x = (br.point.x + tr.point.x) / 2
midright.point.y = (br.point.y + tr.point.y) / 2
midright.point.z = (br.point.z + tr.point.z) / 2
if not GripUtils.grab_point(midleft,
roll=pi / 2,
yaw=-pi / 2,
pitch=pi / 4,
arm="l",
x_offset=-0.01,
z_offset=0.01,
INIT_SCOOT_AMT=0.01):
return FAILURE
if not GripUtils.grab_point(midright,
roll=-pi / 2,
yaw=pi / 2,
pitch=pi / 4,
arm="r",
x_offset=-0.01,
z_offset=0.01,
INIT_SCOOT_AMT=0.01):
return FAILURE
if not GripUtils.go_to_pts(point_l=midleft,
roll_l=pi / 2,
yaw_l=-pi / 2,
pitch_l=pi / 4,
grip_l=True,
point_r=midright,
roll_r=-pi / 2,
yaw_r=pi / 2,
pitch_r=pi / 4,
grip_r=True,
z_offset_l=0.4,
z_offset_r=0.4):
return FAILURE
return SUCCESS
def sendTarget(self, dur, target1, target2=False,grab=False):
resp = False
if grab:
point = target1.point
pitch = target1.pitch
roll = target1.roll
yaw = target1.yaw
GripUtils.grab_point(point=point,pitch=pitch,roll=roll,yaw=yaw,arm=target1.arm,approach=False)
return
target1.point.header.stamp = rospy.Time.now()
if not target2:
try:
srv = rospy.ServiceProxy("move_one_arm",MoveOneArm)
resp = srv(MoveOneArmRequest(target=target1,dur=dur))
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
def execute(self, userdata):
if self.side == 'l':
arm = 'l'
processor = 'far_left_finder_node'
yaw = -pi / 2
else:
arm = 'r'
processor = 'far_right_finder_node'
yaw = pi / 2
process_mono = rospy.ServiceProxy("%s/process_mono" % processor,
ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
pt_opp = resp.pts3d[1]
userdata.arm = 'l' if arm == 'r' else 'r' #Arm returns the arm which is currently gripping
homogeneity = resp.params[resp.param_names.index("homogeneity")]
if homogeneity > 0.13:
print "Too homogeneous: %f" % homogeneity
return FAILURE
userdata.cloth_width = sqrt((pt_opp.point.x - pt.point.x)**2 +
(pt_opp.point.y - pt.point.y)**2)
#userdata.cloth_width = abs(pt_opp.point.y - pt.point.y)
userdata.cloth_height = None
if arm == "l":
x_offset = -0.02 #was -0.01
else:
x_offset = -0.01
if not GripUtils.grab_point(
pt, roll=-pi / 2, yaw=yaw, arm=arm, x_offset=x_offset):
return FAILURE
else:
if self.let_go:
GripUtils.open_gripper(opp_arm(arm))
return SUCCESS
def execute(self, userdata):
if self.side == 'l':
arm = 'l'
processor = 'far_left_finder_node'
yaw = -pi/2
else:
arm = 'r'
processor = 'far_right_finder_node'
yaw = pi/2
process_mono = rospy.ServiceProxy("%s/process_mono"%processor,ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
pt_opp = resp.pts3d[1]
userdata.arm = 'l' if arm == 'r' else 'r' #Arm returns the arm which is currently gripping
homogeneity = resp.params[resp.param_names.index("homogeneity")]
if homogeneity > 0.13:
print "Too homogeneous: %f"%homogeneity
return FAILURE
userdata.cloth_width = sqrt( (pt_opp.point.x - pt.point.x)**2 + (pt_opp.point.y - pt.point.y)**2 )
#userdata.cloth_width = abs(pt_opp.point.y - pt.point.y)
userdata.cloth_height = None
if arm == "l":
x_offset = -0.02 #was -0.01
else:
x_offset = -0.01
if not GripUtils.grab_point(pt,roll=-pi/2,yaw=yaw,arm=arm,x_offset=x_offset):
return FAILURE
else:
if self.let_go:
GripUtils.open_gripper(opp_arm(arm))
return SUCCESS
def execute(self,userdata):
pt_bl = userdata.bl
pt_tl = userdata.tl
pt_br = userdata.br
pt_tr = userdata.tr
#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 sendTarget(self, dur, target1, target2=False, grab=False):
resp = False
if grab:
point = target1.point
pitch = target1.pitch
roll = target1.roll
yaw = target1.yaw
GripUtils.grab_point(point=point,
pitch=pitch,
roll=roll,
yaw=yaw,
arm=target1.arm,
approach=False)
return
target1.point.header.stamp = rospy.Time.now()
if not target2:
try:
srv = rospy.ServiceProxy("move_one_arm", MoveOneArm)
resp = srv(MoveOneArmRequest(target=target1, dur=dur))
except rospy.ServiceException, e:
rospy.loginfo("Service Call Failed: %s" % e)
def execute(self, userdata):
bl = userdata.bl
br = userdata.br
tl = userdata.tl
tr = userdata.tr
midleft = bl
midleft.point.x = (bl.point.x + tl.point.x) / 2
midleft.point.y = (bl.point.y + tl.point.y) / 2
midleft.point.z = (bl.point.z + tl.point.z) / 2
midright = br
midright.point.x = (br.point.x + tr.point.x) / 2
midright.point.y = (br.point.y + tr.point.y) / 2
midright.point.z = (br.point.z + tr.point.z) / 2
if not GripUtils.grab_point(midleft,
roll=pi / 2,
yaw=-pi / 2,
pitch=pi / 4,
arm="l",
x_offset=-0.01,
z_offset=0.01,
INIT_SCOOT_AMT=0.01):
return FAILURE
if not GripUtils.grab_point(midright,
roll=-pi / 2,
yaw=pi / 2,
pitch=pi / 4,
arm="r",
x_offset=-0.01,
z_offset=0.01,
INIT_SCOOT_AMT=0.01):
return FAILURE
if not GripUtils.go_to_relative_multi(0, 0, 0.4, True, 0, 0, 0.4, True,
"base_footprint"):
return FAILURE
return SUCCESS
def execute(self,userdata):
bl = userdata.bl
br = userdata.br
tl = userdata.tl
tr = userdata.tr
midleft = bl
midleft.point.x = (bl.point.x + tl.point.x)/2
midleft.point.y = (bl.point.y + tl.point.y)/2
midleft.point.z = (bl.point.z + tl.point.z)/2
midright = br
midright.point.x = (br.point.x + tr.point.x)/2
midright.point.y = (br.point.y + tr.point.y)/2
midright.point.z = (br.point.z + tr.point.z)/2
if not GripUtils.grab_point(midleft,roll=pi/2,yaw=-pi/2,pitch=pi/4,arm="l",x_offset=-0.01,z_offset=0.01,INIT_SCOOT_AMT = 0.01):
return FAILURE
if not GripUtils.grab_point(midright,roll=-pi/2,yaw=pi/2,pitch=pi/4,arm="r",x_offset=-0.01,z_offset=0.01,INIT_SCOOT_AMT = 0.01):
return FAILURE
if not GripUtils.go_to_pts(point_l=midleft,roll_l=pi/2,yaw_l=-pi/2,pitch_l=pi/4,grip_l=True,point_r=midright,roll_r=-pi/2,yaw_r=pi/2,pitch_r=pi/4,grip_r=True,z_offset_l=0.4,z_offset_r=0.4):
return FAILURE
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 fold_cloth(dir='l'):
print "Folding cloth"
_cloth_tracker.scoot(-0.1)
GripUtils.recall_arm("b")
(waist_l_base,waist_r_base) = get_waist_points()
now = rospy.Time.now()
waist_l_base.header.stamp = now
waist_r_base.header.stamp = now
_listener.waitForTransform("table_height",waist_l_base.header.frame_id,now,rospy.Duration(5.0))
waist_l = _listener.transformPoint("table_height",waist_l_base)
waist_r= _listener.transformPoint("table_height",waist_r_base)
waist_l.point.z = 0
waist_r.point.z = 0
#Grab the waist point
smooth()
GripUtils.grab_point(waist_l,arm="l",yaw=-pi/2,x_offset=0.02)
#Fold over
ctr_x = (waist_l.point.x + waist_r.point.x)/2+0.02
ctr_y = (waist_l.point.y + waist_r.point.y)/2
ctr_z = waist_l.point.y - ctr_y
GripUtils.go_to( x=ctr_x, y=ctr_y, z=ctr_z,
roll=pi/2, pitch=pi/2, yaw=-pi/2,
arm="l", grip=True, frame=waist_l.header.frame_id)
GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=True,y_offset=0.01,x_offset=0.02)
GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=False,y_offset=-0.05,x_offset=0.02,dur=2.5)
GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=False,y_offset=-0.05,x_offset=0.02,z_offset=0.05,dur=2.5)
GripUtils.recall_arm("b")
#Grab waist
scoot_amt = 0.2
_cloth_tracker.scoot(-scoot_amt)
ctr_x = 0.25 * waist_l.point.x + 0.75 * waist_r.point.x + scoot_amt
ctr_y = 0.25 * waist_l.point.y + 0.75 * waist_r.point.y
ctr_z = waist_l.point.z
GripUtils.grab( x=ctr_x, y=ctr_y, z=ctr_z,
roll=pi/2, pitch=pi/4, yaw=0,
arm="r", frame=waist_l.header.frame_id)
sweep_drag_amount = 0.95*TABLE_WIDTH
sweep_lift_amount = 0.6
(x,y,z,r,p,yw) = sweep_cloth_with_scoot("r", sweep_drag_amount, TABLE_WIDTH, sweep_lift_amount,roll=pi/2)
print "y is %f"%y
print "(%f,%f,%f,%f,%f,%f)"%(x,y,z,r,p,y)
smooth("l")
GripUtils.go_to( x=x, y=y+PANTS_LENGTH/4-0.03, z=PANTS_LENGTH/4,
roll=r, pitch=3*pi/8, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH/2-0.03, z=PANTS_LENGTH/2,
roll=r, pitch=pi/2, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+3*PANTS_LENGTH/4-0.03, z=PANTS_LENGTH/4,
roll=r, pitch=5*pi/8, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH-0.03, z=0.01,
roll=r, pitch=pi-p, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH+0.05, z=0.01,
roll=r, pitch=pi-p, yaw=yw,
arm="r", grip=False, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH+0.05, z=0.1,
roll=r, pitch=pi-p, yaw=yw,
arm="r", grip=False, frame="table_height", dur=2.5)
GripUtils.recall_arm("r")
smooth()
_cloth_tracker.scoot(scoot_amt+0.08)
return True
def fold_cloth(dir='l'):
print "Folding cloth"
_cloth_tracker.scoot(-0.1)
GripUtils.recall_arm("b")
(waist_l_base, waist_r_base) = get_waist_points()
now = rospy.Time.now()
waist_l_base.header.stamp = now
waist_r_base.header.stamp = now
_listener.waitForTransform("table_height", waist_l_base.header.frame_id,
now, rospy.Duration(5.0))
waist_l = _listener.transformPoint("table_height", waist_l_base)
waist_r = _listener.transformPoint("table_height", waist_r_base)
waist_l.point.z = 0
waist_r.point.z = 0
#Grab the waist point
smooth()
GripUtils.grab_point(waist_l, arm="l", yaw=-pi / 2, x_offset=0.02)
#Fold over
ctr_x = (waist_l.point.x + waist_r.point.x) / 2 + 0.02
ctr_y = (waist_l.point.y + waist_r.point.y) / 2
ctr_z = waist_l.point.y - ctr_y
GripUtils.go_to(x=ctr_x,
y=ctr_y,
z=ctr_z,
roll=pi / 2,
pitch=pi / 2,
yaw=-pi / 2,
arm="l",
grip=True,
frame=waist_l.header.frame_id)
GripUtils.go_to_pt(waist_r,
arm="l",
roll=pi / 2,
pitch=3 * pi / 4,
yaw=-pi / 2,
grip=True,
y_offset=0.01,
x_offset=0.02)
GripUtils.go_to_pt(waist_r,
arm="l",
roll=pi / 2,
pitch=3 * pi / 4,
yaw=-pi / 2,
grip=False,
y_offset=-0.05,
x_offset=0.02,
dur=2.5)
GripUtils.go_to_pt(waist_r,
arm="l",
roll=pi / 2,
pitch=3 * pi / 4,
yaw=-pi / 2,
grip=False,
y_offset=-0.05,
x_offset=0.02,
z_offset=0.05,
dur=2.5)
GripUtils.recall_arm("b")
#Grab waist
scoot_amt = 0.2
_cloth_tracker.scoot(-scoot_amt)
ctr_x = 0.25 * waist_l.point.x + 0.75 * waist_r.point.x + scoot_amt
ctr_y = 0.25 * waist_l.point.y + 0.75 * waist_r.point.y
ctr_z = waist_l.point.z
GripUtils.grab(x=ctr_x,
y=ctr_y,
z=ctr_z,
roll=pi / 2,
pitch=pi / 4,
yaw=0,
arm="r",
frame=waist_l.header.frame_id)
sweep_drag_amount = 0.95 * TABLE_WIDTH
sweep_lift_amount = 0.6
(x, y, z, r, p, yw) = sweep_cloth_with_scoot("r",
sweep_drag_amount,
TABLE_WIDTH,
sweep_lift_amount,
roll=pi / 2)
print "y is %f" % y
print "(%f,%f,%f,%f,%f,%f)" % (x, y, z, r, p, y)
smooth("l")
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH / 4 - 0.03,
z=PANTS_LENGTH / 4,
roll=r,
pitch=3 * pi / 8,
yaw=yw,
arm="r",
grip=True,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH / 2 - 0.03,
z=PANTS_LENGTH / 2,
roll=r,
pitch=pi / 2,
yaw=yw,
arm="r",
grip=True,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + 3 * PANTS_LENGTH / 4 - 0.03,
z=PANTS_LENGTH / 4,
roll=r,
pitch=5 * pi / 8,
yaw=yw,
arm="r",
grip=True,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH - 0.03,
z=0.01,
roll=r,
pitch=pi - p,
yaw=yw,
arm="r",
grip=True,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH + 0.05,
z=0.01,
roll=r,
pitch=pi - p,
yaw=yw,
arm="r",
grip=False,
frame="table_height",
dur=2.5)
GripUtils.go_to(x=x,
y=y + PANTS_LENGTH + 0.05,
z=0.1,
roll=r,
pitch=pi - p,
yaw=yw,
arm="r",
grip=False,
frame="table_height",
dur=2.5)
GripUtils.recall_arm("r")
smooth()
_cloth_tracker.scoot(scoot_amt + 0.08)
return True
