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 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 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 executeSmooth(self, fold_traj):
if fold_traj == False:
return False
if fold_traj.ignore_smooth:
return False
if pi / 4 <= abs(fold_traj.tilts[0]) <= 3 * pi / 4:
return False
scale = 1
if self.is_backwards(fold_traj):
scale = -1
StanceUtils.open_gripper("b")
self.fold_stance("b")
GRIPPER_LENGTH = 0.195
center = fold_traj.smooth_center
left_start = self.torso_pt(center)
right_start = self.torso_pt(center)
left_start.point.y += 0.07
left_start.point.x += scale * 0.1
left_start.point.z += GRIPPER_LENGTH + 0.01
right_start.point.y -= 0.07
right_start.point.x += scale * 0.1
right_start.point.z += GRIPPER_LENGTH + 0.01
left_end = self.torso_pt(fold_traj.smooth_edges[0])
left_end.point.z += GRIPPER_LENGTH + 0.01
left_end.point.x += scale * 0.08
right_end = self.torso_pt(fold_traj.smooth_edges[1])
right_end.point.z += GRIPPER_LENGTH + 0.01
right_end.point.x += scale * 0.08
left_tilt = 0
right_tilt = 0
#Start moving
target_left = GripTarget(point=left_start,
arm="l",
yaw=left_tilt,
grip=True,
pitch=-pi / 2,
roll=0)
target_right = GripTarget(point=right_start,
arm="r",
yaw=right_tilt,
grip=True,
pitch=-pi / 2,
roll=0)
self.sendTarget(5.0, target_left, target_right)
target_left = GripTarget(point=left_end,
arm="l",
yaw=left_tilt,
grip=True,
pitch=-pi / 2,
roll=0)
target_right = GripTarget(point=right_end,
arm="r",
yaw=right_tilt,
grip=True,
pitch=-pi / 2,
roll=0)
self.sendTarget(5.0, target_left, target_right)
self.fold_stance("b")
return True
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 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 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 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 init_stances(self):
my_stances = {}
folding_height_stance = Stance (set_torso_height=True,torso_height=0.3,arms="n")
my_stances["folding_height"] = folding_height_stance
viewing_height_stance = Stance (set_torso_height=True,torso_height=0.2,arms="n")
my_stances["viewing_height"] = viewing_height_stance
folding_stance = Stance(x_left=0.65,y_left=0.3,z_left=0.2,roll_left=pi/2,pitch_left=pi/4,yaw_left=0,grip_left=False,frame_left="torso_lift_link"
,x_right=0.65,y_right=-0.3,z_right=0.2,roll_right=pi/2,pitch_right=pi/4,yaw_right=0,grip_right=False,frame_right="torso_lift_link"
,arms="b")
my_stances["folding"] = folding_stance
folding_right_stance = Stance(x_left=0.65,y_left=0.3,z_left=0.2,roll_left=pi/2,pitch_left=pi/4,yaw_left=0,grip_left=False,frame_left="torso_lift_link"
,x_right=0.65,y_right=-0.3,z_right=0.2,roll_right=pi/2,pitch_right=pi/4,yaw_right=0,grip_right=False,frame_right="torso_lift_link"
,arms="r")
my_stances["folding_right"] = folding_right_stance
folding_left_stance = Stance(x_left=0.65,y_left=0.3,z_left=0.2,roll_left=pi/2,pitch_left=pi/4,yaw_left=0,grip_left=False,frame_left="torso_lift_link"
,x_right=0.65,y_right=-0.3,z_right=0.2,roll_right=pi/2,pitch_right=pi/4,yaw_right=0,grip_right=False,frame_right="torso_lift_link"
,arms="l")
my_stances["folding_left"] = folding_left_stance
viewing_stance = Stance (x_left=0.1,y_left=0.7,z_left=0.3,roll_left=pi/2,pitch_left=-pi/4,yaw_left=0,grip_left=False,frame_left='torso_lift_link'
,x_right=0.1,y_right=-0.7,z_right=0.3,roll_right=pi/2,pitch_right=-pi/4,yaw_right=0,grip_right=False,frame_right='torso_lift_link'
,arms="b")
my_stances["viewing"] = viewing_stance
viewing_stance_left = Stance (x_left=0.1,y_left=0.7,z_left=0.3,roll_left=pi/2,pitch_left=-pi/4,yaw_left=0,grip_left=False,frame_left='torso_lift_link'
,x_right=0.1,y_right=-0.7,z_right=0.3,roll_right=pi/2,pitch_right=-pi/4,yaw_right=0,grip_right=False,frame_right='torso_lift_link'
,arms="l")
my_stances["viewing_left"] = viewing_stance_left
viewing_stance_right = Stance (x_left=0.1,y_left=0.7,z_left=0.3,roll_left=pi/2,pitch_left=-pi/4,yaw_left=0,grip_left=False,frame_left='torso_lift_link'
,x_right=0.1,y_right=-0.7,z_right=0.3,roll_right=pi/2,pitch_right=-pi/4,yaw_right=0,grip_right=False,frame_right='torso_lift_link'
,arms="r")
my_stances["viewing_right"] = viewing_stance_right
#Head stances
my_stances["look_bottom_left"] = Stance(arms="n",set_head_angle=True,head_pan=0.6,head_tilt=1.25)
my_stances["look_top_left"] = Stance(arms="n",set_head_angle=True,head_pan=0.6,head_tilt=0.9)
my_stances["look_bottom_right"] = Stance(arms="n",set_head_angle=True,head_pan=-0.6,head_tilt=1.25)
my_stances["look_top_right"] = Stance(arms="n",set_head_angle=True,head_pan=-0.6,head_tilt=0.9)
my_stances["look_center"] = Stance(arms="n",set_head_angle=True,head_pan=0.0,head_tilt=1.0)
StanceUtils.add_stances(my_stances)
initialize = PrimitiveUtils.make_primitive(stances=["folding_height","viewing"],durs=[5.0,5.0],params=[[],[]])
PrimitiveUtils.add_primitive(primitive=initialize,name="initialize")
scan_cloth = PrimitiveUtils.make_primitive(
stances=["viewing","look_bottom_left","pause","look_top_left","pause","look_top_right","pause","look_bottom_right","pause"],
durs=[5.0,5.0,25.0,5.0,25.0,5.0,25.0,5.0,25.0],
params=[[],[],[],[],[],[],[],[],[]]
)
PrimitiveUtils.add_primitive(primitive=scan_cloth,name="scan_cloth")
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 executeSmooth(self,fold_traj):
if fold_traj == False:
return False
if fold_traj.ignore_smooth:
return False
if pi/4 <= abs(fold_traj.tilts[0]) <= 3*pi/4:
return False
scale = 1
if self.is_backwards(fold_traj):
scale = -1
StanceUtils.open_gripper("b")
self.fold_stance("b")
GRIPPER_LENGTH = 0.195
center = fold_traj.smooth_center
left_start = self.torso_pt(center)
right_start = self.torso_pt(center)
left_start.point.y += 0.07
left_start.point.x += scale*0.1
left_start.point.z += GRIPPER_LENGTH + 0.01
right_start.point.y -= 0.07
right_start.point.x += scale*0.1
right_start.point.z += GRIPPER_LENGTH + 0.01
left_end = self.torso_pt(fold_traj.smooth_edges[0])
left_end.point.z += GRIPPER_LENGTH + 0.01
left_end.point.x += scale*0.08
right_end = self.torso_pt(fold_traj.smooth_edges[1])
right_end.point.z += GRIPPER_LENGTH + 0.01
right_end.point.x += scale*0.08
left_tilt = 0
right_tilt = 0
#Start moving
target_left =GripTarget(point=left_start,arm="l",yaw=left_tilt,grip=True,pitch=-pi/2,roll=0)
target_right = GripTarget(point=right_start,arm="r",yaw=right_tilt,grip=True,pitch=-pi/2,roll=0)
self.sendTarget(5.0,target_left,target_right)
target_left = GripTarget(point=left_end,arm="l",yaw=left_tilt,grip=True,pitch=-pi/2,roll=0)
target_right = GripTarget(point=right_end,arm="r",yaw=right_tilt,grip=True,pitch=-pi/2,roll=0)
self.sendTarget(5.0,target_left,target_right)
self.fold_stance("b")
return True
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 executeBiGrip(self, fold_traj):
tilts = fold_traj.tilts
approach_points = fold_traj.approach_points
grip_points = fold_traj.grip_points
quarter_points = fold_traj.quarter_points
weight_points = fold_traj.weight_points
vertical_points = fold_traj.vertical_points
goal_points = fold_traj.goal_points
print "hi from bigrip"
rolls = [pi/2,pi/2]
#rolls = [-pi/2,-pi/2]
#rolls = [0,0]
pitches = [pi/4,pi/4]
#pitches = [0,0]
if not fold_traj.red:
self.executeSmooth(self.last_traj)
self.fold_stance("b")
arms = self.getArms(fold_traj)
#Initially, both are in "approach" mode
(target1,target2) = self.gripPoints(point=approach_points[0],grip=False,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/4,preferred_roll=rolls[0],
point2=approach_points[1],grip2=False,tilt2=tilts[1],arm2=arms[1],preferred_pitch2=pi/4,preferred_roll2=rolls[1])
#pitches[0] = target1.pitch
#Do the first pickup and nothing with the other arm
self.gripPoints(point=grip_points[0],grip=False,tilt=tilts[0],arm=arms[0],preferred_pitch=0
,preferred_roll=rolls[0],grab=True)
StanceUtils.close_gripper(arms[0])
#Do the second pickup, with the first arm interpolated
first_dx = vertical_points[0].point.x - grip_points[0].point.x
first_dy = vertical_points[0].point.y - grip_points[0].point.y
first_dz = vertical_points[0].point.z - grip_points[0].point.z
second_dx = vertical_points[1].point.x - grip_points[1].point.x
second_dy = vertical_points[1].point.y - grip_points[1].point.y
second_dz = vertical_points[1].point.z - grip_points[1].point.z
interp_dx = first_dx - second_dx
interp_dy = first_dy - second_dy
interp_dz = first_dz - second_dz
interp_x = grip_points[0].point.x + interp_dx
interp_y = grip_points[0].point.y + interp_dy
interp_z = grip_points[0].point.z + interp_dz
interp_pt = PointStamped()
interp_pt.point.x = interp_x
interp_pt.point.y = interp_y
interp_pt.point.z = interp_z
interp_pt.header.frame_id = grip_points[0].header.frame_id
#Go to the second approach point
(target2,nothing) = self.gripPoints(point=interp_pt,grip=True,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/4,preferred_roll=rolls[0],
point2=approach_points[1],arm2=arms[1],grip2=False,tilt2=tilts[1],preferred_pitch2=pi/4,preferred_roll2=rolls[1])
#pitches[1] = target2.pitch
self.gripPoints (point=interp_pt,grip=True,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/4,preferred_roll=rolls[0]
,point2=grip_points[1],grip2=False,tilt2=tilts[1],arm2=arms[1],preferred_pitch2=pi/4,preferred_roll2=rolls[1])
self.gripPoints(point=grip_points[1],grip=False,tilt=tilts[1],arm=arms[1],preferred_pitch=pi/4,preferred_roll=rolls[1],grab=True)
StanceUtils.close_gripper(arms[1])
else:
arms = self.getArms(fold_traj)
#Bring both to middle
self.gripPoints (point=vertical_points[0],grip=True,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/2,preferred_roll=rolls[0]
,point2=vertical_points[1],grip2=True,tilt2=tilts[1],arm2=arms[1],preferred_pitch2=pi/2,preferred_roll2=rolls[1]) #FIX pi/4 -> pi/2
#Set first one down
first_dx = goal_points[0].point.x - vertical_points[0].point.x
first_dy = goal_points[0].point.y - vertical_points[0].point.y
first_dz = goal_points[0].point.z - vertical_points[0].point.z
second_dx = goal_points[1].point.x - vertical_points[1].point.x
second_dy = goal_points[1].point.y - vertical_points[1].point.y
second_dz = goal_points[1].point.z - vertical_points[1].point.z
interp_dx = first_dx - second_dx
interp_dy = first_dy - second_dy
interp_dz = first_dz - second_dz
interp_x = goal_points[0].point.x - interp_dx
interp_y = goal_points[0].point.y - interp_dy
interp_z = goal_points[0].point.z - interp_dz
interp_pt = PointStamped()
interp_pt.point.x = interp_x
interp_pt.point.y = interp_y
interp_pt.point.z = interp_z
interp_pt.header.frame_id = grip_points[0].header.frame_id
self.gripPoints (point=interp_pt,grip=True,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/2,preferred_roll=rolls[0]
,point2=goal_points[1],grip2=True,tilt2=tilts[1],arm2=arms[1],preferred_pitch2=pi/2,preferred_roll2=rolls[1])
#Finally, set last one down
self.gripPoints(point=goal_points[0],grip=True,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/2,preferred_roll=rolls[0]
,point2=goal_points[1],grip2=True,tilt2=tilts[1],arm2=arms[1],preferred_pitch2=pi/2,preferred_roll2=rolls[1])
GripUtils.open_grippers()
goal_points[0].point.z += 0.03
goal_points[1].point.z += 0.03
self.gripPoints(point=goal_points[0],grip=False,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/2,preferred_roll=rolls[0]
,point2=goal_points[1],grip2=False,tilt2=tilts[1],arm2=arms[1],preferred_pitch2=pi/2,preferred_roll2=rolls[1])
self.last_traj = fold_traj
def init_stances(self):
my_stances = {}
folding_height_stance = Stance(set_torso_height=True,
torso_height=0.3,
arms="n")
my_stances["folding_height"] = folding_height_stance
viewing_height_stance = Stance(set_torso_height=True,
torso_height=0.2,
arms="n")
my_stances["viewing_height"] = viewing_height_stance
folding_stance = Stance(x_left=0.65,
y_left=0.3,
z_left=0.2,
roll_left=pi / 2,
pitch_left=pi / 4,
yaw_left=0,
grip_left=False,
frame_left="torso_lift_link",
x_right=0.65,
y_right=-0.3,
z_right=0.2,
roll_right=pi / 2,
pitch_right=pi / 4,
yaw_right=0,
grip_right=False,
frame_right="torso_lift_link",
arms="b")
my_stances["folding"] = folding_stance
folding_right_stance = Stance(x_left=0.65,
y_left=0.3,
z_left=0.2,
roll_left=pi / 2,
pitch_left=pi / 4,
yaw_left=0,
grip_left=False,
frame_left="torso_lift_link",
x_right=0.65,
y_right=-0.3,
z_right=0.2,
roll_right=pi / 2,
pitch_right=pi / 4,
yaw_right=0,
grip_right=False,
frame_right="torso_lift_link",
arms="r")
my_stances["folding_right"] = folding_right_stance
folding_left_stance = Stance(x_left=0.65,
y_left=0.3,
z_left=0.2,
roll_left=pi / 2,
pitch_left=pi / 4,
yaw_left=0,
grip_left=False,
frame_left="torso_lift_link",
x_right=0.65,
y_right=-0.3,
z_right=0.2,
roll_right=pi / 2,
pitch_right=pi / 4,
yaw_right=0,
grip_right=False,
frame_right="torso_lift_link",
arms="l")
my_stances["folding_left"] = folding_left_stance
viewing_stance = Stance(x_left=0.1,
y_left=0.7,
z_left=0.3,
roll_left=pi / 2,
pitch_left=-pi / 4,
yaw_left=0,
grip_left=False,
frame_left='torso_lift_link',
x_right=0.1,
y_right=-0.7,
z_right=0.3,
roll_right=pi / 2,
pitch_right=-pi / 4,
yaw_right=0,
grip_right=False,
frame_right='torso_lift_link',
arms="b")
my_stances["viewing"] = viewing_stance
viewing_stance_left = Stance(x_left=0.1,
y_left=0.7,
z_left=0.3,
roll_left=pi / 2,
pitch_left=-pi / 4,
yaw_left=0,
grip_left=False,
frame_left='torso_lift_link',
x_right=0.1,
y_right=-0.7,
z_right=0.3,
roll_right=pi / 2,
pitch_right=-pi / 4,
yaw_right=0,
grip_right=False,
frame_right='torso_lift_link',
arms="l")
my_stances["viewing_left"] = viewing_stance_left
viewing_stance_right = Stance(x_left=0.1,
y_left=0.7,
z_left=0.3,
roll_left=pi / 2,
pitch_left=-pi / 4,
yaw_left=0,
grip_left=False,
frame_left='torso_lift_link',
x_right=0.1,
y_right=-0.7,
z_right=0.3,
roll_right=pi / 2,
pitch_right=-pi / 4,
yaw_right=0,
grip_right=False,
frame_right='torso_lift_link',
arms="r")
my_stances["viewing_right"] = viewing_stance_right
#Head stances
my_stances["look_bottom_left"] = Stance(arms="n",
set_head_angle=True,
head_pan=0.6,
head_tilt=1.25)
my_stances["look_top_left"] = Stance(arms="n",
set_head_angle=True,
head_pan=0.6,
head_tilt=0.9)
my_stances["look_bottom_right"] = Stance(arms="n",
set_head_angle=True,
head_pan=-0.6,
head_tilt=1.25)
my_stances["look_top_right"] = Stance(arms="n",
set_head_angle=True,
head_pan=-0.6,
head_tilt=0.9)
my_stances["look_center"] = Stance(arms="n",
set_head_angle=True,
head_pan=0.0,
head_tilt=1.0)
StanceUtils.add_stances(my_stances)
initialize = PrimitiveUtils.make_primitive(
stances=["folding_height", "viewing"],
durs=[5.0, 5.0],
params=[[], []])
PrimitiveUtils.add_primitive(primitive=initialize, name="initialize")
scan_cloth = PrimitiveUtils.make_primitive(
stances=[
"viewing", "look_bottom_left", "pause", "look_top_left",
"pause", "look_top_right", "pause", "look_bottom_right",
"pause"
],
durs=[5.0, 5.0, 25.0, 5.0, 25.0, 5.0, 25.0, 5.0, 25.0],
params=[[], [], [], [], [], [], [], [], []])
PrimitiveUtils.add_primitive(primitive=scan_cloth, name="scan_cloth")
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 viewing_stance(self,arm):
StanceUtils.call_stance("viewing",3.0)
def executeBiGrip(self, fold_traj):
tilts = fold_traj.tilts
approach_points = fold_traj.approach_points
grip_points = fold_traj.grip_points
quarter_points = fold_traj.quarter_points
weight_points = fold_traj.weight_points
vertical_points = fold_traj.vertical_points
goal_points = fold_traj.goal_points
rolls = [pi / 2, pi / 2]
#pitches = [pi/4,pi/4]
pitches = [pi / 4, pi / 4]
if not fold_traj.red:
self.executeSmooth(self.last_traj)
self.fold_stance("b")
arms = self.getArms(fold_traj)
#Initially, both are in "approach" mode
(target1, target2) = self.gripPoints(point=approach_points[0],
grip=False,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 4,
preferred_roll=rolls[0],
point2=approach_points[1],
grip2=False,
tilt2=tilts[1],
arm2=arms[1],
preferred_pitch2=pi / 4,
preferred_roll2=rolls[1])
#pitches[0] = target1.pitch
#Do the first pickup and nothing with the other arm
self.gripPoints(point=grip_points[0],
grip=False,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 4,
preferred_roll=rolls[0],
grab=True)
StanceUtils.close_gripper(arms[0])
#Do the second pickup, with the first arm interpolated
first_dx = vertical_points[0].point.x - grip_points[0].point.x
first_dy = vertical_points[0].point.y - grip_points[0].point.y
first_dz = vertical_points[0].point.z - grip_points[0].point.z
second_dx = vertical_points[1].point.x - grip_points[1].point.x
second_dy = vertical_points[1].point.y - grip_points[1].point.y
second_dz = vertical_points[1].point.z - grip_points[1].point.z
interp_dx = first_dx - second_dx
interp_dy = first_dy - second_dy
interp_dz = first_dz - second_dz
interp_x = grip_points[0].point.x + interp_dx
interp_y = grip_points[0].point.y + interp_dy
interp_z = grip_points[0].point.z + interp_dz
interp_pt = PointStamped()
interp_pt.point.x = interp_x
interp_pt.point.y = interp_y
interp_pt.point.z = interp_z
interp_pt.header.frame_id = grip_points[0].header.frame_id
#Go to the second approach point
(target2, nothing) = self.gripPoints(point=interp_pt,
grip=True,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 4,
preferred_roll=rolls[0],
point2=approach_points[1],
arm2=arms[1],
grip2=False,
tilt2=tilts[1],
preferred_pitch2=pi / 4,
preferred_roll2=rolls[1])
#pitches[1] = target2.pitch
self.gripPoints(point=interp_pt,
grip=True,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 4,
preferred_roll=rolls[0],
point2=grip_points[1],
grip2=False,
tilt2=tilts[1],
arm2=arms[1],
preferred_pitch2=pi / 4,
preferred_roll2=rolls[1])
self.gripPoints(point=grip_points[1],
grip=False,
tilt=tilts[1],
arm=arms[1],
preferred_pitch=pi / 4,
preferred_roll=rolls[1],
grab=True)
StanceUtils.close_gripper(arms[1])
else:
arms = self.getArms(fold_traj)
#Bring both to middle
self.gripPoints(point=vertical_points[0],
grip=True,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 2,
preferred_roll=rolls[0],
point2=vertical_points[1],
grip2=True,
tilt2=tilts[1],
arm2=arms[1],
preferred_pitch2=pi / 2,
preferred_roll2=rolls[1]) #FIX pi/4 -> pi/2
#Set first one down
first_dx = goal_points[0].point.x - vertical_points[0].point.x
first_dy = goal_points[0].point.y - vertical_points[0].point.y
first_dz = goal_points[0].point.z - vertical_points[0].point.z
second_dx = goal_points[1].point.x - vertical_points[1].point.x
second_dy = goal_points[1].point.y - vertical_points[1].point.y
second_dz = goal_points[1].point.z - vertical_points[1].point.z
interp_dx = first_dx - second_dx
interp_dy = first_dy - second_dy
interp_dz = first_dz - second_dz
interp_x = goal_points[0].point.x - interp_dx
interp_y = goal_points[0].point.y - interp_dy
interp_z = goal_points[0].point.z - interp_dz
interp_pt = PointStamped()
interp_pt.point.x = interp_x
interp_pt.point.y = interp_y
interp_pt.point.z = interp_z
interp_pt.header.frame_id = grip_points[0].header.frame_id
self.gripPoints(point=interp_pt,
grip=True,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 2,
preferred_roll=rolls[0],
point2=goal_points[1],
grip2=True,
tilt2=tilts[1],
arm2=arms[1],
preferred_pitch2=pi / 2,
preferred_roll2=rolls[1])
#Finally, set last one down
self.gripPoints(point=goal_points[0],
grip=True,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 2,
preferred_roll=rolls[0],
point2=goal_points[1],
grip2=True,
tilt2=tilts[1],
arm2=arms[1],
preferred_pitch2=pi / 2,
preferred_roll2=rolls[1])
GripUtils.open_grippers()
goal_points[0].point.z += 0.03
goal_points[1].point.z += 0.03
self.gripPoints(point=goal_points[0],
grip=False,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 2,
preferred_roll=rolls[0],
point2=goal_points[1],
grip2=False,
tilt2=tilts[1],
arm2=arms[1],
preferred_pitch2=pi / 2,
preferred_roll2=rolls[1])
self.last_traj = fold_traj
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 StanceUtils.call_stance(self.stance_name, self.dur):
return FAILURE
else:
return SUCCESS
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 execute(self,userdata):
if not StanceUtils.call_stance(self.stance_name,self.dur):
return FAILURE
else:
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
