Variable Stiffness Controller

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
 )
 

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;

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>
 

src/cartesian_controller.cpp

@@ -1,4 +1,5 @@
 
+#include <algorithm>
 #include <cmath>
 #include <cstddef>
 
@@ -18,6 +19,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 +82,11 @@ 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;
+    setStiffnessAndDamping();
+  }
 
   pinocchio::forwardKinematics(model_, data_, q_pin, dq);
   pinocchio::updateFramePlacements(model_, data_);
@@ -201,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);
 
@@ -318,6 +328,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 +385,26 @@ 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);
 
+  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);
   tau_joint_limits = Eigen::VectorXd::Zero(model_.nv);
@@ -431,9 +463,31 @@ 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;
+  }
+
+  // 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
@@ -469,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
@@ -559,6 +613,44 @@ 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;
+  }
+  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() << 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]",
+    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) {
   if (!params_.log.enabled) {
     return;

src/cartesian_controller.yaml

@@ -313,6 +313,30 @@ 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
+      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