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
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
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
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)
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)
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)))