Example #1
0
    def on_frame(self, controller):
        """
        Function called on every frame by the
        leap motion controller

        Hand position analysis is processed here
        and vehicle rc channels are then overridden
        accordingly

        :param controller: Leap Motion controller
        :return: None
        """

        hands = controller.frame().hands

        debug("last hand palm normal: " + str(self.last_hand_palm_normal))

        # We only do something if there is one hand detected
        if len(hands) == 1:
            palm_normal = hands[0].palm_normal
            if _debug:
                debug(str(palm_normal))

            channels_override = \
                {'1': RC_AVG_VALUE - palm_normal.x * (RC_AVG_VALUE - RC_MIN_VALUE),  # RC1: Roll
                 '2': RC_AVG_VALUE - (palm_normal.z + Z_VALUE_CORRECTION) * (RC_AVG_VALUE - RC_MIN_VALUE)}  # RC2: Pitch

            if _rc_throttle:
                debug("Overriding throttle RC channel to " + str(_rc_throttle))
                channels_override[
                    '3'] = _rc_throttle  # RC3: throttle, required to be at the middle to keep altitude

            # Update the channels override only if there is
            # a significant change in the hand orientation
            if (not self.last_hand_palm_normal) \
                or not (self.vectors_equal_with_accurary(self.last_hand_palm_normal, palm_normal, HAND_MOVEMENT_DETECTION_THRESHOLD)):
                debug("Sending drone override RC channels message: " +
                      str(channels_override))
                self.vehicle.channels.overrides = channels_override
                self.last_hand_palm_normal = palm_normal

        else:  # We disable RC 1 and 2 override to stabilize the copter
            channels_override = \
                {'1': None,
                 '2': None}
            if _rc_throttle:
                channels_override['3'] = _rc_throttle

            palm_normal = Vector()
            palm_normal.x = 0
            palm_normal.y = 0
            palm_normal.z = 0

            # Update only if changed
            if (not self.last_hand_palm_normal) \
                or not (self.vectors_equal_with_accurary(self.last_hand_palm_normal, palm_normal, HAND_MOVEMENT_DETECTION_THRESHOLD)):
                debug("Sending drone override RC channels message: " +
                      str(channels_override))
                self.vehicle.channels.overrides = channels_override
                self.last_hand_palm_normal = palm_normal
Example #2
0
    def on_frame(self, controller):
        """
        Function called on every frame by the
        leap motion controller

        Hand position analysis is processed here
        and vehicle rc channels are then overridden
        accordingly

        :param controller: Leap Motion controller
        :return: None
        """

        hands = controller.frame().hands

        debug("last hand palm normal: " + str(self.last_hand_palm_normal))

        # We only do something if there is one hand detected
        if len(hands) == 1:
            palm_normal = hands[0].palm_normal
            if _debug:
                debug(str(palm_normal))

            channels_override = \
                {'1': RC_AVG_VALUE - palm_normal.x * (RC_AVG_VALUE - RC_MIN_VALUE),  # RC1: Roll
                 '2': RC_AVG_VALUE - (palm_normal.z + Z_VALUE_CORRECTION) * (RC_AVG_VALUE - RC_MIN_VALUE)}  # RC2: Pitch

            if _rc_throttle:
                debug("Overriding throttle RC channel to " + str(_rc_throttle))
                channels_override['3'] = _rc_throttle  # RC3: throttle, required to be at the middle to keep altitude

            # Update the channels override only if there is
            # a significant change in the hand orientation
            if (not self.last_hand_palm_normal) \
                or not (self.vectors_equal_with_accurary(self.last_hand_palm_normal, palm_normal, HAND_MOVEMENT_DETECTION_THRESHOLD)):
                debug("Sending drone override RC channels message: " + str(channels_override))
                self.vehicle.channels.overrides = channels_override
                self.last_hand_palm_normal = palm_normal

        else:   # We disable RC 1 and 2 override to stabilize the copter
            channels_override = \
                {'1': None,
                 '2': None}
            if _rc_throttle:
                channels_override['3'] = _rc_throttle

            palm_normal = Vector()
            palm_normal.x = 0
            palm_normal.y = 0
            palm_normal.z = 0

            # Update only if changed
            if (not self.last_hand_palm_normal) \
                or not (self.vectors_equal_with_accurary(self.last_hand_palm_normal, palm_normal, HAND_MOVEMENT_DETECTION_THRESHOLD)):
                debug("Sending drone override RC channels message: " + str(channels_override))
                self.vehicle.channels.overrides = channels_override
                self.last_hand_palm_normal = palm_normal
