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 #include #include +#include #include #include #include @@ -102,6 +103,8 @@ class CartesianController : public controller_interface::ControllerInterface { rclcpp::Subscription::SharedPtr joint_sub_; /** @brief Subscription for target wrench messages */ rclcpp::Subscription::SharedPtr wrench_sub_; + /** @brief Subscription for variable stiffness messages */ + rclcpp::Subscription::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> target_pose_buffer_; @@ -148,6 +158,9 @@ class CartesianController : public controller_interface::ControllerInterface { realtime_tools::RealtimeBuffer> target_wrench_buffer_; + realtime_tools::RealtimeBuffer> + 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 topic_stiffness_ = Eigen::Matrix::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 @@ pinocchio geometry_msgs sensor_msgs + std_msgs pluginlib realtime_tools diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index c16a7c4..2315707 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -1,4 +1,5 @@ +#include #include #include @@ -18,6 +19,8 @@ #include "pinocchio/algorithm/model.hpp" +#include + #include #include #include @@ -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( "target_wrench", rclcpp::QoS(1), target_wrench_callback); + auto target_stiffness_callback = + [this](const std::shared_ptr 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( + 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_.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(), 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(), 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); + } + } 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,43 @@ 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_.variable_max_stiffness.translational; + const double max_k_rot = params_.variable_max_stiffness.rotational; + std::array 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_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_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); + } + } + topic_stiffness_.setZero(); + topic_stiffness_.diagonal() << vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]; + use_topic_stiffness_ = true; + RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100, + "Variable stiffness received: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]", + vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]); +} + 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..a19f68d 100644 --- a/src/cartesian_controller.yaml +++ b/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" + variable_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