update bridges with manipulator

pyrobolearn/tools/bridges/controllers/robots/bridge_controller_manipulator.py

@@ -0,0 +1,232 @@
+# -*- coding: utf-8 -*-
+# !/usr/bin/env python
+"""Define the bridge between the game controller interface and a manipulator's end-effector. Note that this uses a QP 
+controller behind the scene to move the robot. You can use position or impedance control.
+"""
+
+from enum import Enum
+import numpy as np
+
+from pyrobolearn.tools.bridges import Bridge
+from pyrobolearn.robots import Manipulator, Gripper
+from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy
+from pyrobolearn.tools.interfaces.controllers import GameControllerInterface
+from pyrobolearn.priorities.models.robot_model import RobotModelInterface
+from pyrobolearn.priorities.tasks.velocity import CartesianTask
+from pyrobolearn.priorities.tasks.torque import CartesianImpedanceControlTask
+from pyrobolearn.priorities.solvers import QPTaskSolver
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class ControlType(Enum):
+    POSITION = 1
+    IMPEDANCE = 2
+
+
+class BridgeControllerManipulator(Bridge):
+    r"""Bridge between a game controller and a manipulator's end-effector.
+
+    Here is the mapping between the interface and the end-effector:
+    - Left joystick:
+        - left / right: move onto the y axis in the world.
+        - up / down: move onto the x axis in the world.
+    - Right joystick:
+        - up / down: move onto the z axis in the world.
+    - Buttons
+        - L1/R1: roll
+        - L2/R2: pitch
+        - east/west: yaw
+        - north/south: open/close gripper
+    """
+
+    def __init__(self, manipulator, interface, base_link=None, end_effector_link=None, control_type='position',
+                 gripper=None, translation_scale=1., rotation_step=0.001, use_orientation=False,
+                 priority=None, verbose=False):
+        """
+        Initialize the bridge between the Controller and a manipulator's end-effector.
+
+        Args:
+            manipulator (Manipulator): manipulator robot instance.
+            interface (GameControllerInterface): Game controller interface.
+            base_link (int, str): base link id or name. If None, it will be set to -1 (the base)
+            end_effector_link (int, str): end effector link id or name. If None, it will take the first end-effector.
+            control_type (str): type of control to use, select between {'position', 'impedance'}.
+            gripper (None, Gripper): gripper robot instance.
+            translation_scale (float): translation scale coefficient. It will multiply the value by translation offset
+              by that value when moving the joysticks.
+            rotation_step (float): rotation step value. As long as we keep pushing on the corresponding buttons, it
+              will increment the angles by that step value.
+            use_orientation (bool): if we should account for the orientation of the end-effector as well using the
+              interface.
+            priority (int): priority of the bridge.
+            verbose (bool): If True, print information on the standard output.
+        """
+        # set manipulator
+        self.manipulator = manipulator
+        self.gripper = gripper
+        self.base_link = -1 if base_link is None else base_link
+        self.distal_link = manipulator.get_end_effector_ids(end_effector=0) if end_effector_link is None \
+            else end_effector_link
+        self.use_orientation = use_orientation
+        self.grasp_strength = 0
+
+        self.translation_scale = translation_scale
+        self.rotation_step = rotation_step
+
+        # check the game controller interface
+        if not isinstance(interface, GameControllerInterface):
+            raise TypeError("Expecting the given 'interface' to be an instance of `GameControllerInterface`, but got "
+                            "instead: {}".format(type(interface)))
+
+        # create controller
+        model = RobotModelInterface(manipulator)
+        x_des = self.manipulator.get_link_positions(link_ids=end_effector_link, wrt_link_id=base_link)
+        if control_type == 'position':
+            task = CartesianTask(model, distal_link=end_effector_link, base_link=base_link, desired_position=x_des,
+                                 kp_position=50.)
+            control_type = ControlType.POSITION
+        elif control_type == 'impedance':
+            task = CartesianImpedanceControlTask(model, distal_link=end_effector_link, base_link=base_link,
+                                                 desired_position=x_des, kp_position=100, kd_linear=60)
+            control_type = ControlType.IMPEDANCE
+        else:
+            raise NotImplementedError("Please select between {'position', 'impedance'} for the control_type.")
+
+        self._task = task
+        self._control_type = control_type
+        self._solver = QPTaskSolver(task=task)
+
+        # call superclass
+        super(BridgeControllerManipulator, self).__init__(interface, priority, verbose=verbose)
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def manipulator(self):
+        """Return the manipulator instance."""
+        return self._manipulator
+
+    @manipulator.setter
+    def manipulator(self, manipulator):
+        """Set the manipulator instance."""
+        if not isinstance(manipulator, Manipulator):
+            raise TypeError("Expecting the given 'manipulator' to be an instance of `Manipulator`, instead got: "
+                            "{}".format(type(manipulator)))
+        self._manipulator = manipulator
+
+    @property
+    def gripper(self):
+        """Return the gripper instance."""
+        return self._gripper
+
+    @gripper.setter
+    def gripper(self, gripper):
+        """Set the gripper instance."""
+        if not isinstance(gripper, Gripper):
+            raise TypeError("Expecting the given 'gripper' to be an instance of `Gripper`, instead got: "
+                            "{}".format(type(gripper)))
+        self._gripper = gripper
+
+    @property
+    def simulator(self):
+        """Return the simulator instance."""
+        return self._manipulator.simulator
+
+    @property
+    def solver(self):
+        """Return the QP solver."""
+        return self._solver
+
+    @property
+    def task(self):
+        """Return the priority task."""
+        return self._task
+
+    ###########
+    # Methods #
+    ###########
+
+    def step(self, update_interface=False):
+        """Perform a step: map the Controller interface to the end-effector."""
+        # update interface
+        if update_interface:
+            self.interface()
+
+        # update the QP task desired position
+        left_joystick = self.interface.LJ[::-1]  # (y,x)
+        right_joystick = self.interface.RJ[::-1]  # (y,x)
+        translation = np.zeros(3)
+        translation[:2] = left_joystick
+        translation[2] = right_joystick[0]
+
+        position = self.manipulator.get_link_positions(link_ids=self.distal_link, wrt_link_id=self.base_link)
+        position += self.translation_scale * self.interface.translation
+        self.task.desired_position = position
+
+        # update the QP task desired orientation (if specified)
+        if self.use_orientation:
+            drpy = np.zeros(3)
+            if self.interface.BTN_TL:
+                drpy[0] = self.rotation_step
+            elif self.interface.BTN_TR:
+                drpy[0] = -self.rotation_step
+            if self.interface.BTN_TL2:
+                drpy[1] = self.rotation_step
+            elif self.interface.BTN_TR2:
+                drpy[1] = -self.rotation_step
+            if self.interface.BTN_WEST:
+                drpy[2] = self.rotation_step
+            elif self.interface.BTN_EAST:
+                drpy[2] = -self.rotation_step
+
+            orientation = self.manipulator.get_link_orientations(link_ids=self.distal_link, wrt_link_id=self.base_link)
+            orientation = get_rpy_from_quaternion(orientation)
+            orientation += drpy
+            self.task.desired_orientation = get_quaternion_from_rpy(orientation)
+
+        # update the QP task
+        self.task.update(update_model=True)
+
+        # solve QP and set the joint variables
+        if self._control_type == ControlType.POSITION:  # Position control
+            # solve QP task
+            dq = self.solver.solve()
+
+            # set joint positions
+            q = self.manipulator.get_joint_positions()
+            q = q + dq * self.simulator.dt
+            self.manipulator.set_joint_positions(q)
+
+        else:  # Impedance control
+            # solve QP task
+            torques = self.solver.solve()
+
+            # set joint torques
+            self.manipulator.set_joint_torques(torques)
+
+        # check if gripper
+        if self.gripper is not None:
+            # check if button is pressed
+            if self.interface.BTN_NORTH:  # open the gripper
+                if self._control_type == ControlType.POSITION:  # position control
+                    self.gripper.open(factor=1)
+                else:  # impedance control
+                    self.grasp_strength += 0.1
+                    self.gripper.grasp(strength=self.grasp_strength)
+            elif self.interface.BTN_SOUTH:  # close the gripper
+                if self._control_type == ControlType.POSITION:  # position control
+                    self.gripper.close(factor=1)
+                else:  # impedance control
+                    self.grasp_strength -= 0.1
+                    self.gripper.grasp(strength=self.grasp_strength)

pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py

@@ -61,7 +61,8 @@ class BridgeControllerWheeledRobot(Bridge):
 
         # if interface not defined, create one.
         if not isinstance(interface, GameControllerInterface):
-            raise TypeError
+            raise TypeError("Expecting the given 'interface' to be an instance of `GameControllerInterface`, instead "
+                            "got: {}".format(type(interface)))
 
         # call superclass
         super(BridgeControllerWheeledRobot, self).__init__(interface, priority=priority, verbose=verbose)
@@ -128,8 +129,8 @@ class BridgeControllerWheeledRobot(Bridge):
         pitch, yaw = get_rpy_from_quaternion(self.robot.orientation)[1:]
         if self.fpv:  # first-person view
             target_pos = self.robot.position + 2 * np.array([np.cos(yaw) * np.cos(pitch),
-                                                                  np.sin(yaw) * np.cos(pitch),
-                                                                  np.sin(pitch)])
+                                                             np.sin(yaw) * np.cos(pitch),
+                                                             np.sin(pitch)])
             self.camera.reset(distance=2, pitch=-pitch, yaw=yaw - np.pi / 2, target_position=target_pos)
         else:  # third-person view
             self.camera.follow(body_id=self.robot.id, distance=2, yaw=yaw - np.pi / 2, pitch=self.camera_pitch)

pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_manipulator.py

