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 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 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 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
