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):
if self.side == 'l':
arm = 'l'
processor = 'far_left_finder_node'
yaw = -pi / 2
else:
arm = 'r'
processor = 'far_right_finder_node'
yaw = pi / 2
process_mono = rospy.ServiceProxy("%s/process_mono" % processor,
ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
pt_opp = resp.pts3d[1]
userdata.arm = 'l' if arm == 'r' else 'r' #Arm returns the arm which is currently gripping
homogeneity = resp.params[resp.param_names.index("homogeneity")]
if homogeneity > 0.13:
print "Too homogeneous: %f" % homogeneity
return FAILURE
userdata.cloth_width = sqrt((pt_opp.point.x - pt.point.x)**2 +
(pt_opp.point.y - pt.point.y)**2)
#userdata.cloth_width = abs(pt_opp.point.y - pt.point.y)
userdata.cloth_height = None
if arm == "l":
x_offset = -0.02 #was -0.01
else:
x_offset = -0.01
if not GripUtils.grab_point(
pt, roll=-pi / 2, yaw=yaw, arm=arm, x_offset=x_offset):
return FAILURE
else:
if self.let_go:
GripUtils.open_gripper(opp_arm(arm))
return SUCCESS
def execute(self, userdata):
forward_amount = 0.375
height = 0.65
drop_amount = 0.25
lateral_amount = 0.05
if self.arm=="r":
yaw_multiplier = 1
y_multiplier = -1
else:
yaw_multiplier = -1
y_multiplier = 1
for i in range(self.num_shakes):
if i == 0:
duration = 4.0
if not GripUtils.go_to(x=forward_amount,y=0,z=height,
roll=0,pitch=0,yaw=yaw_multiplier*pi/2,grip=True,
frame="table_height",arm=self.arm,dur=duration):
return FAILURE
duration = .5
if not GripUtils.go_to(x=forward_amount,y=lateral_amount*y_multiplier,z=height-drop_amount,
roll=0,pitch=pi/4,yaw=yaw_multiplier*pi/2,grip=True,
frame="table_height",arm=self.arm,dur=duration):
return FAILURE
if not GripUtils.go_to(x=forward_amount,y=0,z=height,
roll=0,pitch=0,yaw=yaw_multiplier*pi/2,grip=True,
frame="table_height",arm=self.arm,dur=duration):
return FAILURE
return SUCCESS
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 execute(self, userdata):
if self.side == 'l':
arm = 'l'
processor = 'far_left_finder_node'
yaw = -pi/2
else:
arm = 'r'
processor = 'far_right_finder_node'
yaw = pi/2
process_mono = rospy.ServiceProxy("%s/process_mono"%processor,ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
pt_opp = resp.pts3d[1]
userdata.arm = 'l' if arm == 'r' else 'r' #Arm returns the arm which is currently gripping
homogeneity = resp.params[resp.param_names.index("homogeneity")]
if homogeneity > 0.13:
print "Too homogeneous: %f"%homogeneity
return FAILURE
userdata.cloth_width = sqrt( (pt_opp.point.x - pt.point.x)**2 + (pt_opp.point.y - pt.point.y)**2 )
#userdata.cloth_width = abs(pt_opp.point.y - pt.point.y)
userdata.cloth_height = None
if arm == "l":
x_offset = -0.02 #was -0.01
else:
x_offset = -0.01
if not GripUtils.grab_point(pt,roll=-pi/2,yaw=yaw,arm=arm,x_offset=x_offset):
return FAILURE
else:
if self.let_go:
GripUtils.open_gripper(opp_arm(arm))
return SUCCESS
def execute(self, userdata):
forward_amount = 0.5
hover_amount = 0.06
if self.direction == "r":
drag_arm = "r"
edge_y = -1.0 * self.table_width / 2.0
drag_yaw = pi/2
final_y = edge_y + self.drag_amount
else:
drag_arm = "l"
edge_y = 1.0 * self.table_width / 2.0
drag_yaw = -pi/2
final_y = edge_y - self.drag_amount
if not GripUtils.go_to(x=forward_amount*.8,y=edge_y*1.25,z=self.lift_amount,
roll=0,pitch=0,yaw=drag_yaw/4,grip=True,
frame="table_heig@staticmethodht",arm=drag_arm):
print 'f1'
return FAILURE
if not GripUtils.go_to(x=forward_amount,y=edge_y,z=hover_amount,
roll=0,pitch=pi/4,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm):
print 'f2'
return FAILURE
if not GripUtils.go_to(x=forward_amount,y=final_y,z=hover_amount,
roll=0,pitch=pi/2,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm):
print 'f3'
return FAILURE
return SUCCESS
def executeFold_right(self,gripPt,endPt,color_current='blue',color_next='blue'):
"""
execute fold with right arm
"""
print "points",gripPt,endPt
if (color_current == 'blue'):
startpoint = self.convert_to_world_frame(gripPt)
if not GripUtils.grab(x = startpoint.point.x,y=startpoint.point.y,z=startpoint.point.z,arm='r',
roll=pi/2,yaw=pi/3,pitch=pi/4,approach= True,frame=startpoint.header.frame_id):
print "failure"
return False
midpoint = self.dupl_PointStamped(endPt)#self.convert_to_world_frame(endPt)
midpoint.point.x = (midpoint.point.x + startpoint.point.x)/2
midpoint.point.y = (midpoint.point.y + startpoint.point.y)/2
midpoint.point.z = (midpoint.point.z + 0.1)
pitch = pi/4
roll = pi/2
yaw=pi/2
grip=True
frame= midpoint.header.frame_id
if not GripUtils.go_to(x=midpoint.point.x,y=midpoint.point.y,z=midpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='r',dur=7.5):
print "failure to go to ",midpoint.point.x,midpoint.point.y,midpoint.point.z
#return False
endpoint = self.dupl_PointStamped(endPt)#self.convert_to_world_frame(endPt)
frame=endpoint.header.frame_id
if not GripUtils.go_to(x=endpoint.point.x,y=endpoint.point.y,z=endpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='r',dur=7.5):
print "failure"
#return False
if(color_next == 'blue'):
initRobot()
return True
def execute(self, userdata):
bl = userdata.bl
br = userdata.br
tl = userdata.tl
tr = userdata.tr
userdata.cloth_width = sqrt((bl.point.x - br.point.x)**2 +
(bl.point.y - br.point.y)**2) * 1.075
userdata.cloth_height = max(
[abs(tl.point.x - bl.point.x),
abs(tr.point.x - br.point.x)])
if not GripUtils.grab_point(bl,
roll=pi / 2,
yaw=-pi / 2,
pitch=pi / 4,
arm="l",
x_offset=0.01,
INIT_SCOOT_AMT=0.01):
return FAILURE
if not GripUtils.grab_point(br,
roll=-pi / 2,
yaw=pi / 2,
pitch=pi / 4,
arm="r",
x_offset=0.01,
INIT_SCOOT_AMT=0.01):
return FAILURE
return SUCCESS
def execute(self, userdata):
bl = userdata.bl
br = userdata.br
tl = userdata.tl
tr = userdata.tr
midleft = bl
midleft.point.x = (bl.point.x + tl.point.x) / 2
midleft.point.y = (bl.point.y + tl.point.y) / 2
midleft.point.z = (bl.point.z + tl.point.z) / 2
midright = br
midright.point.x = (br.point.x + tr.point.x) / 2
midright.point.y = (br.point.y + tr.point.y) / 2
midright.point.z = (br.point.z + tr.point.z) / 2
if not GripUtils.grab_point(
midleft, roll=pi / 2, yaw=-pi / 2, pitch=pi / 4, arm="l", x_offset=-0.01, z_offset=0.01, INIT_SCOOT_AMT=0.01
):
return FAILURE
if not GripUtils.grab_point(
midright,
roll=-pi / 2,
yaw=pi / 2,
pitch=pi / 4,
arm="r",
x_offset=-0.01,
z_offset=0.01,
INIT_SCOOT_AMT=0.01,
):
return FAILURE
if not GripUtils.go_to_relative_multi(0, 0, 0.4, True, 0, 0, 0.4, True, "base_footprint"):
return FAILURE
return SUCCESS
def execute(self, userdata):
height = 0.35
lateral_amount = 0.65
forward_amount = 0.3
if self.arm == 'b':
lateral_amount_r = lateral_amount * -1
lateral_amount_l = lateral_amount
if not GripUtils.go_to_multi( x_l=forward_amount, y_l=lateral_amount_l, z_l=height,
roll_l=0, pitch_l=0, yaw_l=0,
frame_l="torso_lift_link", grip_l=False,
x_r=forward_amount, y_r=lateral_amount_r, z_r=height,
roll_r=0, pitch_r=0, yaw_r=0,
frame_r="torso_lift_link", grip_r=False, dur=4.0):
return FAILURE
else:
return SUCCESS
else:
if self.arm == 'r':
lateral_amount = lateral_amount * -1
if not GripUtils.go_to( x=forward_amount, y=lateral_amount, z=height,
roll=0, pitch=0, yaw=0, grip=False,
frame="torso_lift_link", dur=4.0, arm=self.arm):
return FAILURE
else:
return SUCCESS
def execute(self,userdata):
arm = userdata.arm
multiplier = 1 if arm == 'r' else -1
if not GripUtils.go_to(x=0.4,y=0,z=0.35,roll=0,yaw=pi/2*multiplier,pitch=pi/4,arm=arm,grip=True,frame="table_height",dur=3.0):
return FAILURE
GripUtils.open_gripper(arm)
GripUtils.recall_arm("b")
return SUCCESS
def execute(self,userdata):
arm = userdata.arm
multiplier = 1 if arm == 'r' else -1
if not GripUtils.go_to(x=0.4,y=0,z=0.35,roll=0,yaw=pi/2*multiplier,pitch=pi/4,arm=arm,grip=True,frame="table_height",dur=3.0):
return FAILURE
GripUtils.open_gripper(arm)
GripUtils.recall_arm("b")
return SUCCESS
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 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 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")
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 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):
bl = userdata.bl
br = userdata.br
tl = userdata.tl
tr = userdata.tr
userdata.cloth_width = sqrt( (bl.point.x-br.point.x)**2 + (bl.point.y - br.point.y)**2)*1.075
userdata.cloth_height = max([abs(tl.point.x-bl.point.x),abs(tr.point.x-br.point.x)])
if not GripUtils.grab_point(bl,roll=pi/2,yaw=-pi/2,pitch=pi/4,arm="l",x_offset=0.01,INIT_SCOOT_AMT = 0.01):
return FAILURE
if not GripUtils.grab_point(br,roll=-pi/2,yaw=pi/2,pitch=pi/4,arm="r",x_offset=0.01,INIT_SCOOT_AMT = 0.01):
return FAILURE
return SUCCESS
def execute(self, userdata):
forward_amount = 0.375
height = 0.65
drop_amount = 0.25
lateral_amount = 0.05
if self.arm == "r":
yaw_multiplier = 1
y_multiplier = -1
else:
yaw_multiplier = -1
y_multiplier = 1
for i in range(self.num_shakes):
if i == 0:
duration = 4.0
if not GripUtils.go_to(x=forward_amount,
y=0,
z=height,
roll=0,
pitch=0,
yaw=yaw_multiplier * pi / 2,
grip=True,
frame="table_height",
arm=self.arm,
dur=duration):
return FAILURE
duration = .5
if not GripUtils.go_to(x=forward_amount,
y=lateral_amount * y_multiplier,
z=height - drop_amount,
roll=0,
pitch=pi / 4,
yaw=yaw_multiplier * pi / 2,
grip=True,
frame="table_height",
arm=self.arm,
dur=duration):
return FAILURE
if not GripUtils.go_to(x=forward_amount,
y=0,
z=height,
roll=0,
pitch=0,
yaw=yaw_multiplier * pi / 2,
grip=True,
frame="table_height",
arm=self.arm,
dur=duration):
return FAILURE
return SUCCESS
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 execute(self, userdata):
cloth_width = userdata.cloth_width
if cloth_width < 0.35:
rospy.logwarn("Cannot shake at less than 0.35m apart; tried to do %s" % str(cloth_width))
return FAILURE
if cloth_width > 0.65:
cloth_width = 0.65
forward_amount = 0.45
height = 0.625
drop_amount = 0.3
lateral_amount = 0.1
for i in range(self.num_shakes):
if i == 0:
duration = 4.0
if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height,
roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True,
x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height,
roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=duration):
return FAILURE
duration = .45
return_val = SUCCESS
if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0-lateral_amount,z_l=height-drop_amount,
roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,
x_r=forward_amount,y_r=-cloth_width/2.0+lateral_amount,z_r=height-drop_amount,
roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=duration):
return_val = FAILURE
if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height,
roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True,
x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height,
roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=duration):
return_val = FAILURE
cloth_width = cloth_width * self.clothWidthScaleFactor
if not GripUtils.go_to_multi(x_l=forward_amount+.2,y_l=cloth_width/2.0,z_l=height,
roll_l=0,pitch_l=0,yaw_l=-pi/4,grip_l=True,
x_r=forward_amount+.2,y_r=-cloth_width/2.0,z_r=height,
roll_r=0,pitch_r=0,yaw_r=pi/4,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=4.0):
return FAILURE
return SUCCESS
def go_to_folding_station():
print "Going to folding station"
DryerNavigationUtils.dryerToTable1()
GripUtils.go_to(arm="l",
x=0.5,
y=0,
z=0.35,
roll=0,
yaw=0,
pitch=pi / 2,
grip=True,
frame="table_height")
GripUtils.open_gripper("l")
return True
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):
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):
forward_amount = 0.5
hover_amount = 0.06
if self.direction == "r":
drag_arm = "r"
edge_y = -1.0 * self.table_width / 2.0
drag_yaw = pi / 2
final_y = edge_y + self.drag_amount
else:
drag_arm = "l"
edge_y = 1.0 * self.table_width / 2.0
drag_yaw = -pi / 2
final_y = edge_y - self.drag_amount
if not GripUtils.go_to(x=forward_amount * .8,
y=edge_y * 1.25,
z=self.lift_amount,
roll=0,
pitch=0,
yaw=drag_yaw / 4,
grip=True,
frame="table_height",
arm=drag_arm):
print 'f1'
return FAILURE
if not GripUtils.go_to(x=forward_amount,
y=edge_y,
z=hover_amount,
roll=0,
pitch=pi / 4,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm):
print 'f2'
return FAILURE
if not GripUtils.go_to(x=forward_amount,
y=final_y,
z=hover_amount,
roll=0,
pitch=pi / 2,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm):
print 'f3'
return FAILURE
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):
bl = userdata.bl
br = userdata.br
tl = userdata.tl
tr = userdata.tr
midleft = bl
midleft.point.x = (bl.point.x + tl.point.x) / 2
midleft.point.y = (bl.point.y + tl.point.y) / 2
midleft.point.z = (bl.point.z + tl.point.z) / 2
midright = br
midright.point.x = (br.point.x + tr.point.x) / 2
midright.point.y = (br.point.y + tr.point.y) / 2
midright.point.z = (br.point.z + tr.point.z) / 2
if not GripUtils.grab_point(midleft,
roll=pi / 2,
yaw=-pi / 2,
pitch=pi / 4,
arm="l",
x_offset=-0.01,
z_offset=0.01,
INIT_SCOOT_AMT=0.01):
return FAILURE
if not GripUtils.grab_point(midright,
roll=-pi / 2,
yaw=pi / 2,
pitch=pi / 4,
arm="r",
x_offset=-0.01,
z_offset=0.01,
INIT_SCOOT_AMT=0.01):
return FAILURE
if not GripUtils.go_to_pts(point_l=midleft,
roll_l=pi / 2,
yaw_l=-pi / 2,
pitch_l=pi / 4,
grip_l=True,
point_r=midright,
roll_r=-pi / 2,
yaw_r=pi / 2,
pitch_r=pi / 4,
grip_r=True,
z_offset_l=0.4,
z_offset_r=0.4):
return FAILURE
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 sendTarget(self, dur, target1, target2=False,grab=False):
resp = False
if grab:
point = target1.point
pitch = target1.pitch
roll = target1.roll
yaw = target1.yaw
GripUtils.grab_point(point=point,pitch=pitch,roll=roll,yaw=yaw,arm=target1.arm,approach=False)
return
target1.point.header.stamp = rospy.Time.now()
if not target2:
try:
srv = rospy.ServiceProxy("move_one_arm",MoveOneArm)
resp = srv(MoveOneArmRequest(target=target1,dur=dur))
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
def execute(self, userdata):
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=True,
x_r=forward_amount,
y_r=-lateral_amount,
z_r=height,
roll_r=0,
pitch_r=0,
yaw_r=0,
grip_r=True,
frame_l="torso_lift_link",
frame_r="torso_lift_link",
dur=4.0):
return FAILURE
else:
return SUCCESS
def execute(self, userdata):
if self.arm == "l":
arm = "l"
processor = "far_left_finder_node"
yaw = -pi / 2
roll = -pi / 2
else:
arm = "r"
processor = "far_right_finder_node"
yaw = pi / 2
roll = pi / 2
process_mono = rospy.ServiceProxy("%s/process_mono" % processor, ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
pt_opp = resp.pts3d[1]
userdata.arm = "l" if arm == "r" else "r" # Arm returns the arm which is currently gripping
homogeneity = resp.params[resp.param_names.index("homogeneity")]
if homogeneity > 0.13:
print "Too homogeneous: %f" % homogeneity
return FAILURE
userdata.cloth_width = sqrt((pt_opp.point.x - pt.point.x) ** 2 + (pt_opp.point.y - pt.point.y) ** 2)
# userdata.cloth_width = abs(pt_opp.point.y - pt.point.y)
userdata.cloth_height = None
# Offsets for safety
if arm == "l":
y_offset = -0.02
z_offset = 0.01
else:
y_offset = 0.02
x_offset = 0.02
if self.side == "t":
x_offset = -0.01
elif self.side == "b":
x_offset = 0.01
else:
x_offset = 0
if not GripUtils.grab_point(pt, roll=roll, yaw=yaw, arm=arm, x_offset=x_offset, y_offset=y_offset):
return FAILURE
else:
if self.let_go:
GripUtils.open_gripper(opp_arm(arm))
if self.recall:
GripUtils.recall_arm(arm, grip=True)
return SUCCESS
def execute(self, userdata):
height = 0.35
lateral_amount = 0.65
forward_amount = 0.3
if self.arm == 'b':
lateral_amount_r = lateral_amount * -1
lateral_amount_l = lateral_amount
if not GripUtils.go_to_multi(x_l=forward_amount,
y_l=lateral_amount_l,
z_l=height,
roll_l=0,
pitch_l=0,
yaw_l=0,
frame_l="torso_lift_link",
grip_l=False,
x_r=forward_amount,
y_r=lateral_amount_r,
z_r=height,
roll_r=0,
pitch_r=0,
yaw_r=0,
frame_r="torso_lift_link",
grip_r=False,
dur=4.0):
return FAILURE
else:
return SUCCESS
else:
if self.arm == 'r':
lateral_amount = lateral_amount * -1
if not GripUtils.go_to(x=forward_amount,
y=lateral_amount,
z=height,
roll=0,
pitch=0,
yaw=0,
grip=False,
frame="torso_lift_link",
dur=4.0,
arm=self.arm):
return FAILURE
else:
return SUCCESS
def main(args):
rospy.init_node("lift_demo_node")
listener = tf.TransformListener()
GripUtils.go_to_relative(0, 0, 0.1, True, "l","base_footprint",listener)
sm = OuterStateMachine(DEFAULT_OUTCOMES)
START_STATE = 'Lift_Off'
with sm:
OuterStateMachine.add('Initialize',Initialize(),{SUCCESS:START_STATE,FAILURE:FAILURE})
OuterStateMachine.add('Lift_Off',LiftOff(),{SUCCESS:SUCCESS,FAILURE:FAILURE})
OuterStateMachine.add('Clump_To_Triangle',ClumpToTriangle(),{SUCCESS:'Triangle_To_Rectangle',FAILURE:'Initialize'})
OuterStateMachine.add('Triangle_To_Rectangle',TriangleToRectangle(),{SUCCESS:'Fold_Towel',FAILURE:FAILURE})
OuterStateMachine.add('Fold_Towel',FoldTowel(),{SUCCESS:SUCCESS,FAILURE:FAILURE})
sis = smach_ros.IntrospectionServer('demo_smach_server', sm, '/SM_ROOT')
sis.start()
outcome = sm.execute()
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_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 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):
cloth_width = userdata.cloth_width*.90
if cloth_width < MINIMUM_SHAKE_WIDTH:
rospy.logwarn("Cannot shake at less than %s m apart; tried to do %d" % (MINIMUM_SHAKE_WIDTH, cloth_width))
return FAILURE
forward_amount = 0.45
height = 0.625
if self.violent:
drop_amount = 0.3
else:
drop_amount = 0.1
lateral_amount = 0.1
for i in range(self.num_shakes):
if i == 0:
duration = 4.0
if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height,
roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True,
x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height,
roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=duration):
return_val = FAILURE
duration = .6
return_val = SUCCESS
if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0-lateral_amount,z_l=height-drop_amount,
roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,
x_r=forward_amount,y_r=-cloth_width/2.0+lateral_amount,z_r=height-drop_amount,
roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=duration):
return_val = FAILURE
if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height,
roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True,
x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height,
roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True,
frame_l="table_height",frame_r="table_height",dur=duration):
return_val = FAILURE
return SUCCESS
def executeDrag(self,gripPts,endPts,color='blue'):
"""
drag from gripPts to endPts
"""
startpoints = []
for vertex in gripPts:
pt_world = self.convert_to_world_frame(vertex)
now = rospy.Time.now()
self.listener.waitForTransform("base_footprint",pt_world.header.frame_id,now,rospy.Duration(10.0))
pt_transformed = self.listener.transformPoint("base_footprint",pt_world)
startpoints.append(pt_transformed)
#print "grabbed (x,y,z)",startpoints[0].point.x,startpoints[0].point.y,startpoints[0].point.z
if not GripUtils.grab_points(point_l=startpoints[0],roll_l=-pi/2,yaw_l=-pi/3,pitch_l=pi/4,x_offset_l=-0.01, z_offset_l=0.005,approach= True,
point_r=startpoints[1],roll_r=pi/2,yaw_r= pi/3,pitch_r=pi/4,x_offset_r=-0.01, z_offset_r=0.005):
print "failure"
return False
else:
print "grabbed (x,y,z)",startpoints[0].point.x,startpoints[0].point.y,startpoints[0].point.z
print "done"
midpoints = []
for vertex in endPts:
pt_world = self.dupl_PointStamped(vertex)#self.convert_to_world_frame(vertex)
midpoints.append(pt_world)
frame_l = frame_r = midpoints[0].header.frame_id
if not GripUtils.go_to_multi (x_l=midpoints[0].point.x,y_l=midpoints[0].point.y,z_l=midpoints[0].point.z,roll_l=-pi/2,yaw_l=-pi/2,pitch_l=pi/4,grip_l=True,frame_l=frame_l
,x_r=midpoints[1].point.x, y_r= midpoints[1].point.y ,z_r= midpoints[1].point.z ,roll_r=pi/2,yaw_r=pi/2,pitch_r=pi/4,grip_r=True,frame_r=frame_r,dur=5):
print "failure"
#return False
else:
print "dragging done"
if (color == 'blue'):
GripUtils.open_grippers()
return True
def execute(self, userdata):
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=self.grip,
x_r=forward_amount, y_r=-lateral_amount, z_r=height,
roll_r=0, pitch_r=0, yaw_r=0, grip_r=self.grip,
frame_l="torso_lift_link", frame_r="torso_lift_link", dur=4.0):
return FAILURE
else:
return SUCCESS
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 sendTarget(self, dur, target1, target2=False, grab=False):
resp = False
if grab:
point = target1.point
pitch = target1.pitch
roll = target1.roll
yaw = target1.yaw
GripUtils.grab_point(point=point,
pitch=pitch,
roll=roll,
yaw=yaw,
arm=target1.arm,
approach=False)
return
target1.point.header.stamp = rospy.Time.now()
if not target2:
try:
srv = rospy.ServiceProxy("move_one_arm", MoveOneArm)
resp = srv(MoveOneArmRequest(target=target1, dur=dur))
except rospy.ServiceException, e:
rospy.loginfo("Service Call Failed: %s" % e)
def execute(self, userdata):
process_mono = rospy.ServiceProxy("triangle_fitter_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt_l = resp.pts3d[0]
pt_r = resp.pts3d[1]
pt_tl = resp.pts3d[2]
pt_tr = resp.pts3d[3]
#Compute approximate widths
towel_width = sqrt( (pt_l.point.x - pt_tr.point.x)**2 + (pt_l.point.y - pt_tr.point.y)**2 )
towel_height = sqrt( (pt_l.point.x - pt_tl.point.x)**2 + (pt_l.point.y - pt_tl.point.y)**2 )
userdata.cloth_width = towel_width
userdata.cloth_height = towel_height
if resp.params[resp.param_names.index("left")] == 0:
left = False
else:
left = True
print "Correct?"
tf = raw_input()
if len(tf) > 0 and (tf[0] == 'n' or tf[0] == 'f'):
left = not left
if left:
GripUtils.recall_arm("l")
if GripUtils.grab_point(pt_l,roll=pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=0.005):
return SUCCESS
else:
return FAILURE
else:
GripUtils.recall_arm("r")
if GripUtils.grab_point(pt_r,roll=-pi/2,yaw=pi/3,pitch=pi/4,arm="r",x_offset=0.005):
return SUCCESS
else:
return FAILURE
def table1ToTable2(width):
global numStacked
GripUtils.go_to_relative_multi(0, 0, 0.2, True, 0, 0, 0.2, True, "base_footprint", None)
goFromAToB("Table1", "Table2")
GripUtils.go_to_relative_multi(0, 0, -0.2+0.02*numStacked, True, 0, 0, -0.2+0.02*numStacked, True, "base_footprint", None)
numStacked += 1
GripUtils.open_grippers()
def table1ToTable2(width):
global numStacked
GripUtils.go_to_relative_multi(0, 0, 0.2, True, 0, 0, 0.2, True, "base_footprint", None)
goFromAToB("Table1", "Table2")
GripUtils.go_to_relative_multi(0, 0, -0.2+0.02*numStacked, True, 0, 0, -0.2+0.02*numStacked, True, "base_footprint", None)
numStacked += 1
GripUtils.open_grippers()
def execute(self, userdata):
process_mono = rospy.ServiceProxy("triangle_fitter_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt_l = resp.pts3d[0]
pt_r = resp.pts3d[1]
pt_tl = resp.pts3d[2]
pt_tr = resp.pts3d[3]
#Compute approximate widths
towel_width = sqrt( (pt_l.point.x - pt_tr.point.x)**2 + (pt_l.point.y - pt_tr.point.y)**2 )
towel_height = sqrt( (pt_l.point.x - pt_tl.point.x)**2 + (pt_l.point.y - pt_tl.point.y)**2 )
userdata.cloth_width = towel_width
userdata.cloth_height = towel_height
if resp.params[resp.param_names.index("left")] == 0:
left = False
else:
left = True
print "Correct?"
tf = raw_input()
if len(tf) > 0 and (tf[0] == 'n' or tf[0] == 'f'):
left = not left
if left:
GripUtils.recall_arm("l")
if GripUtils.grab_point(pt_l,roll=pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=0.005):
return SUCCESS
else:
return FAILURE
else:
GripUtils.recall_arm("r")
if GripUtils.grab_point(pt_r,roll=-pi/2,yaw=pi/3,pitch=pi/4,arm="r",x_offset=0.005):
return SUCCESS
else:
return FAILURE
def smooth(pt,side):
y_offset = -0.08
x_offset = 0.02
x = pt.point.x + x_offset
y = pt.point.y + y_offset
z = pt.point.z
frame = "base_footprint"
x_l = x
x_r = x
y_l = y
y_r = y
z_l = z_r = z
roll_l = roll_r = 0
yaw_l = -pi/2
yaw_r = pi/2
pitch_l = pitch_r=pi/6
grip_l = grip_r = True
GripUtils.go_to_multi (x_l=x,y_l=y_l,z_l=z+0.04,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame
,x_r=x,y_r=y_r,z_r=z+0.04,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame
,dur=5.0)
GripUtils.go_to_multi (x_l=x,y_l=y_l,z_l=z,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame
,x_r=x,y_r=y_r,z_r=z,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame
,dur=3.0)
y_l += 0.06
y_r -= 0.06
pitch_l = pi/4
pitch_r = pi/4
GripUtils.go_to_multi (x_l=x,y_l=y_l,z_l=z+0.01,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame
,x_r=x,y_r=y_r,z_r=z+0.01,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame
,dur=5.0)
def execute(self, userdata):
forward_amount = self.forward_amount
hover_amount = 0.06
if self.direction == "r":
pr2_say(talk_drag2)
drag_arm = "r"
edge_y = self.table_width / 2.0
drag_yaw = pi/2
final_y = edge_y - self.drag_amount
else:
pr2_say(talk_drag1)
drag_arm = "l"
edge_y = -1 * self.table_width / 2.0
drag_yaw = -pi/2
final_y = edge_y + self.drag_amount
#roll = 0
roll = pi/2
GripUtils.go_to(x=forward_amount,y=edge_y,z=self.lift_amount,
roll=roll,pitch=0,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm,dur=2.0)
rospy.sleep(1.0) #Give time to stabilize
if not GripUtils.go_to(x=forward_amount,y=edge_y,z=hover_amount,
roll=roll,pitch=pi/4,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm,dur=2.0):
return FAILURE
if not GripUtils.go_to(x=forward_amount,y=(edge_y+final_y)/2.0,z=hover_amount,
roll=roll,pitch=pi/4,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm,dur=2.5):
return FAILURE
if not GripUtils.go_to(x=forward_amount,y=final_y,z=hover_amount,
roll=roll,pitch=pi/4,yaw=drag_yaw,grip=True,
frame="table_height",arm=drag_arm,dur=2.5):
return FAILURE
return SUCCESS
def execute(self,userdata):
pt_bl = userdata.bl
pt_tl = userdata.tl
pt_br = userdata.br
pt_tr = userdata.tr
#if not GripUtils.go_to_pts(pt_tl, -pi/2, -pi/3, pi/4, pt_tr, pi/2, pi/3, pi/4, x_offset_l=-0.04,\
# x_offset_r=-0.04):
# return FAILURE
#FIXME Hard-coded X offsets to compensate for poor calibration. Re-calibrate and REMOVE
'''
if not GripUtils.grab_point(pt_tl,roll=-pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=-0.04):
return FAILURE
if not GripUtils.grab_point(pt_tr,roll=pi/2,yaw= pi/3,pitch=pi/4,arm="r",x_offset=-0.04):
return FAILURE
'''
#FIXME: For some reason large offsets required, #DEBUG
if not GripUtils.grab_points(pt_tl,roll_l=-pi/2,yaw_l=-pi/3,pitch_l=pi/4,x_offset_l=-0.05, z_offset_l=-0.0025
,point_r=pt_tr,roll_r=pi/2,yaw_r= pi/3,pitch_r=pi/4,x_offset_r=-0.06,
y_offset_r=0.00, y_offset_l=-0.00, z_offset_r=0.01):
return FAILURE
(bl_x,bl_y,bl_z) = (pt_bl.point.x,pt_bl.point.y,pt_bl.point.z)
(tl_x,tl_y,tl_z) = (pt_tl.point.x,pt_tl.point.y,pt_tl.point.z)
(br_x,br_y,br_z) = (pt_br.point.x,pt_br.point.y,pt_br.point.z)
(tr_x,tr_y,tr_z) = (pt_tr.point.x,pt_tr.point.y,pt_tr.point.z)
x_l = (tl_x+bl_x)/2.0
y_l = (tl_y+bl_y)/2.0
z_l = bl_z + sqrt( (bl_x-tl_x)**2 + (bl_y-tl_y)**2 )/2.0
x_r = (tr_x+br_x)/2.0
y_r = (tr_y+br_y)/2.0
z_r = br_z + sqrt( (br_x-tr_x)**2 + (br_y-tr_y)**2 )/2.0
pitch_l = pitch_r = pi/4
roll_l = 0
roll_r = 0
yaw_l=-pi/2
yaw_r= pi/2
grip_l=grip_r=True
frame_l=frame_r = pt_bl.header.frame_id
if not GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
,dur=4.0):
return_val = FAILURE
print "Folding down!"
x_l = bl_x + 0.02 # overshoots fold down
x_r = br_x + 0.02
y_l = bl_y-0.01
y_r = br_y+0.01
z_l = z_r = bl_z + 0.02
yaw_l = -3*pi/4
yaw_r = 3*pi/4
pitch_l=pitch_r = pi/4
roll_l = pi/2
roll_r = -pi/2
GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
,dur=7.5)
GripUtils.recall_arm("b")
pr2_say(talk_pose2)
return SUCCESS
def execute(self, userdata):
bl = userdata.bl
br = userdata.br
tl = userdata.tl
tr = userdata.tr
midleft = bl
midleft.point.x = (bl.point.x + tl.point.x) / 2
midleft.point.y = (bl.point.y + tl.point.y) / 2
midleft.point.z = (bl.point.z + tl.point.z) / 2
midright = br
midright.point.x = (br.point.x + tr.point.x) / 2
midright.point.y = (br.point.y + tr.point.y) / 2
midright.point.z = (br.point.z + tr.point.z) / 2
if not GripUtils.grab_point(midleft,
roll=pi / 2,
yaw=-pi / 2,
pitch=pi / 4,
arm="l",
x_offset=-0.01,
z_offset=0.01,
INIT_SCOOT_AMT=0.01):
return FAILURE
if not GripUtils.grab_point(midright,
roll=-pi / 2,
yaw=pi / 2,
pitch=pi / 4,
arm="r",
x_offset=-0.01,
z_offset=0.01,
INIT_SCOOT_AMT=0.01):
return FAILURE
if not GripUtils.go_to_relative_multi(0, 0, 0.4, True, 0, 0, 0.4, True,
"base_footprint"):
return FAILURE
return SUCCESS
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 execute(self,userdata):
pt_bl = userdata.bl
pt_tl = userdata.tl
pt_br = userdata.br
pt_tr = userdata.tr
#FIXME Hard-coded X offsets to compensate for poor calibration. Re-calibrate and REMOVE
if not GripUtils.grab_point(pt_tl,roll=-pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=-0.04):
return FAILURE
if not GripUtils.grab_point(pt_tr,roll=pi/2,yaw= pi/3,pitch=pi/4,arm="r",x_offset=-0.04):
return FAILURE
(bl_x,bl_y,bl_z) = (pt_bl.point.x,pt_bl.point.y,pt_bl.point.z)
(tl_x,tl_y,tl_z) = (pt_tl.point.x,pt_tl.point.y,pt_tl.point.z)
(br_x,br_y,br_z) = (pt_br.point.x,pt_br.point.y,pt_br.point.z)
(tr_x,tr_y,tr_z) = (pt_tr.point.x,pt_tr.point.y,pt_tr.point.z)
x_l = (tl_x+bl_x)/2.0
y_l = (tl_y+bl_y)/2.0
z_l = bl_z + sqrt( (bl_x-tl_x)**2 + (bl_y-tl_y)**2 )/2.0
x_r = (tr_x+br_x)/2.0
y_r = (tr_y+br_y)/2.0
z_r = br_z + sqrt( (br_x-tr_x)**2 + (br_y-tr_y)**2 )/2.0
pitch_l = pitch_r = pi/4
roll_l = 0
roll_r = 0
yaw_l=-pi/2
yaw_r= pi/2
grip_l=grip_r=True
frame_l=frame_r = pt_bl.header.frame_id
if not GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
,dur=7.5):
return_val = FAILURE
print "Folding down!"
x_l = bl_x-0.015
y_l = bl_y+0.025
z_l = z_r = bl_z + 0.02
x_r = br_x-0.015
y_r = br_y-0.025
yaw_l = -3*pi/4
yaw_r = 3*pi/4
pitch_l=pitch_r = pi/4
roll_l = pi/2
roll_r = -pi/2
GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l
,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
,dur=7.5)
GripUtils.recall_arm("b")
return SUCCESS
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 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 sweep_cloth_with_scoot(direction,
drag_amount,
table_width,
lift_amount,
roll=0,
scoot=0):
forward_amount = 0.5
hover_amount = 0.06
edge_distance = (table_width / 2.0) + .04
edge_distance_low = (table_width / 2.0)
if direction == "r":
drag_arm = "r"
edge_y_high = edge_distance
edge_y_low = edge_distance_low
drag_yaw = pi / 2
waypoint_y = edge_y_low - (drag_amount / 2.0)
final_y = edge_y_low - drag_amount
else:
drag_arm = "l"
edge_y_high = -1 * edge_distance
edge_y_low = -1 * edge_distance_low
drag_yaw = -pi / 2
waypoint_y = edge_y_low + (drag_amount / 2.0)
final_y = edge_y_low + drag_amount
# Take it to table edge and hold it high
if not GripUtils.go_to(x=forward_amount - .21,
y=edge_y_high,
z=lift_amount + 0.01,
roll=roll,
pitch=0,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=3):
return False
for i in range(2):
GripUtils.go_to(x=forward_amount - .21,
y=edge_y_high - .02,
z=lift_amount - .2,
roll=roll,
pitch=0,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=1.0 / (0.7 * i + 1))
# Lower it at table edge
if not GripUtils.go_to(x=forward_amount,
y=edge_y_low,
z=hover_amount,
roll=roll,
pitch=pi / 4,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=1):
return False
# Sweep it back halfway
if not GripUtils.go_to(x=forward_amount,
y=waypoint_y,
z=hover_amount,
roll=roll,
pitch=pi / 4,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=2.5):
return False
# Sweep it back the rest of the way
if not GripUtils.go_to(x=forward_amount,
y=final_y,
z=hover_amount,
roll=roll,
pitch=pi / 4,
yaw=drag_yaw,
grip=True,
frame="table_height",
arm=drag_arm,
dur=2.5):
return False
print "Final_y is: %f" % final_y
return (forward_amount, final_y, hover_amount, roll, pi / 4, drag_yaw)
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):
GripUtils.recall_arm('b')
return SUCCESS
