diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 3c303511..f3cc4e47 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,13 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package LBR FRI ROS 2 Stack ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Humble v2.1.2 (2024-10-18) +-------------------------- +* Adds MoveIt Servo demo, related to https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/50 and https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/211 + + * ``lbr_bringup``: New launch mixin and launch file for MoveIt Servo + * ``lbr_moveit``: New keyboard driver to interface with MoveIt Servo + Humble v2.1.1 (2024-09-27) -------------------------- * Adds support for the new Gazebo and removes support for Gazebo Classic (End-of-Life January 2025, refer https://community.gazebosim.org/t/gazebo-classic-end-of-life/2563). diff --git a/CITATION.cff b/CITATION.cff index 82c41b4c..203549cd 100644 --- a/CITATION.cff +++ b/CITATION.cff @@ -21,4 +21,4 @@ authors: title: "LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots" version: 2.1.2 doi: 10.48550/arXiv.2311.12709 -date-released: 2024-10-17 +date-released: 2024-10-18 diff --git a/lbr_demos/doc/lbr_demos.rst b/lbr_demos/doc/lbr_demos.rst index 7bcbf9e9..7194d431 100644 --- a/lbr_demos/doc/lbr_demos.rst +++ b/lbr_demos/doc/lbr_demos.rst @@ -29,8 +29,8 @@ MoveIt .. toctree:: :titlesonly: - lbr_moveit2_py <../lbr_moveit_py/doc/lbr_moveit_py.rst> - lbr_moveit2_cpp <../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst> + lbr_moveit_py <../lbr_moveit/doc/lbr_moveit.rst> + lbr_moveit_cpp <../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst> Integration ----------- diff --git a/lbr_demos/lbr_moveit/config/forward_keyboard.yaml b/lbr_demos/lbr_moveit/config/forward_keyboard.yaml new file mode 100644 index 00000000..4c6f9dc2 --- /dev/null +++ b/lbr_demos/lbr_moveit/config/forward_keyboard.yaml @@ -0,0 +1,52 @@ +/**/forward_keyboard: + ros__parameters: + velocity_scales: + translation: + x: 0.2 + y: 0.2 + z: 0.2 + rotation: + x: 0.2 + y: 0.2 + z: 0.2 + joints: + - 0.2 + - 0.2 + - 0.2 + - 0.2 + - 0.2 + - 0.2 + - 0.2 + keyboard_layout: + # uses pynput: https://pypi.org/project/pynput/ + translation: + x: + increase: "w" + decrease: "s" + y: + increase: "a" + decrease: "d" + z: + increase: "Key.up" + decrease: "Key.down" + rotation: + x: + increase: "u" + decrease: "j" + y: + increase: "h" + decrease: "k" + z: + increase: "Key.left" + decrease: "Key.right" + joints: + - "1" + - "2" + - "3" + - "4" + - "5" + - "6" + - "7" + escape: "Key.esc" + pause: "p" + reverse_joints: "r" diff --git a/lbr_demos/lbr_moveit_py/doc/img/iiwa7_moveit_rviz.png b/lbr_demos/lbr_moveit/doc/img/iiwa7_moveit_rviz.png similarity index 100% rename from lbr_demos/lbr_moveit_py/doc/img/iiwa7_moveit_rviz.png rename to lbr_demos/lbr_moveit/doc/img/iiwa7_moveit_rviz.png diff --git a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst new file mode 100644 index 00000000..a00c49b5 --- /dev/null +++ b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst @@ -0,0 +1,114 @@ +lbr_moveit +========== +.. warning:: + On hardware, do always execute in ``T1`` mode first. + +.. contents:: Table of Contents + :depth: 2 + :local: + :backlinks: none + +MoveIt Servo +------------ + +Simulation +~~~~~~~~~~ +#. Run the mock setup: + + .. code-block:: bash + + ros2 launch lbr_bringup mock.launch.py \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. Run MoveIt Servo: + + .. code-block:: bash + + ros2 launch lbr_moveit moveit_servo.launch.py \ + mode:=mock \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. Publish to ``/lbr/servo_node/delta_joint_cmds`` and ``/lbr/servo_node/delta_twist_cmds``. For this demo, we provide a keyboard driver (keyboard layout is printed to terminal): + + .. code-block:: bash + + ros2 launch lbr_moveit keyboard_driver.launch.py + +You can now experiment with + +- Modifying the MoveIt Servo parameters in `moveit_servo.yaml `_:octicon:`link-external`. E.g. the ``robot_link_command_frame`` to change the commanding frame. +- Connect a joystick or game controller. +- Or changing the veloctiy scales for this keyboard driver in `forward_keyboard.yaml `_:octicon:`linkt-external`. + +Hardware +~~~~~~~~ +#. Client side configurations: + + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + +#. Remote side configurations: + + #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` + + .. thumbnail:: ../../doc/img/applications_lbr_server.png + + #. Select + + - ``FRI send period``: ``10 ms`` + - ``IP address``: ``your configuration`` + - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` + - ``FRI client command mode``: ``POSITION`` + +#. Proceed with steps 1, 2 and 3 from `Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py`` in step 1. + +MoveIt via RViz +--------------- +.. image:: img/iiwa7_moveit_rviz.png + :align: center + :alt: MoveIt via RViz +**IIWA 7 R800 in RViz** + +To run MoveIt via RViz, simply follow: + +Simulation +~~~~~~~~~~ +#. Run the mock setup: + + .. code-block:: bash + + ros2 launch lbr_bringup mock.launch.py \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. Run MoveIt with RViz: + + .. code-block:: bash + + ros2 launch lbr_bringup move_group.launch.py \ + mode:=mock \ + rviz:=true \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. You can now move the robot via MoveIt in RViz! + +Hardware +~~~~~~~~ +#. Client side configurations: + + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + +#. Remote side configurations: + + #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` + + .. thumbnail:: ../../doc/img/applications_lbr_server.png + + #. Select + + - ``FRI send period``: ``10 ms`` + - ``IP address``: ``your configuration`` + - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` + - ``FRI client command mode``: ``POSITION`` + +#. Proceed with steps 1 and 2 from `Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py`` in step 1. diff --git a/lbr_demos/lbr_moveit/launch/keyboard_driver.launch.py b/lbr_demos/lbr_moveit/launch/keyboard_driver.launch.py new file mode 100644 index 00000000..c2368065 --- /dev/null +++ b/lbr_demos/lbr_moveit/launch/keyboard_driver.launch.py @@ -0,0 +1,47 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from lbr_bringup.description import LBRDescriptionMixin + + +def generate_launch_description() -> LaunchDescription: + ld = LaunchDescription() + + # launch arguments + ld.add_action( + DeclareLaunchArgument( + name="keyboard_config_pkg", + default_value="lbr_moveit", + description="The package containing the keyboard configurations.", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="keyboard_config", + default_value="config/forward_keyboard.yaml", + description="Location of the keyboard configuration file relative to keyboard_config_pkg.", + ) + ) + ld.add_action(LBRDescriptionMixin.arg_robot_name()) + + # forward keyboard node + keyboard_driver_config = PathJoinSubstitution( + [ + FindPackageShare(LaunchConfiguration("keyboard_config_pkg")), + LaunchConfiguration("keyboard_config"), + ] + ) + ld.add_action( + Node( + package="lbr_moveit", + executable="forward_keyboard", + output="screen", + parameters=[ + keyboard_driver_config, + ], + namespace=LaunchConfiguration("robot_name"), + ) + ) + return ld diff --git a/lbr_demos/lbr_moveit/lbr_moveit/__init__.py b/lbr_demos/lbr_moveit/lbr_moveit/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lbr_demos/lbr_moveit/lbr_moveit/forward_keyboard_node.py b/lbr_demos/lbr_moveit/lbr_moveit/forward_keyboard_node.py new file mode 100644 index 00000000..3af1d950 --- /dev/null +++ b/lbr_demos/lbr_moveit/lbr_moveit/forward_keyboard_node.py @@ -0,0 +1,384 @@ +from dataclasses import dataclass + +import numpy as np +import rclpy +from control_msgs.msg import JointJog +from geometry_msgs.msg import TwistStamped +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import JointState + + +class ForwardKeyboardNode(Node): + @dataclass + class VeloctiyScales: + @dataclass + class Translation: + x: float = 0.0 + y: float = 0.0 + z: float = 0.0 + + @dataclass + class Rotation: + x: float = 0.0 + y: float = 0.0 + z: float = 0.0 + + joints: list + translation: Translation = Translation() + rotation: Rotation = Rotation() + + @dataclass + class KeyboardLayout: + @dataclass + class Translation: + @dataclass + class X: + increase: str = "w" + decrease: str = "s" + + @dataclass + class Y: + increase: str = "a" + decrease: str = "d" + + @dataclass + class Z: + increase: str = "q" + decrease: str = "e" + + x: X = X() + y: Y = Y() + z: Z = Z() + + @dataclass + class Rotation: + @dataclass + class X: + increase: str = "r" + decrease: str = "f" + + @dataclass + class Y: + increase: str = "t" + decrease: str = "g" + + @dataclass + class Z: + increase: str = "y" + decrease: str = "h" + + x: X = X() + y: Y = Y() + z: Z = Z() + + joints: list + translation: Translation = Translation() + rotation: Rotation = Rotation() + escape: str = "Key.esc" + pause: str = "p" + reverse_joints: str = "r" + + def __init__(self): + super().__init__("forward_keyboard") + self._joint_cmd_pub = self.create_publisher( + JointJog, "servo_node/delta_joint_cmds", qos_profile_sensor_data + ) + self._twist_cmd_pub = self.create_publisher( + TwistStamped, "servo_node/delta_twist_cmds", qos_profile_sensor_data + ) + self._joint_state_sub = self.create_subscription( + JointState, + "joint_states", + self._on_joint_state, + qos_profile_sensor_data, + ) + self._cmd_timer = self.create_timer(0.1, self._on_cmd_timer) + + self._joint_state = None + self._dof = None + self._twist_cmd = TwistStamped() + self._joint_cmd = JointJog() + + while not self._joint_state: + self.get_logger().info( + f"Waiting for joint state on {self._joint_state_sub.topic_name}..." + ) + rclpy.spin_once(self, timeout_sec=1.0) + self._veloctiy_scales = self.VeloctiyScales(joints=[0.0] * self._dof) + self._keyboard_layout = self.KeyboardLayout( + joints=[str(i) for i in range(self._dof)] + ) + self._declare_parameters() + self._get_parameters() + + @property + def joint_state(self) -> JointState: + return self._joint_state + + @property + def dof(self) -> int: + return self._dof + + @property + def twist_cmd(self) -> np.ndarray: + return np.array( + [ + self._twist_cmd.twist.linear.x, + self._twist_cmd.twist.linear.y, + self._twist_cmd.twist.linear.z, + self._twist_cmd.twist.angular.x, + self._twist_cmd.twist.angular.y, + self._twist_cmd.twist.angular.z, + ] + ) + + @property + def keyboard_layout(self) -> KeyboardLayout: + return self._keyboard_layout + + def _declare_parameters(self): + # veloctiy scales + self.declare_parameters( + namespace="", + parameters=[ + ("velocity_scales.joints", [0.1] * self._dof), + ("velocity_scales.translation.x", 0.1), + ("velocity_scales.translation.y", 0.1), + ("velocity_scales.translation.z", 0.1), + ("velocity_scales.rotation.x", 0.1), + ("velocity_scales.rotation.y", 0.1), + ("velocity_scales.rotation.z", 0.1), + ], + ) + + # keyboard layout + self.declare_parameters( + namespace="", + parameters=[ + ("keyboard_layout.joints", [str(i) for i in range(self._dof)]), + ("keyboard_layout.translation.x.increase", "w"), + ("keyboard_layout.translation.x.decrease", "s"), + ("keyboard_layout.translation.y.increase", "a"), + ("keyboard_layout.translation.y.decrease", "d"), + ("keyboard_layout.translation.z.increase", "Key.up"), + ("keyboard_layout.translation.z.decrease", "Key.down"), + ("keyboard_layout.rotation.x.increase", "u"), + ("keyboard_layout.rotation.x.decrease", "j"), + ("keyboard_layout.rotation.y.increase", "h"), + ("keyboard_layout.rotation.y.decrease", "k"), + ("keyboard_layout.rotation.z.increase", "Key.left"), + ("keyboard_layout.rotation.z.decrease", "Key.right"), + ("keyboard_layout.escape", "Key.esc"), + ("keyboard_layout.pause", "p"), + ("keyboard_layout.reverse_joints", "r"), + ], + ) + + def _get_parameters(self): + # veloctiy scales + self._veloctiy_scales.joints = ( + self.get_parameter("velocity_scales.joints") + .get_parameter_value() + .double_array_value + ) + if len(self._veloctiy_scales.joints) != self._dof: + raise ValueError( + f"Number of joint velocity scales ({len(self._veloctiy_scales.joints)}) " + f"does not match the number of joints ({self._dof})." + ) + self._veloctiy_scales.translation.x = ( + self.get_parameter("velocity_scales.translation.x") + .get_parameter_value() + .double_value + ) + self._veloctiy_scales.translation.y = ( + self.get_parameter("velocity_scales.translation.y") + .get_parameter_value() + .double_value + ) + self._veloctiy_scales.translation.z = ( + self.get_parameter("velocity_scales.translation.z") + .get_parameter_value() + .double_value + ) + self._veloctiy_scales.rotation.x = ( + self.get_parameter("velocity_scales.rotation.x") + .get_parameter_value() + .double_value + ) + self._veloctiy_scales.rotation.y = ( + self.get_parameter("velocity_scales.rotation.y") + .get_parameter_value() + .double_value + ) + self._veloctiy_scales.rotation.z = ( + self.get_parameter("velocity_scales.rotation.z") + .get_parameter_value() + .double_value + ) + + # keyboard layout + self._keyboard_layout.joints = ( + self.get_parameter("keyboard_layout.joints") + .get_parameter_value() + .string_array_value + ) + self._keyboard_layout.translation.x.increase = ( + self.get_parameter("keyboard_layout.translation.x.increase") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.translation.x.decrease = ( + self.get_parameter("keyboard_layout.translation.x.decrease") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.translation.y.increase = ( + self.get_parameter("keyboard_layout.translation.y.increase") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.translation.y.decrease = ( + self.get_parameter("keyboard_layout.translation.y.decrease") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.translation.z.increase = ( + self.get_parameter("keyboard_layout.translation.z.increase") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.translation.z.decrease = ( + self.get_parameter("keyboard_layout.translation.z.decrease") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.rotation.x.increase = ( + self.get_parameter("keyboard_layout.rotation.x.increase") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.rotation.x.decrease = ( + self.get_parameter("keyboard_layout.rotation.x.decrease") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.rotation.y.increase = ( + self.get_parameter("keyboard_layout.rotation.y.increase") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.rotation.y.decrease = ( + self.get_parameter("keyboard_layout.rotation.y.decrease") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.rotation.z.increase = ( + self.get_parameter("keyboard_layout.rotation.z.increase") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.rotation.z.decrease = ( + self.get_parameter("keyboard_layout.rotation.z.decrease") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.escape = ( + self.get_parameter("keyboard_layout.escape") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.pause = ( + self.get_parameter("keyboard_layout.pause") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.reverse_joints = ( + self.get_parameter("keyboard_layout.reverse_joints") + .get_parameter_value() + .string_value + ) + + self.get_logger().info("Parameters:") + # veloctiy scales + self.get_logger().info(" Velocity Scales:") + self.get_logger().info(" Translation:") + self.get_logger().info(f" x: {self._veloctiy_scales.translation.x}") + self.get_logger().info(f" y: {self._veloctiy_scales.translation.y}") + self.get_logger().info(f" z: {self._veloctiy_scales.translation.z}") + self.get_logger().info(" Rotation:") + self.get_logger().info(f" x: {self._veloctiy_scales.rotation.x}") + self.get_logger().info(f" y: {self._veloctiy_scales.rotation.y}") + self.get_logger().info(f" z: {self._veloctiy_scales.rotation.z}") + self.get_logger().info(f" Joints: {self._veloctiy_scales.joints.tolist()}") + + # keyboard layout + self.get_logger().info(" Keyboard Layout:") + self.get_logger().info(" Translation:") + self.get_logger().info( + f" x: {self._keyboard_layout.translation.x.increase} / {self._keyboard_layout.translation.x.decrease}" + ) + self.get_logger().info( + f" y: {self._keyboard_layout.translation.y.increase} / {self._keyboard_layout.translation.y.decrease}" + ) + self.get_logger().info( + f" z: {self._keyboard_layout.translation.z.increase} / {self._keyboard_layout.translation.z.decrease}" + ) + self.get_logger().info(" Rotation:") + self.get_logger().info( + f" x: {self._keyboard_layout.rotation.x.increase} / {self._keyboard_layout.rotation.x.decrease}" + ) + self.get_logger().info( + f" y: {self._keyboard_layout.rotation.y.increase} / {self._keyboard_layout.rotation.y.decrease}" + ) + self.get_logger().info( + f" z: {self._keyboard_layout.rotation.z.increase} / {self._keyboard_layout.rotation.z.decrease}" + ) + self.get_logger().info(f" Joints: {self._keyboard_layout.joints}") + self.get_logger().info(f" Escape: {self._keyboard_layout.escape}") + self.get_logger().info(f" Pause: {self._keyboard_layout.pause}") + self.get_logger().info( + f" Reverse Joints: {self._keyboard_layout.reverse_joints}" + ) + + def _on_cmd_timer(self) -> None: + self._twist_cmd.header.stamp = self.get_clock().now().to_msg() + self._twist_cmd_pub.publish(self._twist_cmd) + self._joint_cmd.header.stamp = self.get_clock().now().to_msg() + self._joint_cmd_pub.publish(self._joint_cmd) + + @twist_cmd.setter + def twist_cmd(self, twist: np.ndarray) -> None: + if len(twist) != 6: + raise ValueError("Twist command must be a 6-element array.") + self._twist_cmd.twist.linear.x = twist[0] * self._veloctiy_scales.translation.x + self._twist_cmd.twist.linear.y = twist[1] * self._veloctiy_scales.translation.y + self._twist_cmd.twist.linear.z = twist[2] * self._veloctiy_scales.translation.z + self._twist_cmd.twist.angular.x = twist[3] * self._veloctiy_scales.rotation.x + self._twist_cmd.twist.angular.y = twist[4] * self._veloctiy_scales.rotation.y + self._twist_cmd.twist.angular.z = twist[5] * self._veloctiy_scales.rotation.z + + @property + def joint_veloctiy_cmd(self) -> np.ndarray: + return np.array(self._joint_cmd.velocities) + + @joint_veloctiy_cmd.setter + def joint_veloctiy_cmd(self, velocities: np.ndarray) -> None: + if not self._joint_state: + return + if len(velocities) != self._dof: + raise ValueError(f"Joint command must be a {self._dof}-element array.") + + self._joint_cmd.joint_names = self._joint_state.name + # account for potentially unsorted joint state broadcaster + index_map = np.argsort(self._joint_cmd.joint_names) + self._joint_cmd.velocities = velocities[index_map].tolist() + self._joint_cmd.velocities = [ + v * s + for v, s in zip(self._joint_cmd.velocities, self._veloctiy_scales.joints) + ] + + def _on_joint_state(self, msg: JointState): + self._joint_state = msg + self._dof = len(self._joint_state.name) diff --git a/lbr_demos/lbr_moveit/lbr_moveit/keyboard_listener.py b/lbr_demos/lbr_moveit/lbr_moveit/keyboard_listener.py new file mode 100644 index 00000000..1e93e154 --- /dev/null +++ b/lbr_demos/lbr_moveit/lbr_moveit/keyboard_listener.py @@ -0,0 +1,192 @@ +import numpy as np +from pynput.keyboard import Key, Listener + +from .forward_keyboard_node import ForwardKeyboardNode + + +class KeyboardListener: + def __init__(self, command_forward_node: ForwardKeyboardNode): + self._command_forward_node = command_forward_node + self._key_listener = Listener( + on_press=self._on_key_press, on_release=self._on_key_release + ) + self._valid_numbers = None + self._twist_cmd = np.zeros(6) + self._joint_velocity_cmd = None + self._joint_vel_direction = 1.0 + + def __enter__(self): + self._key_listener.start() + + def __exit__(self, exc_type, exc_value, traceback): + self._command_forward_node.get_logger().info("Exiting keyboard listener with.") + self._key_listener.stop() + + def _on_key_release(self, key: Key) -> None: + key_str = str(key).replace("'", "") + # zero out the velocity commands + try: + if ( + key_str + == self._command_forward_node.keyboard_layout.translation.z.increase + ): + self._twist_cmd[2] = 0.0 # translation along z-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.z.decrease + ): + self._twist_cmd[2] = 0.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.z.increase + ): + self._twist_cmd[5] = 0.0 # rotation about z-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.z.decrease + ): + self._twist_cmd[5] = 0.0 + elif key_str == self._command_forward_node.keyboard_layout.pause: # pause + self._twist_cmd = np.zeros(6) + self._joint_velocity_cmd = np.zeros(self._command_forward_node.dof) + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.x.increase + ): + self._twist_cmd[0] = 0.0 # translation along x-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.x.decrease + ): + self._twist_cmd[0] = 0.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.y.increase + ): + self._twist_cmd[1] = 0.0 # translation along y-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.y.decrease + ): + self._twist_cmd[1] = 0.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.x.increase + ): + self._twist_cmd[3] = 0.0 # rotation about x-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.x.decrease + ): + self._twist_cmd[3] = 0.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.y.increase + ): + self._twist_cmd[3] = 0.0 # rotation about y-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.y.decrease + ): + self._twist_cmd[3] = 0.0 + elif key_str in self._valid_numbers: + # get index of key_str within valid_numbers + index = self._valid_numbers.index(key_str) + if 0 <= index < self._command_forward_node.dof: + self._joint_velocity_cmd[index] = 0.0 + self._command_forward_node.twist_cmd = self._twist_cmd + self._command_forward_node.joint_veloctiy_cmd = self._joint_velocity_cmd + except AttributeError: + pass + + def _on_key_press(self, key: Key) -> None: + key_str = str(key).replace("'", "") + if key_str == self._command_forward_node.keyboard_layout.escape: + self._command_forward_node.get_logger().info("Exiting keyboard listener.") + self._twist_cmd = np.zeros(6) + self._joint_velocity_cmd = np.zeros(self._command_forward_node.dof) + self._command_forward_node.twist_cmd = self._twist_cmd + self._command_forward_node.joint_veloctiy_cmd = self._joint_velocity_cmd + self._key_listener.stop() + if not self._command_forward_node.dof: + return + if not self._valid_numbers: + self._valid_numbers = "".join( + [str(i) for i in self._command_forward_node.keyboard_layout.joints] + ) + self._joint_velocity_cmd = np.zeros(self._command_forward_node.dof) + try: + if ( + key_str + == self._command_forward_node.keyboard_layout.translation.z.increase + ): + self._twist_cmd[2] = 1.0 # translation along z-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.z.decrease + ): + self._twist_cmd[2] = -1.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.z.increase + ): + self._twist_cmd[5] = 1.0 # rotation about z-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.z.decrease + ): + self._twist_cmd[5] = -1.0 + elif key_str == self._command_forward_node.keyboard_layout.pause: # pause + self._twist_cmd = np.zeros(6) + self._joint_velocity_cmd = np.zeros(self._command_forward_node.dof) + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.x.increase + ): + self._twist_cmd[0] = 1.0 # translation along x-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.x.decrease + ): + self._twist_cmd[0] = -1.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.y.increase + ): + self._twist_cmd[1] = 1.0 # translation along y-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.y.decrease + ): + self._twist_cmd[1] = -1.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.x.increase + ): + self._twist_cmd[3] = 1.0 # rotation about x-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.x.decrease + ): + self._twist_cmd[3] = -1.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.y.increase + ): + self._twist_cmd[3] = 1.0 # rotation about y-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.y.decrease + ): + self._twist_cmd[3] = -1.0 + elif key_str == self._command_forward_node.keyboard_layout.reverse_joints: + self._joint_vel_direction *= -1.0 + elif key_str in self._valid_numbers: + # get index of key_str within valid_numbers + index = self._valid_numbers.index(key_str) + if 0 <= index < self._command_forward_node.dof: + self._joint_velocity_cmd[index] = self._joint_vel_direction + self._command_forward_node.twist_cmd = self._twist_cmd + self._command_forward_node.joint_veloctiy_cmd = self._joint_velocity_cmd + except AttributeError: + pass diff --git a/lbr_demos/lbr_moveit/package.xml b/lbr_demos/lbr_moveit/package.xml new file mode 100644 index 00000000..f2116fe8 --- /dev/null +++ b/lbr_demos/lbr_moveit/package.xml @@ -0,0 +1,18 @@ + + + + lbr_moveit + 2.1.2 + MoveIt demos for the LBRs + mhubii + Apache-2.0 + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/lbr_demos/lbr_moveit/resource/lbr_moveit b/lbr_demos/lbr_moveit/resource/lbr_moveit new file mode 100644 index 00000000..e69de29b diff --git a/lbr_demos/lbr_moveit/scripts/__init__.py b/lbr_demos/lbr_moveit/scripts/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lbr_demos/lbr_moveit/scripts/forward_keyboard.py b/lbr_demos/lbr_moveit/scripts/forward_keyboard.py new file mode 100644 index 00000000..62d6531a --- /dev/null +++ b/lbr_demos/lbr_moveit/scripts/forward_keyboard.py @@ -0,0 +1,13 @@ +import rclpy +from lbr_moveit.forward_keyboard_node import ForwardKeyboardNode +from lbr_moveit.keyboard_listener import KeyboardListener + + +def main(): + rclpy.init() + forward_keyboard_node = ForwardKeyboardNode() + try: + with KeyboardListener(forward_keyboard_node): + rclpy.spin(forward_keyboard_node) + except KeyboardInterrupt: + pass diff --git a/lbr_demos/lbr_moveit/setup.cfg b/lbr_demos/lbr_moveit/setup.cfg new file mode 100644 index 00000000..54a2c888 --- /dev/null +++ b/lbr_demos/lbr_moveit/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/lbr_moveit +[install] +install_scripts=$base/lib/lbr_moveit diff --git a/lbr_demos/lbr_moveit/setup.py b/lbr_demos/lbr_moveit/setup.py new file mode 100644 index 00000000..a4d342ce --- /dev/null +++ b/lbr_demos/lbr_moveit/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = "lbr_moveit" + +setup( + name=package_name, + version="2.1.2", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/config", ["config/forward_keyboard.yaml"]), + ("share/" + package_name + "/launch", ["launch/keyboard_driver.launch.py"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="mhubii", + maintainer_email="m.huber_1994@hotmail.de", + description="MoveIt demos for the LBRs.", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "forward_keyboard = scripts.forward_keyboard:main", + ], + }, +) diff --git a/lbr_demos/lbr_moveit/test/test_copyright.py b/lbr_demos/lbr_moveit/test/test_copyright.py new file mode 100644 index 00000000..ceffe896 --- /dev/null +++ b/lbr_demos/lbr_moveit/test/test_copyright.py @@ -0,0 +1,27 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/lbr_demos/lbr_moveit/test/test_flake8.py b/lbr_demos/lbr_moveit/test/test_flake8.py new file mode 100644 index 00000000..ee79f31a --- /dev/null +++ b/lbr_demos/lbr_moveit/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/lbr_demos/lbr_moveit/test/test_pep257.py b/lbr_demos/lbr_moveit/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/lbr_demos/lbr_moveit/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst index d7b52b15..5d907950 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -76,7 +76,7 @@ The ``MoveGroup`` configurations are parsed conveniently through a mixin: .. code-block:: python - from lbr_bringup.move_group import LBRMoveGroupMixin + from lbr_bringup.moveit import LBRMoveGroupMixin ... diff --git a/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py index fa080dfe..3e1577e7 100644 --- a/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py +++ b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py @@ -5,7 +5,7 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from lbr_bringup.description import LBRDescriptionMixin -from lbr_bringup.move_group import LBRMoveGroupMixin +from lbr_bringup.moveit import LBRMoveGroupMixin def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: diff --git a/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst b/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst deleted file mode 100644 index 31654530..00000000 --- a/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst +++ /dev/null @@ -1,55 +0,0 @@ -lbr_demos_moveit_py -=================== -.. warning:: - On hardware, do always execute in ``T1`` mode first. - -MoveIt via RViz ------------------ -.. image:: img/iiwa7_moveit_rviz.png - :align: center - :alt: MoveIt via RViz -**IIWA 7 R800 in RViz** - -To run MoveIt via RViz, simply follow: - -Simulation -~~~~~~~~~~ -#. Run the robot driver: - - .. code-block:: bash - - ros2 launch lbr_bringup mock.launch.py \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] - -#. Run MoveIt with RViz: - - .. code-block:: bash - - ros2 launch lbr_bringup move_group.launch.py \ - mode:=mock \ - rviz:=true \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] - -#. You can now move the robot via MoveIt in RViz! - -Hardware -~~~~~~~~ -#. Client side configurations: - - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` - -#. Remote side configurations: - - #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` - - .. thumbnail:: ../../doc/img/applications_lbr_server.png - - #. Select - - - ``FRI send period``: ``10 ms`` - - ``IP address``: ``your configuration`` - - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` - - ``FRI client command mode``: ``POSITION`` - -#. Proceed with steps 1 and 2 from `Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py``.