From f772ffb3fb8aa489db798d6f36986dbdec84281d Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Fri, 2 Oct 2026 12:27:49 +0000 Subject: [PATCH 1/4] TRY updating params instead of waiting on mutex --- src/cartesian_controller.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index f1d8bd8..98c4190 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -228,9 +228,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & tau_previous = tau_d; - params_listener_->refresh_dynamic_parameters(); - if (params_listener_->is_old(params_)) { - params_ = params_listener_->get_params(); + if (params_listener_->try_update_params(params_)) { setStiffnessAndDamping(); } From b8ffb9d2ad4540ab00cbef1395057fe39c0c9f03 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 19 Nov 2026 00:04:32 +0100 Subject: [PATCH 2/4] fix: reset the CartesianController state on activation and deactivation on_activate: - Refuse to activate when a joint state is missing or not finite. - Drop target pose, joint, wrench, and stiffness messages received while inactive. - Reset target_wrench_, dq_ref, tau_previous, and the topic stiffness. - Apply pending parameter changes. on_deactivate: - Set the effort commands to zero. - Drop pending target messages. Make the new_target_* flags atomic. The subscription callbacks write them and update() reads them. Co-Authored-By: Frank --- .../cartesian_controller.hpp | 10 ++-- src/cartesian_controller.cpp | 51 ++++++++++++++++++- 2 files changed, 56 insertions(+), 5 deletions(-) diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index 32df937..1e41c5b 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -5,6 +5,7 @@ * @brief Cartesian controller implementation for robot manipulation (supports impedance and OSC) */ +#include #include #include #include @@ -143,10 +144,11 @@ class CartesianController : public controller_interface::ControllerInterface { */ void parse_target_stiffness_(); - bool new_target_pose_; - bool new_target_joint_; - bool new_target_wrench_; - bool new_target_stiffness_ = false; + // Written by the subscription callbacks, read by update(). + std::atomic new_target_pose_{false}; + std::atomic new_target_joint_{false}; + std::atomic new_target_wrench_{false}; + std::atomic new_target_stiffness_{false}; bool use_topic_stiffness_ = false; realtime_tools::RealtimeBuffer> diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index 98c4190..5e48015 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -596,7 +596,37 @@ void CartesianController::updateCurrentState(bool initialize) { CallbackReturn CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - + // Do not start from missing or invalid joint states: the target pose comes from them. + for (size_t i = 0; i < state_interfaces_.size(); ++i) { +#if ROS2_VERSION_ABOVE_HUMBLE + const auto value = state_interfaces_[i].get_optional(); + const bool valid = value.has_value() && std::isfinite(value.value()); +#else + const bool valid = std::isfinite(state_interfaces_[i].get_value()); +#endif + if (!valid) { + RCLCPP_ERROR( + get_node()->get_logger(), + "State interface '%s' has no valid value. Refusing to activate.", + state_interfaces_[i].get_name().c_str()); + return CallbackReturn::ERROR; + } + } + + // Drop targets received while the controller was inactive and reset the + // values of the previous activation. + new_target_pose_ = false; + new_target_joint_ = false; + new_target_wrench_ = false; + new_target_stiffness_ = false; + use_topic_stiffness_ = false; + target_wrench_.setZero(); + dq_ref.setZero(); + tau_previous.setZero(); + + params_listener_->try_update_params(params_); + setStiffnessAndDamping(); + // Update the current state with initial measurements (no EMA filtering) // to avoid large initial errors updateCurrentState(true); @@ -617,6 +647,25 @@ CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_stat controller_interface::CallbackReturn CartesianController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { + // Do not leave the last torque on the command interfaces. + for (auto & command_interface : command_interfaces_) { +#if ROS2_VERSION_ABOVE_HUMBLE + if (!command_interface.set_value(0.0)) { + RCLCPP_WARN( + get_node()->get_logger(), "Failed to set '%s' to zero on deactivation.", + command_interface.get_name().c_str()); + } +#else + command_interface.set_value(0.0); +#endif + } + + new_target_pose_ = false; + new_target_joint_ = false; + new_target_wrench_ = false; + new_target_stiffness_ = false; + + RCLCPP_INFO(get_node()->get_logger(), "Controller deactivated."); return CallbackReturn::SUCCESS; } From 42810e90f60efcbb8d6a31e29e4ccc38ff940d6f Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 19 Nov 2026 00:16:35 +0100 Subject: [PATCH 3/4] fix: bound the target joint copy to the controller joints parse_target_joint_() copied velocities with the size of the position array and copied positions without a size check. Copy at most as many entries as the controller has joints, for positions and velocities. Co-Authored-By: Frank --- src/cartesian_controller.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index 5e48015..dacefca 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -682,13 +682,15 @@ void CartesianController::parse_target_pose_() { void CartesianController::parse_target_joint_() { auto msg = *target_joint_buffer_.readFromRT(); if (msg->position.size()) { - for (size_t i = 0; i < msg->position.size(); ++i) { + const auto size = std::min(msg->position.size(), q_target.size()); + for (size_t i = 0; i < size; ++i) { q_target[i] = msg->position[i]; } /*filterJointValues(msg->name, msg->position, params_.joints, q_ref);*/ } if (msg->velocity.size()) { - for (size_t i = 0; i < msg->position.size(); ++i) { + const auto size = std::min(msg->velocity.size(), dq_ref.size()); + for (size_t i = 0; i < size; ++i) { dq_ref[i] = msg->velocity[i]; } /*filterJointValues(msg->name, msg->velocity, params_.joints, dq_ref);*/ From bf4f9f5f76cc30b535505b181b0df4dcc9373df7 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 19 Nov 2026 16:05:00 +0100 Subject: [PATCH 4/4] fix: keep allocations and joint name lookups out of the realtime path - on_configure(): store the q_pin index and the continuous flag of each joint. updateCurrentState() uses them instead of a string copy and a name lookup per joint and cycle. - updateNullspaceWeights(): looks up the nullspace weights by joint name after a parameter change only. setStiffnessAndDamping() uses the stored weights and does not allocate. - on_activate(): computes the weights again only if try_update_params() returns new parameters. No log messages in on_activate() and on_deactivate(). Co-Authored-By: Frank --- .../cartesian_controller.hpp | 14 ++++++ src/cartesian_controller.cpp | 45 ++++++++++++------- 2 files changed, 42 insertions(+), 17 deletions(-) diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index 1e41c5b..618d2a3 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include // NOLINT(build/include_order) @@ -118,6 +119,12 @@ class CartesianController : public controller_interface::ControllerInterface { */ void setStiffnessAndDamping(); + /** + * @brief Set the nullspace weight of each joint from the parameters. Call it after a parameter + * change, not in every cycle: it looks up the joints by name. + */ + void updateNullspaceWeights(); + /** * @brief Get the current state of the robot from hardware interfaces and update internal variables * @param initialize If set to true, initialize the exponential moving average filter with the current state @@ -198,6 +205,13 @@ class CartesianController : public controller_interface::ControllerInterface { Eigen::MatrixXd nullspace_stiffness; /** @brief Nullspace damping matrix for posture control */ Eigen::MatrixXd nullspace_damping; + /** @brief Nullspace weight of each joint, set by updateNullspaceWeights() */ + Eigen::VectorXd nullspace_weights_; + + /** @brief Index in q_pin of each controller joint, set in on_configure() */ + std::vector joint_idx_q_; + /** @brief True for a continuous joint (two entries in q_pin), set in on_configure() */ + std::vector joint_is_continuous_; /** @brief Current joint positions with dimension nv. */ Eigen::VectorXd q; diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index dacefca..93902eb 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -229,6 +229,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & tau_previous = tau_d; if (params_listener_->try_update_params(params_)) { + updateNullspaceWeights(); setStiffnessAndDamping(); } @@ -350,7 +351,18 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta nullspace_stiffness = Eigen::MatrixXd::Zero(model_.nv, model_.nv); nullspace_damping = Eigen::MatrixXd::Zero(model_.nv, model_.nv); + nullspace_weights_ = Eigen::VectorXd::Zero(model_.nv); + // Look up the joints by name once: updateCurrentState() runs in the realtime loop. + joint_idx_q_.clear(); + joint_is_continuous_.clear(); + for (const auto & joint_name : params_.joints) { + const auto & joint = model_.joints[model_.getJointId(joint_name)]; + joint_idx_q_.push_back(joint.idx_q()); + joint_is_continuous_.push_back(continous_joint_types.count(joint.shortname()) > 0); + } + + updateNullspaceWeights(); setStiffnessAndDamping(); new_target_pose_ = false; @@ -541,26 +553,26 @@ void CartesianController::setStiffnessAndDamping() { nullspace_stiffness.setZero(); nullspace_damping.setZero(); - auto weights = Eigen::VectorXd(model_.nv); - for (size_t i = 0; i < params_.joints.size(); ++i) { - weights[i] = params_.nullspace.weights.joints_map.at(params_.joints.at(i)).value; - } - nullspace_stiffness.diagonal() << params_.nullspace.stiffness * weights; + nullspace_stiffness.diagonal() = params_.nullspace.stiffness * nullspace_weights_; // For nullspace, use explicit damping if > 0, otherwise compute from stiffness if (params_.nullspace.damping > 0) { - nullspace_damping.diagonal() = params_.nullspace.damping * weights; + nullspace_damping.diagonal() = params_.nullspace.damping * nullspace_weights_; } else { nullspace_damping.diagonal() = 2.0 * nullspace_stiffness.diagonal().cwiseSqrt(); } } +void CartesianController::updateNullspaceWeights() { + for (size_t i = 0; i < params_.joints.size(); ++i) { + nullspace_weights_[i] = params_.nullspace.weights.joints_map.at(params_.joints.at(i)).value; + } +} + void CartesianController::updateCurrentState(bool initialize) { auto num_joints = params_.joints.size(); for (size_t i = 0; i < num_joints; i++) { - auto joint_name = params_.joints[i]; - auto joint_id = model_.getJointId(joint_name); // pinocchio joind id might be different - auto joint = model_.joints[joint_id]; + const int idx_q = joint_idx_q_[i]; // pinocchio index might be different #if ROS2_VERSION_ABOVE_HUMBLE double q_meas = state_interfaces_[i].get_optional().value_or(q[i]); @@ -578,12 +590,11 @@ void CartesianController::updateCurrentState(bool initialize) { ? q_meas : exponential_moving_average(q[i], q_meas, params_.filter.q); - if (continous_joint_types.count(joint.shortname())) { // Then we are handling a continous - // joint that is SO(2) - q_pin[joint.idx_q()] = std::cos(q[i]); - q_pin[joint.idx_q() + 1] = std::sin(q[i]); + if (joint_is_continuous_[i]) { // continuous joint, SO(2) + q_pin[idx_q] = std::cos(q[i]); + q_pin[idx_q + 1] = std::sin(q[i]); } else { // simple revolute joint case - q_pin[joint.idx_q()] = q[i]; + q_pin[idx_q] = q[i]; } dq[i] = initialize @@ -624,7 +635,9 @@ CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_stat dq_ref.setZero(); tau_previous.setZero(); - params_listener_->try_update_params(params_); + if (params_listener_->try_update_params(params_)) { + updateNullspaceWeights(); + } setStiffnessAndDamping(); // Update the current state with initial measurements (no EMA filtering) @@ -641,7 +654,6 @@ CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_stat desired_position_ = target_position_; desired_orientation_ = target_orientation_; - RCLCPP_INFO(get_node()->get_logger(), "Controller activated."); return CallbackReturn::SUCCESS; } @@ -665,7 +677,6 @@ CartesianController::on_deactivate(const rclcpp_lifecycle::State & /*previous_st new_target_wrench_ = false; new_target_stiffness_ = false; - RCLCPP_INFO(get_node()->get_logger(), "Controller deactivated."); return CallbackReturn::SUCCESS; }