Example #3
0
    def __init__(self, frames, numframes, *args, **kwargs):
        norm = 1. / numframes

        self.ts = frames[0].timestamp / 1000000.0
        self.av_numhands = sum(len(fr.hands) for fr in frames) * norm
        # Calculates the hand's average pitch, roll and yaw
        self.av_pitch = sum(fr.hands[0].direction.pitch
                            for fr in frames) * norm
        self.av_roll = sum(fr.hands[0].palm_normal.roll
                           for fr in frames) * norm
        self.av_yaw = sum(fr.hands[0].direction.yaw for fr in frames) * norm
        # Calculates the hand's average palm position
        try:
            self.av_palm_pos = Vector(0, 0, 0)
            for fr in frames:
                self.av_palm_pos += fr.hands[0].palm_position
            self.av_palm_pos *= norm
        except:
            self.av_palm_pos = NaN
        # Calculates the average number of fingers
        self.av_fingers = sum(len(fr.hands[0].fingers) for fr in frames) * norm
        # Calculates the hand's average finger tip position
        try:
            self.av_tip_pos = Vector(0, 0, 0)
            for fr in frames:
                self.av_tip_pos += sum(
                    (fg.tip_position for fg in fr.hands[0].fingers),
                    Vector()) / len(fr.hands[0].fingers)
            self.av_tip_pos *= norm
        except:
            self.av_tip_pos = NaN
        # Check whether the hand is horizontal
        if self.av_numhands > 0.5:
            self.is_horizontal = 0
            self.is_vertical = 0
            if self.av_pitch > -10 and self.av_pitch < 20:
                self.is_horizontal = 1
            if self.av_pitch > 55 and self.av_pitch < 70:
                self.is_vertical = 1
            if self.av_pitch < -60 and self.av_pitch > -90:
                self.is_vertical = -1
        # Compute the hand's average palm velocity
        self.av_palm_vel = sum(fr.hands[0].palm_velocity[1]
                               for fr in frames) * norm
        # Compute the hand's average fingers speed
        try:
            self.av_fingers_speed = 0
            for fr in frames:
                self.av_fingers_speed += sum(
                    fg.tip_velocity[1]
                    for fg in fr.hands[0].fingers) / len(fr.hands[0].fingers)
            self.av_fingers_speed *= norm
        except:
            self.av_fingers_speed = NaN
Example #4
0
    def update(self, frame, data):
        # Get the required data
        hand_state = data['leap_state']['current']
        hand_state_prev = data['leap_state']['prev']

        elapsed = NaN  # (s)
        d_av_tip_pos = Vector(NaN, NaN, NaN)  # (mm)
        velocity = NaN  # (mm/s)
        try:
            # Computes some differences between the current and previous frames sets
            d_av_palm_pos = hand_state.av_palm_pos - hand_state_prev.av_palm_pos
            d_av_fingers = hand_state.av_fingers - hand_state_prev.av_fingers
            d_av_tip_pos = hand_state.av_tip_pos - hand_state_prev.av_tip_pos
            # Computes the elapsed time between the current and previous frames sets
            elapsed = max(0.000001, hand_state.ts - hand_state_prev.ts)
            # Computes velocity and acceleration
            velocity = d_av_palm_pos.magnitude / elapsed
        except:
            pass

        if not math.isnan(velocity) and elapsed <= (
                self.max_elapsed / 1000) and velocity <= self.max_vel:
            if d_av_fingers > 0:
                self.last_change = hand_state.ts
            if hand_state.ts - self.last_change < (self.finger_pause / 1000):
                d_av_palm_pos *= 0  # don't move pointer when fingers changes

            if hand_state.av_numhands >= 0.5 and hand_state.av_fingers >= 1:
                # The cursor speed will be inversely proportional to the number of fingers detected by the controller
                d_av_palm_pos *= 1. / hand_state.av_fingers

                # Move it!
                self.move(d_av_palm_pos.x, -d_av_palm_pos.y)
        elif self.active_fist:
            # Move it!
            self.move(d_av_palm_pos.x, -d_av_palm_pos.y)

        if data['actions']['click']:
            self.click()
        if data['actions']['press']:
            self.press()
        if data['actions']['release']:
            self.release()
        if data['actions']['scroll_up']:
            self.scroll_up(repeats=data['actions']['scroll_up'])
        if data['actions']['scroll_down']:
            self.scroll_down(repeats=data['actions']['scroll_down'])
        if data['actions']['switch_desk']:
            self.switch_desktop(data['actions']['switch_desk'])
	def update(self,fingers=None):
		self.touched = 0
		if (not fingers) or len(fingers) < 1:
			return
		# else...
		for finger in fingers:
			center = self.get_sphere_center()
			radius = self.get_sphere_radius()
			# first decide whether it's touched
			if finger.tip_position.distance_to(center) <= radius + 20:
				self.touched += 1
			# then slide if necessary
			if finger.tip_position.distance_to(center) < radius:
				rel_to_sphere = finger.tip_position - center
				flattened = Vector(rel_to_sphere.x,0,rel_to_sphere.z)
				pitch_radians = flattened.angle_to(rel_to_sphere)
				radius_at_height = math.cos(pitch_radians)*radius
				# print "%f -> %f" % (math.degrees(pitch_radians),radius_at_height)
				z_dist = rel_to_sphere.z
				x_radius = math.sqrt(radius_at_height**2 - z_dist**2)
				x_dist = x_radius - math.fabs(rel_to_sphere.x)
				if finger.tip_position.x >= center.x:
					x_dist *= -1
				self.slide_sphere(x_dist)
