Merge pull request #96 from KCL-BMEIS/dev-humble-moveit

Humble v1.2.0

Brings back MoveIt 2 🥳

Closes:

#52 
#60

Author: mhubii

.gitignore

@@ -1 +1,2 @@
+.vscode
 **/__pycache__

README.md

@@ -19,7 +19,7 @@ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
 Next, launch the simulation via
 ```shell
 source install/setup.bash
-ros2 launch lbr_bringup lbr_bringup.launch.py model:=med7 sim:=true # model:=[iiwa7/iiwa14/med7/med14]
+ros2 launch lbr_bringup bringup.launch.py model:=med7 sim:=true # model:=[iiwa7/iiwa14/med7/med14]
 ```
 
 Now, run the [demos](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Documentation](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/index.html) above.

lbr_bringup/CMakeLists.txt

@@ -7,13 +7,13 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(ament_cmake_python REQUIRED)
 
 install(
   DIRECTORY config launch
   DESTINATION share/${PROJECT_NAME}
 )
 
+# Launch mixins
 ament_python_install_package(${PROJECT_NAME})
 
 ament_package()

lbr_bringup/doc/lbr_bringup.rst

@@ -0,0 +1,85 @@
+LBR Bringup
+===========
+The ``lbr_fri_ros2_stack`` is designed for research **and** deployment. It runs standalone, with ``ros2_control``, and thus also with ``MoveIt 2``. Details are described in below sections
+
+- :ref:`ROS 2 Control and MoveIt 2 Launch`
+- :ref:`Standalone Launch`
+
+Users may also refer to :ref:`Software Architecture` for a better understanding of the underlying ``lbr_fri_ros2`` package.
+
+.. note::
+    For the real robot, make sure you have followed :ref:`Robot Setup` first.
+
+.. warning::
+    On the real robot, do always execute in ``T1`` mode first.
+
+General Information on the FRI
+------------------------------
+The ``FRI`` lets the user select a ``FRI control mode`` and a ``FRI client command mode``. When running the ``LBRServer``:
+
+- .. dropdown:: Select ``FRI control mode``
+
+    .. thumbnail:: ../../lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_control_mode.png
+
+- .. dropdown:: Select ``FRI client command mode``
+    
+    .. thumbnail:: ../../lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_client_command_mode.png
+
+The ``FRI control mode`` specifies the mode in which the robot is controlled, and the ``FRI client command mode`` specifies the commands that the user sends.
+
+ROS 2 Control and MoveIt 2 Launch
+---------------------------------
+The ``lbr_bringup`` works for the simulation and the real robot. Run:
+
+.. code:: bash
+
+    ros2 launch lbr_bringup bringup.launch.py \
+        model:=iiwa7 # [iiwa7, iiwa14, med7, med14] \
+        sim:=true # [true, false] \
+        rviz:=true # [true, false] \
+        moveit:=true # [true, false]
+
+.. note::
+    For a list of available parameters, call ``ros2 launch lbr_bringup bringup.launch.py -s``.
+
+When using the real robot
+
+.. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD``
+
+    .. thumbnail:: ../../lbr_demos/doc/img/applications_lbr_server.png
+
+and select:
+
+- ``FRI send period``: ``10 ms``
+- ``IP address``: ``your configuration``
+- ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` (will put the robot into a compliant mode)
+- ``FRI client command mode``: ``POSITION``
+
+Make sure that the ``update_rate`` in `lbr_controllers.yml <https://github.com/KCL-BMEIS/lbr_fri_ros2_stack/tree/humble/lbr_hardware_interface/config/lbr_controllers.yml>`_ is greater or equal ``100`` (``FRI send period``).
+
+For using other ``FRI send period``, also change the ``sample_time`` in the `lbr.ros2_control.xacro <https://github.com/KCL-BMEIS/lbr_fri_ros2_stack/blob/humble/lbr_description/ros2_control/lbr.ros2_control.xacro>`_ (automated in the future).
+
+Standalone Launch
+-----------------
+Standalone launch is great for research. Only the the real robot is supported. It can be launched through:
+
+.. code:: bash
+
+    ros2 launch lbr_fri_ros2 lbr_app.launch.py \
+        model:=iiwa7 # [iiwa7, iiwa14, med7, med14] \
+        robot_name:=lbr # any robot name
+
+This runs the :lbr_fri_ros2:`LBRAppComponentLBRApp <lbr_fri_ros2::LBRAppComponent>`, which creates 2 topics, ``/robot_name/command`` for commands and ``/robot_name/state``. See :ref:`LBR Demos FRI ROS 2` for more examples and :ref:`LBR FRI ROS 2` for more documentation.
+
+.. note::
+    For a list of available parameters, call ``ros2 launch lbr_fri_ros2 lbr_app.launch.py -s``.
+
+Troubleshooting
+---------------
+Noisy Execution
+~~~~~~~~~~~~~~~
+Three main causes:
+
+- Frequency: Make sure the ``ros2_control_node`` runs at the same or a higher rate of the ``FRI send period``, change ``update_rate`` in `lbr_controllers.yml <https://github.com/KCL-BMEIS/lbr_fri_ros2_stack/tree/humble/lbr_hardware_interface/config/lbr_controllers.yml>`_. 
+- Standalone noise: Smoothing might be required, see :ref:`LBR Demos FRI ROS 2`.
+- Realtime priority: Set real time priority in ``code /etc/security/limits.conf``, add the line: ``user - rtprio 99``, where user is your username.

lbr_bringup/launch/bringup.launch.py

@@ -0,0 +1,47 @@
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, RegisterEventHandler
+from launch.conditions import IfCondition, UnlessCondition
+from launch.event_handlers import OnIncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+from lbr_description import LBRDescriptionMixin, RVizMixin
+
+
+def generate_launch_description() -> LaunchDescription:
+    ld = LaunchDescription()
+
+    ld.add_action(LBRDescriptionMixin.arg_sim())
+
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("lbr_bringup"),
+                        "launch",
+                        "sim.launch.py",
+                    ]
+                )
+            ),
+            condition=IfCondition(LaunchConfiguration("sim")),
+        )
+    )
+
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("lbr_bringup"),
+                        "launch",
+                        "real.launch.py",
+                    ]
+                )
+            ),
+            condition=UnlessCondition(LaunchConfiguration("sim")),
+        )
+    )
+
+    return ld

lbr_bringup/launch/lbr_bringup.launch.py

@@ -0,0 +1,61 @@
+from typing import List
+
+from launch import LaunchDescription, LaunchDescriptionEntity
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+
+from lbr_bringup import LBRMoveGroupMixin
+from lbr_description import LBRDescriptionMixin, RVizMixin
+
+
+def launch_setup(context) -> List[LaunchDescriptionEntity]:
+    ld = LaunchDescription()
+
+    ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution())
+    ld.add_action(LBRMoveGroupMixin.arg_capabilities())
+    ld.add_action(LBRMoveGroupMixin.arg_disable_capabilities())
+    ld.add_action(LBRMoveGroupMixin.arg_monitor_dynamics())
+    ld.add_action(LBRMoveGroupMixin.args_publish_monitored_planning_scene())
+
+    model = LaunchConfiguration("model").perform(context)
+    moveit_configs_builder = LBRMoveGroupMixin.moveit_configs_builder(
+        model,
+        package_name=f"{model}_moveit_config",
+    )
+    movegroup_params = LBRMoveGroupMixin.params_move_group()
+
+    # MoveGroup
+    ld.add_action(
+        LBRMoveGroupMixin.node_move_group(
+            parameters=[
+                moveit_configs_builder.to_dict(),
+                movegroup_params,
+                {"use_sim_time": LaunchConfiguration("sim")},
+            ],
+        )
+    )
+
+    # RViz
+    rviz = RVizMixin.node_rviz(
+        rviz_config_pkg=f"{model}_moveit_config",
+        rviz_config="config/moveit.rviz",
+        parameters=LBRMoveGroupMixin.params_rviz(
+            moveit_configs=moveit_configs_builder.to_moveit_configs()
+        ),
+    )
+
+    ld.add_action(rviz)
+
+    return ld.entities
+
+
+def generate_launch_description() -> LaunchDescription:
+    ld = LaunchDescription()
+
+    ld.add_action(LBRDescriptionMixin.arg_model())
+    ld.add_action(LBRDescriptionMixin.arg_robot_name())
+    ld.add_action(LBRDescriptionMixin.arg_sim())
+
+    ld.add_action(OpaqueFunction(function=launch_setup))
+
+    return ld