@@ -0,0 +1,241 @@
+# -*- coding: utf-8 -*-
+# !/usr/bin/env python
+"""Define the bridge between the mouse+keyboard interface and a manipulator's end-effector. Note that this uses a QP
+controller behind the scene to move the robot. You can use position or impedance control.
+"""
+
+from enum import Enum
+import numpy as np
+
+from pyrobolearn.tools.bridges import Bridge
+from pyrobolearn.robots import Manipulator, Gripper
+from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy
+from pyrobolearn.tools.interfaces.mouse_keyboard.mousekeyboard import MouseKeyboardInterface
+from pyrobolearn.priorities.models.robot_model import RobotModelInterface
+from pyrobolearn.priorities.tasks.velocity import CartesianTask
+from pyrobolearn.priorities.tasks.torque import CartesianImpedanceControlTask
+from pyrobolearn.priorities.solvers import QPTaskSolver
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class ControlType(Enum):
+    POSITION = 1
+    IMPEDANCE = 2
+
+
+class BridgeMouseKeyboardManipulator(Bridge):
+    r"""Bridge between a mouse+keyboard and a manipulator's end-effector.
+
+    Here is the mapping between the interface and the end-effector:
+    - left and right arrows: move on the y axis in the world.
+    - up and down arrows: move on the x axis in the world.
+    - shift and enter: move on the z axis in the world.
+    - ctrl + left/right arrow: move around the z axis (yaw)
+    - ctrl + up/down arrow: move around the y axis (pitch)
+    - ctrl + shift/enter: move around the x axis (roll)
+    - numpad 0: open the gripper.
+    - numpad 1: close the gripper.
+    """
+
+    def __init__(self, manipulator, interface=None, base_link=None, end_effector_link=None, control_type='position',
+                 gripper=None, translation_step=0.001, rotation_step=0.001, use_orientation=False,
+                 priority=None, verbose=False):
+        """
+        Initialize the bridge between the mouse+keyboard interface and a manipulator's end-effector.
+
+        Args:
+            manipulator (Manipulator): manipulator robot instance.
+            interface (None, SpaceMouseInterface): SpaceMouse interface. If None, it will create one.
+            base_link (int, str): base link id or name. If None, it will be set to -1 (the base)
+            end_effector_link (int, str): end effector link id or name. If None, it will take the first end-effector.
+            control_type (str): type of control to use, select between {'position', 'impedance'}.
+            gripper (None, Gripper): gripper robot instance.
+            translation_step (float): translation step value. As long as we keep pushing on the corresponding buttons,
+              it will increment the positions by that step value.
+            rotation_step (float): rotation step value. As long as we keep pushing on the corresponding buttons, it
+              will increment the angles by that step value.
+            use_orientation (bool): if we should account for the orientation of the end-effector as well using the
+              interface.
+            priority (int): priority of the bridge.
+            verbose (bool): If True, print information on the standard output.
+        """
+        # set manipulator
+        self.manipulator = manipulator
+        self.gripper = gripper
+        self.base_link = -1 if base_link is None else base_link
+        self.distal_link = manipulator.get_end_effector_ids(end_effector=0) if end_effector_link is None \
+            else end_effector_link
+        self.use_orientation = use_orientation
+        self.grasp_strength = 0
+
+        self.translation_step = translation_step
+        self.rotation_step = rotation_step
+
+        # if interface not defined, create one.
+        if not isinstance(interface, MouseKeyboardInterface):
+            interface = MouseKeyboardInterface(self.simulator)
+
+        # create controller
+        model = RobotModelInterface(manipulator)
+        x_des = self.manipulator.get_link_positions(link_ids=end_effector_link, wrt_link_id=base_link)
+        if control_type == 'position':
+            task = CartesianTask(model, distal_link=end_effector_link, base_link=base_link, desired_position=x_des,
+                                 kp_position=50.)
+            control_type = ControlType.POSITION
+        elif control_type == 'impedance':
+            task = CartesianImpedanceControlTask(model, distal_link=end_effector_link, base_link=base_link,
+                                                 desired_position=x_des, kp_position=100, kd_linear=60)
+            control_type = ControlType.IMPEDANCE
+        else:
+            raise NotImplementedError("Please select between {'position', 'impedance'} for the control_type.")
+
+        self._task = task
+        self._control_type = control_type
+        self._solver = QPTaskSolver(task=task)
+
+        # call superclass
+        super(BridgeMouseKeyboardManipulator, self).__init__(interface, priority, verbose=verbose)
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def manipulator(self):
+        """Return the manipulator instance."""
+        return self._manipulator
+
+    @manipulator.setter
+    def manipulator(self, manipulator):
+        """Set the manipulator instance."""
+        if not isinstance(manipulator, Manipulator):
+            raise TypeError("Expecting the given 'manipulator' to be an instance of `Manipulator`, instead got: "
+                            "{}".format(type(manipulator)))
+        self._manipulator = manipulator
+
+    @property
+    def gripper(self):
+        """Return the gripper instance."""
+        return self._gripper
+
+    @gripper.setter
+    def gripper(self, gripper):
+        """Set the gripper instance."""
+        if not isinstance(gripper, Gripper):
+            raise TypeError("Expecting the given 'gripper' to be an instance of `Gripper`, instead got: "
+                            "{}".format(type(gripper)))
+        self._gripper = gripper
+
+    @property
+    def simulator(self):
+        """Return the simulator instance."""
+        return self._manipulator.simulator
+
+    @property
+    def solver(self):
+        """Return the QP solver."""
+        return self._solver
+
+    @property
+    def task(self):
+        """Return the priority task."""
+        return self._task
+
+    ###########
+    # Methods #
+    ###########
+
+    def step(self, update_interface=False):
+        """Perform a step: map the SpaceMouse interface to the end-effector."""
+        # update interface
+        if update_interface:
+            self.interface()
+
+        # get interface values
+        key, pressed, down = self.interface.key, self.interface.key_pressed, self.interface.key_down
+
+        # update the QP task desired position
+        translation = np.zeros(3)
+        if key.ctrl not in pressed:
+            if key.top_arrow in down:
+                translation[0] = self.translation_step
+            elif key.bottom_arrow in down:
+                translation[0] = -self.translation_step
+            elif key.left_arrow in down:
+                translation[1] = self.translation_step
+            elif key.right_arrow in down:
+                translation[1] = -self.translation_step
+            elif key.enter in down:
+                translation[2] = self.translation_step
+            elif key.shift in down:
+                translation[2] = -self.translation_step
+
+        position = self.manipulator.get_link_positions(link_ids=self.distal_link, wrt_link_id=self.base_link)
+        position += translation
+        self.task.desired_position = position
+
+        # update the QP task desired orientation (if specified)
+        if self.use_orientation:
+            drpy = np.zeros(3)
+            if key.ctrl in pressed:
+                if key.top_arrow in down:
+                    drpy[1] = self.rotation_step
+                elif key.bottom_arrow in down:
+                    drpy[1] = -self.rotation_step
+                elif key.left_arrow in down:
+                    drpy[2] = self.rotation_step
+                elif key.right_arrow in down:
+                    drpy[2] = -self.rotation_step
+                elif key.enter in down:
+                    drpy[0] = self.rotation_step
+                elif key.shift in down:
+                    drpy[0] = -self.rotation_step
+            orientation = self.manipulator.get_link_orientations(link_ids=self.distal_link, wrt_link_id=self.base_link)
+            orientation = get_rpy_from_quaternion(orientation)
+            orientation += drpy
+            self.task.desired_orientation = get_quaternion_from_rpy(orientation)
+
+        # update the QP task
+        self.task.update(update_model=True)
+
+        # solve QP and set the joint variables
+        if self._control_type == ControlType.POSITION:  # Position control
+            # solve QP task
+            dq = self.solver.solve()
+
+            # set joint positions
+            q = self.manipulator.get_joint_positions()
+            q = q + dq * self.simulator.dt
+            self.manipulator.set_joint_positions(q)
+
+        else:  # Impedance control
+            # solve QP task
+            torques = self.solver.solve()
+
+            # set joint torques
+            self.manipulator.set_joint_torques(torques)
+
+        # check if gripper
+        if self.gripper is not None:
+            # check if button is pressed
+            if key.n0 in down:  # open the gripper
+                if self._control_type == ControlType.POSITION:  # position control
+                    self.gripper.open(factor=1)
+                else:  # impedance control
+                    self.grasp_strength += 0.1
+                    self.gripper.grasp(strength=self.grasp_strength)
+            elif key.n1 in down:  # close the gripper
+                if self._control_type == ControlType.POSITION:  # position control
+                    self.gripper.close(factor=1)
+                else:  # impedance control
+                    self.grasp_strength -= 0.1
+                    self.gripper.grasp(strength=self.grasp_strength)

