def execute(self, userdata):
rospy.loginfo('Executing state LowerHand')
#x = userdata.object_of_desire.pose.pose.position.x
#y = userdata.object_of_desire.pose.pose.position.y
#xdes = x
#ydes = y
#mc = MixingClient.get_mixing_client(use_right_arm = False)
#status = mc.plunge_spoon() # will probably want to modify mc to take x and y, but we'll wait and see
ac = ArmClient.get_arm_client()
isRightArm = userdata.grasp_with_right_hand
(trans, rot) = ac.get_transform(isRightArm)
print trans, rot
xdes = trans[0]
ydes = trans[1]
zdes = trans[2]
dz = .02
zobj = userdata.object_of_desire.pose.pose.position.z
zdim = userdata.object_of_desire.box_dims[2]
status = 0
#while status is 0: # TODO: add minimum z threshold here
#while True: # TODO: add minimum z threshold here
#zdes = zdes - dz
#zgoal = zobj + .08 + zdim
#print 'zdes: ', zdes, '\tzobj: ', zobj, '\tzdim: ', zdim, '\tzgoal: ', zgoal
#if zdes < zgoal:
#break
#status = ac.move_to_pose(isRightArm, True, xdes, ydes, zdes, rot[0], rot[1], rot[2], rot[3], pos_thresh = 0.01)
#time.sleep(1)
#
#print status
zgoal = zobj + .09 + zdim #TODO should fix this
if userdata.object_of_desire_name == 'cookie_sheet':
print 'hard setting z'
zgoal = .74
else:
print 'hard setting z for mixing bowl'
zgoal = .86
print 'zgoal:', zgoal, ' userdata.zgoal:', userdata.zgoal
#TODO: change this for iros demo
print '***************************************'
print 'zgoal: ', zgoal
print 'zobj: ', zobj
print 'zobj+.09:', (zobj + .09)
print '***************************************'
status = ac.move_to_pose(isRightArm,
True,
xdes,
ydes,
zgoal,
rot[0],
rot[1],
rot[2],
rot[3],
pos_thresh=0.01)
if status is 0:
return 'done'
else:
return 'fail'
def __init__(self, name=None):
FState.__init__(
self, input_keys=['mixing_bowl', 'pre_mix_height_above_table'])
self.arm_client = ArmClient.get_arm_client()
self.papm = PickAndPlaceManager.get_pick_and_place_manager()
self.name = name
self.isRightArm = None
def __init__(self):
FState.__init__(
self,
input_keys=['acceptable_height_above_table', 'table_height'],
output_keys=['current_height_above_table'])
self.arm_client = ArmClient.get_arm_client()
self.tf_listener = self.arm_client.tf_listener
def execute(self, userdata):
rospy.loginfo('Executing state LowerSheet')
logger = EventLoggerClient.startfsm('LowerSheet (ovening fsm)')
ac = ArmClient.get_arm_client()
trans, rot = ac.get_transform(False)
delta = 1.06 - trans[
2] # TODO: need to measure this to get it into oven
status = ac.move_delta_const_ee(False, delta, 'z')
if status == 0:
logger.stops()
return 'done'
else:
rospy.logwarn('unable to lower sheet, trying again')
status = ac.move_delta_const_ee(False, delta, 'z')
if status == 0:
logger.stops()
return 'done'
rospy.logwarn(
'unable to lower sheet, trying again with smaller delta')
status = ac.move_delta_const_ee(False, delta + .01, 'z')
if status == 0:
logger.stops()
return 'done'
else:
logger.stopf()
return 'fail'
def __init__(self):
smach.State.__init__(
self,
outcomes=['done', 'fail'],
input_keys=['mixing_bowl', 'pre_mix_height_above_table'])
self.arm_client = ArmClient.get_arm_client()
self.papm = PickAndPlaceManager.get_pick_and_place_manager()
def __init__(self):
smach.State.__init__(self,
outcomes=['success', 'fail'],
input_keys=['current_known_pose_name'],
output_keys=['current_known_pose_name'])
self.arm_client = ArmClient.get_arm_client()
self.tf_listener = self.arm_client.tf_listener
self.pose_manager = None
def __init__(self):
smach.State.__init__(self,
outcomes=['done', 'fail'],
input_keys=[
'barcode_up', 'mixing_bowl_radius',
'mixing_bowl_height', 'mixing_bowl'
])
self.ac = ArmClient.get_arm_client()
def __init__(self):
FState.__init__(self,
input_keys=[
'cookie_sheet', 'pour_height_above_centroid',
'current_known_pose_name', 'mixing_bowl_radius'
],
output_keys=['current_known_pose_name'])
self.arm_client = ArmClient.get_arm_client()
self.tf_listener = self.arm_client.tf_listener
def __init__(self):
FState.__init__(
self,
input_keys=['grasp_pose', 'bowl_to_deal', 'mixing_bowl'],
output_keys=[
'current_known_pose_name', 'hand_poses_exhausted',
'use_new_pose_manager'
])
self.arm_client = ArmClient.get_arm_client()
self.tf_listener = self.arm_client.tf_listener
def __init__(self):
FState.__init__(self,
input_keys=[
'mixing_bowl', 'cookie_sheet',
'pre_mix_height_above_table', 'skipped_debug_init'
],
output_keys=['cookie_sheet'])
self.arm_client = ArmClient.get_arm_client()
self.papm = PickAndPlaceManager.get_pick_and_place_manager()
self.fail_count = 0
def __init__(self):
smach.State.__init__(self,
outcomes=['success', 'fail'],
input_keys=[
'acceptable_height_above_table',
'table_height', 'is_right_arm'
],
output_keys=['current_height_above_table'])
self.arm_client = ArmClient.get_arm_client()
self.tf_listener = self.arm_client.tf_listener
def __init__(self):
FState.__init__(self,
input_keys=[
'bowl_to_deal', 'table_height',
'acceptable_height_above_table'
])
self.arm_client = ArmClient.get_arm_client()
self.pick_and_place_manager = PickAndPlaceManager.get_pick_and_place_manager(
)
self.tf_listener = self.arm_client.tf_listener
self.fail_count = 0
def execute(self, userdata):
desired = .88
rospy.loginfo('Executing state MoveRobotToPreOpen')
logger = EventLoggerClient.startfsm('MoveRobotToPreOpen (ovening fsm)')
odc = OvenDoorClient.get_oven_door_client()
bc = BaseClient.get_base_client()
actual = odc.get_door_pos()
if actual > 500:
rospy.logwarn('could not find the oven')
self.num_fails = self.num_fails + 1
if self.num_fails > 5:
return 'failure'
else:
rospy.logwarn('could not find the oven, recursing')
rospy.logwarn('number of failures to find oven: ' +
str(self.num_fails))
rospy.logwarn('backing up to try again')
bc.translate(True, -.15)
return self.execute(userdata)
drivex = actual
status = bc.translate(True, drivex)
if not status:
self.first_pass = False
logger.stopf()
return 'failure'
else:
logger.stops()
#return 'wait'
rospy.loginfo('Executing state OpenOven')
logger = EventLoggerClient.startfsm('OpenOven (ovening fsm)')
oc = OvenDoorClient.get_oven_door_client()
status = oc.open_door()
print '\n\n\n\t\topen door status: ', status
papm = PickAndPlaceManager.get_pick_and_place_manager()
papm.open_gripper(1)
time.sleep(3)
ac = ArmClient.get_arm_client()
print 'move arm back status: ', ac.move_delta_const_ee(
False, -.10, 'x')
print 'move arm back status: ', ac.move_delta_const_ee(
False, -.10, 'x')
bc = BaseClient.get_base_client()
bc.translate(True, -.15)
papm.move_arm_to_side(0)
papm.move_arm_to_side(1)
if status:
logger.stops()
return 'success'
else:
logger.stopf()
return 'fail'
def execute(self, userdata):
rospy.loginfo('Executing state OpenOven')
logger = EventLoggerClient.startfsm('OpenOven (ovening fsm)')
oc = OvenDoorClient.get_oven_door_client()
status = oc.open_door()
print '\n\n\n\t\topen door status: ', status
papm = PickAndPlaceManager.get_pick_and_place_manager()
papm.open_gripper(1)
time.sleep(3)
ac = ArmClient.get_arm_client()
print 'move arm back status: ', ac.move_delta_const_ee(
False, -.10, 'x')
print 'move arm back status: ', ac.move_delta_const_ee(
False, -.10, 'x')
bc = BaseClient.get_base_client()
bc.translate(True, -.15)
if status:
logger.stops()
if not self.gone_once:
self.gone_once = True
return 'get cs'
else:
# time.sleep(10)
# print 'waited 10'
# time.sleep(10)
# print 'waited 10'
# time.sleep(10)
# print 'waited 10'
# time.sleep(10)
# print 'waited 10'
# time.sleep(10)
# print 'waited 10'
# time.sleep(10)
# print '1waited 10'
# time.sleep(10)
# print '1waited 10'
# time.sleep(10)
# print '1waited 10'
# time.sleep(10)
# print '1waited 10'
# time.sleep(10)
# print '1waited 10'
# time.sleep(10)
# print 'done waiting'
return 'done'
else:
self.gone_once = True
logger.stopf()
return 'fail'
def __init__(self):
smach.State.__init__(self,
outcomes=['success', 'fail'],
output_keys=[
'object_of_desire', 'table_frame_x_offset',
'table_frame_y_offset',
'grasp_with_right_hand', 'clock_position'
])
self.pick_and_place_manager = PickAndPlaceManager.get_pick_and_place_manager(
)
print 'constructing detect object'
self.arm_client = ArmClient.get_arm_client()
self.btom = BroadTabletopObjectManager.get_broad_tabletop_object_manager(
self.pick_and_place_manager)
print 'done'
def execute(self, userdata):
rospy.loginfo('Executing state RetreatArm')
logger = EventLoggerClient.startfsm('RetreatArm (ovening fsm)')
ac = ArmClient.get_arm_client()
status = ac.move_to_pose_const_ee(False, .39, .41, 1.03)
if status == 0:
logger.stops()
return 'done'
else:
status = ac.move_to_pose_const_ee(False, .39, .41, 1.03)
if status == 0:
logger.stops()
return 'done'
else:
logger.stopf()
return 'fail'
def __init__(self):
FState.__init__(self,
input_keys=[
'cookie_sheet', 'nominal_cookie_sheet_pos',
'table_frame_x_offset',
'acceptable_height_above_table', 'table_height',
'table_frame_y_offset'
],
output_keys=[
'cookie_sheet', 'table_frame_y_offset',
'table_frame_x_offset'
])
self.num_attempts = 0
self.first_time = True
self.arm_client = ArmClient.get_arm_client()
self.tf_listener = self.arm_client.tf_listener
def __init__(self, debug=False):
FState.__init__(self,
input_keys=[
'cookie_sheet', 'mixing_bowl', 'is_right_arm',
'table_frame_x_offset', 'table_frame_y_offset',
'table_frame_x_offset'
],
output_keys=[
'current_known_pose_name', 'cookie_sheet',
'is_right_arm', 'grasp_pose', 'global_grasp_pose'
])
self.pick_and_place_manager = PickAndPlaceManager.get_pick_and_place_manager(
)
self.arm_client = ArmClient.get_arm_client()
self.broad_object_manager = BroadTabletopObjectManager.get_broad_tabletop_object_manager(
self.pick_and_place_manager)
def execute(self, userdata):
logger = EventLoggerClient.startfsm('LiftSpoon')
rospy.loginfo('Executing state LiftSpoon')
ac = ArmClient.get_arm_client()
(trans, rot) = ac.get_transform(True)
dz = .2
status = ac.move_to_pose(True, False, trans[0], trans[1],
trans[2] + dz, rot[0], rot[1], rot[2], rot[3])
if status:
logger.stops()
return 'done'
else:
status = ac.move_to_pose(True, False, trans[0], trans[1],
trans[2] + dz / 2, rot[0], rot[1], rot[2],
rot[3])
logger.stopf()
return 'fail'
def execute(self, userdata):
rospy.loginfo('Executing state MoveSheetIntoOven')
logger = EventLoggerClient.startfsm('MoveSheetIntoOven (ovening fsm)')
#delta = userdata.insert_sheet_delta + .10
ac = ArmClient.get_arm_client()
#ac.move_delta_const_ee(False, delta, 'x')
status = ac.move_to_pose_const_ee(False, .63, -.06, 1.)
#status = ac.move_to_pose_const_ee(False, .71, -.10, .97)
if status == 0:
logger.stops()
return 'done'
else:
status = ac.move_to_pose_const_ee(False, .63, -.06, 1.0)
if status == 0:
logger.stops()
return 'done'
else:
return 'fail'
def execute(self, userdata):
rospy.loginfo('Executing state MoveHandToPreOvenPos')
logger = EventLoggerClient.startfsm(
'MoveHandToPreOvenPos (ovening fsm)')
rospy.logerr('Not yet implemented')
self.arm_client = ArmClient.get_arm_client()
#status = self.arm_client.move_to_pose_const_ee(False, .54, -.09, 1.05)
status = self.arm_client.move_to_pose_const_ee(False, .55, -.06, 1.06)
if status == 0:
rospy.loginfo('done')
logger.stops()
return 'done'
else:
status = self.arm_client.move_to_pose_const_ee(
False, .55, -.06, 1.06)
if status == 0:
rospy.loginfo('done')
logger.stops()
return 'done'
else:
logger.stopf()
return 'fail'
def execute(self, userdata):
rospy.loginfo('Executing state LiftGripper')
logger = EventLoggerClient.startfsm('LiftGripper (ovening fsm)')
arm_client = ArmClient.get_arm_client()
(trans, rot) = arm_client.get_transform(False)
(x, y, z) = trans
zdes = 1.05
status = arm_client.move_to_pose_const_ee(False, x, y, zdes)
if status == 0:
logger.stops()
return 'done'
else:
rospy.logwarn('arm client failed to lift')
status = arm_client.move_to_pose_const_ee(False, x, y, zdes)
if status == 0:
logger.stops()
return 'done'
status = arm_client.move_to_pose_const_ee(False, x, y, zdes)
if status == 0:
logger.stops()
return 'done'
return 'fail'
def execute(self, userdata):
rospy.loginfo('Executing state RaiseHand')
ac = ArmClient.get_arm_client()
isRightArm = userdata.grasp_with_right_hand
(trans, rot) = ac.get_transform(isRightArm)
print trans, rot
xdes = trans[0]
ydes = trans[1]
zdes = trans[2] + .3
status = ac.move_to_pose(isRightArm,
True,
xdes,
ydes,
zdes,
rot[0],
rot[1],
rot[2],
rot[3],
pos_thresh=0.01)
if status is 0:
return 'done'
else:
status = ac.move_to_pose(isRightArm,
True,
xdes,
ydes,
zdes,
rot[0],
rot[1],
rot[2],
rot[3],
pos_thresh=0.01)
if status is 0:
return 'done'
else:
return 'fail'
def execute(self, userdata):
rospy.loginfo('Executing state MoveToPreOven')
logger = EventLoggerClient.startfsm('MoveToPreOven (ovening fsm)')
bc = BaseClient.get_base_client()
#bc.fake_rotate(1)
#bc.translate(True, -.03)
logger.stops()
rospy.loginfo('Executing state MoveHandToPreOvenPos')
logger = EventLoggerClient.startfsm(
'MoveHandToPreOvenPos (ovening fsm)')
rospy.logerr('Not yet implemented')
self.arm_client = ArmClient.get_arm_client()
#status = self.arm_client.move_to_pose_const_ee(False, .54, -.09, 1.05)
status = self.arm_client.move_to_pose_const_ee(False, .55, -.06, 1.06)
if status == 0:
rospy.loginfo('done')
logger.stops()
#return 'done'
else:
status = self.arm_client.move_to_pose_const_ee(
False, .55, -.06, 1.06)
if status == 0:
rospy.loginfo('done')
logger.stops()
#return 'done'
else:
logger.stopf()
return 'failure'
#return 'done'
rospy.loginfo('Executing state MoveSheetIntoOven')
logger = EventLoggerClient.startfsm('MoveSheetIntoOven (ovening fsm)')
ac = ArmClient.get_arm_client()
status = ac.move_to_pose_const_ee(False, .63, -.06, 1.)
if status == 0:
logger.stops()
#return 'done'
else:
status = ac.move_to_pose_const_ee(False, .63, -.06, 1.0)
if status == 0:
logger.stops()
#return 'done'
else:
return 'failure'
rospy.loginfo('Executing state LowerSheet')
logger = EventLoggerClient.startfsm('LowerSheet (ovening fsm)')
ac = ArmClient.get_arm_client()
trans, rot = ac.get_transform(False)
delta = 1.06 - trans[
2] # TODO: need to measure this to get it into oven
print 'delta: ', delta
status = ac.move_delta_const_ee(False, delta, 'z')
if status == 0:
logger.stops()
#return 'done'
else:
rospy.logwarn('unable to lower sheet, trying again')
status = ac.move_delta_const_ee(False, delta, 'z')
if status == 0:
logger.stops()
#return 'done'
rospy.logwarn(
'unable to lower sheet, trying again with smaller delta')
status = ac.move_delta_const_ee(False, delta + .01, 'z')
if status == 0:
logger.stops()
#return 'done'
else:
logger.stopf()
return 'failure'
rospy.loginfo('Executing state OpenGripper')
logger = EventLoggerClient.startfsm('OpenGripper (ovening fsm)')
papm = PickAndPlaceManager.get_pick_and_place_manager()
papm.open_gripper(1)
logger.stops()
#return 'done'
rospy.loginfo('Executing state LiftGripper')
logger = EventLoggerClient.startfsm('LiftGripper (ovening fsm)')
arm_client = ArmClient.get_arm_client()
(trans, rot) = arm_client.get_transform(False)
(x, y, z) = trans
zdes = 1.05
status = arm_client.move_to_pose_const_ee(False, x, y, zdes)
if status == 0:
logger.stops()
#return 'done'
else:
rospy.logwarn('arm client failed to lift')
status = arm_client.move_to_pose_const_ee(False, x, y, zdes)
if status == 0:
logger.stops()
#return 'done'
status = arm_client.move_to_pose_const_ee(False, x, y, zdes)
if status == 0:
logger.stops()
#return 'done'
return 'failure'
rospy.loginfo('Executing state RetreatArm')
logger = EventLoggerClient.startfsm('RetreatArm (ovening fsm)')
ac = ArmClient.get_arm_client()
status = ac.move_to_pose_const_ee(False, .39, .41, 1.03)
if status == 0:
logger.stops()
return 'success'
else:
status = ac.move_to_pose_const_ee(False, .39, .41, 1.03)
if status == 0:
logger.stops()
return 'success'
else:
logger.stopf()
return 'failure'
def execute(self, userdata):
rospy.loginfo('Executing state MoveHandOverRim')
#xdim = userdata.object_of_desire.box_dims.x
#ydim = userdata.object_of_desire.box_dims.y
xdim = userdata.object_of_desire.box_dims[0]
ydim = userdata.object_of_desire.box_dims[1]
zdim = userdata.object_of_desire.box_dims[2]
r = (math.sqrt(xdim**2 + ydim**2)) / 2.0
rospy.loginfo('calculated radius: ' + str(r))
rcor = .02
r = r - rcor
rospy.loginfo('subtracting correction: ' + str(rcor))
rhard = .123 if userdata.object_of_desire_name == 'cookie_sheet' else .14 #.13
r = rhard
rospy.logwarn('overriding radius with hardcodedd value: ' + str(rhard))
rospy.loginfo('zdim: ' + str(zdim))
h = .4 # TODO tune this
#h = 0.2
x = userdata.object_of_desire.pose.pose.position.x
y = userdata.object_of_desire.pose.pose.position.y
z = userdata.object_of_desire.pose.pose.position.z
pose_to_point_at = [x, y, .65]
pick_and_place_manager = PickAndPlaceManager.get_pick_and_place_manager(
)
print 'point the head at xyz', pose_to_point_at
pick_and_place_manager.point_head(pose_to_point_at, 'base_link')
if userdata.clock_position is 'twelve_o_clock':
xoff = r
yoff = 0
elif userdata.clock_position is 'three_o_clock':
xoff = 0
yoff = -r
elif userdata.clock_position is 'six_o_clock':
xoff = -r
yoff = 0
else:
xoff = 0
yoff = r
xdes = x + xoff #+ userdata.table_frame_x_offset
ydes = y + yoff #+ userdata.table_frame_y_offset
zdes = z + h
print '\n\n\n*******************************************'
print 'x', x
print 'y', y
print 'r: ', r
print 'xoff: ', xoff
print 'yoff: ', yoff
print 'desired: ', xdes, ydes, zdes
print 'xdim', xdim
print 'ydim', ydim
print 'zdim', zdim
print '*******************************************'
posenamein = userdata.clock_position + '_bi'
posenameout = userdata.clock_position + '_bo'
rotin = GripperBowlPoseManager.ALL_POSES[posenamein]
rotout = GripperBowlPoseManager.ALL_POSES[posenameout]
ac = ArmClient.get_arm_client()
rot = rotin
if self.first_time:
self.first_time = False
ac.advance_arm(userdata.grasp_with_right_hand)
status = ac.move_to_pose(userdata.grasp_with_right_hand,
False,
xdes,
ydes,
zdes,
rot[0],
rot[1],
rot[2],
rot[3],
pos_thresh=.025,
rot_thresh=.25)
status = ac.move_to_pose(userdata.grasp_with_right_hand,
False,
xdes,
ydes,
zdes,
rot[0],
rot[1],
rot[2],
rot[3],
pos_thresh=.035,
rot_thresh=.25)
#goalpose = Pose()
#goalpose.position.x = xdes
#goalpose.position.y = ydes
#goalpose.position.z = zdes
#goalpose.orientation.x = rot[0]
#goalpose.orientation.y = rot[1]
#goalpose.orientation.z = rot[2]
#goalpose.orientation.w = rot[3]
#papm = PickAndPlaceManager.get_pick_and_place_manager()
#btom = BroadTabletopObjectManager.get_broad_tabletop_object_manager(papm)
#armstotry = (0,0) if userdata.grasp_with_right_hand else (1,1)
#(resultval, whicharm, grasp) = btom.refine_and_grasp(userdata.object_of_desire.collision_name, armstotry, 0, 0, goalpose)
#print 'resutlval: ', resultval
if status is not 0:
rot = rotout
status = ac.move_to_pose(userdata.grasp_with_right_hand, True,
xdes, ydes, zdes, rot[0], rot[1], rot[2],
rot[3])
if status is not 0:
return 'fail'
return 'done'
def __init__(self):
smach.State.__init__(self, outcomes=['done', 'fail'])
self.arm_client = ArmClient.get_arm_client()
self.papm = PickAndPlaceManager.get_pick_and_place_manager()
def __init__(self):
FState.__init__(self,
input_keys=['current_known_pose_name'],
output_keys=['current_known_pose_name'])
self.arm_client = ArmClient.get_arm_client()
self.tf_listener = self.arm_client.tf_listener
def execute(self, userdata):
rospy.loginfo('Executing state DumpSpoon')
arm_client = ArmClient.get_arm_client()
arm_client.retreat_arm(True)
return 'success'
def __init__(self, isRightArm, retreat_first=False):
smach.State.__init__(self, outcomes=['done', 'fail'])
self.arm_client = ArmClient.get_arm_client()
self.papm = PickAndPlaceManager.get_pick_and_place_manager()
self.isRightArm = isRightArm
self.retreat_first = retreat_first
def __init__(self):
smach.State.__init__(self, outcomes=['done'])
self.arm_client = ArmClient.get_arm_client()
