diff --git a/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_manipulator.py b/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_manipulator.py new file mode 100644 index 0000000..bc495c7 --- /dev/null +++ b/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) diff --git a/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py b/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py index 9c3fc39..58512c3 100644 --- a/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py +++ b/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) diff --git a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_manipulator.py b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_manipulator.py new file mode 100644 index 0000000..d4a0322 --- /dev/null +++ b/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) diff --git a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_spacemouse_manipulator.py b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_spacemouse_manipulator.py new file mode 100644 index 0000000..3376cab --- /dev/null +++ b/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) diff --git a/pyrobolearn/tools/bridges/sensors/__init__.py b/pyrobolearn/tools/bridges/sensors/__init__.py new file mode 100644 index 0000000..633f866 --- /dev/null +++ b/pyrobolearn/tools/bridges/sensors/__init__.py @@ -0,0 +1,2 @@ +# -*- coding: utf-8 -*- + diff --git a/pyrobolearn/tools/bridges/sensors/bridge_leapmotion_manipulator.py b/pyrobolearn/tools/bridges/sensors/bridge_leapmotion_manipulator.py new file mode 100644 index 0000000..dd9afe0 --- /dev/null +++ b/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) + diff --git a/pyrobolearn/tools/interfaces/sensors/leapmotion.py b/pyrobolearn/tools/interfaces/sensors/leapmotion.py index 3ff5a51..75c8791 100644 --- a/pyrobolearn/tools/interfaces/sensors/leapmotion.py +++ b/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!")