diff --git a/pyrobolearn/tools/interfaces/sensors/install_leapmotion_ubuntu.txt b/pyrobolearn/tools/interfaces/sensors/install_leapmotion_ubuntu.txt new file mode 100644 index 0000000..7943441 --- /dev/null +++ b/pyrobolearn/tools/interfaces/sensors/install_leapmotion_ubuntu.txt @@ -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/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 diff --git a/pyrobolearn/tools/interfaces/sensors/leapmotion.py b/pyrobolearn/tools/interfaces/sensors/leapmotion.py index 3413e4c..f433029 100644 --- a/pyrobolearn/tools/interfaces/sensors/leapmotion.py +++ b/pyrobolearn/tools/interfaces/sensors/leapmotion.py @@ -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!")