Example #6
0
class RpsListener(Leap.Listener):
    logger = None
    finger_names = ['thumb', 'index', 'middle', 'ring', 'pinky']
    bone_names = ['Metacarpal', 'Proximal', 'Intermediate', 'Distal']
    state_names = ['STATE_INVALID', 'STATE_START', 'STATE_UPDATE', 'STATE_END']
    xyz = ["x", "y", "z"]
    origin = Vector(0, 0, 0)
    is_measuring = False
    rock_state = None
    rps_state = None

    def on_init(self, controller):
        if self.logger != None:
            self.logger.info("Initialized")

    def on_connect(self, controller):
        if self.logger != None:
            self.logger.info("Connected")

    def on_disconnect(self, controller):
        if self.logger != None:
            # Note: not dispatched when running in a debugger.
            self.logger.info("Disconnected")

    def on_exit(self, controller):
        if self.logger != None:
            self.logger.info("Exited")

    def set_logger(self, logger):
        self.logger = logger

    def get_hand_data(self, hand):
        # heuristic way
        hand_data = [None] * 7
        hand_data[0] = hand.id
        hand_data[1] = hand.confidence
        idx = 2
        hand_direction = hand.direction
        palm_normal = hand.palm_normal
        for fng in hand.fingers:
            angle_hand_fng = hand_direction.angle_to(
                fng.direction) * 180 / math.pi
            angle_norm_fng = palm_normal.angle_to(
                fng.direction) * 180 / math.pi
            if angle_norm_fng > 90:
                angle_hand_fng = angle_hand_fng * -1
            hand_data[idx + fng.type] = angle_hand_fng
        return hand_data

    def start_measure(self):
        self.is_measuring = True

    def start_rock_check(self):
        self.rock_state = RockState(self.logger)
        return self.rock_state

    def stop_rock_check(self):
        self.rock_state = None

    def start_rps_recognition(self):
        self.rps_state = RpsState(self.logger)
        return self.rps_state

    def stop_rps_recognition(self):
        self.rps_state = None

    def stop_measure(self):
        """
        Reset all state info
        """
        self.is_measuring = False
        self.stop_rock_check()
        self.stop_rps_recognition()

    def on_frame(self, controller):
        if self.is_measuring == False:
            return
        frame = controller.frame()
        if len(frame.hands) == 0:
            #self.logger.info("No data in this frame")
            return
        # use first hand
        hand = frame.hands[0]
        # they do not contain valid tracking data and do not correspond to a physical entity
        if hand.is_valid == False:
            self.logger.info("Not valid hand")
            return
        # store hand data to rock state
        if self.rock_state != None:
            self.rock_state.update_hand_data(hand)
        if self.rps_state != None:
            self.rps_state.update_hand_data(hand)
Example #7
0
from Leap import Vector

# MAGIIIIIC NUMBERS !
CENTERED_ORIGIN = Vector(0, 200, 0)
MAX_X = 250
MAX_Y = 350
MAX_Z = 150


def rescale_position(old):
    """
    Fit the given vector back into the centered coordinate system.
    Return a vector with coordinates values in [-1;1]
    """
    # Translate
    new = old - CENTERED_ORIGIN

    # Scale
    new.x /= MAX_X
    new.y /= MAX_Y
    new.z /= MAX_Z

    return new
