wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-09 11:31:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

add leapmotion interface

Browse Source
This commit is contained in:
Brian Delhaisse
2019-07-07 03:41:14 +02:00
parent 49b70d0e2d
commit 9666faa497
2 changed files with 846 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/tools/interfaces/sensors/install_leapmotion_ubuntu.txt
+124
View File
@@ -0,0 +1,124 @@
Installation of LeapMotion on Ubuntu 16.04/18.04
================================================
After downloading the "Desktop Setup" available here: https://www.leapmotion.com/setup/desktop/
If you have an error during the Linux installation such as the following:
$ sudo dpkg --install Leap-2.3.1+31549-x64.deb
Selecting previously unselected package leap.
(Reading database ... 478480 files and directories currently installed.)
Preparing to unpack Leap-2.3.1+31549-x64.deb ...
Unpacking leap (2.3.1+31549) ...
Setting up leap (2.3.1+31549) ...
Leap Motion installed under /usr/bin and /usr/sbin
Failed to start leapd.service: Unit leapd.service not found.
dpkg: error processing package leap (--install):
subprocess installed post-installation script returned error exit status 5
Processing triggers for ureadahead (0.100.0-19) ...
Errors were encountered while processing:
leap
Please follow the next steps (taken from https://forums.leapmotion.com/t/linux-install-of-sdk-fails/5158/6):
1. After the install failure, I simply uninstalled with:
sudo dpkg -r leap
2. Now, with no leap installed yet, I followed the post above and just created the file as mentioned there (I used the
terminal to write the following command, to use the nano text editor as root user):
sudo nano /lib/systemd/system/leapd.service
3. I pasted that content (reproduced here for easiness):
(also please note that on Ubuntu terminal, you paste with CTRL+SHIFT+V instead of just CTRL+V)
# Found by Kevin Cole 2014.11.22 at
# https://github.com/atejeda/leap-fedora-rpm
#
# Remember to:
#
# ln -s /lib/systemd/system/leapd.service /etc/systemd/system/leapd.service
# systemctl daemon-reload
#
[Unit]
Description=LeapMotion Daemon
After=syslog.target
[Service]
Type=simple
ExecStart=/usr/sbin/leapd
[Install]
WantedBy=multi-user.target
4. Saved (CTRL+O then ENTER), and exited (CTRL+X)
5. Still using the terminal, executed the two commands from that same post, BUT I included the sudo prefix to run them
as root user:
sudo ln -s /lib/systemd/system/leapd.service /etc/systemd/system/leapd.service
sudo systemctl daemon-reload
6. All is right now and we can properly install. So, I just installed the original package:as in the official Leap
guide:
sudo dpkg --install Leap-2.3.1+31549-x64.deb
(please just note that I'm using a x64 Ubuntu, so I installed the 64 bits package)
The above command should output someting similar to:
Selecting previously unselected package leap.
(Reading database ... 478480 files and directories currently installed.)
Preparing to unpack Leap-2.3.1+31549-x64.deb ...
Unpacking leap (2.3.1+31549) ...
Setting up leap (2.3.1+31549) ...
Leap Motion installed under /usr/bin and /usr/sbin
Check Leap Motion daemon with:
service leapd status
Open the Leap Motion GUI with:
LeapControlPanel
See /usr/share/Leap/README.linux for more information.
7. Done!!!
8. You can try to open the Leap Motion GUI
$ LeapControlPanel
Installation of the LeapMotion SDK
==================================
To install the SDK, go to: https://developer.leapmotion.com/sdk/v2
and download the SDK (this will require you to sign up).
You can check the following link for more information on how to use the SDK in Python:
https://developer-archive.leapmotion.com/documentation/python/index.html
You can copy-paste the following files:
- Leap.py
- LeapPython.so
- libLeap.so
in your /usr/local/lib/python<version>/dist-packages/. Or set your PYTHONPATH environment variable to where these files
are on your system.
By default, Leap only supports Python 2.7 as described here:
"The LeapPython library included in the Leap Motion SDK supports only Python 2.7. However, the SDK also includes the
SWIG interface file used to generate the LeapPython source code, so advanced users can generate and compile their own
version of LeapPython. For instructions, refer to Generating a Python 3.3.0 Wrapper with SWIG 2.0.9 in our support
knowledge base."
For more info, check the following link:
https://developer-archive.leapmotion.com/documentation/python/devguide/Project_Setup.html
If at one point, the daemon is not running, you can run it again using:
$ sudo service leapd restart
pyrobolearn/tools/interfaces/sensors/leapmotion.py
+722 -16
View File
@@ -4,22 +4,21 @@
References:
- Leap Motion: https://www.leapmotion.com/
- Installation (Ubuntu): https://www.leapmotion.com/setup/desktop/linux/
- V2 tracking toolkit: https://developer.leapmotion.com/sdk/v2
- Python API: https://developer-archive.leapmotion.com/documentation/python/api/Leap_Classes.html
- Tutorials: https://www.youtube.com/playlist?list=PLgTGpidiW0iTELuljcIdTkA5SjHa5tudP
- V2 tracking toolkit (SDK): https://developer.leapmotion.com/sdk/v2
- Python API documentation: https://developer-archive.leapmotion.com/documentation/python/index.html
"""
# TODO: implement this interface
import time
import numpy as np
try:
import Leap
except ImportError as e:
raise ImportError(repr(e) + '\nTry to install `Leap` (see references in class documentation)')
raise ImportError(repr(e) + '\nTry to install `Leap`, see the `install_leapmotion_ubuntu.txt` file.')
from pyrobolearn.tools.interfaces.sensors import SensorInterface
from pyrobolearn.utils.transformation import get_quaternion_from_matrix, get_rpy_from_matrix
__author__ = "Brian Delhaisse"
@@ -35,19 +34,40 @@ __status__ = "Development"
class LeapMotionInterface(SensorInterface):
r"""Leap Motion Interface
This class implements the Leap motion interface. Note that the right-handed Cartesian coordinate system used by
the Leap Motion is different from the one used in robotics (see [5]).
In robotics, the x-axis points forward, the y-axis points to the left, and the z-axis points upward.
In the Leap's coordinate system (when the Leap motion is placed in front of the user on a table), the x-axis
points to the right, the y-axis points upward, and the z-axis points toward the user.
Also, the physical quantities measured by the Leap motion are in the following units:
- Distance: millimeter
- Time: microseconds (unless otherwise noted)
- Speed: millimeter/second
- Angle: radians
In this class, the returned physical quantities accessed through the given methods, are converted to the standard
units (meter, second, radian), and to the coordinate system used in robotics.
Part of the documentation for the methods has been copied-pasted from [4] for completeness purposes.
References:
- Leap Motion: https://www.leapmotion.com/
- Installation (Ubuntu): https://www.leapmotion.com/setup/desktop/linux/
- V2 tracking toolkit: https://developer.leapmotion.com/sdk/v2
- Python API: https://developer-archive.leapmotion.com/documentation/python/api/Leap_Classes.html
- Tutorials: https://www.youtube.com/playlist?list=PLgTGpidiW0iTELuljcIdTkA5SjHa5tudP
- [1] Leap Motion: https://www.leapmotion.com/
- [2] Installation (Ubuntu): https://www.leapmotion.com/setup/desktop/linux/
- [3] V2 tracking toolkit (SDK): https://developer.leapmotion.com/sdk/v2
- [4] Python API documentation: https://developer-archive.leapmotion.com/documentation/python/index.html
- [5] Leap motion - Coordinate systems:
https://developer-archive.leapmotion.com/documentation/python/devguide/Leap_Coordinate_Mapping.html
"""
def __init__(self, use_thread=False, sleep_dt=0., verbose=False, use_rgb=True, use_depth=True):
def __init__(self, bounding_box=None, use_thread=False, sleep_dt=0., verbose=False):
"""
Initialize the RealSense input interface.
Initialize the Leap motion input interface.
Args:
bounding_box (None, np.array[2,3]):
use_thread (bool): If True, it will run the interface in a separate thread than the main one.
The interface will update its data automatically.
sleep_dt (float): If :attr:`use_thread` is True, it will sleep the specified amount before acquiring or
@@ -56,19 +76,705 @@ class LeapMotionInterface(SensorInterface):
programmer what he / she wishes to print.
"""
# TODO
# create Leap controller
self.controller = Leap.Controller()
# wait until the controller connects to the daemon
while not self.controller.is_connected:
if verbose:
print("Waiting the Leap controller to connect to the Leap Daemon...")
time.sleep(0.01)
if verbose:
print("The Leap controller is now connected.")
# get the first frame
self._frame = self.controller.frame()
self._interaction_box = self._frame.interaction_box
self._left_hand = None
self._right_hand = None
super(LeapMotionInterface, self).__init__(use_thread, sleep_dt, verbose)
##############
# Properties #
##############
@property
def frame(self):
"""Return the last frame."""
return self._frame
@property
def interaction_box(self):
"""Return the last interaction box."""
return self._interaction_box
@property
def hands(self):
"""Return the list of hands."""
return self.frame.hands
@property
def left_hand(self):
"""Return the left hand."""
return self._left_hand
@property
def leftmost_hand(self):
"""Return the left most hand."""
return self.hands.leftmost
@property
def right_hand(self):
"""Return the right hand."""
return self._right_hand
@property
def rightmost_hand(self):
"""Return the right most hand."""
return self.hands.rightmost
###########
# Methods #
###########
@staticmethod
def get_hand_direction(hand):
"""
Get the direction vector; the direction from the palm position toward the fingers.
Args:
hand (Leap.Hand): hand instance.
Returns:
np.array[3]: hand direction (unit vector)
"""
d = hand.direction
return np.array([-d.z, -d.x, d.y])
@staticmethod
def get_hand_palm_normal(hand):
"""
Get the normal vector to the palm. If your hand is flat, this vector will point downward, or 'out' of the
front surface of your palm.
Args:
hand (Leap.Hand): hand instance.
Returns:
np.array[3]: palm normal (unit vector)
"""
d = hand.palm_normal
return np.array([-d.z, -d.x, d.y])
@staticmethod
def get_hand_rotation_matrix(hand):
"""
Get the hand orientation as a rotation matrix.
Args:
hand (Leap.Hand): hand instance.
Returns:
np.array[3, 3]: rotation matrix.
"""
basis = hand.basis
x_basis = basis.x_basis.to_float_array()
y_basis = basis.y_basis.to_float_array()
z_basis = basis.z_basis.to_float_array()
return np.array([-z_basis, -x_basis, y_basis])
@staticmethod
def get_hand_quaternion(hand):
"""
Get the hand orientation as a quaternion [x,y,z,w].
Args:
hand (Leap.Hand): hand instance.
Returns:
np.array[4]: quaternion [x,y,z,w]
"""
return get_quaternion_from_matrix(LeapMotionInterface.get_hand_rotation_matrix(hand))
@staticmethod
def get_hand_rpy(hand):
"""
Get the hand orientation as roll-pitch-yaw angles (in radians).
Args:
hand (Leap.Hand): hand instance.
Returns:
np.array[3]: roll-pitch-yaw angles (in radians)
"""
return get_rpy_from_matrix(LeapMotionInterface.get_hand_rotation_matrix(hand))
@staticmethod
def get_hand_position(hand):
"""
Get the hand position (center position of the palm) in meter from the Leap Motion Controller origin.
Args:
hand (Leap.Hand): hand instance.
Returns:
np.array[3]: position (in meter)
"""
d = hand.palm_position
return np.array([-d.z, -d.x, d.y]) / 1000.
@staticmethod
def get_hand_stable_position(hand):
"""
Get the stabilized hand position (center position of the palm) in meter.
Smoothing and stabilization is performed in order to make this value more suitable for interaction with 2D
content. The stabilized position lags behind the palm position by a variable amount, depending primarily on
the speed of movement.
Args:
hand (Leap.Hand): hand instance.
Returns:
np.array[3]: stabilized hand position (in meter)
"""
d = hand.stabilized_palm_position
return np.array([-d.z, -d.x, d.y]) / 1000.
@staticmethod
def get_hand_homogeneous_transform(hand):
"""
Get the homogeneous transformation matrix of the hand.
Args:
hand (Leap.Hand): hand instance.
Returns:
np.array[4,4]: homogeneous transformation matrix
"""
rotation = LeapMotionInterface.get_hand_rotation_matrix(hand)
position = LeapMotionInterface.get_hand_position(hand)
return np.vstack((np.hstack((rotation, position.reshape(-1, 1))),
np.array([0., 0., 0., 1.])))
@staticmethod
def get_wrist_position(hand):
"""
Get the wrist position (in meter) associated with the given hand.
Args:
hand (Leap.Hand): hand instance.
Returns:
np.array[3]: wrist position (in meter)
"""
d = hand.wrist_position
return np.array([-d.z, -d.x, d.y]) / 1000.
@staticmethod
def get_hand_velocity(hand):
"""
Get the hand velocity in meter.
Args:
hand (Leap.Hand): hand instance.
Returns:
np.array[3]: linear velocity (in meter/second)
"""
d = hand.palm_velocity
return np.array([-d.z, -d.x, d.y]) / 1000.
@staticmethod
def is_left_hand(hand):
"""
Return True if the given hand is a left hand.
Args:
hand (Leap.Hand): hand instance.
Returns:
bool: True if left hand
"""
return hand.is_left
@staticmethod
def is_right_hand(hand):
"""
Return True if the given hand is a right hand.
Args:
hand (Leap.Hand): hand instance.
Returns:
bool: True if right hand
"""
return hand.is_right
@staticmethod
def get_hand_confidence(hand):
"""
Return the hand confidence; how well the internal hand model fits the observed data.
A low value indicates that there are significant discrepancies; finger positions, even hand identification
could be incorrect. The significance of the confidence value to your application can vary with context.
Args:
hand (Leap.Hand): hand instance.
Returns:
float: hand confidence
"""
return hand.confidence
@staticmethod
def get_hand_grab_strength(hand):
"""
Get the strength of a grab.
The strength of a grab hand pose as a value in the range [0..1]. An open hand has a grab strength of zero. As
a hand closes into a fist, its grab strength increases to one.
Args:
hand (Leap.Hand): hand instance.
Returns:
float: value in [0..1].
"""
return hand.grab_strength
@staticmethod
def get_hand_pinch_strength(hand):
"""
Get the strength of a pinch.
The strength of a pinch pose between the thumb and the closest finger tip as a value in the range [0..1]. An
open, flat hand has a grab strength of zero. As the tip of the thumb approaches the tip of a finger, the pinch
strength increases to one.
Args:
hand (Leap.Hand): hand instance.
Returns:
float: value in [0..1]
"""
return hand.pinch_strength
@staticmethod
def get_hand_sphere_center(hand):
"""
Get the center of a sphere fit to the curvature of the given hand. The sphere is placed roughly as if the hand
were holding a ball. Thus the size of the sphere decreases as the fingers are curled into a fist.
Args:
hand (Leap.Hand): hand instance.
Returns:
np.array[3]: position of the center of the "hold" sphere (in meter)
"""
d = hand.sphere_center
return np.array([-d.z, -d.x, d.y]) / 1000.
@staticmethod
def get_hand_sphere_radius(hand):
"""
Get the radius of a sphere fit to the curvature of the given hand. This sphere is placed roughly as if the
hand were holding a ball. Thus the size of the sphere decreases as the fingers are curled into a fist.
Args:
hand (Leap.Hand): hand instance.
Returns:
float: sphere radius.
"""
return hand.sphere_radius / 1000.
@staticmethod
def get_hand_palm_width(hand):
"""
Get the hand palm width; the average width of the hand (not including fingers or thumb)
Args:
hand (Leap.Hand): hand instance.
Returns:
float: palm width (in meter)
"""
return hand.palm_width / 1000.
@staticmethod
def get_fingers(hand):
"""
Get the list of fingers associated with the given hand.
Args:
hand (Leap.Hand): hand instance.
Returns:
Leap.FingerList: list of finger objects (given in arbitrary order).
"""
return hand.fingers
@staticmethod
def get_finger(hand, idx=0):
"""
Get the `idx`th finger from the hand.
Args:
hand (Leap.Hand): hand instance.
idx (int): index of the finger. 0 is for the thumb, 1 for the index, 2 for the middle, 3 for the ring, and
4 for the pinky.
Returns:
Leap.Finger: finger instance.
"""
f = Leap.Finger
f = {0: f.TYPE_THUMB, 1: f.TYPE_INDEX, 2: f.TYPE_MIDDLE, 3: f.TYPE_RING, 4: f.TYPE_PINKY}
for finger in hand.fingers:
if finger.type == f[idx]:
return finger
@staticmethod
def get_finger_type(finger):
"""
Get the finger type which is between {'thumb', 'index', 'middle', 'ring', 'pinky'}.
Args:
finger (Leap.Finger): finger instance.
Returns:
str: finger type
"""
if finger.type == finger.TYPE_THUMB:
return 'thumb'
if finger.type == finger.TYPE_INDEX:
return 'index'
if finger.type == finger.TYPE_MIDDLE:
return 'middle'
if finger.type == finger.TYPE_RING:
return 'ring'
if finger.type == finger.TYPE_PINKY:
return 'pinky'
@staticmethod
def get_arm(hand):
"""
Get the arm associated with the hand.
Args:
hand (Leap.Hand): hand instance.
Returns:
Arm: arm instance.
"""
return hand.arm
@staticmethod
def get_elbow_position(hand_or_arm):
"""
Get the elbow position (in meter) associated to the given hand or arm.
Args:
hand_or_arm (Leap.Hand, Leap.Arm): hand or arm instance.
Returns:
np.array[3]: elbow position (in meter)
"""
if isinstance(hand_or_arm, Leap.Hand):
d = hand_or_arm.arm.elbow_position
elif isinstance(hand_or_arm, Leap.Arm):
d = hand_or_arm.elbow_position
else:
raise TypeError("Expecting the given parameter to be an instance of `Leap.Hand` or `Leap.Arm`, instead "
"got: {}".format(type(hand_or_arm)))
return np.array([-d.z, -d.x, d.y]) / 1000.
@staticmethod
def get_finger_tip_position(finger):
"""
Get the finger tip position (in meter).
Args:
finger (Leap.Finger): finger instance.
Returns:
np.array[3]: finger tip position (in meter)
"""
d = finger.tip_position
return np.array([-d.z, -d.x, d.y]) / 1000.
@staticmethod
def get_finger_tip_stable_position(finger):
"""
Get the finger filtered and stabilized position (in meter) using velocity and past positions.
Args:
finger (Leap.Finger): finger instance.
Returns:
np.array[3]: finger tip stabilized position
"""
d = finger.stabilized_tip_position
return np.array([-d.z, -d.x, d.y]) / 1000.
@staticmethod
def get_finger_tip_velocity(finger):
"""
Get the finger tip velocity (in meter/second).
Args:
finger (Leap.Finger): finger instance.
Returns:
np.array[3]: finger tip velocity (in meter/second)
"""
d = finger.tip_velocity
return np.array([-d.z, -d.x, d.y]) / 1000.
@staticmethod
def get_finger_direction(finger):
"""
Get the finger direction.
Args:
finger (Leap.Finger): finger instance.
Returns:
np.array[3]: finger direction (unit vector)
"""
d = finger.direction
return np.array([-d.z, -d.x, d.y])
@staticmethod
def get_finger_length(finger):
"""
Get the finger length (in meter).
Args:
finger (Leap.Finger): finger instance.
Returns:
np.array[3]: finger length (in meter)
"""
return finger.length / 1000.
@staticmethod
def get_finger_width(finger):
"""
Get the finger width (in meter).
Args:
finger (Leap.Finger): finger instance.
Returns:
np.array[3]: finger witdh (in meter)
"""
return finger.width / 1000.
@staticmethod
def get_interaction_box(frame):
"""
Get the interaction box from the specified frame.
Args:
frame (Leap.Frame): frame instance.
Returns:
Leap.InteractionBox: interaction box
"""
return frame.interaction_box
@staticmethod
def get_interaction_box_center(box):
"""
Get the interaction box center position (in meter).
Args:
box (Leap.InteractionBox): interaction box instance.
Returns:
np.array[3]: interaction box center (in meter).
"""
d = box.center
return np.array([-d.z, -d.x, d.y]) / 1000.
@staticmethod
def get_interaction_box_depth(box):
"""
Get the interaction box depth (in meter) measured along the x-axis.
Args:
box (Leap.InteractionBox): interaction box instance.
Returns:
float: depth (in meter)
"""
return box.depth
@staticmethod
def get_interaction_box_height(box):
"""
Get the interaction box height (in meter) measured along the z-axis.
Args:
box (Leap.InteractionBox): interaction box instance.
Returns:
float: height (in meter)
"""
return box.height
@staticmethod
def get_interaction_box_width(box):
"""
Get the interaction box width (in meter) measured along the y-axis.
Args:
box (Leap.InteractionBox): interaction box instance.
Returns:
float: width (in meter)
"""
return box.width
@staticmethod
def get_interaction_box_dimensions(box):
"""
Get the interaction box dimensions (depth, width, height) in meter along the (x, y, z) axis.
Args:
box (Leap.InteractionBox): interaction box instance.
Returns:
np.array[3]: box dimensions (depth, width, height)
"""
return np.array([box.depth, box.width, box.height])
def normalize_point(self, position, clamp=True):
"""
Normalize the given position such that the position is in the range of [0..1].
Args:
position (np.array[3]): position to normalized.
clamp (bool): Whether or not to limit the output value to the range [0,1] when the input position is
outside the InteractionBox. Defaults to True.
Returns:
np.array[3]: normalized position
"""
pass
def transform_normalized_position(self, position, clamp=True, ranges=None):
"""
Transform the normalized position
Args:
position (np.array[3]): normalized position.
clamp (bool): Whether or not to limit the output value to the range [0,1] when the input position is
outside the InteractionBox. Defaults to True.
ranges (None, np.array[2,3]):
Returns:
np.array[3]: transformed position.
"""
# normalize the position
position = self.normalize_point(position, clamp=clamp)
def transform_position(self, position, clamp=True, ranges=None):
"""
Transform the position
Args:
position (np.array[3]): normalized position.
clamp (bool): Whether or not to limit the output value to the range [0,1] when the input position is
outside the InteractionBox. Defaults to True.
ranges (None, np.array[2,3]):
Returns:
np.array[3]: transformed position.
"""
pass
def get_hand_transformed_position(self, hand):
"""
Get the hand transformed position (in meter) in the application coordinate system.
Args:
hand (Leap.Hand): hand instance.
Returns:
np.array[3]: hand transformed position (in meter)
"""
pass
@staticmethod
def get_left_image(frame):
"""
Get the left image.
Args:
frame (Leap.Frame): frame instance.
Returns:
np.array[H, W]: image.
"""
image = frame.images[0]
height, width = image.height, image.width
return np.frombuffer(image.data, dtype=np.uint8).reshape(height, width)
@staticmethod
def get_right_image(frame):
"""
Get the right image.
Args:
frame (Leap.Frame): frame instance.
Returns:
np.array[H, W]: image
"""
image = frame.images[1]
height, width = image.height, image.width
return np.frombuffer(image.data, dtype=np.uint8).reshape(height, width)
def run(self):
"""Run the interface."""
pass # TODO
if self.controller.is_connected:
# get the last frame from the Leap motion controller
self._frame = self.controller.frame()
# get the last interaction box
# self._interaction_box = self._frame.interaction_box
# get the left and right hand
for hand in self.frame.hands:
if hand.is_left:
self._left_hand = hand
elif hand.is_right:
self._right_hand = hand
# Test the interface
if __name__ == '__main__':
pass
import time
# create the myo interface
myo = LeapMotionInterface(verbose=True)
try:
while True:
myo.step()
print("RPY: {}".format(myo.rpy))
print("Quaternion: {}".format(myo.quaternion))
print("EMG: {}".format(myo.emg))
print("Accel: {}".format(myo.acceleration))
print("Gyro: {}".format(myo.gyro))
print("")
time.sleep(0.01)
except KeyboardInterrupt:
print("Keyboard Interrupt")
finally:
myo.close()
print("Bye!")