pyrobolearn/tools/bridges/mouse_keyboard/bridge_spacemouse_manipulator.py

@@ -0,0 +1,208 @@
+# -*- coding: utf-8 -*-
+# !/usr/bin/env python
+"""Define the bridge between the SpaceMouse interface and a manipulator's end-effector. Note that this uses a QP 
+controller behind the scene to move the robot. You can use position or impedance control.
+"""
+
+from enum import Enum
+
+from pyrobolearn.tools.bridges import Bridge
+from pyrobolearn.robots import Manipulator, Gripper
+from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy
+from pyrobolearn.tools.interfaces.mouse_keyboard.spacemouse import SpaceMouseInterface
+from pyrobolearn.priorities.models.robot_model import RobotModelInterface
+from pyrobolearn.priorities.tasks.velocity import CartesianTask
+from pyrobolearn.priorities.tasks.torque import CartesianImpedanceControlTask
+from pyrobolearn.priorities.solvers import QPTaskSolver
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class ControlType(Enum):
+    POSITION = 1
+    IMPEDANCE = 2
+
+
+class BridgeSpaceMouseManipulator(Bridge):
+    r"""Bridge between SpaceMouse and a manipulator's end-effector.
+
+    Here is the mapping between the interface and the end-effector:
+    - translation: translate the end-effector link in the 3D space.
+    - rotation: rotate the end-effector link in the 3D space.
+    - left button: open the gripper.
+    - right button: close the gripper.
+    """
+
+    def __init__(self, manipulator, interface=None, base_link=None, end_effector_link=None, control_type='position',
+                 gripper=None, translation_range=(-1, 1), rotation_range=(-1, 1), use_orientation=False,
+                 priority=None, verbose=False):
+        """
+        Initialize the bridge between the SpaceMouse and a manipulator's end-effector.
+
+        Args:
+            manipulator (Manipulator): manipulator robot instance.
+            interface (None, SpaceMouseInterface): SpaceMouse interface. If None, it will create one.
+            base_link (int, str): base link id or name. If None, it will be set to -1 (the base)
+            end_effector_link (int, str): end effector link id or name. If None, it will take the first end-effector.
+            control_type (str): type of control to use, select between {'position', 'impedance'}.
+            gripper (None, Gripper): gripper robot instance.
+            translation_range (np.array[float[2]], np.array[float[2,3]], tuple[float[2]],
+              tuple[np.array[float[3]][2]]): the lower and higher bounds for the (x, y, z). This is used to normalize
+              the translation range to be between [-1, 1]. The frame (x,y,z) is defined with x pointing forward, y to
+              the left, and z up.
+            rotation_range (np.array[float[2]], np.array[float[2,3]], tuple[float[2]], tuple[np.array[3][2]]): the
+              lower and higher bounds for the roll-pitch-yaw angles. This is used to normalize the rotation range to
+              be between [-1, 1]. The frame (x,y,z) is defined with x pointing forward, y to the left, and z up.
+            use_orientation (bool): if we should account for the orientation of the end-effector as well using the
+              interface.
+            priority (int): priority of the bridge.
+            verbose (bool): If True, print information on the standard output.
+        """
+        # set manipulator
+        self.manipulator = manipulator
+        self.gripper = gripper
+        self.base_link = -1 if base_link is None else base_link
+        self.distal_link = manipulator.get_end_effector_ids(end_effector=0) if end_effector_link is None \
+            else end_effector_link
+        self.use_orientation = use_orientation
+        self.grasp_strength = 0
+
+        # if interface not defined, create one.
+        if interface is None:
+            interface = SpaceMouseInterface(verbose=verbose, translation_range=translation_range,
+                                            rotation_range=rotation_range)
+        if not isinstance(interface, SpaceMouseInterface):
+            raise TypeError("Expecting the given 'interface' to be an instance of `SpaceMouseInterface`, but got "
+                            "instead: {}".format(type(interface)))
+
+        # create controller
+        model = RobotModelInterface(manipulator)
+        x_des = self.manipulator.get_link_positions(link_ids=end_effector_link, wrt_link_id=base_link)
+        if control_type == 'position':
+            task = CartesianTask(model, distal_link=end_effector_link, base_link=base_link, desired_position=x_des,
+                                 kp_position=50.)
+            control_type = ControlType.POSITION
+        elif control_type == 'impedance':
+            task = CartesianImpedanceControlTask(model, distal_link=end_effector_link, base_link=base_link,
+                                                 desired_position=x_des, kp_position=100, kd_linear=60)
+            control_type = ControlType.IMPEDANCE
+        else:
+            raise NotImplementedError("Please select between {'position', 'impedance'} for the control_type.")
+
+        self._task = task
+        self._control_type = control_type
+        self._solver = QPTaskSolver(task=task)
+
+        # call superclass
+        super(BridgeSpaceMouseManipulator, self).__init__(interface, priority, verbose=verbose)
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def manipulator(self):
+        """Return the manipulator instance."""
+        return self._manipulator
+
+    @manipulator.setter
+    def manipulator(self, manipulator):
+        """Set the manipulator instance."""
+        if not isinstance(manipulator, Manipulator):
+            raise TypeError("Expecting the given 'manipulator' to be an instance of `Manipulator`, instead got: "
+                            "{}".format(type(manipulator)))
+        self._manipulator = manipulator
+
+    @property
+    def gripper(self):
+        """Return the gripper instance."""
+        return self._gripper
+
+    @gripper.setter
+    def gripper(self, gripper):
+        """Set the gripper instance."""
+        if not isinstance(gripper, Gripper):
+            raise TypeError("Expecting the given 'gripper' to be an instance of `Gripper`, instead got: "
+                            "{}".format(type(gripper)))
+        self._gripper = gripper
+
+    @property
+    def simulator(self):
+        """Return the simulator instance."""
+        return self._manipulator.simulator
+
+    @property
+    def solver(self):
+        """Return the QP solver."""
+        return self._solver
+
+    @property
+    def task(self):
+        """Return the priority task."""
+        return self._task
+
+    ###########
+    # Methods #
+    ###########
+
+    def step(self, update_interface=False):
+        """Perform a step: map the SpaceMouse interface to the end-effector."""
+        # update interface
+        if update_interface:
+            self.interface()
+
+        # update the QP task desired position
+        position = self.manipulator.get_link_positions(link_ids=self.distal_link, wrt_link_id=self.base_link)
+        position += self.interface.translation
+        self.task.desired_position = position
+
+        # update the QP task desired orientation (if specified)
+        if self.use_orientation:
+            orientation = self.manipulator.get_link_orientations(link_ids=self.distal_link, wrt_link_id=self.base_link)
+            orientation = get_rpy_from_quaternion(orientation)
+            orientation += self.interface.rotation
+            self.task.desired_orientation = get_quaternion_from_rpy(orientation)
+
+        # update the QP task
+        self.task.update(update_model=True)
+
+        # solve QP and set the joint variables
+        if self._control_type == ControlType.POSITION:  # Position control
+            # solve QP task
+            dq = self.solver.solve()
+
+            # set joint positions
+            q = self.manipulator.get_joint_positions()
+            q = q + dq * self.simulator.dt
+            self.manipulator.set_joint_positions(q)
+
+        else:  # Impedance control
+            # solve QP task
+            torques = self.solver.solve()
+
+            # set joint torques
+            self.manipulator.set_joint_torques(torques)
+
+        # check if gripper
+        if self.gripper is not None:
+            # check if button is pressed
+            if self.interface.left_button_pressed:  # open the gripper
+                if self._control_type == ControlType.POSITION:  # position control
+                    self.gripper.open(factor=1)
+                else:  # impedance control
+                    self.grasp_strength += 1
+                    self.gripper.grasp(strength=self.grasp_strength)
+            elif self.interface.right_button_pressed:  # close the gripper
+                if self._control_type == ControlType.POSITION:  # position control
+                    self.gripper.close(factor=1)
+                else:  # impedance control
+                    self.grasp_strength -= 1
+                    self.gripper.grasp(strength=self.grasp_strength)