class RpsListener(Leap.Listener):
    finger_names = ['thumb', 'index', 'middle', 'ring', 'pinky']
    bone_names = ['Metacarpal', 'Proximal', 'Intermediate', 'Distal']
    state_names = ['STATE_INVALID', 'STATE_START', 'STATE_UPDATE', 'STATE_END']
    xyz = ["x", "y", "z"]
    origin = Vector(0, 0, 0)
    logger = None
    log_filename = ""
    header_len = 0
    is_measuring = False
    data_dir_path = ""

    def on_init(self, controller):
        print("Initialized")

    def on_connect(self, controller):
        print("Connected")

    def on_disconnect(self, controller):
        # Note: not dispatched when running in a debugger.
        print("Disconnected")

    def on_exit(self, controller):
        print("Exited")

    def init_logger(self, log_filename):
        now = datetime.now()
        now_str = now.strftime("%Y%m%d-%H%M%S%f")
        data_dir_path = os.path.abspath("data/{}".format(now_str))
        os.makedirs(data_dir_path)
        self.data_dir_path = data_dir_path

        self.log_filename = log_filename
        # initialize logger
        self.logger = self.setup_logger(self.data_dir_path, self.log_filename)
        data_header = self.create_data_header()
        data_header_str = "\t".join(data_header)
        self.header_len = len(data_header)
        # write header of tsv
        self.logger.info(data_header_str)

    def setup_logger(self, data_dir_path, log_filename, level=DEBUG):
        logger = getLogger(__name__)
        logger.setLevel(level)
        # create log in tsv
        fh = FileHandler(
            filename="{}/{}.tsv".format(data_dir_path, log_filename))
        fm = Formatter("%(asctime)s\t%(message)s")
        fh.setLevel(DEBUG)
        fh.setFormatter(fm)
        logger.addHandler(fh)
        # don't propagate to parent logger
        logger.propagate = False
        return logger

    def create_data_header(self):
        data_header = []
        # hand_header = 7 + 3*3 = 16
        hand_header = [
            "hand_type", "hand_id", "hand_confidence", "palm_x", "palm_y",
            "palm_z", "palm_width"
        ]
        for bs in ["x_basis", "y_basis", "z_basis"]:
            hand_header.extend(
                ["hand_{}_{}".format(bs, ax) for ax in self.xyz])
        # finger_header = 5 * (2 + 3 + 3 + 4*3) = 100
        finger_header = []
        for f in self.finger_names:
            # length and width are decided when your hand is first detected (static)
            finger_header.extend(
                ["{}_{}".format(f, t) for t in ["len", "width"]])
            finger_header.extend(
                ["{}_direction_{}".format(f, ax) for ax in self.xyz])
            finger_header.extend(
                ["{}_velocity_{}".format(f, ax) for ax in self.xyz])
            # finger bones, all fingers contain 4 bones
            for b in range(0, 4):
                finger_header.extend(
                    ["{}_{}_center_{}".format(f, b, ax) for ax in self.xyz])
        data_header.extend(hand_header)
        data_header.extend(finger_header)
        return data_header

    def get_hand_data(self, hand):
        # fast array initializaion
        d = [None] * self.header_len
        d[0] = "L" if hand.is_left else "R"
        d[1] = hand.id
        d[2] = hand.confidence
        pp = hand.palm_position
        d[3] = pp.x
        d[4] = pp.y
        d[5] = pp.z
        d[6] = hand.palm_width
        idx = 7
        basis = hand.basis
        for bs in [basis.x_basis, basis.y_basis, basis.z_basis]:
            for val in [bs.x, bs.y, bs.z]:
                d[idx] = val
                idx += 1
        for fng in hand.fingers:
            d[idx] = fng.length
            idx += 1
            d[idx] = fng.width
            idx += 1
            fng_dir = fng.direction
            fng_vel = fng.tip_velocity
            for val in [fng_dir.x, fng_dir.y, fng_dir.z]:
                d[idx] = val
                idx += 1
            for val in [fng_vel.x, fng_vel.y, fng_vel.z]:
                d[idx] = val
                idx += 1
            for b in range(0, 4):
                bn = fng.bone(b)
                d[idx] = bn.center.x
                idx += 1
                d[idx] = bn.center.y
                idx += 1
                d[idx] = bn.center.z
                idx += 1
        return d

    def start_measure(self):
        self.is_measuring = True

    def stop_measure(self):
        self.is_measuring = False

    def on_frame(self, controller):
        if self.is_measuring == False:
            return
        frame = controller.frame()
        if len(frame.hands) == 0:
            print("No data in this frame")
            return
        # use first hand
        hand = frame.hands[0]
        # they do not contain valid tracking data and do not correspond to a physical entity
        if hand.is_valid == False:
            print("Not valid hand")
            return
        hand_data = self.get_hand_data(hand)
        self.logger.info("\t".join(map(str, hand_data)))