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 adjust_fold(self, req):
#Go to viewing stance
StanceUtils.call_stance("open_both", 5.0)
StanceUtils.call_stance("viewing", 5.0)
last_model = pickle.load(open("/tmp/last_model.pickle"))
camera_model = image_geometry.PinholeCameraModel()
info = RosUtils.get_next_message("wide_stereo/left/camera_info",
CameraInfo)
cam_frame = info.header.frame_id
camera_model.fromCameraInfo(info)
now = rospy.Time.now()
req.start.header.stamp = now
req.end.header.stamp = now
self.listener.waitForTransform(cam_frame, req.start.header.frame_id,
now, rospy.Duration(20.0))
start_cam = self.listener.transformPoint(cam_frame, req.start)
end_cam = self.listener.transformPoint(cam_frame, req.end)
start = camera_model.project3dToPixel(
(start_cam.point.x, start_cam.point.y, start_cam.point.z))
end = camera_model.project3dToPixel(
(end_cam.point.x, end_cam.point.y, end_cam.point.z))
folded_model = Models.Point_Model_Folded(last_model, start, end)
folded_model.image = None
folded_model.initial_model.image = None
pickle.dump(folded_model, open("/tmp/last_model.pickle", 'w'))
adjust_folded_model = rospy.ServiceProxy(
"fold_finder_node/process_mono", ProcessMono)
resp = adjust_folded_model("wide_stereo/left")
new_start = resp.pts3d[0]
new_end = resp.pts3d[1]
return AdjustFoldResponse(start=new_start, end=new_end)
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 adjust_fold(self,req):
#Go to viewing stance
StanceUtils.call_stance("open_both",5.0)
StanceUtils.call_stance("viewing",5.0)
last_model = pickle.load(open("/tmp/last_model.pickle"))
camera_model = image_geometry.PinholeCameraModel()
info = RosUtils.get_next_message("wide_stereo/left/camera_info",CameraInfo)
cam_frame = info.header.frame_id
camera_model.fromCameraInfo(info)
now = rospy.Time.now()
req.start.header.stamp = now
req.end.header.stamp=now
self.listener.waitForTransform(cam_frame,req.start.header.frame_id,now,rospy.Duration(20.0))
start_cam = self.listener.transformPoint(cam_frame,req.start)
end_cam = self.listener.transformPoint(cam_frame,req.end)
start = camera_model.project3dToPixel((start_cam.point.x,start_cam.point.y,start_cam.point.z))
end = camera_model.project3dToPixel((end_cam.point.x,end_cam.point.y,end_cam.point.z))
folded_model = Models.Point_Model_Folded(last_model,start,end)
folded_model.image = None
folded_model.initial_model.image = None
pickle.dump(folded_model,open("/tmp/last_model.pickle",'w'))
adjust_folded_model = rospy.ServiceProxy("fold_finder_node/process_mono",ProcessMono)
resp = adjust_folded_model("wide_stereo/left")
new_start = resp.pts3d[0]
new_end = resp.pts3d[1]
return AdjustFoldResponse(start=new_start,end=new_end)
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 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_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 locate_polygon(self,req):
#Go to viewing stance
StanceUtils.call_stance("open_both",2.0)
StanceUtils.call_stance("viewing",2.0)
locate_poly = rospy.ServiceProxy("shape_fitter_node/process_mono",ProcessMono)
print "Calling locate_poly"
resp = locate_poly("wide_stereo/left")
print "Received response"
pts = resp.pts3d
for pt in pts:
self.publish(pt)
return LocatePolygonResponse()
def locate_polygon(self, req):
#Go to viewing stance
StanceUtils.call_stance("open_both", 2.0)
StanceUtils.call_stance("viewing", 2.0)
locate_poly = rospy.ServiceProxy("shape_fitter_node/process_mono",
ProcessMono)
print "Calling locate_poly"
resp = locate_poly("wide_stereo/left")
print "Received response"
pts = resp.pts3d
for pt in pts:
self.publish(pt)
return LocatePolygonResponse()
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 pair_socks():
pickup_sock("l")
open_sock()
put_on_dopple(depth=0.28)
StanceUtils.call_stance("arms_up",5.0)
pickup_sock("l") #Node: we do "l" because this is now the only sock on the table. It is now the "left" one.
open_sock()
put_on_dopple(depth=DOPPLE_HEIGHT-0.1)
#put_on_dopple(depth=0.29)
StanceUtils.call_stance("arms_up",5.0)
take_off_dopple_pair()
return
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 pair_socks():
pickup_sock("l")
open_sock()
put_on_dopple(depth=0.28)
StanceUtils.call_stance("arms_up", 5.0)
pickup_sock(
"l"
) #Node: we do "l" because this is now the only sock on the table. It is now the "left" one.
open_sock()
put_on_dopple(depth=DOPPLE_HEIGHT - 0.1)
#put_on_dopple(depth=0.29)
StanceUtils.call_stance("arms_up", 5.0)
take_off_dopple_pair()
return
def execute_primitive(self, primitive, req):
for weight_point in primitive.weight_points:
stance_name = weight_point.stance_name
dur = weight_point.dur
params = weight_point.params
resp = StanceUtils.call_stance(stance_name, dur, params)
#if not resp:
# return ExecutePrimitiveResponse(False)
return ExecutePrimitiveResponse(True)
def initial_pickup(arm):
success = True
StanceUtils.call_stance("look_down",5.0)
rospy.sleep(2.5)
"""
process_mono = rospy.ServiceProxy("clump_center_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
z_offset = 0.02
GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm=arm,z_offset=z_offset,grip=False,dur=5.0)
GripUtils.close_gripper(arm)
while not GripUtils.has_object(arm):
z_offset -= 0.005
GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm=arm,z_offset=z_offset,grip=False,dur=5.0)
GripUtils.close_gripper(arm)
"""
pt = grab_far_right(arm)
return pt
def initial_pickup(arm):
success = True
StanceUtils.call_stance("look_down", 5.0)
rospy.sleep(2.5)
"""
process_mono = rospy.ServiceProxy("clump_center_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
z_offset = 0.02
GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm=arm,z_offset=z_offset,grip=False,dur=5.0)
GripUtils.close_gripper(arm)
while not GripUtils.has_object(arm):
z_offset -= 0.005
GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm=arm,z_offset=z_offset,grip=False,dur=5.0)
GripUtils.close_gripper(arm)
"""
pt = grab_far_right(arm)
return pt
def execute_primitive(self,primitive,req):
for weight_point in primitive.weight_points:
stance_name = weight_point.stance_name
dur = weight_point.dur
params = weight_point.params
resp = StanceUtils.call_stance(stance_name,dur,params)
#if not resp:
# return ExecutePrimitiveResponse(False)
return ExecutePrimitiveResponse(True)
def executeSingleGrip(self, fold_traj):
print "hi from single grip"
approach_point = fold_traj.approach_points[0]
grip_point = fold_traj.grip_points[0]
quarter_point = fold_traj.quarter_points[0]
vertical_point = fold_traj.vertical_points[0]
goal_point = fold_traj.goal_points[0]
smooth_center = fold_traj.smooth_center
smooth_edges = fold_traj.smooth_edges
first_adjustment = False
tilt = fold_traj.tilts[0]
roll = -pi/2 #pi/2
pitch = pi/4
if fold_traj.red:
tilt = self.last_traj.tilts[0]%(2*pi)
rospy.loginfo("Tilt: %s"%tilt)
#We do this so that we don't let go of the vertex during a red fold
if not fold_traj.red:
self.executeSmooth(self.last_traj)
StanceUtils.open_gripper("b")
self.fold_stance("b")
arm = self.getArm(fold_traj)
#Move the unused arm out of the way
if arm=="l":
StanceUtils.call_stance("viewing_right",5.0)
else:
StanceUtils.call_stance("viewing_left",5.0)
(target,nothing) = self.gripPoints(point=approach_point,arm=arm,tilt=tilt,grip=False,preferred_pitch = pi/4,preferred_roll=roll,red=fold_traj.red)
pitch = target.pitch
self.gripPoints(point=grip_point,arm=arm,tilt=tilt,grip=False,preferred_pitch = pi/4,preferred_roll=roll,red=fold_traj.red,grab=True)
StanceUtils.close_gripper(arm)
else:
arm = self.last_arm
StanceUtils.close_gripper(arm)
self.gripPoints(point=vertical_point,grip=True,tilt=tilt,arm=arm,preferred_pitch=pi/2,preferred_roll=roll,red=fold_traj.red)
self.gripPoints(point=goal_point,grip=True,tilt=tilt,arm=arm,preferred_pitch=3*pi/5,preferred_roll=roll,red=fold_traj.red)
self.last_traj = fold_traj
self.last_arm = arm
def get_grip_point(sock):
StanceUtils.call_stance("look_down",5.0)
rospy.sleep(2.5)
print "Give me the grip point"
process_mono = rospy.ServiceProxy("click_point_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
resp.param_names = ("l","r")
resp.params = (0.0,0.0)
output = {}
for i,val in enumerate(resp.params):
output[resp.param_names[i]] = val
if sock == "l":
pt = resp.pts3d[0]
angle = -1*output["l"]
else:
pt = resp.pts3d[1]
angle = -1*output["r"]
return (pt,angle)
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 get_grip_point(sock):
StanceUtils.call_stance("look_down", 5.0)
rospy.sleep(2.5)
print "Give me the grip point"
process_mono = rospy.ServiceProxy("click_point_node/process_mono",
ProcessMono)
resp = process_mono("wide_stereo/left")
resp.param_names = ("l", "r")
resp.params = (0.0, 0.0)
output = {}
for i, val in enumerate(resp.params):
output[resp.param_names[i]] = val
if sock == "l":
pt = resp.pts3d[0]
angle = -1 * output["l"]
else:
pt = resp.pts3d[1]
angle = -1 * output["r"]
return (pt, angle)
def fold_stance(self,arm):
if arm=="b":
StanceUtils.call_stance("folding",3.0)
elif arm=="l":
StanceUtils.call_stance("folding_left",3.0)
elif arm=="r":
StanceUtils.call_stance("folding_right",3.0)
def fold_stance(self, arm):
if arm == "b":
StanceUtils.call_stance("folding", 3.0)
elif arm == "l":
StanceUtils.call_stance("folding_left", 3.0)
elif arm == "r":
StanceUtils.call_stance("folding_right", 3.0)
def initRobot():
# Look Down
if not StanceUtils.call_stance('look_down',5.0):
print "Look Down: Failure"
return False
# ARMS UP
height = 0.35
lateral_amount = 0.65
forward_amount = 0.3
if not GripUtils.go_to_multi( x_l=forward_amount, y_l=lateral_amount, z_l=height,
roll_l=0, pitch_l=0, yaw_l=0, grip_l=False,
x_r=forward_amount, y_r=-lateral_amount, z_r=height,
roll_r=0, pitch_r=0, yaw_r=0, grip_r=False,
frame_l="torso_lift_link", frame_r="torso_lift_link", dur=4.0):
return False
else:
return True
def viewing_stance(self,arm):
StanceUtils.call_stance("viewing",3.0)
def execute(self, userdata):
if not StanceUtils.call_stance(self.stance_name, self.dur):
return FAILURE
else:
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)
def flip_sock(pt, arm):
wiggle_down_dopple()
StanceUtils.call_stance("arms_up", 5.0)
take_off_dopple(pt, arm=arm)
smooth(pt, arm)
StanceUtils.call_stance("arms_up", 5.0)
def executeSingleGrip(self, fold_traj):
approach_point = fold_traj.approach_points[0]
grip_point = fold_traj.grip_points[0]
quarter_point = fold_traj.quarter_points[0]
vertical_point = fold_traj.vertical_points[0]
goal_point = fold_traj.goal_points[0]
smooth_center = fold_traj.smooth_center
smooth_edges = fold_traj.smooth_edges
first_adjustment = False
tilt = fold_traj.tilts[0]
roll = pi / 2
pitch = pi / 4
if fold_traj.red:
tilt = self.last_traj.tilts[0] % (2 * pi)
rospy.loginfo("Tilt: %s" % tilt)
#We do this so that we don't let go of the vertex during a red fold
if not fold_traj.red:
self.executeSmooth(self.last_traj)
StanceUtils.open_gripper("b")
self.fold_stance("b")
arm = self.getArm(fold_traj)
#Move the unused arm out of the way
if arm == "l":
StanceUtils.call_stance("viewing_right", 5.0)
else:
StanceUtils.call_stance("viewing_left", 5.0)
(target, nothing) = self.gripPoints(point=approach_point,
arm=arm,
tilt=tilt,
grip=False,
preferred_pitch=pi / 4,
preferred_roll=roll,
red=fold_traj.red)
pitch = target.pitch
self.gripPoints(point=grip_point,
arm=arm,
tilt=tilt,
grip=False,
preferred_pitch=pi / 4,
preferred_roll=roll,
red=fold_traj.red,
grab=True)
StanceUtils.close_gripper(arm)
else:
arm = self.last_arm
StanceUtils.close_gripper(arm)
self.gripPoints(point=vertical_point,
grip=True,
tilt=tilt,
arm=arm,
preferred_pitch=pi / 2,
preferred_roll=roll,
red=fold_traj.red)
self.gripPoints(point=goal_point,
grip=True,
tilt=tilt,
arm=arm,
preferred_pitch=3 * pi / 5,
preferred_roll=roll,
red=fold_traj.red)
self.last_traj = fold_traj
self.last_arm = arm
def flip_sock(pt,arm):
wiggle_down_dopple()
StanceUtils.call_stance("arms_up",5.0)
take_off_dopple(pt,arm=arm)
smooth(pt,arm)
StanceUtils.call_stance("arms_up",5.0)
def viewing_stance(self, arm):
StanceUtils.call_stance("viewing", 3.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):
if not StanceUtils.call_stance(self.stance_name,self.dur):
return FAILURE
else:
return SUCCESS
