Add admittance as ros2 controller

lbr_bringup/doc/lbr_bringup.rst

@@ -183,5 +183,5 @@ Troubleshooting
 ---------------
 Noisy Execution
 ~~~~~~~~~~~~~~~
-- Frequency: Make sure the ``ros2_control_node`` frequency and the ``FRI send period`` are compatible, consider changing ``update_rate`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`. 
+- Frequency: Make sure the ``ros2_control_node`` frequency and the ``FRI send period`` are compatible, consider changing ``update_rate`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`. 
 - 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/gazebo.launch.py

@@ -13,7 +13,7 @@ def generate_launch_description() -> LaunchDescription:
     ld.add_action(LBRDescriptionMixin.arg_robot_name())
     ld.add_action(
         LBRROS2ControlMixin.arg_ctrl()
-    )  # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml
+    )  # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_description/ros2_control/lbr_controllers.yaml
 
     # static transform world -> <robot_name>_floating_link
     world_robot_tf = [0, 0, 0, 0, 0, 0]  # keep zero

lbr_bringup/lbr_bringup/ros2_control.py

@@ -11,15 +11,15 @@ class LBRROS2ControlMixin:
     def arg_ctrl_cfg_pkg() -> DeclareLaunchArgument:
         return DeclareLaunchArgument(
             name="ctrl_cfg_pkg",
-            default_value="lbr_ros2_control",
+            default_value="lbr_description",
             description="Controller configuration package. The package containing the ctrl_cfg.",
         )
 
     @staticmethod
     def arg_ctrl_cfg() -> DeclareLaunchArgument:
         return DeclareLaunchArgument(
             name="ctrl_cfg",
-            default_value="config/lbr_controllers.yaml",
+            default_value="ros2_control/lbr_controllers.yaml",
             description="Relative path from ctrl_cfg_pkg to the controllers.",
         )
 
@@ -30,6 +30,7 @@ def arg_ctrl() -> DeclareLaunchArgument:
             default_value="joint_trajectory_controller",
             description="Desired default controller. One of specified in ctrl_cfg.",
             choices=[
+                "admittance_controller",
                 "joint_trajectory_controller",
                 "forward_position_controller",
                 "lbr_joint_position_command_controller",
@@ -82,11 +83,11 @@ def node_ros2_control(
                     [
                         FindPackageShare(
                             LaunchConfiguration(
-                                "ctrl_cfg_pkg", default="lbr_ros2_control"
+                                "ctrl_cfg_pkg", default="lbr_description"
                             )
                         ),
                         LaunchConfiguration(
-                            "ctrl_cfg", default="config/lbr_controllers.yaml"
+                            "ctrl_cfg", default="ros2_control/lbr_controllers.yaml"
                         ),
                     ]
                 ),

lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml

@@ -0,0 +1,37 @@
+# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface
+hardware:
+  fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro
+    major_version: 1
+    minor_version: 15
+  client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench]
+  port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups
+  remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection
+  rt_prio: 80 # real-time priority for the control loop
+  # exponential moving average time constant for joint position commands [s]:
+  #   Set tau > robot sample time for more smoothing on the commands
+  #   Set tau << robot sample time for no smoothing on the commands
+  joint_position_tau: 0.2
+  command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop]
+  # exponential moving average time constant for external joint torque measurements [s]:
+  #   Set tau > robot sample time for more smoothing on the external torque measurements
+  #   Set tau << robot sample time for more smoothing on the external torque measurements
+  external_torque_tau: 0.4
+  # exponential moving average time constant for joint torque measurements [s]:
+  #   Set tau > robot sample time for more smoothing on the raw torque measurements
+  #   Set tau << robot sample time for more smoothing on the raw torque measurements
+  measured_torque_tau: 0.4
+  open_loop: true # KUKA works the best in open_loop control mode
+
+estimated_ft_sensor: # estimates the external force-torque from the external joint torque values
+  enabled: true # whether to enable the force-torque estimation
+  update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate)
+  rt_prio: 30 # real-time priority for the force-torque estimation
+  chain_root: lbr_link_0
+  chain_tip: lbr_link_ee
+  damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian
+  force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered
+  force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered
+  force_z_th: 2.0 # z-force threshold. Only if the force exceeds this value, the force will be considered
+  torque_x_th: 0.5 # x-torque threshold. Only if the torque exceeds this value, the torque will be considered
+  torque_y_th: 0.5 # y-torque threshold. Only if the torque exceeds this value, the torque will be considered
+  torque_z_th: 0.5 # z-torque threshold. Only if the torque exceeds this value, the torque will be considered

lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst

@@ -8,14 +8,9 @@ lbr_demos_advanced_cpp
    :local:
    :backlinks: none
 
-Admittance Controller
----------------------
-This demo implements a simple admittance controller.
-
-#. Client side configurations:
-
-    #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+Twist Controller
+----------------
+This demo uses the twist controller.
 
 #. Remote side configurations:
 
@@ -35,16 +30,47 @@ This demo implements a simple admittance controller.
     .. code-block:: bash
 
         ros2 launch lbr_bringup hardware.launch.py \
-            ctrl:=lbr_joint_position_command_controller \
+            ctrl:=twist_controller \
             model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
 
-#. Launch the `admittance_control <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp>`_:octicon:`link-external`:
+#. Next, publish to the ``/lbr/command/twist`` topic:
 
-    .. code-block:: bash    
-
-        ros2 run lbr_demos_advanced_cpp admittance_control --ros-args \
-            -r __ns:=/lbr \
-            --params-file `ros2 pkg prefix lbr_demos_advanced_cpp`/share/lbr_demos_advanced_cpp/config/admittance_control.yaml
+    .. code-block:: bash
+
+        ros2 topic pub \
+            --rate 100 \
+            /lbr/command/twist \
+            geometry_msgs/msg/Twist \
+            "{linear: {x: 0.0, y: 0.0, z: 0.05}, angular: {x: 0.0, y: 0.0, z: 0.0}}"
+
+#. If you ``Ctrl+C`` the publisher, the ``twist_controller`` throws an error as it expects a continuous stream of twist commands.
+
+Admittance Controller
+---------------------
+This demo uses the admittance controller.
+
+#. 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``
+        - ``FRI client command mode``: ``POSITION``
+
+#. Launch the robot driver (please note that a different system configuration file is used with heavier smoothing!):
+
+    .. code-block:: bash
+
+        ros2 launch lbr_bringup hardware.launch.py \
+            ctrl:=admittance_controller \
+            sys_cfg_pkg:=lbr_demos_advanced_cpp \
+            sys_cfg:=config/lbr_system_config.yaml \
+            model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
 
 #. Now gently move the robot at the end-effector.
 
@@ -56,7 +82,7 @@ kinematics to move the robot's end-effector along the z-axis in Cartesian space.
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations:
 

lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst

@@ -15,7 +15,7 @@ This demo implements a simple admittance controller.
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations:
 
@@ -55,7 +55,7 @@ This demo implements an admittance controller with a remote center of motion (RC
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations:
 

lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst

@@ -15,7 +15,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations:
 
@@ -73,7 +73,7 @@ Hardware
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations:
 
@@ -97,7 +97,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations:
 
@@ -135,7 +135,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations:
 

lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst

@@ -15,7 +15,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations:
 
@@ -73,7 +73,7 @@ Hardware
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations:
 
@@ -97,7 +97,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations:
 
@@ -135,7 +135,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations:
 

lbr_demos/lbr_moveit/doc/lbr_moveit.rst

@@ -63,7 +63,7 @@ MoveIt Servo - Hardware
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations:
 
@@ -123,7 +123,7 @@ MoveIt via RViz - Hardware
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations:
 

lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst

@@ -42,7 +42,7 @@ Hardware
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_config.yaml>`_:octicon:`link-external`
-    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_controllers.yaml>`_:octicon:`link-external`
 
 #. Remote side configurations: