Ejemplo n.º 1
0
    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)
Ejemplo n.º 2
0
    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)