Merge pull request #317 from lbr-stack/dev-humble-twist-fix

mhubii · web-flow · commit 2e932d4e0629 · 2025-11-30T14:09:21.000Z
Twist clipping fix
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -1,6 +1,10 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package LBR FRI ROS 2 Stack
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Humble v2.2.2 (2025-11-30)
+--------------------------
+* ``lbr_fri_ros2``: Fix the twist clipping bug: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/313
+
 Humble v2.2.1 (2025-03-26)
 --------------------------
 * ``lbr_bringup``: Removed static broadcasters from launch files: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/264
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: 2.2.1
+version: 2.2.2
 doi: 10.21105/joss.06138
-date-released: 2025-03-26
+date-released: 2025-11-30
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>2.2.1</version>
+  <version>2.2.2</version>
   <description>LBR launch files.</description>
   <maintainer email="m.huber_1994@hotmail.de">mhubii</maintainer>
   <license>Apache-2.0</license>
diff --git a/lbr_demos/lbr_demos_advanced_cpp/package.xml b/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>2.2.1</version>
+  <version>2.2.2</version>
   <description>Advanced C++ demos for the lbr_ros2_control.</description>
   <maintainer email="m.huber_1994@hotmail.de">mhubii</maintainer>
   <license>Apache-2.0</license>
diff --git a/lbr_demos/lbr_demos_advanced_py/package.xml b/lbr_demos/lbr_demos_advanced_py/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_py</name>
-  <version>2.2.1</version>
+  <version>2.2.2</version>
   <description>Advanced Python demos for the lbr_ros2_control.</description>
   <maintainer email="m.huber_1994@hotmail.de">mhubii</maintainer>
   <maintainer email="mower.chris@gmail.com">cmower</maintainer>
diff --git a/lbr_demos/lbr_demos_advanced_py/setup.py b/lbr_demos/lbr_demos_advanced_py/setup.py
@@ -6,7 +6,7 @@
 
 setup(
     name=package_name,
-    version="2.2.1",
+    version="2.2.2",
     packages=[package_name],
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_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_cpp</name>
-  <version>2.2.1</version>
+  <version>2.2.2</version>
   <description>C++ demos for lbr_ros2_control.</description>
   <maintainer email="m.huber_1994@hotmail.de">mhubii</maintainer>
   <license>Apache-2.0</license>
diff --git a/lbr_demos/lbr_demos_py/package.xml b/lbr_demos/lbr_demos_py/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_py</name>
-  <version>2.2.1</version>
+  <version>2.2.2</version>
   <description>Python demos for lbr_ros2_control.</description>
   <maintainer email="m.huber_1994@hotmail.de">mhubii</maintainer>
   <license>Apache-2.0</license>
diff --git a/lbr_demos/lbr_demos_py/setup.py b/lbr_demos/lbr_demos_py/setup.py
@@ -4,7 +4,7 @@
 
 setup(
     name=package_name,
-    version="2.2.1",
+    version="2.2.2",
     packages=[package_name],
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
diff --git a/lbr_demos/lbr_moveit/package.xml b/lbr_demos/lbr_moveit/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_moveit</name>
-  <version>2.2.1</version>
+  <version>2.2.2</version>
   <description>MoveIt demos for the LBRs</description>
   <maintainer email="m.huber_1994@hotmail.de">mhubii</maintainer>
   <license>Apache-2.0</license>
diff --git a/lbr_demos/lbr_moveit/setup.py b/lbr_demos/lbr_moveit/setup.py
@@ -4,7 +4,7 @@
 
 setup(
     name=package_name,
-    version="2.2.1",
+    version="2.2.2",
     packages=find_packages(exclude=["test"]),
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
diff --git a/lbr_demos/lbr_moveit_cpp/package.xml b/lbr_demos/lbr_moveit_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_moveit_cpp</name>
-  <version>2.2.1</version>
+  <version>2.2.2</version>
   <description>Demo for using MoveIt C++ API.</description>
   <maintainer email="m.huber_1994@hotmail.de">mhubii</maintainer>
   <license>Apache-2.0</license>
diff --git a/lbr_description/package.xml b/lbr_description/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_description</name>
-  <version>2.2.1</version>
+  <version>2.2.2</version>
   <description>KUKA LBR description files</description>
   <maintainer email="m.huber_1994@hotmail.de">mhubii</maintainer>
   <license>Apache-2.0</license>
diff --git a/lbr_description/ros2_control/lbr_controllers.yaml b/lbr_description/ros2_control/lbr_controllers.yaml
@@ -105,7 +105,7 @@
       chain_tip: lbr_link_ee
       damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian
       max_linear_velocity: 2.0 # maximum linear velocity
-      max_angular_velocity: 2.0 # maximum linear acceleration
+      max_angular_velocity: 2.0 # maximum angular velocity
       joint_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # joint gains
       cartesian_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # cartesian gains
 
@@ -116,9 +116,9 @@
       chain_root: lbr_link_0
       chain_tip: lbr_link_ee
       twist_in_tip_frame: true # if true, the twist command is expressed in the tip frame, otherwise in the root frame
-      damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian
+      damping: 0.02 # damping factor for the pseudo-inverse of the Jacobian
       max_linear_velocity: 0.1 # maximum linear velocity
-      max_angular_velocity: 0.1 # maximum linear acceleration
+      max_angular_velocity: 0.1 # maximum angular velocity
       joint_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # joint gains
       cartesian_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # cartesian gains
     timeout: 0.2 # stop controller if no command is received within this time [s]
diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/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_fri_ros2</name>
-  <version>2.2.1</version>
+  <version>2.2.2</version>
   <description>The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS
     2. Robot states can be extracted and commanded.</description>
   <maintainer email="m.huber_1994@hotmail.de">mhubii</maintainer>
diff --git a/lbr_fri_ros2/src/control.cpp b/lbr_fri_ros2/src/control.cpp
@@ -101,10 +101,10 @@ void AdmittanceImpl::log_info() const {
 
 void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) {
   // clip velocity
-  twist_target_.head(3).unaryExpr([&](double v) {
+  twist_target_.head(3) = twist_target_.head(3).unaryExpr([&](double v) {
     return std::clamp(v, -parameters_.max_linear_velocity, parameters_.max_linear_velocity);
   });
-  twist_target_.tail(3).unaryExpr([&](double v) {
+  twist_target_.tail(3) = twist_target_.tail(3).unaryExpr([&](double v) {
     return std::clamp(v, -parameters_.max_angular_velocity, parameters_.max_angular_velocity);
   });
 
diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/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_fri_ros2_stack</name>
-  <version>2.2.1</version>
+  <version>2.2.2</version>
   <description>ROS 2 stack for KUKA LBRs.</description>
   <maintainer email="m.huber_1994@hotmail.de">mhubii</maintainer>
   <license>Apache-2.0</license>
diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/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_ros2_control</name>
-  <version>2.2.1</version>
+  <version>2.2.2</version>
   <description>ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI).</description>
   <maintainer email="m.huber_1994@hotmail.de">mhubii</maintainer>
   <license>Apache-2.0</license>