pyrobolearn/tools/bridges/sensors/__init__.py

@@ -0,0 +1,2 @@
+# -*- coding: utf-8 -*-
+

pyrobolearn/tools/bridges/sensors/bridge_leapmotion_manipulator.py

@@ -0,0 +1,218 @@
+# -*- coding: utf-8 -*-
+#!/usr/bin/env python
+"""Define the bridge between the LeapMotion and a manipulator's end-effector. Note that this uses a QP controller
+behind the scene to move the robot. You can use position or impedance control.
+"""
+
+from enum import Enum
+import numpy as np
+
+from pyrobolearn.tools.bridges import Bridge
+from pyrobolearn.robots import Manipulator
+from pyrobolearn.tools.interfaces.sensors.leapmotion import LeapMotionInterface
+from pyrobolearn.priorities.models.robot_model import RobotModelInterface
+from pyrobolearn.priorities.tasks.velocity import CartesianTask
+from pyrobolearn.priorities.tasks.torque import CartesianImpedanceControlTask
+from pyrobolearn.priorities.solvers import QPTaskSolver
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class ControlType(Enum):
+    POSITION = 1
+    IMPEDANCE = 2
+
+
+class Hand(Enum):
+    LEFT = 1
+    RIGHT = 2
+
+
+class Position(Enum):
+    ABSOLUTE = 1
+    RELATIVE = 2
+
+
+class BridgeLeapMotionManipulator(Bridge):
+    r"""Bridge between LeapMotion and a manipulator's end-effector.
+
+    Just move your hand in front of the LeapMotion camera to move the end-effector.
+    """
+
+    def __init__(self, manipulator, interface=None, base_link=None, end_effector_link=None, control_type='position',
+                 gripper=None, bounding_box=None, use_orientation=True, hand='right', position='relative',
+                 priority=None, verbose=False):
+        """
+
+        Args:
+            manipulator (Manipulator): manipulator robot instance.
+            interface (None, LeapMotionInterface): LeapMotion interface. If None, it will create one.
+            base_link (int, str): base link id or name.
+            end_effector_link (int, str): end effector link id or name.
+            control_type (str): type of control to use, select between {'position', 'impedance'}.
+            gripper (None, Gripper): gripper robot instance.
+            bounding_box (None, np.array[float[2,3]]): bounding box limits.
+            use_orientation (bool): if we should account for the orientation of the end-effector as well using the
+              interface.
+            hand (str): the hand that we will use to move the end-effector, select between {'right', 'left'}.
+            position (str): if the position of the hand should be absolute or relative when moving the end-effector.
+              If relative, by moving the hand from the center it will compute the distance from it and move the
+              end-effector accordingly. Note that the orientation is always absolute here.
+            priority (int): priority of the bridge.
+            verbose (bool): If True, print information on the standard output.
+        """
+        # set manipulator
+        self.manipulator = manipulator
+        self.gripper = gripper
+        self.base_link = -1 if base_link is None else base_link
+        self.distal_link = manipulator.get_end_effector_ids(end_effector=0) if end_effector_link is None \
+            else end_effector_link
+        self.use_orientation = use_orientation
+        self.hand_type = Hand.RIGHT if hand == 'right' else Hand.LEFT
+        self.position_type = Position.RELATIVE if position == 'relative' else Position.ABSOLUTE
+        self.grasp_strength = 5
+
+        # if interface not defined, create one.
+        if interface is None:
+            interface = LeapMotionInterface(bounding_box=bounding_box, verbose=verbose)
+        if not isinstance(interface, LeapMotionInterface):
+            raise TypeError("Expecting the given 'interface' to be an instance of `LeapMotionInterface`, but got "
+                            "instead: {}".format(type(interface)))
+
+        # create controller
+        model = RobotModelInterface(manipulator)
+        x_des = self.manipulator.get_link_positions(link_ids=end_effector_link, wrt_link_id=base_link)
+        if control_type == 'position':
+            task = CartesianTask(model, distal_link=end_effector_link, base_link=base_link, desired_position=x_des,
+                                 kp_position=50.)
+            control_type = ControlType.POSITION
+        elif control_type == 'impedance':
+            task = CartesianImpedanceControlTask(model, distal_link=end_effector_link, base_link=base_link,
+                                                 desired_position=x_des, kp_position=100, kd_linear=60)
+            control_type = ControlType.IMPEDANCE
+        else:
+            raise NotImplementedError("Please select between {'position', 'impedance'} for the control_type.")
+
+        self._task = task
+        self._control_type = control_type
+        self._solver = QPTaskSolver(task=task)
+
+        # call superclass
+        super(BridgeLeapMotionManipulator, self).__init__(interface, priority, verbose=verbose)
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def manipulator(self):
+        """Return the manipulator instance."""
+        return self._manipulator
+
+    @manipulator.setter
+    def manipulator(self, manipulator):
+        """Set the manipulator instance."""
+        if not isinstance(manipulator, Manipulator):
+            raise TypeError("Expecting the given 'manipulator' to be an instance of `Manipulator`, instead got: "
+                            "{}".format(type(manipulator)))
+        self._manipulator = manipulator
+
+    @property
+    def gripper(self):
+        """Return the gripper instance."""
+        return self._gripper
+
+    @gripper.setter
+    def gripper(self, gripper):
+        """Set the gripper instance."""
+        if not isinstance(gripper, Gripper):
+            raise TypeError("Expecting the given 'gripper' to be an instance of `Gripper`, instead got: "
+                            "{}".format(type(gripper)))
+        self._gripper = gripper
+
+    @property
+    def simulator(self):
+        """Return the simulator instance."""
+        return self._manipulator.simulator
+
+    @property
+    def solver(self):
+        """Return the QP solver."""
+        return self._solver
+
+    @property
+    def task(self):
+        """Return the priority task."""
+        return self._task
+
+    ###########
+    # Methods #
+    ###########
+
+    def step(self, update_interface=False):
+        """Perform a step: map the LeapMotion interface to the end-effector."""
+        # update interface
+        if update_interface:
+            self.interface()
+
+        hand = None
+        if self.hand_type == Hand.RIGHT:
+            hand = self.interface.right_hand
+        elif self.hand_type == Hand.LEFT:
+            hand = self.interface.left_hand
+
+        if hand is not None:
+            # update the QP task desired position
+            # position = self.manipulator.get_link_positions(link_ids=self.distal_link, wrt_link_id=self.base_link)
+            # position += self.interface.translation
+            self.task.desired_position = self.interface.get_hand_stable_position(hand)
+
+            # update the QP task desired orientation (if specified)
+            if self.use_orientation:
+                # orientation = self.manipulator.get_link_orientations(link_ids=self.distal_link,
+                #                                                      wrt_link_id=self.base_link)
+                # orientation = get_rpy_from_quaternion(orientation)
+                self.task.desired_orientation = self.interface.get_hand_quaternion(hand)
+
+            # update the QP task
+            self.task.update(update_model=True)
+
+            # solve QP and set the joint variables
+            if self._control_type == ControlType.POSITION:  # Position control
+                # solve QP task
+                dq = self.solver.solve()
+
+                # set joint positions
+                q = self.manipulator.get_joint_positions()
+                q = q + dq * self.simulator.dt
+                self.manipulator.set_joint_positions(q)
+
+            else:  # Impedance control
+                # solve QP task
+                torques = self.solver.solve()
+
+                # set joint torques
+                self.manipulator.set_joint_torques(torques)
+
+            # check if gripper
+            if self.gripper is not None:
+                # check if button is pressed
+                if self.interface.left_button_pressed:  # open the gripper
+                    if self._control_type == ControlType.POSITION:  # position control
+                        self.gripper.open(factor=1)
+                    else:  # impedance control
+                        self.gripper.grasp(strength=self.grasp_strength)
+                elif self.interface.right_button_pressed:  # close the gripper
+                    if self._control_type == ControlType.POSITION:  # position control
+                        self.gripper.close(factor=1)
+                    else:  # impedance control
+                        self.gripper.grasp(strength=-self.grasp_strength)
+

pyrobolearn/tools/interfaces/sensors/leapmotion.py

@@ -2,6 +2,12 @@
 #!/usr/bin/env python
 """Define the Leap Motion hand tracking sensor input interface.
 
+You can run the Leap control panel by typing in the terminal:
+$ LeapControlPanel
+
+If you need to restart the daemon (if necessary), just run:
+$ sudo service leapd restart
+
 References:
     - Leap Motion: https://www.leapmotion.com/
     - Installation (Ubuntu): https://www.leapmotion.com/setup/desktop/linux/
@@ -68,7 +74,7 @@ class LeapMotionInterface(SensorInterface):
         Initialize the Leap motion input interface.
 
         Args:
-            bounding_box (None, np.array[2,3]):
+            bounding_box (None, np.array[float[2,3]]): bounding box limits.
             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
@@ -149,7 +155,7 @@ class LeapMotionInterface(SensorInterface):
             hand (Leap.Hand): hand instance.
 
         Returns:
-            np.array[3]: hand direction (unit vector)
+            np.array[float[3]]: hand direction (unit vector)
         """
         d = hand.direction
         return np.array([-d.z, -d.x, d.y])
@@ -164,7 +170,7 @@ class LeapMotionInterface(SensorInterface):
             hand (Leap.Hand): hand instance.
 
         Returns:
