Merge pull request #174 from lbr-stack/dev-humble-launch

Humble v2.0.0 Release

README.md

@@ -70,7 +70,7 @@ Full documentation available on [Read the Docs](https://lbr-stack.readthedocs.io
 Now, run the [demos](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Hardware Setup](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_fri_ros2_stack/doc/hardware_setup.html).
 
 ## Citation
-If you enjoyed using this repository for your work, we would really appreciate ❤️ if you could cite it, as it helps us to continue offering support.
+If you enjoyed using this repository for your work, we would really appreciate ❤️ if you could leave a ⭐ and / or cite it, as it helps us to continue offering support.
 
 ```
 @misc{huber2023lbrstack,

docker/doc/docker.rst

@@ -30,7 +30,7 @@ To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions
 
     ros2 launch lbr_bringup bringup.launch.py model:=iiwa7 sim:=true moveit:=true
 
-   .. note::
+   .. hint::
 
     List all arguments for the launch file via
 

lbr_bringup/doc/lbr_bringup.rst

@@ -52,5 +52,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/tree/humble/lbr_ros2_control/config/lbr_controllers.yaml>`_. 
+- 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/tree/humble/lbr_ros2_control/config/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_mixins/lbr_ros2_control.py

@@ -32,7 +32,7 @@ def arg_ctrl() -> DeclareLaunchArgument:
             choices=[
                 "joint_trajectory_controller",
                 "forward_position_controller",
-                "lbr_position_command_controller",
+                "lbr_joint_position_command_controller",
                 "lbr_torque_command_controller",
                 "lbr_wrench_command_controller",
             ],

lbr_bringup/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_bringup</name>
-  <version>1.4.3</version>
+  <version>2.0.0</version>
   <description>LBR launch files.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>Apache License 2.0</license>

lbr_demos/doc/lbr_demos.rst

@@ -1,6 +1,6 @@
 lbr_demos
 =========
-Contolling the LBR through ``ros2_control``.
+All demos build on :ref:`lbr_ros2_control`. Different ``ros2_controllers`` are loaded to command the robot.
 
 .. note::
     Some demos require a real robot. Make sure you followed :doc:`Hardware Setup <../../lbr_fri_ros2_stack/doc/hardware_setup>` first.
@@ -24,9 +24,12 @@ Advanced
     lbr_demos_advanced_cpp <../lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst>
     lbr_demos_advanced_py <../lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst>
 
-MoveIt 2
---------------
-TODO moveit simple action client demos
+MoveIt 
+-------
+.. toctree::
+    :titlesonly:
+
+    lbr_moveit2_py <../lbr_moveit_py/doc/lbr_moveit_py.rst>
 
 Integration
 -----------

lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst

@@ -1,29 +1,45 @@
 lbr_demos_advanced_cpp
 ======================
-Collection of advanced usage examples for the ``lbr_fri_ros2`` package through C++.
-
 .. warning::
-    Do always execute in ``T1`` mode first.
+    On hardware, do always execute in ``T1`` mode first.
+
+.. contents:: Table of Contents
+   :depth: 2
+   :local:
+   :backlinks: none
 
 Admittance Controller
 ---------------------
-.. warning::
-    Not well behaved around singularities, put the robot in a well-behaved configuration first, e.g. ``A1 = 0°``, ``A2 = -30°``, ``A3 = 0°``, ``A4 = 60°``, ``A5 = 0°``, ``A6 = -90°``, ``A7 = 0°``. This can be done using the ``smartPAD`` in ``T1`` mode.
+This demo implements a simple admittance controller.
+
+#. Client side configurations:
+
+    #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/lbr_system_parameters.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+
+#. Remote side configurations:
+
+    #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD``
 
-#. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD``
+        .. thumbnail:: ../../doc/img/applications_lbr_server.png
 
-    .. 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:
 
     .. code-block:: bash
 
         ros2 launch lbr_bringup bringup.launch.py \
             sim:=false \
-            ctrl:=lbr_position_command_controller \
+            ctrl:=lbr_joint_position_command_controller \
             model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
 
-#. Launch the `admittance_control <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.    cpp>`_:
+#. Launch the `admittance_control <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp>`_:octicon:`link-external`:
 
     .. code-block:: bash    
     
@@ -38,17 +54,31 @@ Pose Controller
 This demo uses ``KDL`` to calculate forward kinematics and inverse
 kinematics to move the robot's end-effector along the z-axis in Cartesian space.
 
-#. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD``
+#. Client side configurations:
+
+    #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/lbr_system_parameters.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/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
 
-    .. thumbnail:: ../../doc/img/applications_lbr_server.png
+        - ``FRI send period``: ``10 ms``
+        - ``IP address``: ``your configuration``
+        - ``FRI control mode``: ``POSITION_CONTROL``
+        - ``FRI client command mode``: ``POSITION``
 
 #. Launch the robot driver:
 
     .. code-block:: bash
 
         ros2 launch lbr_bringup bringup.launch.py \
             sim:=false \
-            ctrl:=lbr_position_command_controller \
+            ctrl:=lbr_joint_position_command_controller \
             model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
 
 #. Launch the pose control

lbr_demos/lbr_demos_advanced_cpp/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_demos_advanced_cpp</name>
-  <version>1.4.3</version>
+  <version>2.0.0</version>
   <description>Advanced C++ demos for the lbr_ros2_control.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>Apache License 2.0</license>

lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp

@@ -62,7 +62,7 @@ class AdmittanceControlNode : public LBRBasePositionCommandNode {
     smooth_lbr_state_(lbr_state);
 
     auto lbr_command = admittance_controller_->update(lbr_state_, dt_);
-    lbr_position_command_pub_->publish(lbr_command);
+    lbr_joint_position_command_pub_->publish(lbr_command);
   };
 
   void smooth_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) {

lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp

@@ -12,7 +12,7 @@
 
 #include "friLBRState.h"
 
-#include "lbr_fri_idl/msg/lbr_position_command.hpp"
+#include "lbr_fri_idl/msg/lbr_joint_position_command.hpp"
 #include "lbr_fri_idl/msg/lbr_state.hpp"
 #include "lbr_fri_ros2/pinv.hpp"
 
@@ -41,8 +41,8 @@ class AdmittanceController {
     q_.resize(chain_.getNrOfJoints());
   };
 
-  const lbr_fri_idl::msg::LBRPositionCommand &update(const lbr_fri_idl::msg::LBRState &lbr_state,
-                                                      const double &dt) {
+  const lbr_fri_idl::msg::LBRJointPositionCommand &
+  update(const lbr_fri_idl::msg::LBRState &lbr_state, const double &dt) {
     std::memcpy(q_.data.data(), lbr_state.measured_joint_position.data(),
                 sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
     std::memcpy(tau_ext_.data(), lbr_state.external_torque.data(),
@@ -64,14 +64,15 @@ class AdmittanceController {
     dq_ = dq_gains_.asDiagonal() * jacobian_inv_ * f_ext_;
 
     for (int i = 0; i < 7; i++) {
-      lbr_position_command_.joint_position[i] = lbr_state.measured_joint_position[i] + dq_[i] * dt;
+      lbr_joint_position_command_.joint_position[i] =
+          lbr_state.measured_joint_position[i] + dq_[i] * dt;
     }
 
-    return lbr_position_command_;
+    return lbr_joint_position_command_;
   };
 
 protected:
-  lbr_fri_idl::msg::LBRPositionCommand lbr_position_command_;
+  lbr_fri_idl::msg::LBRJointPositionCommand lbr_joint_position_command_;
   KDL::Tree tree_;
   KDL::Chain chain_;
   std::unique_ptr<KDL::ChainJntToJacSolver> jacobian_solver_;

lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp

@@ -6,7 +6,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "lbr_fri_idl/msg/lbr_position_command.hpp"
+#include "lbr_fri_idl/msg/lbr_joint_position_command.hpp"
 #include "lbr_fri_idl/msg/lbr_state.hpp"
 
 namespace lbr_demos {
@@ -21,16 +21,17 @@ class LBRBasePositionCommandNode : public rclcpp::Node {
     dt_ = 1.0 / static_cast<double>(update_rate_);
 
     // publishers and subscribers
-    lbr_position_command_pub_ =
-        create_publisher<lbr_fri_idl::msg::LBRPositionCommand>("command/joint_position", 1);
+    lbr_joint_position_command_pub_ =
+        create_publisher<lbr_fri_idl::msg::LBRJointPositionCommand>("command/joint_position", 1);
 
     lbr_state_sub_ = create_subscription<lbr_fri_idl::msg::LBRState>(
         "state", 1,
         std::bind(&LBRBasePositionCommandNode::on_lbr_state_, this, std::placeholders::_1));
   }
 
 protected:
-  rclcpp::Publisher<lbr_fri_idl::msg::LBRPositionCommand>::SharedPtr lbr_position_command_pub_;
+  rclcpp::Publisher<lbr_fri_idl::msg::LBRJointPositionCommand>::SharedPtr
+      lbr_joint_position_command_pub_;
   rclcpp::Subscription<lbr_fri_idl::msg::LBRState>::SharedPtr lbr_state_sub_;
 
   std::string robot_description_;

lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp

@@ -69,9 +69,9 @@ class PoseControlNode : public LBRBasePositionCommandNode {
         current_joint_positions(i) = current_lbr_state_.measured_joint_position[i];
       }
 
-      lbr_fri_idl::msg::LBRPositionCommand joint_position_command =
+      lbr_fri_idl::msg::LBRJointPositionCommand joint_position_command =
           compute_ik_(msg, current_joint_positions);
-      lbr_position_command_pub_->publish(joint_position_command);
+      lbr_joint_position_command_pub_->publish(joint_position_command);
     }
   }
 
@@ -106,11 +106,12 @@ class PoseControlNode : public LBRBasePositionCommandNode {
     return pose;
   }
 
-  lbr_fri_idl::msg::LBRPositionCommand compute_ik_(const geometry_msgs::msg::Pose &desired_pose,
-                                                   KDL::JntArray &current_joint_positions) {
+  lbr_fri_idl::msg::LBRJointPositionCommand
+  compute_ik_(const geometry_msgs::msg::Pose &desired_pose,
+              KDL::JntArray &current_joint_positions) {
     KDL::ChainIkSolverPos_LMA ik_solver(chain_);
     KDL::JntArray result_joint_positions = KDL::JntArray(chain_.getNrOfJoints());
-    lbr_fri_idl::msg::LBRPositionCommand joint_position_command;
+    lbr_fri_idl::msg::LBRJointPositionCommand joint_position_command;
 
     // transfer data type 'geometry::msg::Pose' to be 'KDL::Frame'
     KDL::Vector position(desired_pose.position.x, desired_pose.position.y, desired_pose.position.z);

lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst

@@ -1,31 +1,45 @@
 lbr_demos_advanced_py
 =====================
-Collection of advanced usage examples for the ``lbr_ros2_control`` package through Python.
-
 .. warning::
-    Do always execute in ``T1`` mode first.
+    On hardware, do always execute in ``T1`` mode first.
+
+.. contents:: Table of Contents
+   :depth: 2
+   :local:
+   :backlinks: none
 
 Admittance Controller
 ---------------------
 This demo implements a simple admittance controller.
 
-.. warning::
-    May not be well behaved around singularities, put the robot in a well-behaved configuration first, e.g. ``A1 = 0°``, ``A2 = -30°``, ``A3 = 0°``, ``A4 = 60°``, ``A5 = 0°``, ``A6 = -90°``, ``A7 = 0°``. This can be done using the ``smartPAD`` in ``T1`` mode.
+#. Client side configurations:
+
+    #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/lbr_system_parameters.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+
+#. Remote side configurations:
+
+    #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD``
 
-#. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD``
+        .. thumbnail:: ../../doc/img/applications_lbr_server.png
 
-    .. 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:
 
     .. code-block:: bash
 
         ros2 launch lbr_bringup bringup.launch.py \
             sim:=false \
-            ctrl:=lbr_position_command_controller \
+            ctrl:=lbr_joint_position_command_controller \
             model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
 
-#. Run the `admittance_control <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py>`_ with remapping and parameter file:
+#. Run the `admittance_control <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py>`_:octicon:`link-external` with remapping and parameter file:
 
     .. code-block:: bash
 
@@ -39,20 +53,34 @@ Admittance Controller with Remote Center of Motion
 --------------------------------------------------
 This demo implements an admittance controller with a remote center of motion (RCM).
 
-#. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD``
+#. Client side configurations:
+
+    #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/lbr_system_parameters.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/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
 
-    .. thumbnail:: ../../doc/img/applications_lbr_server.png
+        - ``FRI send period``: ``10 ms``
+        - ``IP address``: ``your configuration``
+        - ``FRI control mode``: ``POSITION_CONTROL``
+        - ``FRI client command mode``: ``POSITION``
 
 #. Launch the robot driver:
 
     .. code-block:: bash
 
         ros2 launch lbr_bringup bringup.launch.py \
             sim:=false \
-            ctrl:=lbr_position_command_controller \
+            ctrl:=lbr_joint_position_command_controller \
             model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
 
-#. Run the `admittance_rcm_control <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py>`_ with remapping and parameter file:
+#. Run the `admittance_rcm_control <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py>`_:octicon:`link-external` with remapping and parameter file:
 
     .. code-block:: bash
 

lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py

@@ -59,7 +59,7 @@ def _on_lbr_state(self, lbr_state: LBRState) -> None:
         self._smooth_lbr_state(lbr_state)
 
         lbr_command = self._controller(self._lbr_state, self._dt)
-        self._lbr_position_command_pub.publish(lbr_command)
+        self._lbr_joint_position_command_pub.publish(lbr_command)
 
     def _smooth_lbr_state(self, lbr_state: LBRState) -> None:
         if not self._init: