From f300c84e3322e9af4f2be88c0d3d3edcbc5afb44 Mon Sep 17 00:00:00 2001
From: domrachev03 <domrachev03@users.noreply.github.com>
Date: Fri, 20 Mar 2026 21:22:45 +0900
Subject: [PATCH 1/9] feat: add runtime variable stiffness support via ROS2
 topic

Add ability to update Cartesian stiffness at runtime through a
std_msgs/Float64MultiArray topic (6 elements: diagonal stiffness).
Gated behind variable_stiffness.enabled parameter (default: false).
Uses RealtimeBuffer pattern consistent with existing target_wrench.
Damping remains parameter-controlled only.
---
 CMakeLists.txt                                |  3 +
 .../cartesian_controller.hpp                  | 15 +++++
 package.xml                                   |  1 +
 src/cartesian_controller.cpp                  | 57 ++++++++++++++++++-
 src/cartesian_controller.yaml                 | 10 ++++
 5 files changed, 83 insertions(+), 3 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index fc56aa9..3e2c731 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -60,6 +60,7 @@ find_package(hardware_interface REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(pinocchio REQUIRED)
 find_package(realtime_tools REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(generate_parameter_library REQUIRED)
 
 generate_parameter_library(
@@ -129,6 +130,7 @@ ament_target_dependencies(${PROJECT_NAME}
     pinocchio
     generate_parameter_library
     realtime_tools
+    std_msgs
 )
 
 pluginlib_export_plugin_description_file(
@@ -156,6 +158,7 @@ ament_export_dependencies(
     Eigen3
     pinocchio
     realtime_tools
+    std_msgs
     generate_parameter_library
 )
 
diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp
index ab3fbbf..32df937 100644
--- a/include/crisp_controllers/cartesian_controller.hpp
+++ b/include/crisp_controllers/cartesian_controller.hpp
@@ -14,6 +14,7 @@
 #include <controller_interface/controller_interface.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
 #include <pinocchio/multibody/fwd.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -102,6 +103,8 @@ class CartesianController : public controller_interface::ControllerInterface {
   rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_sub_;
   /** @brief Subscription for target wrench messages */
   rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_sub_;
+  /** @brief Subscription for variable stiffness messages */
+  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr stiffness_sub_;
 
   /** @brief Flag to indicate if multiple publishers detected */
   bool multiple_publishers_detected_;
@@ -135,9 +138,16 @@ class CartesianController : public controller_interface::ControllerInterface {
    */
   void parse_target_wrench_();
 
+  /**
+   * @brief Reads the target stiffness in realtime loop from the buffer and parses it to be used in the controller.
+   */
+  void parse_target_stiffness_();
+
   bool new_target_pose_;
   bool new_target_joint_;
   bool new_target_wrench_;
+  bool new_target_stiffness_ = false;
+  bool use_topic_stiffness_ = false;
 
   realtime_tools::RealtimeBuffer<std::shared_ptr<geometry_msgs::msg::PoseStamped>>
     target_pose_buffer_;
@@ -148,6 +158,9 @@ class CartesianController : public controller_interface::ControllerInterface {
   realtime_tools::RealtimeBuffer<std::shared_ptr<geometry_msgs::msg::WrenchStamped>>
     target_wrench_buffer_;
 
+  realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>>
+    target_stiffness_buffer_;
+
   /** @brief Target position in Cartesian space */
   Eigen::Vector3d target_position_;
   /** @brief Target orientation as quaternion */
@@ -176,6 +189,8 @@ class CartesianController : public controller_interface::ControllerInterface {
   Eigen::MatrixXd stiffness = Eigen::MatrixXd::Zero(6, 6);
   /** @brief Cartesian damping matrix (6x6) */
   Eigen::MatrixXd damping = Eigen::MatrixXd::Zero(6, 6);
+  /** @brief Topic-provided stiffness matrix (6x6 diagonal) */
+  Eigen::Matrix<double, 6, 6> topic_stiffness_ = Eigen::Matrix<double, 6, 6>::Zero();
 
   /** @brief Nullspace stiffness matrix for posture control */
   Eigen::MatrixXd nullspace_stiffness;
diff --git a/package.xml b/package.xml
index c85aa21..b263bd8 100644
--- a/package.xml
+++ b/package.xml
@@ -16,6 +16,7 @@
   <depend>pinocchio</depend>
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
   <depend>pluginlib</depend>
   <depend>realtime_tools</depend>
 
diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp
index c16a7c4..d890ed5 100644
--- a/src/cartesian_controller.cpp
+++ b/src/cartesian_controller.cpp
@@ -18,6 +18,8 @@
 
 #include "pinocchio/algorithm/model.hpp"
 
+#include <std_msgs/msg/float64_multi_array.hpp>
+
 #include <crisp_controllers/cartesian_controller.hpp>
 #include <crisp_controllers/pch.hpp>
 #include <crisp_controllers/utils/friction_model.hpp>
@@ -79,6 +81,10 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration &
     parse_target_wrench_();
     new_target_wrench_ = false;
   }
+  if (new_target_stiffness_) {
+    parse_target_stiffness_();
+    new_target_stiffness_ = false;
+  }
 
   pinocchio::forwardKinematics(model_, data_, q_pin, dq);
   pinocchio::updateFramePlacements(model_, data_);
@@ -318,6 +324,8 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
   new_target_pose_ = false;
   new_target_joint_ = false;
   new_target_wrench_ = false;
+  new_target_stiffness_ = false;
+  use_topic_stiffness_ = false;
 
   multiple_publishers_detected_ = false;
   max_allowed_publishers_ = 1;
@@ -373,6 +381,28 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
   wrench_sub_ = get_node()->create_subscription<geometry_msgs::msg::WrenchStamped>(
     "target_wrench", rclcpp::QoS(1), target_wrench_callback);
 
+  if (params_.variable_stiffness.enabled) {
+    auto target_stiffness_callback =
+      [this](const std::shared_ptr<std_msgs::msg::Float64MultiArray> msg) -> void {
+      if (!check_topic_publisher_count(params_.variable_stiffness.topic)) {
+        RCLCPP_WARN_THROTTLE(
+          get_node()->get_logger(),
+          *get_node()->get_clock(),
+          1000,
+          "Ignoring target_stiffness message due to multiple publishers detected!");
+        return;
+      }
+      target_stiffness_buffer_.writeFromNonRT(msg);
+      new_target_stiffness_ = true;
+    };
+
+    stiffness_sub_ = get_node()->create_subscription<std_msgs::msg::Float64MultiArray>(
+      params_.variable_stiffness.topic, rclcpp::QoS(1), target_stiffness_callback);
+
+    RCLCPP_INFO(get_node()->get_logger(), "Variable stiffness enabled on topic: %s",
+      params_.variable_stiffness.topic.c_str());
+  }
+
   // Initialize all control vectors with appropriate dimensions
   tau_task = Eigen::VectorXd::Zero(model_.nv);
   tau_joint_limits = Eigen::VectorXd::Zero(model_.nv);
@@ -431,9 +461,13 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
 }
 
 void CartesianController::setStiffnessAndDamping() {
-  stiffness.setZero();
-  stiffness.diagonal() << params_.task.k_pos_x, params_.task.k_pos_y, params_.task.k_pos_z,
-    params_.task.k_rot_x, params_.task.k_rot_y, params_.task.k_rot_z;
+  if (use_topic_stiffness_) {
+    stiffness = topic_stiffness_;
+  } else {
+    stiffness.setZero();
+    stiffness.diagonal() << params_.task.k_pos_x, params_.task.k_pos_y, params_.task.k_pos_z,
+      params_.task.k_rot_x, params_.task.k_rot_y, params_.task.k_rot_z;
+  }
 
   damping.setZero();
   // For each axis, use explicit damping if > 0, otherwise compute from stiffness
@@ -559,6 +593,23 @@ void CartesianController::parse_target_wrench_() {
     msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z;
 }
 
+void CartesianController::parse_target_stiffness_() {
+  auto msg = *target_stiffness_buffer_.readFromRT();
+  if (msg->data.size() != 6) {
+    RCLCPP_WARN_THROTTLE(
+      get_node()->get_logger(),
+      *get_node()->get_clock(),
+      1000,
+      "Variable stiffness message must have exactly 6 elements, got %zu. Ignoring.",
+      msg->data.size());
+    return;
+  }
+  topic_stiffness_.setZero();
+  topic_stiffness_.diagonal() << msg->data[0], msg->data[1], msg->data[2],
+    msg->data[3], msg->data[4], msg->data[5];
+  use_topic_stiffness_ = true;
+}
+
 void CartesianController::log_debug_info(const rclcpp::Time & time) {
   if (!params_.log.enabled) {
     return;
diff --git a/src/cartesian_controller.yaml b/src/cartesian_controller.yaml
index b59eb26..53fd6ea 100644
--- a/src/cartesian_controller.yaml
+++ b/src/cartesian_controller.yaml
@@ -313,6 +313,16 @@ cartesian_controller:
       default_value: [ 0.039533, 0.025882, -0.04607, 0.036194, 0.026226, -0.021047, 0.0035526]
       description: "Friction parameters part 3"
 
+  variable_stiffness:
+    enabled:
+      type: bool
+      default_value: false
+      description: "Enable receiving stiffness updates via a ROS2 topic. When enabled, stiffness values from the topic override parameter-based stiffness."
+    topic:
+      type: string
+      default_value: "target_stiffness"
+      description: "Topic name for receiving variable stiffness updates (std_msgs/Float64MultiArray with 6 elements: [k_pos_x, k_pos_y, k_pos_z, k_rot_x, k_rot_y, k_rot_z])"
+
   enable_introspection:
     type: bool
     default_value: false

From b755ac6e2cba49c7db7f8b76bdf274bec61fb853 Mon Sep 17 00:00:00 2001
From: domrachev03 <domrachev03@users.noreply.github.com>
Date: Sat, 21 Mar 2026 16:28:14 +0900
Subject: [PATCH 2/9] fix: adapt to ROS2 Jazzy

---
 src/cartesian_controller.cpp       | 2 +-
 src/pose_broadcaster.cpp           | 2 +-
 src/torque_feedback_controller.cpp | 4 ++--
 src/twist_broadcaster.cpp          | 2 +-
 4 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp
index d890ed5..c401c84 100644
--- a/src/cartesian_controller.cpp
+++ b/src/cartesian_controller.cpp
@@ -503,7 +503,7 @@ void CartesianController::updateCurrentState(bool initialize) {
     auto joint_id = model_.getJointId(joint_name);  // pinocchio joind id might be different
     auto joint = model_.joints[joint_id];
 
-#if ROS2_VERSION_ABOVE_HUMBLE
+#if ROS2_VERSION_ABOVE_JAZZY
     double q_meas = state_interfaces_[i].get_optional().value_or(q[i]);
     double dq_meas = state_interfaces_[num_joints + i].get_optional().value_or(dq[i]);
 #else
diff --git a/src/pose_broadcaster.cpp b/src/pose_broadcaster.cpp
index c09dc39..1cf82fa 100644
--- a/src/pose_broadcaster.cpp
+++ b/src/pose_broadcaster.cpp
@@ -45,7 +45,7 @@ PoseBroadcaster::update(const rclcpp::Time & time, const rclcpp::Duration & peri
     auto joint_id = model_.getJointId(joint_name);
     auto joint = model_.joints[joint_id];
 
-#if ROS2_VERSION_ABOVE_HUMBLE
+#if ROS2_VERSION_ABOVE_JAZZY
     q[i] = state_interfaces_[i].get_optional().value_or(q[i]);
 #else
     q[i] = state_interfaces_[i].get_value();
diff --git a/src/torque_feedback_controller.cpp b/src/torque_feedback_controller.cpp
index cb55c55..0c209e6 100644
--- a/src/torque_feedback_controller.cpp
+++ b/src/torque_feedback_controller.cpp
@@ -50,7 +50,7 @@ controller_interface::return_type TorqueFeedbackController::update(
   const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
   // Update joint states
   for (int i = 0; i < num_joints_; i++) {
-#if ROS2_VERSION_ABOVE_HUMBLE
+#if ROS2_VERSION_ABOVE_JAZZY
     q_[i] = state_interfaces_[i].get_optional().value_or(q_[i]);
     dq_[i] = state_interfaces_[num_joints_ + i].get_optional().value_or(dq_[i]);
 #else
@@ -261,7 +261,7 @@ TorqueFeedbackController::on_configure(const rclcpp_lifecycle::State & /*previou
 CallbackReturn
 TorqueFeedbackController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
   for (int i = 0; i < num_joints_; i++) {
-#if ROS2_VERSION_ABOVE_HUMBLE
+#if ROS2_VERSION_ABOVE_JAZZY
     q_[i] = state_interfaces_[i].get_optional().value_or(q_[i]);
     dq_[i] = state_interfaces_[num_joints_ + i].get_optional().value_or(dq_[i]);
 #else
diff --git a/src/twist_broadcaster.cpp b/src/twist_broadcaster.cpp
index 37c46ce..953bacb 100644
--- a/src/twist_broadcaster.cpp
+++ b/src/twist_broadcaster.cpp
@@ -47,7 +47,7 @@ TwistBroadcaster::update(const rclcpp::Time & time, const rclcpp::Duration & per
     auto joint_id = model_.getJointId(joint_name);
     auto joint = model_.joints[joint_id];
 
-#if ROS2_VERSION_ABOVE_HUMBLE
+#if ROS2_VERSION_ABOVE_JAZZY
     q[i] = state_interfaces_[i * 2].get_optional().value_or(q[i]);
     q_dot[i] = state_interfaces_[i * 2 + 1].get_optional().value_or(q_dot[i]);
 #else

From 890289aa81b526982e816c48ccdedf0b4415d9fa Mon Sep 17 00:00:00 2001
From: domrachev03 <domrachev03@users.noreply.github.com>
Date: Sat, 21 Mar 2026 17:05:30 +0900
Subject: [PATCH 3/9] feat: add INFO logging for variable stiffness debugging

---
 src/cartesian_controller.cpp | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp
index c401c84..794479a 100644
--- a/src/cartesian_controller.cpp
+++ b/src/cartesian_controller.cpp
@@ -463,10 +463,18 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
 void CartesianController::setStiffnessAndDamping() {
   if (use_topic_stiffness_) {
     stiffness = topic_stiffness_;
+    RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 2000,
+      "Using TOPIC stiffness: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]",
+      stiffness(0,0), stiffness(1,1), stiffness(2,2),
+      stiffness(3,3), stiffness(4,4), stiffness(5,5));
   } else {
     stiffness.setZero();
     stiffness.diagonal() << params_.task.k_pos_x, params_.task.k_pos_y, params_.task.k_pos_z,
       params_.task.k_rot_x, params_.task.k_rot_y, params_.task.k_rot_z;
+    RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 2000,
+      "Using PARAM stiffness: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]",
+      stiffness(0,0), stiffness(1,1), stiffness(2,2),
+      stiffness(3,3), stiffness(4,4), stiffness(5,5));
   }
 
   damping.setZero();
@@ -608,6 +616,10 @@ void CartesianController::parse_target_stiffness_() {
   topic_stiffness_.diagonal() << msg->data[0], msg->data[1], msg->data[2],
     msg->data[3], msg->data[4], msg->data[5];
   use_topic_stiffness_ = true;
+  RCLCPP_INFO(get_node()->get_logger(),
+    "Variable stiffness received: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]",
+    msg->data[0], msg->data[1], msg->data[2],
+    msg->data[3], msg->data[4], msg->data[5]);
 }
 
 void CartesianController::log_debug_info(const rclcpp::Time & time) {

From 449ed0004aa6c131dd1ea98b2741744e3ed7eb05 Mon Sep 17 00:00:00 2001
From: domrachev03 <domrachev03@users.noreply.github.com>
Date: Sat, 21 Mar 2026 17:10:26 +0900
Subject: [PATCH 4/9] fix: always create variable stiffness subscriber
 regardless of enabled param

---
 src/cartesian_controller.cpp | 40 +++++++++++++++++-------------------
 1 file changed, 19 insertions(+), 21 deletions(-)

diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp
index 794479a..6c692c6 100644
--- a/src/cartesian_controller.cpp
+++ b/src/cartesian_controller.cpp
@@ -381,27 +381,25 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
   wrench_sub_ = get_node()->create_subscription<geometry_msgs::msg::WrenchStamped>(
     "target_wrench", rclcpp::QoS(1), target_wrench_callback);
 
-  if (params_.variable_stiffness.enabled) {
-    auto target_stiffness_callback =
-      [this](const std::shared_ptr<std_msgs::msg::Float64MultiArray> msg) -> void {
-      if (!check_topic_publisher_count(params_.variable_stiffness.topic)) {
-        RCLCPP_WARN_THROTTLE(
-          get_node()->get_logger(),
-          *get_node()->get_clock(),
-          1000,
-          "Ignoring target_stiffness message due to multiple publishers detected!");
-        return;
-      }
-      target_stiffness_buffer_.writeFromNonRT(msg);
-      new_target_stiffness_ = true;
-    };
-
-    stiffness_sub_ = get_node()->create_subscription<std_msgs::msg::Float64MultiArray>(
-      params_.variable_stiffness.topic, rclcpp::QoS(1), target_stiffness_callback);
-
-    RCLCPP_INFO(get_node()->get_logger(), "Variable stiffness enabled on topic: %s",
-      params_.variable_stiffness.topic.c_str());
-  }
+  auto target_stiffness_callback =
+    [this](const std::shared_ptr<std_msgs::msg::Float64MultiArray> msg) -> void {
+    if (!check_topic_publisher_count(params_.variable_stiffness.topic)) {
+      RCLCPP_WARN_THROTTLE(
+        get_node()->get_logger(),
+        *get_node()->get_clock(),
+        1000,
+        "Ignoring target_stiffness message due to multiple publishers detected!");
+      return;
+    }
+    target_stiffness_buffer_.writeFromNonRT(msg);
+    new_target_stiffness_ = true;
+  };
+
+  stiffness_sub_ = get_node()->create_subscription<std_msgs::msg::Float64MultiArray>(
+    params_.variable_stiffness.topic, rclcpp::QoS(1), target_stiffness_callback);
+
+  RCLCPP_INFO(get_node()->get_logger(), "Variable stiffness topic: %s",
+    params_.variable_stiffness.topic.c_str());
 
   // Initialize all control vectors with appropriate dimensions
   tau_task = Eigen::VectorXd::Zero(model_.nv);

From e8434d3de862b1259d642bedb400ede22c7f8c90 Mon Sep 17 00:00:00 2001
From: domrachev03 <domrachev03@users.noreply.github.com>
Date: Fri, 27 Mar 2026 22:32:16 +0900
Subject: [PATCH 5/9] fix: bound stiffness values

---
 src/cartesian_controller.cpp  | 40 +++++++++++++++++++++++++++++++++--
 src/cartesian_controller.yaml | 14 ++++++++++++
 2 files changed, 52 insertions(+), 2 deletions(-)

diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp
index 6c692c6..6ef740f 100644
--- a/src/cartesian_controller.cpp
+++ b/src/cartesian_controller.cpp
@@ -1,4 +1,5 @@
 
+#include <algorithm>
 #include <cmath>
 #include <cstddef>
 
@@ -475,6 +476,24 @@ void CartesianController::setStiffnessAndDamping() {
       stiffness(3,3), stiffness(4,4), stiffness(5,5));
   }
 
+  // Clamp stiffness to [0, max_stiffness]
+  const double max_k_trans = params_.max_stiffness.translational;
+  const double max_k_rot = params_.max_stiffness.rotational;
+  for (int i = 0; i < 3; ++i) {
+    if (stiffness(i, i) < 0.0 || stiffness(i, i) > max_k_trans) {
+      RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
+        "Translational stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, stiffness(i, i), max_k_trans);
+      stiffness(i, i) = std::clamp(stiffness(i, i), 0.0, max_k_trans);
+    }
+  }
+  for (int i = 3; i < 6; ++i) {
+    if (stiffness(i, i) < 0.0 || stiffness(i, i) > max_k_rot) {
+      RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
+        "Rotational stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, stiffness(i, i), max_k_rot);
+      stiffness(i, i) = std::clamp(stiffness(i, i), 0.0, max_k_rot);
+    }
+  }
+
   damping.setZero();
   // For each axis, use explicit damping if > 0, otherwise compute from stiffness
   damping.diagonal()
@@ -610,9 +629,26 @@ void CartesianController::parse_target_stiffness_() {
       msg->data.size());
     return;
   }
+  const double max_k_trans = params_.max_stiffness.translational;
+  const double max_k_rot = params_.max_stiffness.rotational;
+  std::array<double, 6> vals = {msg->data[0], msg->data[1], msg->data[2],
+                                msg->data[3], msg->data[4], msg->data[5]};
+  for (int i = 0; i < 3; ++i) {
+    if (vals[i] < 0.0 || vals[i] > max_k_trans) {
+      RCLCPP_WARN(get_node()->get_logger(),
+        "Topic stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, vals[i], max_k_trans);
+      vals[i] = std::clamp(vals[i], 0.0, max_k_trans);
+    }
+  }
+  for (int i = 3; i < 6; ++i) {
+    if (vals[i] < 0.0 || vals[i] > max_k_rot) {
+      RCLCPP_WARN(get_node()->get_logger(),
+        "Topic stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, vals[i], max_k_rot);
+      vals[i] = std::clamp(vals[i], 0.0, max_k_rot);
+    }
+  }
   topic_stiffness_.setZero();
-  topic_stiffness_.diagonal() << msg->data[0], msg->data[1], msg->data[2],
-    msg->data[3], msg->data[4], msg->data[5];
+  topic_stiffness_.diagonal() << vals[0], vals[1], vals[2], vals[3], vals[4], vals[5];
   use_topic_stiffness_ = true;
   RCLCPP_INFO(get_node()->get_logger(),
     "Variable stiffness received: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]",
diff --git a/src/cartesian_controller.yaml b/src/cartesian_controller.yaml
index 53fd6ea..751928d 100644
--- a/src/cartesian_controller.yaml
+++ b/src/cartesian_controller.yaml
@@ -313,6 +313,20 @@ cartesian_controller:
       default_value: [ 0.039533, 0.025882, -0.04607, 0.036194, 0.026226, -0.021047, 0.0035526]
       description: "Friction parameters part 3"
 
+  max_stiffness:
+    translational:
+      type: double
+      default_value: 900.0
+      description: "Maximum allowed translational stiffness value. Stiffness values above this will be clamped."
+      validation:
+        bounds<>: [0.0, 5000.0]
+    rotational:
+      type: double
+      default_value: 60.0
+      description: "Maximum allowed rotational stiffness value. Stiffness values above this will be clamped."
+      validation:
+        bounds<>: [0.0, 100.0]
+
   variable_stiffness:
     enabled:
       type: bool

From c412d605d096833794e3b8cae646753328e73b19 Mon Sep 17 00:00:00 2001
From: domrachev03 <domrachev03@users.noreply.github.com>
Date: Mon, 30 Mar 2026 16:10:15 +0900
Subject: [PATCH 6/9] Revert "fix: adapt to ROS2 Jazzy"

This reverts commit b755ac6e2cba49c7db7f8b76bdf274bec61fb853.
---
 src/cartesian_controller.cpp       | 2 +-
 src/pose_broadcaster.cpp           | 2 +-
 src/torque_feedback_controller.cpp | 4 ++--
 src/twist_broadcaster.cpp          | 2 +-
 4 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp
index 6ef740f..b94500c 100644
--- a/src/cartesian_controller.cpp
+++ b/src/cartesian_controller.cpp
@@ -528,7 +528,7 @@ void CartesianController::updateCurrentState(bool initialize) {
     auto joint_id = model_.getJointId(joint_name);  // pinocchio joind id might be different
     auto joint = model_.joints[joint_id];
 
-#if ROS2_VERSION_ABOVE_JAZZY
+#if ROS2_VERSION_ABOVE_HUMBLE
     double q_meas = state_interfaces_[i].get_optional().value_or(q[i]);
     double dq_meas = state_interfaces_[num_joints + i].get_optional().value_or(dq[i]);
 #else
diff --git a/src/pose_broadcaster.cpp b/src/pose_broadcaster.cpp
index 1cf82fa..c09dc39 100644
--- a/src/pose_broadcaster.cpp
+++ b/src/pose_broadcaster.cpp
@@ -45,7 +45,7 @@ PoseBroadcaster::update(const rclcpp::Time & time, const rclcpp::Duration & peri
     auto joint_id = model_.getJointId(joint_name);
     auto joint = model_.joints[joint_id];
 
-#if ROS2_VERSION_ABOVE_JAZZY
+#if ROS2_VERSION_ABOVE_HUMBLE
     q[i] = state_interfaces_[i].get_optional().value_or(q[i]);
 #else
     q[i] = state_interfaces_[i].get_value();
diff --git a/src/torque_feedback_controller.cpp b/src/torque_feedback_controller.cpp
index 0c209e6..cb55c55 100644
--- a/src/torque_feedback_controller.cpp
+++ b/src/torque_feedback_controller.cpp
@@ -50,7 +50,7 @@ controller_interface::return_type TorqueFeedbackController::update(
   const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
   // Update joint states
   for (int i = 0; i < num_joints_; i++) {
-#if ROS2_VERSION_ABOVE_JAZZY
+#if ROS2_VERSION_ABOVE_HUMBLE
     q_[i] = state_interfaces_[i].get_optional().value_or(q_[i]);
     dq_[i] = state_interfaces_[num_joints_ + i].get_optional().value_or(dq_[i]);
 #else
@@ -261,7 +261,7 @@ TorqueFeedbackController::on_configure(const rclcpp_lifecycle::State & /*previou
 CallbackReturn
 TorqueFeedbackController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
   for (int i = 0; i < num_joints_; i++) {
-#if ROS2_VERSION_ABOVE_JAZZY
+#if ROS2_VERSION_ABOVE_HUMBLE
     q_[i] = state_interfaces_[i].get_optional().value_or(q_[i]);
     dq_[i] = state_interfaces_[num_joints_ + i].get_optional().value_or(dq_[i]);
 #else
diff --git a/src/twist_broadcaster.cpp b/src/twist_broadcaster.cpp
index 953bacb..37c46ce 100644
--- a/src/twist_broadcaster.cpp
+++ b/src/twist_broadcaster.cpp
@@ -47,7 +47,7 @@ TwistBroadcaster::update(const rclcpp::Time & time, const rclcpp::Duration & per
     auto joint_id = model_.getJointId(joint_name);
     auto joint = model_.joints[joint_id];
 
-#if ROS2_VERSION_ABOVE_JAZZY
+#if ROS2_VERSION_ABOVE_HUMBLE
     q[i] = state_interfaces_[i * 2].get_optional().value_or(q[i]);
     q_dot[i] = state_interfaces_[i * 2 + 1].get_optional().value_or(q_dot[i]);
 #else

From fbe31071a07c56fb5236b4ea4d77dcdae9106b7a Mon Sep 17 00:00:00 2001
From: domrachev03 <domrachev03@users.noreply.github.com>
Date: Mon, 30 Mar 2026 16:21:14 +0900
Subject: [PATCH 7/9] fix: remove extra debug prints

---
 src/cartesian_controller.cpp | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp
index b94500c..d6945ff 100644
--- a/src/cartesian_controller.cpp
+++ b/src/cartesian_controller.cpp
@@ -462,18 +462,10 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
 void CartesianController::setStiffnessAndDamping() {
   if (use_topic_stiffness_) {
     stiffness = topic_stiffness_;
-    RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 2000,
-      "Using TOPIC stiffness: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]",
-      stiffness(0,0), stiffness(1,1), stiffness(2,2),
-      stiffness(3,3), stiffness(4,4), stiffness(5,5));
   } else {
     stiffness.setZero();
     stiffness.diagonal() << params_.task.k_pos_x, params_.task.k_pos_y, params_.task.k_pos_z,
       params_.task.k_rot_x, params_.task.k_rot_y, params_.task.k_rot_z;
-    RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 2000,
-      "Using PARAM stiffness: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]",
-      stiffness(0,0), stiffness(1,1), stiffness(2,2),
-      stiffness(3,3), stiffness(4,4), stiffness(5,5));
   }
 
   // Clamp stiffness to [0, max_stiffness]

From ac78f838cfaa8c24021fc5ee1321cfbe3969f8b2 Mon Sep 17 00:00:00 2001
From: domrachev03 <domrachev03@users.noreply.github.com>
Date: Tue, 31 Mar 2026 14:33:13 +0900
Subject: [PATCH 8/9] fix: debug update stiffness

---
 src/cartesian_controller.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp
index d6945ff..eb980fc 100644
--- a/src/cartesian_controller.cpp
+++ b/src/cartesian_controller.cpp
@@ -85,6 +85,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration &
   if (new_target_stiffness_) {
     parse_target_stiffness_();
     new_target_stiffness_ = false;
+    setStiffnessAndDamping();
   }
 
   pinocchio::forwardKinematics(model_, data_, q_pin, dq);
@@ -208,8 +209,10 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration &
   tau_previous = tau_d;
 
   params_listener_->refresh_dynamic_parameters();
-  params_ = params_listener_->get_params();
-  setStiffnessAndDamping();
+  if (params_listener_->is_old(params_)) {
+    params_ = params_listener_->get_params();
+    setStiffnessAndDamping();
+  }
 
   log_debug_info(time);
 
@@ -520,7 +523,7 @@ void CartesianController::updateCurrentState(bool initialize) {
     auto joint_id = model_.getJointId(joint_name);  // pinocchio joind id might be different
     auto joint = model_.joints[joint_id];
 
-#if ROS2_VERSION_ABOVE_HUMBLE
+#if ROS2_VERSION_ABOVE_JAZZY
     double q_meas = state_interfaces_[i].get_optional().value_or(q[i]);
     double dq_meas = state_interfaces_[num_joints + i].get_optional().value_or(dq[i]);
 #else

From f08923bcf219d70427f48b46b3ce47f49c64d306 Mon Sep 17 00:00:00 2001
From: domrachev03 <domrachev03@users.noreply.github.com>
Date: Tue, 31 Mar 2026 16:08:52 +0900
Subject: [PATCH 9/9] refactor: add throttling to ROS2 loggers & rename
 max_stiffness -> variable_max_stiffness

---
 src/cartesian_controller.cpp  | 21 ++++++++++-----------
 src/cartesian_controller.yaml |  2 +-
 2 files changed, 11 insertions(+), 12 deletions(-)

diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp
index eb980fc..2315707 100644
--- a/src/cartesian_controller.cpp
+++ b/src/cartesian_controller.cpp
@@ -472,18 +472,18 @@ void CartesianController::setStiffnessAndDamping() {
   }
 
   // Clamp stiffness to [0, max_stiffness]
-  const double max_k_trans = params_.max_stiffness.translational;
-  const double max_k_rot = params_.max_stiffness.rotational;
+  const double max_k_trans = params_.variable_max_stiffness.translational;
+  const double max_k_rot = params_.variable_max_stiffness.rotational;
   for (int i = 0; i < 3; ++i) {
     if (stiffness(i, i) < 0.0 || stiffness(i, i) > max_k_trans) {
-      RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
+      RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
         "Translational stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, stiffness(i, i), max_k_trans);
       stiffness(i, i) = std::clamp(stiffness(i, i), 0.0, max_k_trans);
     }
   }
   for (int i = 3; i < 6; ++i) {
     if (stiffness(i, i) < 0.0 || stiffness(i, i) > max_k_rot) {
-      RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
+      RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
         "Rotational stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, stiffness(i, i), max_k_rot);
       stiffness(i, i) = std::clamp(stiffness(i, i), 0.0, max_k_rot);
     }
@@ -624,20 +624,20 @@ void CartesianController::parse_target_stiffness_() {
       msg->data.size());
     return;
   }
-  const double max_k_trans = params_.max_stiffness.translational;
-  const double max_k_rot = params_.max_stiffness.rotational;
+  const double max_k_trans = params_.variable_max_stiffness.translational;
+  const double max_k_rot = params_.variable_max_stiffness.rotational;
   std::array<double, 6> vals = {msg->data[0], msg->data[1], msg->data[2],
                                 msg->data[3], msg->data[4], msg->data[5]};
   for (int i = 0; i < 3; ++i) {
     if (vals[i] < 0.0 || vals[i] > max_k_trans) {
-      RCLCPP_WARN(get_node()->get_logger(),
+      RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
         "Topic stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, vals[i], max_k_trans);
       vals[i] = std::clamp(vals[i], 0.0, max_k_trans);
     }
   }
   for (int i = 3; i < 6; ++i) {
     if (vals[i] < 0.0 || vals[i] > max_k_rot) {
-      RCLCPP_WARN(get_node()->get_logger(),
+      RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
         "Topic stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, vals[i], max_k_rot);
       vals[i] = std::clamp(vals[i], 0.0, max_k_rot);
     }
@@ -645,10 +645,9 @@ void CartesianController::parse_target_stiffness_() {
   topic_stiffness_.setZero();
   topic_stiffness_.diagonal() << vals[0], vals[1], vals[2], vals[3], vals[4], vals[5];
   use_topic_stiffness_ = true;
-  RCLCPP_INFO(get_node()->get_logger(),
+  RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
     "Variable stiffness received: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]",
-    msg->data[0], msg->data[1], msg->data[2],
-    msg->data[3], msg->data[4], msg->data[5]);
+    vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]);
 }
 
 void CartesianController::log_debug_info(const rclcpp::Time & time) {
diff --git a/src/cartesian_controller.yaml b/src/cartesian_controller.yaml
index 751928d..a19f68d 100644
--- a/src/cartesian_controller.yaml
+++ b/src/cartesian_controller.yaml
@@ -313,7 +313,7 @@ cartesian_controller:
       default_value: [ 0.039533, 0.025882, -0.04607, 0.036194, 0.026226, -0.021047, 0.0035526]
       description: "Friction parameters part 3"
 
-  max_stiffness:
+  variable_max_stiffness:
     translational:
       type: double
       default_value: 900.0