def execute(self, userdata):
process_stereo = rospy.ServiceProxy("clump_center_node/process_mono",
ProcessMono)
resp = process_stereo("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():
z_offset -= 0.02
if (z_offset < 0):
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")
return SUCCESS
def main(args):
rospy.init_node("unfolding_click")
StanceUtils.call_stance("look_down", 3.0)
StanceUtils.call_stance("arms_up", 3.0)
while True and not rospy.is_shutdown():
process_mono = rospy.ServiceProxy("click_node/process_mono",
ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
z_offset = 0.06
link_frame = "r_wrist_roll_link"
GripUtils.go_to_pt(pt,
roll=pi / 2,
yaw=0,
pitch=pi / 2,
arm="r",
z_offset=z_offset,
grip=True,
dur=5.0,
link_frame=link_frame)
z_offset = 0.00
GripUtils.go_to_pt(pt,
roll=pi / 2,
yaw=0,
pitch=pi / 2,
arm="r",
z_offset=z_offset,
grip=True,
dur=5.0,
link_frame=link_frame)
rospy.sleep(5.0)
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):
process_mono = rospy.ServiceProxy("clump_center_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
z_offset = 0.16
GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",z_offset=z_offset,grip=False,dur=5.0)
GripUtils.open_gripper("l")
return SUCCESS
def main(args):
rospy.init_node("unfolding_click")
while True and not rospy.is_shutdown():
process_mono = rospy.ServiceProxy("click_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
print pt
GripUtils.go_to_pt(point=pt,roll=pi/2,pitch=pi/2,yaw=0,grip=True,arm="l",dur=5.0,link_name="l_tip_frame")
print "waiting.."
a = raw_input()
def execute(self, userdata):
process_mono = rospy.ServiceProxy("clump_center_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
pt.point.z += 0.05
self.listener.waitForTransform("/map", pt.header.frame_id, pt.header.stamp, rospy.Duration(15.0))
transpt = self.listener.transformPoint("/map", pt)
GripUtils.go_to_pt(transpt,roll=0,yaw=0,pitch=pi/2,arm="l",grip=True,dur=5.0)
userdata.locations['mark'] = transpt
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):
process_mono = rospy.ServiceProxy("clump_center_node_white/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():
z_offset -= 0.02
if(z_offset < 0):
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")
return SUCCESS
def main(args):
rospy.init_node("unfolding_click")
StanceUtils.call_stance("look_down",3.0)
StanceUtils.call_stance("arms_up",3.0)
while True and not rospy.is_shutdown():
process_mono = rospy.ServiceProxy("click_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
z_offset = 0.06
link_frame = "r_wrist_roll_link"
GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",z_offset=z_offset,grip=True,dur=5.0,link_frame=link_frame)
z_offset = 0.00
GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",z_offset=z_offset,grip=True,dur=5.0,link_frame=link_frame)
rospy.sleep(5.0)
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):
self.location = userdata.location
self.distance = userdata.distance
initial_separation = 0.11
if self.arm=="b":
#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):
return FAILURE
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.04,
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.04,
link_frame_r="r_wrist_back_frame",dur=2.0):
return FAILURE
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.03,
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.03,
link_frame_r="r_wrist_back_frame",dur=2.0):
return FAILURE
else:
#Right is negative in the y axis
if self.arm=="r":
y_multiplier = -1
else:
y_multiplier = 1.0
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):
return FAILURE
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):
return FAILURE
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.00,arm=self.arm,
link_frame="%s_wrist_back_frame"%self.arm,dur=2.0):
return FAILURE
return SUCCESS
def main(args):
rospy.init_node("unfolding_click")
while True and not rospy.is_shutdown():
process_mono = rospy.ServiceProxy("click_node/process_mono",
ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
print pt
GripUtils.go_to_pt(point=pt,
roll=pi / 2,
pitch=pi / 2,
yaw=0,
grip=True,
arm="l",
dur=5.0,
link_name="l_tip_frame")
print "waiting.."
a = raw_input()
def take_off_dopple(pt,arm):
x = DOPPLE_X
y = DOPPLE_Y
if arm == "l":
yaw = -pi/2
y -= 0.005
else:
yaw = pi/2
y += 0.005
z = DOPPLE_HEIGHT+TABLE_HEIGHT - 0.045
frame = "base_footprint"
GripUtils.go_to(x=x,y=y,z=z,roll=0,yaw=yaw,pitch=pi/4,arm=arm,grip=False,frame=frame,dur=5.0)
GripUtils.close_gripper(arm,extra_tight=False)
GripUtils.go_to(x=x,y=y,z=z+0.1,roll=0,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0)
GripUtils.go_to(x=x,y=y,z=z+0.2,roll=0,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0)
roll = 0
if arm=="l":
yaw = -pi/2
else:
yaw = pi/2
GripUtils.go_to(x=x-0.15,y=y,z=z+0.2,roll=roll,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0)
#Spreads out
GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=0,arm=arm,grip=True,z_offset = 0.2,y_offset=0.2,dur=5.0) # previously z_o= 0.08 y_o 0.1
GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=pi/2,arm=arm,grip=True,dur=7.5,y_offset = -0.2,z_offset=0.015)
GripUtils.open_gripper(arm)
GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=pi/4,arm=arm,grip=False,dur=2.5,y_offset = -0.2,z_offset=0.025)
StanceUtils.call_stance("arms_up",5.0)
def pickup_sock(sock):
z_off = 0.0
"""
while not rospy.is_shutdown():
StanceUtils.call_stance("close_right",5.0)
(grip_pt,angle) = get_grip_point(sock)
GripUtils.go_to_pt(grip_pt,roll=pi/2,yaw=pi/2-angle,pitch=pi/2,arm="r",grip=True,z_offset=z_off)
rospy.sleep(5.0)
z_off = float(raw_input("What is the new z_offset?"))
StanceUtils.call_stance("arms_up",5.0)
"""
(grip_pt,angle) = get_grip_point(sock)
if MODE==THICK_SOCK:
OFFSET = 0.02
else:
OFFSET = 0.025
y_offset = OFFSET*cos(angle)
x_offset = OFFSET*sin(angle)
#x_offset += 0.005
z_offset = 0.02
pitch = pi/2
roll = pi/2
yaw = pi/2-angle
GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0)
z_offset = -0.02
z_offset -= 0.001
GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0)
#Drag thin socks
if MODE == THIN_SOCK:
y_offset += 0.02
GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0)
while not GripUtils.has_object("r"):
StanceUtils.call_stance("open_right",2.0)
pitch -= ANGLE_INCREMENT
y_offset -= 0.005*cos(angle)
x_offset -= 0.005*sin(angle)
z_offset += 0.0015
GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0)
GripUtils.close_gripper("r",extra_tight=True)
break
return grip_pt
def pickup_clump(arm):
init_pt = initial_pickup("r")
GripUtils.go_to_pt(init_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",grip=True,z_offset = 0.08,dur=3.0)
GripUtils.go_to_pt(init_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",grip=True,z_offset = 0.01,y_offset=0.1,dur=3.0)
GripUtils.go_to_pt(init_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",grip=True,z_offset = 0.01,y_offset=-0.1,dur=3.0)
StanceUtils.call_stance("arms_up",5.0)
seam_pt = grab_far_left("l")
GripUtils.go_to_pt(seam_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",grip=True,z_offset = 0.08,dur=3.0)
GripUtils.go_to_pt(seam_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",grip=True,z_offset = 0.01,y_offset=+0.1,dur=3.0)
GripUtils.go_to_pt(seam_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",grip=True,z_offset = 0.01,y_offset=-0.1,dur=3.0)
StanceUtils.call_stance("arms_up",5.0)
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):
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 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 = userdata.locations['mark']
GripUtils.go_to_pt(pt,roll=0,yaw=0,pitch=pi/2,arm="l",z_offset=0,grip=True,dur=5.0)
return SUCCESS
def pickup_clump(arm):
init_pt = initial_pickup("r")
GripUtils.go_to_pt(init_pt,
roll=pi / 2,
yaw=0,
pitch=pi / 2,
arm="r",
grip=True,
z_offset=0.08,
dur=3.0)
GripUtils.go_to_pt(init_pt,
roll=pi / 2,
yaw=0,
pitch=pi / 2,
arm="r",
grip=True,
z_offset=0.01,
y_offset=0.1,
dur=3.0)
GripUtils.go_to_pt(init_pt,
roll=pi / 2,
yaw=0,
pitch=pi / 2,
arm="r",
grip=True,
z_offset=0.01,
y_offset=-0.1,
dur=3.0)
StanceUtils.call_stance("arms_up", 5.0)
seam_pt = grab_far_left("l")
GripUtils.go_to_pt(seam_pt,
roll=pi / 2,
yaw=0,
pitch=pi / 2,
arm="l",
grip=True,
z_offset=0.08,
dur=3.0)
GripUtils.go_to_pt(seam_pt,
roll=pi / 2,
yaw=0,
pitch=pi / 2,
arm="l",
grip=True,
z_offset=0.01,
y_offset=+0.1,
dur=3.0)
GripUtils.go_to_pt(seam_pt,
roll=pi / 2,
yaw=0,
pitch=pi / 2,
arm="l",
grip=True,
z_offset=0.01,
y_offset=-0.1,
dur=3.0)
StanceUtils.call_stance("arms_up", 5.0)
def pickup_sock(sock):
z_off = 0.0
"""
while not rospy.is_shutdown():
StanceUtils.call_stance("close_right",5.0)
(grip_pt,angle) = get_grip_point(sock)
GripUtils.go_to_pt(grip_pt,roll=pi/2,yaw=pi/2-angle,pitch=pi/2,arm="r",grip=True,z_offset=z_off)
rospy.sleep(5.0)
z_off = float(raw_input("What is the new z_offset?"))
StanceUtils.call_stance("arms_up",5.0)
"""
(grip_pt, angle) = get_grip_point(sock)
if MODE == THICK_SOCK:
OFFSET = 0.02
else:
OFFSET = 0.025
y_offset = OFFSET * cos(angle)
x_offset = OFFSET * sin(angle)
#x_offset += 0.005
z_offset = 0.02
pitch = pi / 2
roll = pi / 2
yaw = pi / 2 - angle
GripUtils.go_to_pt(grip_pt,
roll=roll,
yaw=yaw,
pitch=pitch,
arm="r",
x_offset=x_offset,
y_offset=y_offset,
z_offset=z_offset,
grip=False,
dur=5.0)
z_offset = -0.02
z_offset -= 0.001
GripUtils.go_to_pt(grip_pt,
roll=roll,
yaw=yaw,
pitch=pitch,
arm="r",
x_offset=x_offset,
y_offset=y_offset,
z_offset=z_offset,
grip=False,
dur=5.0)
#Drag thin socks
if MODE == THIN_SOCK:
y_offset += 0.02
GripUtils.go_to_pt(grip_pt,
roll=roll,
yaw=yaw,
pitch=pitch,
arm="r",
x_offset=x_offset,
y_offset=y_offset,
z_offset=z_offset,
grip=False,
dur=5.0)
while not GripUtils.has_object("r"):
StanceUtils.call_stance("open_right", 2.0)
pitch -= ANGLE_INCREMENT
y_offset -= 0.005 * cos(angle)
x_offset -= 0.005 * sin(angle)
z_offset += 0.0015
GripUtils.go_to_pt(grip_pt,
roll=roll,
yaw=yaw,
pitch=pitch,
arm="r",
x_offset=x_offset,
y_offset=y_offset,
z_offset=z_offset,
grip=False,
dur=5.0)
GripUtils.close_gripper("r", extra_tight=True)
break
return grip_pt
def execute(self, userdata):
self.location = userdata.location
self.distance = userdata.distance
initial_separation = 0.11
if self.arm == "b":
#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):
return FAILURE
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.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):
return FAILURE
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.03,
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.03,
link_frame_r="r_wrist_back_frame",
dur=2.0):
return FAILURE
else:
#Right is negative in the y axis
if self.arm == "r":
y_multiplier = -1
else:
y_multiplier = 1
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):
return FAILURE
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):
return FAILURE
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):
return FAILURE
return SUCCESS
def take_off_dopple(pt, arm):
x = DOPPLE_X
y = DOPPLE_Y
if arm == "l":
yaw = -pi / 2
y -= 0.005
else:
yaw = pi / 2
y += 0.005
z = DOPPLE_HEIGHT + TABLE_HEIGHT - 0.045
frame = "base_footprint"
GripUtils.go_to(x=x,
y=y,
z=z,
roll=0,
yaw=yaw,
pitch=pi / 4,
arm=arm,
grip=False,
frame=frame,
dur=5.0)
GripUtils.close_gripper(arm, extra_tight=False)
GripUtils.go_to(x=x,
y=y,
z=z + 0.1,
roll=0,
yaw=yaw,
pitch=0,
arm=arm,
grip=True,
frame=frame,
dur=5.0)
GripUtils.go_to(x=x,
y=y,
z=z + 0.2,
roll=0,
yaw=yaw,
pitch=0,
arm=arm,
grip=True,
frame=frame,
dur=5.0)
roll = 0
if arm == "l":
yaw = -pi / 2
else:
yaw = pi / 2
GripUtils.go_to(x=x - 0.15,
y=y,
z=z + 0.2,
roll=roll,
yaw=yaw,
pitch=0,
arm=arm,
grip=True,
frame=frame,
dur=5.0)
#Spreads out
GripUtils.go_to_pt(pt,
roll=roll,
yaw=yaw,
pitch=0,
arm=arm,
grip=True,
z_offset=0.2,
y_offset=0.2,
dur=5.0) # previously z_o= 0.08 y_o 0.1
GripUtils.go_to_pt(pt,
roll=roll,
yaw=yaw,
pitch=pi / 2,
arm=arm,
grip=True,
dur=7.5,
y_offset=-0.2,
z_offset=0.015)
GripUtils.open_gripper(arm)
GripUtils.go_to_pt(pt,
roll=roll,
yaw=yaw,
pitch=pi / 4,
arm=arm,
grip=False,
dur=2.5,
y_offset=-0.2,
z_offset=0.025)
StanceUtils.call_stance("arms_up", 5.0)