-            np.array[3]: palm normal (unit vector)
+            np.array[float[3]]: palm normal (unit vector)
         """
         d = hand.palm_normal
         return np.array([-d.z, -d.x, d.y])
@@ -178,7 +184,7 @@ class LeapMotionInterface(SensorInterface):
             hand (Leap.Hand): hand instance.
 
         Returns:
-            np.array[3, 3]: rotation matrix.
+            np.array[float[3,3]]: rotation matrix.
         """
         basis = hand.basis
         x_basis = basis.x_basis.to_float_array()
@@ -195,7 +201,7 @@ class LeapMotionInterface(SensorInterface):
             hand (Leap.Hand): hand instance.
 
         Returns:
-            np.array[4]: quaternion [x,y,z,w]
+            np.array[float[4]]: quaternion [x,y,z,w]
         """
         return get_quaternion_from_matrix(LeapMotionInterface.get_hand_rotation_matrix(hand))
 
@@ -208,7 +214,7 @@ class LeapMotionInterface(SensorInterface):
             hand (Leap.Hand): hand instance.
 
         Returns:
-            np.array[3]: roll-pitch-yaw angles (in radians)
+            np.array[float[3]]: roll-pitch-yaw angles (in radians)
         """
         return get_rpy_from_matrix(LeapMotionInterface.get_hand_rotation_matrix(hand))
 
@@ -221,7 +227,7 @@ class LeapMotionInterface(SensorInterface):
             hand (Leap.Hand): hand instance.
 
         Returns:
-            np.array[3]: position (in meter)
+            np.array[float[3]]: position (in meter)
         """
         d = hand.palm_position
         return np.array([-d.z, -d.x, d.y]) / 1000.
@@ -239,7 +245,7 @@ class LeapMotionInterface(SensorInterface):
             hand (Leap.Hand): hand instance.
 
         Returns:
-            np.array[3]: stabilized hand position (in meter)
+            np.array[float[3]]: stabilized hand position (in meter)
         """
         d = hand.stabilized_palm_position
         return np.array([-d.z, -d.x, d.y]) / 1000.
@@ -269,7 +275,7 @@ class LeapMotionInterface(SensorInterface):
             hand (Leap.Hand): hand instance.
 
         Returns:
-            np.array[3]: wrist position (in meter)
+            np.array[float[3]]: wrist position (in meter)
         """
         d = hand.wrist_position
         return np.array([-d.z, -d.x, d.y]) / 1000.
@@ -283,7 +289,7 @@ class LeapMotionInterface(SensorInterface):
             hand (Leap.Hand): hand instance.
 
         Returns:
-            np.array[3]: linear velocity (in meter/second)
+            np.array[float[3]]: linear velocity (in meter/second)
         """
         d = hand.palm_velocity
         return np.array([-d.z, -d.x, d.y]) / 1000.
@@ -372,7 +378,7 @@ class LeapMotionInterface(SensorInterface):
             hand (Leap.Hand): hand instance.
 
         Returns:
