def return_object(self):
"""Hand the object back to the user.
Extend the arm out toward the user and (A) wait for the arm to move and
(B) close the fingers and detect no object. Whichever happens first
stops the other task. Finally open the hand and then joke.
"""
self.logger.debug('call return_object')
if not self.run:
return
self.say_and_animate('done', 'Grasping', 'ReturnObject') # offer object
time.sleep(1) # try to ensure that arm monitoring doesn't false positive...
object_dropped = qi.Promise()
fut = object_dropped.future()
# 1. Register a callback to arm movement and start detecting arm movement
def handle_arm_moved(object_dropped, value):
self.logger.debug('call handle_arm_moved')
if not self.run:
return
utils.stop_monitoring_grasp_status()
object_dropped.setValue(1)
signal = utils.start_monitoring_arm_move(self.sesh, 'R')
signal.connect(partial(handle_arm_moved, object_dropped))
# 2. asynchronously close the hand and keep checking if its empty
def grasp_status_changed(object_dropped, stat):
self.logger.debug('call grasp_status_changed: {}'.format(stat))
if not self.run:
return
utils.nearly_close_hand(self, 'RHand')
if stat == 'absent':
utils.stop_monitoring_arm_move()
object_dropped.setValue(1)
signal = utils.start_monitoring_grasp_status(self.sesh, 'present')
signal.connect(partial(grasp_status_changed, object_dropped))
# proceed when one of the above two has finished
try:
fut.value(self.return_duration)
self.run_anim('OpenHand')
time.sleep(1) # for continuity
self.say_and_animate('giveback', 'Grasping', 'DontForget')
time.sleep(1) # for continuity
except RuntimeError:
self.logger.warning("give up on returning the grasped object!")
self.run_anim('OpenHand')
self.box.onStopped(1)
def wait_for_object(self):
"""Wait for the hand to grasp_demo an object.
1. detect movement of the right arm
2. nearly close the right hand
3. check if an object is in the hand continuously
4a. if it is, fulfill object_dropped
4b. if it is not, say 'missed' and start over
"""
self.logger.info('Wait for object')
if not self.run:
return
def grasp_status_changed(stat):
self.logger.debug('call grasp_status_changed: {}'.format(stat))
if not self.run:
return
utils.stop_periodic_say()
if stat == 'present':
self.motion.stiffnessInterpolation("RArm", 1.0, 0.5)
self.say_and_animate('thanks', 'Grasping', 'ThankYou')
self.grasp_object.setValue(1)
elif stat == 'absent':
self.say_and_animate('missed', 'Grasping', 'Missed')
self.run_anim('OpenHand')
self.motion.stiffnessInterpolation("RArm", 1.0, 0.5)
self.missed_count += 1
if self.missed_count >= 3:
self.grasp_object.setCanceled()
else:
self.run_anim('ExtendHand')
self.wait_for_object()
def close_and_check_hand(value):
self.logger.debug('call close_and_check_hand')
if not self.run:
return
utils.nearly_close_hand(self, 'RHand')
signal = utils.start_monitoring_grasp_status(self.sesh, 'moving')
signal.connect(grasp_status_changed)
utils.start_periodic_say(self.sesh, 'waiting', 'Grasping', 15)
signal = utils.start_monitoring_arm_move(self.sesh, 'R')
signal.connect(close_and_check_hand)
