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 open_sock():
x=0.5
y=0.0
z=TABLE_HEIGHT+0.4
frame="base_footprint"
#old strategy
GripUtils.go_to(x=x,y=y+0.007,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/8,grip=True,frame=frame,arm="r",dur=5.0)
GripUtils.go_to(x=x,y=y+0.05,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=False,frame=frame,arm="l",dur=5.0)
GripUtils.go_to(x=x,y=y-0.004,z=z+0.01,roll=pi/2,yaw=-pi/2,pitch=pi/5,grip=False,frame=frame,arm="l",dur=5.0)
GripUtils.go_to(x=x,y=y-0.0035,z=z+0.001,roll=pi/2,yaw=-pi/2,pitch=pi/5,grip=True,frame=frame,arm="l",dur=2.0) #changed from -0.007 to -0.0065
#GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=0,grip=True,frame=frame,arm="r",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.04,z=z+0.05,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.04,z=z+0.005,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
#GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/16,grip=True,frame=frame,arm="r",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.06,z=z-0.025,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.025,z=z-0.225,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=True,frame=frame,arm="l",dur=1.0)
#GripUtils.go_to(x=x,y=y+0.025,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=True,frame=frame,arm="l",dur=5.0)
GripUtils.close_gripper("l",extra_tight=True)
open_amt = 0.025
GripUtils.go_to_multi (x_l=x,y_l=y+open_amt,z_l=z+0.01,roll_l=pi/2,yaw_l=-pi/2,pitch_l=0,grip_l=True,frame_l=frame
,x_r=x,y_r=y-open_amt,z_r=z+0.01,roll_r=pi/2,yaw_r=pi/2,pitch_r=0,grip_r=True,frame_r=frame
,dur=5.0)
def 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 __init__(self, title=None, transitions=None):
NestedStateMachine.__init__(self,
title,
transitions=transitions,
outcomes=DEFAULT_OUTCOMES)
pr2_say(talk_greet)
self.add('Arms_Up', ArmsUp(grip=False), {
SUCCESS: 'Look_Down',
FAILURE: FAILURE
})
self.add('Look_Down', StanceState('look_down'), {
SUCCESS: SUCCESS,
FAILURE: FAILURE
})
# Calibrate open/close tolerances automatically
GripUtils.close_gripper('b')
rospy.sleep(1.0)
joint_states_msg = rospy.wait_for_message('joint_states', JointState)
joint_states = dict(
zip(joint_states_msg.name, joint_states_msg.position))
l_eps = joint_states['l_gripper_joint']
r_eps = joint_states['r_gripper_joint']
rel_tol = 0.1
abs_tol = 0.0001
l_has_obj_min = l_eps + max(abs_tol, abs(l_eps * rel_tol))
r_has_obj_min = r_eps + max(abs_tol, abs(l_eps * rel_tol))
rospy.set_param('l_has_obj_min', l_has_obj_min)
rospy.set_param('r_has_obj_min', r_has_obj_min)
pr2_say(talk_initialize)
def add_default_stances(self):
rospy.Service("stances/pause", ExecuteStance, self.pause)
rospy.Service("stances/open_left", ExecuteStance, lambda req: GripUtils.open_gripper("l"))
rospy.Service("stances/open_right", ExecuteStance, lambda req: GripUtils.open_gripper("r"))
rospy.Service("stances/open_both", ExecuteStance, lambda req: GripUtils.open_gripper("b"))
rospy.Service("stances/close_left", ExecuteStance, lambda req: GripUtils.close_gripper("l"))
rospy.Service("stances/close_right", ExecuteStance, lambda req: GripUtils.close_gripper("r"))
rospy.Service("stances/close_both", ExecuteStance, lambda req: GripUtils.close_gripper("b"))
self.default_stances = ()
def take_out_cloth():
print "Taking out cloth"
RosUtils.call_service("move_torso",MoveTorso,height=0.01)
DryerNavigationUtils.goToPosition("enter_dryer")
GripUtils.go_to(x=-0.1,y=-0.33,z=-0.4,roll=0,pitch=0,yaw=0,grip=False,arm="l",frame="dryer")
DryerNavigationUtils.goToPosition("into_dryer")
GripUtils.go_to(x=0.15,y=-0.33,z=-0.65,roll=0,pitch=pi/2,yaw=0,grip=False,arm="l",frame="dryer")
GripUtils.close_gripper("l")
GripUtils.go_to(x=0.15,y=-0.33,z=-0.4,roll=0,pitch=0,yaw=0,grip=True,arm="l",frame="dryer")
DryerNavigationUtils.goToPosition("enter_dryer")
RosUtils.call_service("move_torso",MoveTorso,height=0.3)
return True
def take_off_dopple_pair():
x = DOPPLE_X
y = DOPPLE_Y
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="r",grip=False,frame=frame,dur=5.0)
GripUtils.close_gripper("r",extra_tight=False)
GripUtils.go_to(x=x,y=y,z=z+0.1,roll=pi/2,yaw=yaw,pitch=0,arm="r",grip=True,frame=frame,dur=5.0)
GripUtils.go_to(x=x,y=y,z=z+0.2,roll=pi/2,yaw=yaw,pitch=0,arm="r",grip=True,frame=frame,dur=5.0)
GripUtils.go_to(x=x-0.2,y=y,z=z+0.2,roll=pi/2,yaw=yaw,pitch=0,arm="r",grip=True,frame=frame,dur=5.0)
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 take_off_dopple_pair():
x = DOPPLE_X
y = DOPPLE_Y
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="r",
grip=False,
frame=frame,
dur=5.0)
GripUtils.close_gripper("r", extra_tight=False)
GripUtils.go_to(x=x,
y=y,
z=z + 0.1,
roll=pi / 2,
yaw=yaw,
pitch=0,
arm="r",
grip=True,
frame=frame,
dur=5.0)
GripUtils.go_to(x=x,
y=y,
z=z + 0.2,
roll=pi / 2,
yaw=yaw,
pitch=0,
arm="r",
grip=True,
frame=frame,
dur=5.0)
GripUtils.go_to(x=x - 0.2,
y=y,
z=z + 0.2,
roll=pi / 2,
yaw=yaw,
pitch=0,
arm="r",
grip=True,
frame=frame,
dur=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 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 __init__(self, title=None, transitions=None):
NestedStateMachine.__init__(self, title, transitions=transitions, outcomes=DEFAULT_OUTCOMES)
pr2_say(talk_greet)
self.add("Arms_Up", ArmsUp(grip=False), {SUCCESS: "Look_Down", FAILURE: FAILURE})
self.add("Look_Down", StanceState("look_down"), {SUCCESS: SUCCESS, FAILURE: FAILURE})
# Calibrate open/close tolerances automatically
GripUtils.close_gripper("b")
rospy.sleep(1.0)
joint_states_msg = rospy.wait_for_message("joint_states", JointState)
joint_states = dict(zip(joint_states_msg.name, joint_states_msg.position))
l_eps = joint_states["l_gripper_joint"]
r_eps = joint_states["r_gripper_joint"]
rel_tol = 0.1
abs_tol = 0.0001
l_has_obj_min = l_eps + max(abs_tol, abs(l_eps * rel_tol))
r_has_obj_min = r_eps + max(abs_tol, abs(l_eps * rel_tol))
rospy.set_param("l_has_obj_min", l_has_obj_min)
rospy.set_param("r_has_obj_min", r_has_obj_min)
pr2_say(talk_initialize)
def take_out_cloth():
print "Taking out cloth"
RosUtils.call_service("move_torso", MoveTorso, height=0.01)
DryerNavigationUtils.goToPosition("enter_dryer")
GripUtils.go_to(x=-0.1,
y=-0.33,
z=-0.4,
roll=0,
pitch=0,
yaw=0,
grip=False,
arm="l",
frame="dryer")
DryerNavigationUtils.goToPosition("into_dryer")
GripUtils.go_to(x=0.15,
y=-0.33,
z=-0.65,
roll=0,
pitch=pi / 2,
yaw=0,
grip=False,
arm="l",
frame="dryer")
GripUtils.close_gripper("l")
GripUtils.go_to(x=0.15,
y=-0.33,
z=-0.4,
roll=0,
pitch=0,
yaw=0,
grip=True,
arm="l",
frame="dryer")
DryerNavigationUtils.goToPosition("enter_dryer")
RosUtils.call_service("move_torso", MoveTorso, height=0.3)
return True
def open_sock():
x = 0.5
y = 0.0
z = TABLE_HEIGHT + 0.4
frame = "base_footprint"
#old strategy
GripUtils.go_to(x=x,
y=y + 0.007,
z=z,
roll=pi / 2,
yaw=pi / 2,
pitch=-pi / 8,
grip=True,
frame=frame,
arm="r",
dur=5.0)
GripUtils.go_to(x=x,
y=y + 0.05,
z=z,
roll=pi / 2,
yaw=-pi / 2,
pitch=0,
grip=False,
frame=frame,
arm="l",
dur=5.0)
GripUtils.go_to(x=x,
y=y - 0.004,
z=z + 0.01,
roll=pi / 2,
yaw=-pi / 2,
pitch=pi / 5,
grip=False,
frame=frame,
arm="l",
dur=5.0)
GripUtils.go_to(x=x,
y=y - 0.0035,
z=z + 0.001,
roll=pi / 2,
yaw=-pi / 2,
pitch=pi / 5,
grip=True,
frame=frame,
arm="l",
dur=2.0) #changed from -0.007 to -0.0065
#GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=0,grip=True,frame=frame,arm="r",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.04,z=z+0.05,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.04,z=z+0.005,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
#GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/16,grip=True,frame=frame,arm="r",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.06,z=z-0.025,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
#GripUtils.go_to(x=x,y=y+0.025,z=z-0.225,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=True,frame=frame,arm="l",dur=1.0)
#GripUtils.go_to(x=x,y=y+0.025,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=True,frame=frame,arm="l",dur=5.0)
GripUtils.close_gripper("l", extra_tight=True)
open_amt = 0.025
GripUtils.go_to_multi(x_l=x,
y_l=y + open_amt,
z_l=z + 0.01,
roll_l=pi / 2,
yaw_l=-pi / 2,
pitch_l=0,
grip_l=True,
frame_l=frame,
x_r=x,
y_r=y - open_amt,
z_r=z + 0.01,
roll_r=pi / 2,
yaw_r=pi / 2,
pitch_r=0,
grip_r=True,
frame_r=frame,
dur=5.0)
def 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 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 execute(self, userdata):
if not GripUtils.close_gripper(self.arm):
return FAILURE
else:
return SUCCESS
def execute(self, userdata):
if not GripUtils.close_gripper(self.arm):
return FAILURE
else:
return SUCCESS
