Merge pull request #142 from lbr-stack/dev-humble-v1.4.0

To be addressed in `v1.4.1`: - [ ] LBR wrench command controller (TODO: gpio) - [ ] Depract app.launch.py https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_fri_ros2/launch/app.launch.py - [ ] Gazebo floating base #129 - [ ] `ros2_control` admittance control demo https://github.com/ros-controls/ros2_controllers/tree/master/admittance_controller
lbr-stack · Dec 8, 2023 · 65461c1 · 65461c1
2 parents c10d0a5 + e39b85a
commit 65461c1
Show file tree

Hide file tree

Showing 89 changed files with 2,355 additions and 2,067 deletions.
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
@@ -17,7 +17,7 @@ jobs:
       matrix:
         os: [ubuntu-latest]
     steps:
-      - uses: ros-tooling/setup-ros@v0.3
+      - uses: ros-tooling/setup-ros@v0.7
       - uses: ros-tooling/[email protected]
         with:
           package-name: lbr_fri_ros2_stack

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -0,0 +1,88 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package LBR FRI ROS 2 Stack
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Humble v1.4.0 (2023-12-08)
+--------------------------
+* The general goal of this release is a tighter ``ros2_control`` integration. The ``lbr_bringup``
+  will serve as single entry point in the future. For now, ``app_component`` and ``app.launch.py`` are kept
+* Changes to ``lbr_fri_ros2``:
+
+  * Removes logging / parameter interfaces from ``lbr_fri_ros2`` (so ``lbr_ros2_control`` serves as single interaction point)
+  * Updates legacy ``app_component`` in ``lbr_fri_ros2`` for changes. To be depracted in the future
+  * Adds force-torque estimator to ``lbr_fri_ros2``
+* Changes to ``lbr_ros2_control``:
+
+  * Removes now redundant node from ``lbr_ros2_control``
+  * Adds forward position and forward torque controllers to ``lbr_ros2_control``
+  * Removes estimated force-torque broadcaster from ``lbr_ros2_control`` in favor of ``ros2_control`` default implementation
+
+    * Force-torque now available under ``/lbr/force_torque_broadcaster/wrench`` 
+    * Namespace issues since ``lbr_controllers.yaml`` includes namespace in ``frame_id`` parameter
+  * Adds ``lbr_fri_ros2`` force-torque estimator to ``lbr_ros2_control`` as sensor
+  * Adds configurations to ``lbr_system_interface.xacro``
+  * Simplifies ``lbr_ros2_control`` class names
+* ``/lbr/command/position`` topic now under ``/lbr/command/joint_position``
+* Adds this changelog with release notes
+* Refers to https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/142
+
+Humble v1.3.1 (2023-11-21)
+--------------------------
+* v1.3.0 Gazebo namespace fixes in https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/123
+* Fix iiwa ee link in https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/126
+* Humble v.1.3.1 in https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/130
+* Full log: https://github.com/lbr-stack/lbr_fri_ros2_stack/compare/humble-v1.3.0-beta...humble-v1.3.1
+
+Humble v1.3.0 beta (2023-10-03)
+-------------------------------
+* Namespaced robot_description and joint_states
+* De-coupled commands, user will interact through LBRPositionCommand, LBRTorqueCommand, LBRWrenchCommand
+* Multi-robot support
+* New command / state interfaces in lbr_fri_ros2
+* Topic free ros2_control support through command / state interfaces in lbr_fri_ros2
+* Intraprocess cpp admittance demo
+* New app component based on command / state interfaces in lbr_fri_ros2
+* Refers to https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/121
+
+Humble v1.2.5 (2023-09-08)
+--------------------------
+* Updated visualization (STL -> DAE files with materials, might occur dark in Gazebo, caused by lack of light)
+* Fixes joint bug in Gazebo
+* Improved logging in command guard
+
+Humble v1.2.4 (2023-08-09)
+--------------------------
+* Remove robot name from configs and use frame_prefix from robot state publisher instead
+* Removed robot name from joint names, e.g. lbr_A1 -> A1
+* Added PID for asynchronous control rate
+* Simplified class names, e.g. LBRApp -> App
+* Add utils.hpp for PID and exponential filter
+
+Humble v1.2.3 (2023-08-07)
+--------------------------
+* Utilizes FRI through vendor package for common fri source in https://github.com/lbr-stack/
+* Addresses some of https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/85
+* Give command guard only logger interface
+* Fix open loop bug
+* Adds real-time priority via rt_prio parameter
+
+Humble v1.2.2 (2023-08-05)
+--------------------------
+* Adds base frame parameter to URDF and launch
+* Adds an open loop option to control the robot, which works extremely well
+* Updates logo in readme
+* Updates joint names to KUKA convention, i.e. A1,...
+
+Humble v1.2.1 (2023-08-04)
+--------------------------
+* Stack's new home at: https://github.com/lbr-stack
+
+Humble v1.2.0 (2023-08-03)
+--------------------------
+* Re-introduces MoveIt, refer to https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/52
+* Moves demo prefix to front for improved package overview
+* Single node for hardware interface
+* Static executors where possible
+* Adds plenty documentation
+* Introduce /lbr, i.e. robot name, namespace to LBRClient for better multi-robot support. Commands / states now e.g. published to /lbr/command / /lbr/state
+* Hardware interface exact limits (stand-alone use has safety-limits)
+* Gives command guard a node handle
diff --git a/CITATION.cff b/CITATION.cff
@@ -19,6 +19,6 @@ authors:
 
 
 title: "LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots"