-            np.array[3]: position of the center of the "hold" sphere (in meter)
+            np.array[float[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.
@@ -480,7 +486,7 @@ class LeapMotionInterface(SensorInterface):
             hand_or_arm (Leap.Hand, Leap.Arm): hand or arm instance.
 
         Returns:
-            np.array[3]: elbow position (in meter)
+            np.array[float[3]]: elbow position (in meter)
         """
         if isinstance(hand_or_arm, Leap.Hand):
             d = hand_or_arm.arm.elbow_position
@@ -500,7 +506,7 @@ class LeapMotionInterface(SensorInterface):
             finger (Leap.Finger): finger instance.
 
         Returns:
-            np.array[3]: finger tip position (in meter)
+            np.array[float[3]]: finger tip position (in meter)
         """
         d = finger.tip_position
         return np.array([-d.z, -d.x, d.y]) / 1000.
@@ -514,7 +520,7 @@ class LeapMotionInterface(SensorInterface):
             finger (Leap.Finger): finger instance.
 
         Returns:
-            np.array[3]: finger tip stabilized position
+            np.array[float[3]]: finger tip stabilized position
         """
         d = finger.stabilized_tip_position
         return np.array([-d.z, -d.x, d.y]) / 1000.
@@ -528,7 +534,7 @@ class LeapMotionInterface(SensorInterface):
             finger (Leap.Finger): finger instance.
 
         Returns:
-            np.array[3]: finger tip velocity (in meter/second)
+            np.array[float[3]]: finger tip velocity (in meter/second)
         """
         d = finger.tip_velocity
         return np.array([-d.z, -d.x, d.y]) / 1000.
@@ -542,7 +548,7 @@ class LeapMotionInterface(SensorInterface):
             finger (Leap.Finger): finger instance.
 
         Returns:
-            np.array[3]: finger direction (unit vector)
+            np.array[float[3]]: finger direction (unit vector)
         """
         d = finger.direction
         return np.array([-d.z, -d.x, d.y])
@@ -556,7 +562,7 @@ class LeapMotionInterface(SensorInterface):
             finger (Leap.Finger): finger instance.
 
         Returns:
-            np.array[3]: finger length (in meter)
+            np.array[float[3]]: finger length (in meter)
         """
         return finger.length / 1000.
 
@@ -569,7 +575,7 @@ class LeapMotionInterface(SensorInterface):
             finger (Leap.Finger): finger instance.
 
         Returns:
-            np.array[3]: finger witdh (in meter)
+            np.array[float[3]]: finger witdh (in meter)
         """
         return finger.width / 1000.
 
@@ -595,7 +601,7 @@ class LeapMotionInterface(SensorInterface):
             box (Leap.InteractionBox): interaction box instance.
 
         Returns:
-            np.array[3]: interaction box center (in meter).
+            np.array[float[3]]: interaction box center (in meter).
         """
         d = box.center
         return np.array([-d.z, -d.x, d.y]) / 1000.
@@ -648,7 +654,7 @@ class LeapMotionInterface(SensorInterface):
             box (Leap.InteractionBox): interaction box instance.
 
         Returns:
-            np.array[3]: box dimensions (depth, width, height)
+            np.array[float[3]]: box dimensions (depth, width, height)
         """
         return np.array([box.depth, box.width, box.height])
 
@@ -657,27 +663,27 @@ class LeapMotionInterface(SensorInterface):
         Normalize the given position such that the position is in the range of [0..1].
 
         Args:
-            position (np.array[3]): position to normalized.
+            position (np.array[float[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
+            np.array[float[3]]: normalized position
         """
         pass
 
     def transform_normalized_position(self, position, clamp=True, ranges=None):
         """
-        Transform the normalized position
+        Transform the normalized position.
 
         Args:
-            position (np.array[3]): normalized position.
+            position (np.array[float[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]):
+            ranges (None, np.array[float[2,3]]):
 
         Returns:
-            np.array[3]: transformed position.
+            np.array[float[3]]: transformed position.
         """
         # normalize the position
         position = self.normalize_point(position, clamp=clamp)
@@ -687,13 +693,13 @@ class LeapMotionInterface(SensorInterface):
         Transform the position
 
         Args:
-            position (np.array[3]): normalized position.
+            position (np.array[float[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]):
+            ranges (None, np.array[float[2,3]]):
 
         Returns:
-            np.array[3]: transformed position.
+            np.array[float[3]]: transformed position.
         """
         pass
 
@@ -705,7 +711,7 @@ class LeapMotionInterface(SensorInterface):
             hand (Leap.Hand): hand instance.
 
         Returns:
-            np.array[3]: hand transformed position (in meter)
+            np.array[float[3]]: hand transformed position (in meter)
         """
         pass
 
@@ -718,7 +724,7 @@ class LeapMotionInterface(SensorInterface):
             frame (Leap.Frame): frame instance.
 
         Returns:
-            np.array[H, W]: image.
+            np.array[uint8[H,W]]: image.
         """
         image = frame.images[0]
         height, width = image.height, image.width
@@ -733,7 +739,7 @@ class LeapMotionInterface(SensorInterface):
             frame (Leap.Frame): frame instance.
 
         Returns:
-            np.array[H, W]: image
+            np.array[uint8[H,W]]: image
         """
         image = frame.images[1]
         height, width = image.height, image.width
@@ -762,20 +768,15 @@ if __name__ == '__main__':
     import time
 
     # create the myo interface
-    myo = LeapMotionInterface(verbose=True)
+    leap = 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))
+            leap.step()
             print("")
             time.sleep(0.01)
     except KeyboardInterrupt:
         print("Keyboard Interrupt")
     finally:
-        myo.close()
+        leap.close()
         print("Bye!")