-version: 1.3.1
+version: 1.4.0
 doi: 10.48550/arXiv.2311.12709
-date-released: 2023-11-22
+date-released: 2023-12-08
diff --git a/LICENSE b/LICENSE
@@ -1,6 +1,6 @@
 MIT License
 
-Copyright (c) 2021 Martin Huber
+Copyright (c) 2023 Martin Huber
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal

diff --git a/lbr_bringup/config/config.rviz b/lbr_bringup/config/config.rviz
@@ -8,8 +8,9 @@ Panels:
         - /Status1
         - /RobotModel1
         - /RobotModel1/Description Topic1
-      Splitter Ratio: 0.5
-    Tree Height: 724
+        - /Wrench1
+      Splitter Ratio: 0.40760868787765503
+    Tree Height: 717
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -123,6 +124,25 @@ Visualization Manager:
       Update Interval: 0
       Value: true
       Visual Enabled: true
+    - Accept NaN Values: false
+      Alpha: 1
+      Arrow Width: 0.20000000298023224
+      Class: rviz_default_plugins/Wrench
+      Enabled: true
+      Force Arrow Scale: 0.10000000149011612
+      Force Color: 0; 85; 255
+      History Length: 1
+      Name: Wrench
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /lbr/force_torque_broadcaster/wrench
+      Torque Arrow Scale: 0.30000001192092896
+      Torque Color: 170; 85; 255
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 21; 21; 26
@@ -195,7 +215,7 @@ Window Geometry:
   Height: 1021
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000015b0000035ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c40000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000002120000035bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000035b000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f0000035b000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002ed00fffffffb0000000800540069006d00650100000000000004500000000000000000000005220000035b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst
@@ -57,7 +57,7 @@ and select:
 
 Make sure that the ``update_rate`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/humble/lbr_ros2_control/config/lbr_controllers.yaml>`_ 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/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_description/ros2_control/lbr.ros2_control.xacro>`_ (automated in the future).
+For using other ``FRI send period``, also change the ``sample_time`` in the `lbr_system_interface.xacro <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/lbr_system_interface.xacro>`_ (automated in the future).
 
 Standalone Launch
 -----------------

diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py
@@ -29,12 +29,12 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
     joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
         controller="joint_state_broadcaster"
     )
+    force_torque_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
+        controller="force_torque_broadcaster"
+    )
     lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
         controller="lbr_state_broadcaster"
     )
-    lbr_estimated_ft_broadcast = LBRROS2ControlMixin.node_controller_spawner(
-        controller="lbr_estimated_ft_broadcaster"
-    )
     controller = LBRROS2ControlMixin.node_controller_spawner(
         controller=LaunchConfiguration("ctrl")
     )
@@ -44,8 +44,8 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
             target_action=ros2_control_node,
             on_start=[
                 joint_state_broadcaster,
+                force_torque_broadcaster,
                 lbr_state_broadcaster,
-                lbr_estimated_ft_broadcast,
                 controller,
             ],
         )

diff --git a/lbr_bringup/package.xml b/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.3.1</version>
+  <version>1.4.0</version>
   <description>LBR launch files.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>

diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt
@@ -11,8 +11,6 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(eigen3_cmake_module REQUIRED)
-find_package(Eigen3 REQUIRED)
 find_package(fri_vendor REQUIRED)
 find_package(FRIClient REQUIRED)
 find_package(kdl_parser REQUIRED)
@@ -34,7 +32,6 @@ target_include_directories(admittance_control_component
 
 ament_target_dependencies(
   admittance_control_component
-  Eigen3
   kdl_parser
   lbr_fri_ros2
   lbr_fri_msgs

diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml
@@ -2,17 +2,15 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_demos_fri_ros2_advanced_cpp</name>
-  <version>1.3.1</version>
+  <version>1.4.0</version>
   <description>Advanced C++ demos for the lbr_fri_ros2.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
   <exec_depend>lbr_description</exec_depend>
 
-  <depend>eigen</depend>
   <depend>fri_vendor</depend>
   <depend>kdl_parser</depend>
   <depend>lbr_fri_ros2</depend>

diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp
@@ -8,7 +8,6 @@
 #include "lbr_fri_ros2/app.hpp"
 
 #include "admittance_controller.hpp"
-#include "damped_least_squares.hpp"
 
 namespace lbr_fri_ros2 {
 class AdmittanceControlNode : public rclcpp::Node {
@@ -31,7 +30,7 @@ class AdmittanceControlNode : public rclcpp::Node {
                                                this->get_parameter("dx_gains").as_double_array());
 
     lbr_position_command_pub_ =
-        create_publisher<lbr_fri_msgs::msg::LBRPositionCommand>("/lbr/command/position", 1);
+        create_publisher<lbr_fri_msgs::msg::LBRPositionCommand>("/lbr/command/joint_position", 1);
     lbr_state_sub_ = create_subscription<lbr_fri_msgs::msg::LBRState>(
         "/lbr/state", 1,
         std::bind(&AdmittanceControlNode::on_lbr_state, this, std::placeholders::_1));

diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp
@@ -14,8 +14,7 @@
 
 #include "lbr_fri_msgs/msg/lbr_position_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
-
-#include "damped_least_squares.hpp"
+#include "lbr_fri_ros2/pinv.hpp"
 
 namespace lbr_fri_ros2 {
 class AdmittanceController {
@@ -51,7 +50,7 @@ class AdmittanceController {
 
     jacobian_solver_->JntToJac(q_, jacobian_);
 
-    jacobian_inv_ = damped_least_squares(jacobian_.data);
+    jacobian_inv_ = pinv(jacobian_.data);
     f_ext_ = jacobian_inv_.transpose() * tau_ext_;
 
     for (int i = 0; i < 6; i++) {

diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/damped_least_squares.hpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/damped_least_squares.hpp
diff --git a/...os_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py b/...os_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py
@@ -30,7 +30,9 @@ def __init__(self, node_name="admittance_control_node") -> None:
             LBRState, "/lbr/state", self.on_lbr_state_, 1
         )
         self.lbr_position_command_pub_ = self.create_publisher(
-            LBRPositionCommand, "/lbr/command/position", 1
+            LBRPositionCommand,
+            "/lbr/command/joint_position",
+            1,
         )
 
     def on_lbr_state_(self, lbr_state: LBRState) -> None:

diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml b/lbr_demos/lbr_demos_fri_ros2_advanced_python/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_fri_ros2_advanced_python</name>
-  <version>1.3.1</version>
+  <version>1.4.0</version>
   <description>Advanced Python demos for the lbr_fri_ros2.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>

diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py
@@ -6,7 +6,7 @@
 
 setup(
     name=package_name,
-    version="1.3.1",
+    version="1.4.0",
     packages=[package_name],
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),

diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml b/lbr_demos/lbr_demos_fri_ros2_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_fri_ros2_cpp</name>
-  <version>1.3.1</version>
+  <version>1.4.0</version>
   <description>C++ demos for the lbr_fri_ros2.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>

diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp b/lbr_demos/lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp
@@ -18,9 +18,9 @@ class JointSineOverlayNode : public rclcpp::Node {
 
 public:
   JointSineOverlayNode(const std::string &node_name) : Node(node_name), phase_(0.) {
-    // create publisher to /lbr/command/position
-    lbr_position_command_pub_ =
-        this->create_publisher<lbr_fri_msgs::msg::LBRPositionCommand>("/lbr/command/position", 1);
+    // create publisher to /lbr/command/joint_position
+    lbr_position_command_pub_ = this->create_publisher<lbr_fri_msgs::msg::LBRPositionCommand>(
+        "/lbr/command/joint_position", 1);
 
     // create subscription to /lbr/state
     lbr_state_sub_ = this->create_subscription<lbr_fri_msgs::msg::LBRState>(

diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py
@@ -16,9 +16,11 @@ def __init__(self, node_name: str) -> None:
         self.phase_ = 0.0
         self.lbr_position_command_ = LBRPositionCommand()
 
-        # create publisher to /lbr/command/position
+        # create publisher to /lbr/command/joint_position
         self.lbr_position_command_pub_ = self.create_publisher(
-            LBRPositionCommand, "/lbr/command/position", 1
+            LBRPositionCommand,
+            "/lbr/command/joint_position",
+            1,
         )
 
         # create subscription to /lbr_state

diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py
@@ -4,7 +4,7 @@
 from rclpy.node import Node
 
 # import lbr_fri_msgs
-from lbr_fri_msgs.msg import LBRTorqueCommand, LBRState
+from lbr_fri_msgs.msg import LBRState, LBRTorqueCommand
 
 
 class TorqueSineOverlayNode(Node):

diff --git a/lbr_demos/lbr_demos_fri_ros2_python/package.xml b/lbr_demos/lbr_demos_fri_ros2_python/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_fri_ros2_python</name>
-  <version>1.3.1</version>
+  <version>1.4.0</version>
   <description>Python demos for the lbr_fri_ros2.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>

diff --git a/lbr_demos/lbr_demos_fri_ros2_python/setup.py b/lbr_demos/lbr_demos_fri_ros2_python/setup.py
@@ -6,7 +6,7 @@
 
 setup(
     name=package_name,
-    version="1.3.1",
+    version="1.4.0",
     packages=[package_name],
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),