diff --git a/CMakeLists.txt b/CMakeLists.txt index d930b97..1f792c6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -215,6 +215,16 @@ if(BUILD_TESTING) target_link_libraries(test_pseudo_inverse Eigen3::Eigen ) + + ament_add_gtest(test_nullspace_gains + tests/test_nullspace_gains.cpp + ) + target_include_directories(test_nullspace_gains PUBLIC + $ + ) + target_link_libraries(test_nullspace_gains + Eigen3::Eigen + ) endif() ament_package() diff --git a/docs/deprecated/new_robot_setup.md b/docs/deprecated/new_robot_setup.md index 1dc26f1..929c2ad 100644 --- a/docs/deprecated/new_robot_setup.md +++ b/docs/deprecated/new_robot_setup.md @@ -80,7 +80,7 @@ For more information on the controllers, check the [available controllers and br k_rot_z: 30.0 nullspace: - stiffness: 0.0 + stiffness: [0.0] use_friction: true use_coriolis_compensation: true @@ -105,13 +105,13 @@ For more information on the controllers, check the [available controllers and br max_delta_tau: 0.5 nullspace: - stiffness: 5.0 + # A single value applies to every joint, or give one value per joint + # to tune them individually, e.g. stiffness: [5.0, 4.0, 4.0, 3.0, 3.0, 3.0] + stiffness: [5.0] projector_type: none # So we are directly controlling the joints! - damping: 0.5 - max_tau: 5.0 + damping: [0.5] + max_tau: [5.0] regularization: 1.0e-06 - weights: - use_friction: true use_coriolis_compensation: true @@ -137,7 +137,7 @@ For more information on the controllers, check the [available controllers and br k_rot_z: 30.0 nullspace: - stiffness: 2.0 + stiffness: [2.0] use_friction: false use_coriolis_compensation: true diff --git a/docs/getting_started_controller_details.md b/docs/getting_started_controller_details.md index e6dac3a..93d24ca 100644 --- a/docs/getting_started_controller_details.md +++ b/docs/getting_started_controller_details.md @@ -68,6 +68,23 @@ where: The nullspace position can be set with `robot.set_target_joint(...)` when using the Cartesian controller. It will publish a target joint position which is interpreted as the nullspace target. +The `nullspace.stiffness`, `nullspace.damping` and `nullspace.max_tau` parameters fill the diagonals of +\( \mathbf{K}_{p,\text{ns}} \) and \( \mathbf{K}_{d,\text{ns}} \) and the torque limit. +Each of them takes either a single value, which is applied to every joint, +or one value per joint so that each joint can be tuned individually: + +```yaml +nullspace: + stiffness: [5.0, 4.0, 4.0, 3.0, 3.0, 3.0] + damping: [1.0, 0.8, 0.8, 0.75, 0.75, 0.75] + max_tau: [5.0] + projector_type: none +``` + +Any other number of values is rejected and the controller refuses to configure. +A negative damping value asks for the critical damping \( 2\sqrt{k} \) of that same joint, +so `damping: [-1.0]` derives all the derivative gains from the stiffnesses. + ## Variable Stiffness diff --git a/docs/new_robot_setup.md b/docs/new_robot_setup.md index 79121e2..1b7255e 100644 --- a/docs/new_robot_setup.md +++ b/docs/new_robot_setup.md @@ -86,7 +86,7 @@ For more information on the controllers, check the [controllers details](getting k_rot_z: 30.0 nullspace: - stiffness: 0.0 + stiffness: [0.0] use_friction: true use_coriolis_compensation: true @@ -111,13 +111,13 @@ For more information on the controllers, check the [controllers details](getting max_delta_tau: 0.5 nullspace: - stiffness: 5.0 + # A single value applies to every joint, or give one value per joint + # to tune them individually, e.g. stiffness: [5.0, 4.0, 4.0, 3.0, 3.0, 3.0] + stiffness: [5.0] projector_type: none # So we are directly controlling the joints! - damping: 0.5 - max_tau: 5.0 + damping: [0.5] + max_tau: [5.0] regularization: 1.0e-06 - weights: - use_friction: true use_coriolis_compensation: true @@ -143,7 +143,7 @@ For more information on the controllers, check the [controllers details](getting k_rot_z: 30.0 nullspace: - stiffness: 2.0 + stiffness: [2.0] use_friction: false use_coriolis_compensation: true diff --git a/include/crisp_controllers/cartesian_admittance_controller.hpp b/include/crisp_controllers/cartesian_admittance_controller.hpp index 59dc183..d943f61 100644 --- a/include/crisp_controllers/cartesian_admittance_controller.hpp +++ b/include/crisp_controllers/cartesian_admittance_controller.hpp @@ -19,11 +19,11 @@ #include #include #include -#include #include #include #include #include +#include #include @@ -127,9 +127,11 @@ class CartesianAdmittanceController : public controller_interface::ControllerInt // ---- Methods ---- /** - * @brief Set the impedance stiffness and damping matrices based on parameters + * @brief Set impedance stiffness and damping matrices and nullspace torque limit from parameters. + * @return false if a nullspace parameter holds neither a single value nor one value per + * degree of freedom, in which case the previous values are kept. */ - void setStiffnessAndDamping(); + [[nodiscard]] bool setStiffnessAndDamping(); /** * @brief Set the admittance mass, stiffness, and damping matrices from parameters @@ -242,6 +244,8 @@ class CartesianAdmittanceController : public controller_interface::ControllerInt Eigen::MatrixXd nullspace_stiffness; /** @brief Nullspace damping matrix for posture control */ Eigen::MatrixXd nullspace_damping; + /** @brief Maximum nullspace torque per degree of freedom */ + Eigen::VectorXd nullspace_max_tau; // ---- Admittance inner loop state ---- diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index 32df937..e46801d 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -14,10 +14,10 @@ #include #include #include -#include #include #include #include +#include #include @@ -113,9 +113,11 @@ class CartesianController : public controller_interface::ControllerInterface { size_t max_allowed_publishers_; /** - * @brief Set the stiffness and damping matrices based on parameters + * @brief Set stiffness and damping matrices and nullspace torque limit from parameters. + * @return false if a nullspace parameter holds neither a single value nor one value per + * degree of freedom, in which case the previous values are kept. */ - void setStiffnessAndDamping(); + [[nodiscard]] bool setStiffnessAndDamping(); /** * @brief Get the current state of the robot from hardware interfaces and update internal variables @@ -196,6 +198,8 @@ class CartesianController : public controller_interface::ControllerInterface { Eigen::MatrixXd nullspace_stiffness; /** @brief Nullspace damping matrix for posture control */ Eigen::MatrixXd nullspace_damping; + /** @brief Maximum nullspace torque per degree of freedom */ + Eigen::VectorXd nullspace_max_tau; /** @brief Current joint positions with dimension nv. */ Eigen::VectorXd q; diff --git a/include/crisp_controllers/torque_feedback_controller.hpp b/include/crisp_controllers/torque_feedback_controller.hpp index 0bc9475..00b5503 100644 --- a/include/crisp_controllers/torque_feedback_controller.hpp +++ b/include/crisp_controllers/torque_feedback_controller.hpp @@ -132,6 +132,13 @@ class TorqueFeedbackController : public controller_interface::ControllerInterfac */ void publish_wrench_callback_(); + /** + * @brief Expand the nullspace parameters into one gain per joint + * @return false if a nullspace parameter holds neither a single value nor one value per + * joint, in which case the previous values are kept + */ + [[nodiscard]] bool setNullspaceGains(); + /// Names of controlled joints std::vector joint_names_; /// Number of controlled joints @@ -151,8 +158,12 @@ class TorqueFeedbackController : public controller_interface::ControllerInterfac /// Initial joint positions recorded on activation (nullspace target) Eigen::VectorXd q_init_; - /// Nullspace weights (computed from parameters) - Eigen::VectorXd nullspace_weights_; + /// Nullspace stiffness per joint (expanded from parameters) + Eigen::VectorXd nullspace_stiffness_; + /// Nullspace damping per joint (expanded from parameters) + Eigen::VectorXd nullspace_damping_; + /// Maximum nullspace torque per joint (expanded from parameters) + Eigen::VectorXd nullspace_max_tau_; /// Friction parameters as Eigen vectors Eigen::VectorXd friction_fp1_; diff --git a/include/crisp_controllers/utils/nullspace_gains.hpp b/include/crisp_controllers/utils/nullspace_gains.hpp new file mode 100644 index 0000000..a65d618 --- /dev/null +++ b/include/crisp_controllers/utils/nullspace_gains.hpp @@ -0,0 +1,51 @@ +#pragma once + +#include +#include +#include + +#include + +namespace crisp_controllers { + +/** + * @brief Expand a nullspace gain parameter into one gain per degree of freedom. + * + * A parameter holding a single value is broadcast to all degrees of freedom. + * Otherwise, it has to provide one value per degree of freedom already. + * + * @param values Gain values as given in the parameters. + * @param nv Number of degrees of freedom of the model. + * @return The expanded gains, or std::nullopt if values has neither 1 nor nv elements. + */ +inline std::optional +expand_nullspace_gains(const std::vector & values, Eigen::Index nv) { + if (values.size() == 1) { + return Eigen::VectorXd::Constant(nv, values.front()); + } + if (static_cast(values.size()) == nv) { + return Eigen::Map(values.data(), nv); + } + return std::nullopt; +} + +/** + * @brief Resolve the damping gains, replacing negative entries by their critical damping. + * + * A negative damping gain means "pick a sensible value for me", which is 2 sqrt(k) for the + * stiffness k of that same degree of freedom. Zero is a valid request for no damping at all. + * + * @param damping Damping gains, one per degree of freedom. + * @param stiffness Stiffness gains, one per degree of freedom. + * @return The resolved damping gains. + */ +inline Eigen::VectorXd +resolve_nullspace_damping(const Eigen::VectorXd & damping, const Eigen::VectorXd & stiffness) { + Eigen::VectorXd resolved(damping.size()); + for (Eigen::Index i = 0; i < damping.size(); ++i) { + resolved[i] = damping[i] < 0.0 ? 2.0 * std::sqrt(stiffness[i]) : damping[i]; + } + return resolved; +} + +} // namespace crisp_controllers diff --git a/src/cartesian_admittance_controller.cpp b/src/cartesian_admittance_controller.cpp index e58013d..fcb4636 100644 --- a/src/cartesian_admittance_controller.cpp +++ b/src/cartesian_admittance_controller.cpp @@ -12,11 +12,11 @@ #include #include #include +#include #include #include #include #include -#include #include "pinocchio/algorithm/model.hpp" @@ -26,6 +26,7 @@ #include #include #include +#include #include #include "crisp_controllers/utils/fiters.hpp" #include "crisp_controllers/utils/torque_rate_saturation.hpp" @@ -66,7 +67,6 @@ CartesianAdmittanceController::state_interface_configuration() const { controller_interface::return_type CartesianAdmittanceController::update(const rclcpp::Time & time, const rclcpp::Duration & period) { - // 1. Update current state information with EMA filtered values updateCurrentState(); @@ -86,7 +86,9 @@ CartesianAdmittanceController::update(const rclcpp::Time & time, const rclcpp::D if (new_target_stiffness_) { parse_target_stiffness_(); new_target_stiffness_ = false; - setStiffnessAndDamping(); + // An invalid live update must not bring down an active torque controller, so the previous + // gains are kept and the failure is only reported. + (void)setStiffnessAndDamping(); } // 3. Parse F/T sensor data @@ -107,8 +109,7 @@ CartesianAdmittanceController::update(const rclcpp::Time & time, const rclcpp::D pinocchio::updateFramePlacements(model_, data_); end_effector_pose = data_.oMf[end_effector_frame_id]; - if (!end_effector_pose.translation().allFinite() || - !end_effector_pose.rotation().allFinite()) { + if (!end_effector_pose.translation().allFinite() || !end_effector_pose.rotation().allFinite()) { RCLCPP_ERROR_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), @@ -166,8 +167,10 @@ CartesianAdmittanceController::update(const rclcpp::Time & time, const rclcpp::D // 10. Admittance dynamics: accel = M_inv * (F_ext - D * vel + K * error) // Spring term +K*(desired - inner) is a restoring force toward the desired pose - Eigen::Matrix K_adm = use_topic_adm_stiffness_ ? topic_adm_stiffness_ : adm_stiffness_; - Eigen::Vector adm_force = ft_wrench_world - adm_damping_ * inner_motion_ + K_adm * adm_error; + Eigen::Matrix K_adm = + use_topic_adm_stiffness_ ? topic_adm_stiffness_ : adm_stiffness_; + Eigen::Vector adm_force = + ft_wrench_world - adm_damping_ * inner_motion_ + K_adm * adm_error; Eigen::Vector accel = adm_mass_inv_ * adm_force; // 10. Semi-implicit Euler integration of the admittance target @@ -256,8 +259,7 @@ CartesianAdmittanceController::update(const rclcpp::Time & time, const rclcpp::D tau_secondary << nullspace_stiffness * (q_ref - q) + nullspace_damping * (dq_ref - dq); tau_nullspace << nullspace_projection * tau_secondary; - tau_nullspace = - tau_nullspace.cwiseMin(params_.nullspace.max_tau).cwiseMax(-params_.nullspace.max_tau); + tau_nullspace = tau_nullspace.cwiseMin(nullspace_max_tau).cwiseMax(-nullspace_max_tau); tau_friction = params_.use_friction ? get_friction(dq, fp1, fp2, fp3) : Eigen::VectorXd::Zero(model_.nv); @@ -301,7 +303,7 @@ CartesianAdmittanceController::update(const rclcpp::Time & time, const rclcpp::D params_listener_->refresh_dynamic_parameters(); if (params_listener_->is_old(params_)) { params_ = params_listener_->get_params(); - setStiffnessAndDamping(); + (void)setStiffnessAndDamping(); setAdmittanceParameters(); } @@ -398,16 +400,17 @@ CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*pr RCLCPP_ERROR_STREAM( get_node()->get_logger(), "end_effector_frame '" << params_.end_effector_frame - << "' is not present in the robot model. Refusing to configure: " - "activating with an invalid frame results in undefined behavior " - "(out-of-bounds access into pinocchio::Data, manifesting as a " - "segfault or NaN/Inf in computed torques)."); + << "' is not present in the robot model. Refusing to configure: " + "activating with an invalid frame results in undefined behavior " + "(out-of-bounds access into pinocchio::Data, manifesting as a " + "segfault or NaN/Inf in computed torques)."); return CallbackReturn::ERROR; } end_effector_frame_id = model_.getFrameId(params_.end_effector_frame); if (params_.ft_sensor.frame.empty()) { ft_sensor_frame_id = end_effector_frame_id; - RCLCPP_WARN(get_node()->get_logger(), + RCLCPP_WARN( + get_node()->get_logger(), "ft_sensor.frame is not set, using end_effector_frame for F/T wrench transformation. " "Set ft_sensor.frame to the actual sensor measurement frame for correct results."); } else { @@ -444,8 +447,11 @@ CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*pr nullspace_stiffness = Eigen::MatrixXd::Zero(model_.nv, model_.nv); nullspace_damping = Eigen::MatrixXd::Zero(model_.nv, model_.nv); + nullspace_max_tau = Eigen::VectorXd::Zero(model_.nv); - setStiffnessAndDamping(); + if (!setStiffnessAndDamping()) { + return CallbackReturn::ERROR; + } setAdmittanceParameters(); new_target_pose_ = false; @@ -506,8 +512,8 @@ CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*pr }; auto target_wrench_callback = - [this, target_wrench_topic]( - const std::shared_ptr msg) -> void { + [this, + target_wrench_topic](const std::shared_ptr msg) -> void { if (!check_topic_publisher_count(target_wrench_topic)) { RCLCPP_WARN_THROTTLE( get_node()->get_logger(), @@ -548,7 +554,9 @@ CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*pr stiffness_sub_ = get_node()->create_subscription( params_.variable_stiffness.topic, rclcpp::QoS(1), target_stiffness_callback); - RCLCPP_INFO(get_node()->get_logger(), "Variable impedance stiffness enabled on topic: %s", + RCLCPP_INFO( + get_node()->get_logger(), + "Variable impedance stiffness enabled on topic: %s", params_.variable_stiffness.topic.c_str()); } @@ -564,7 +572,9 @@ CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*pr ft_sensor_sub_ = get_node()->create_subscription( params_.ft_sensor.topic, rclcpp::SensorDataQoS(), ft_sensor_callback); - RCLCPP_INFO(get_node()->get_logger(), "F/T sensor subscription on topic: %s", + RCLCPP_INFO( + get_node()->get_logger(), + "F/T sensor subscription on topic: %s", params_.ft_sensor.topic.c_str()); // Variable admittance stiffness subscription @@ -578,7 +588,9 @@ CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*pr adm_stiffness_sub_ = get_node()->create_subscription( params_.variable_admittance_stiffness.topic, rclcpp::QoS(1), target_adm_stiffness_callback); - RCLCPP_INFO(get_node()->get_logger(), "Variable admittance stiffness enabled on topic: %s", + RCLCPP_INFO( + get_node()->get_logger(), + "Variable admittance stiffness enabled on topic: %s", params_.variable_admittance_stiffness.topic.c_str()); } @@ -639,7 +651,25 @@ CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*pr return CallbackReturn::SUCCESS; } -void CartesianAdmittanceController::setStiffnessAndDamping() { +bool CartesianAdmittanceController::setStiffnessAndDamping() { + const auto nullspace_stiffness_gains = + expand_nullspace_gains(params_.nullspace.stiffness, model_.nv); + const auto nullspace_damping_gains = expand_nullspace_gains(params_.nullspace.damping, model_.nv); + const auto nullspace_max_tau_gains = expand_nullspace_gains(params_.nullspace.max_tau, model_.nv); + if (!nullspace_stiffness_gains || !nullspace_damping_gains || !nullspace_max_tau_gains) { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "Each nullspace parameter must hold either a single value or one value per degree of " + "freedom (%d), but stiffness has %zu, damping has %zu and max_tau has %zu.", + model_.nv, + params_.nullspace.stiffness.size(), + params_.nullspace.damping.size(), + params_.nullspace.max_tau.size()); + return false; + } + if (use_topic_stiffness_) { stiffness = topic_stiffness_; } else { @@ -653,15 +683,27 @@ void CartesianAdmittanceController::setStiffnessAndDamping() { const double max_k_rot = params_.variable_max_impedance_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); + 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); + 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); } } @@ -678,19 +720,12 @@ void CartesianAdmittanceController::setStiffnessAndDamping() { nullspace_stiffness.setZero(); nullspace_damping.setZero(); + nullspace_stiffness.diagonal() = *nullspace_stiffness_gains; + nullspace_damping.diagonal() = + resolve_nullspace_damping(*nullspace_damping_gains, *nullspace_stiffness_gains); + nullspace_max_tau = *nullspace_max_tau_gains; - 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_damping.diagonal() << 2.0 * nullspace_stiffness.diagonal().cwiseSqrt(); - - if (params_.nullspace.damping >= 0.0) { - nullspace_damping.diagonal() = params_.nullspace.damping * weights; - } else { - nullspace_damping.diagonal() = 2.0 * nullspace_stiffness.diagonal().cwiseSqrt(); - } + return true; } void CartesianAdmittanceController::setAdmittanceParameters() { @@ -714,17 +749,27 @@ void CartesianAdmittanceController::setAdmittanceParameters() { const double max_ak_rot = params_.variable_max_admittance_stiffness.rotational; for (int i = 0; i < 3; ++i) { if (adm_stiffness_(i, i) < 0.0 || adm_stiffness_(i, i) > max_ak_trans) { - RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100, + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 100, "Admittance translational stiffness[%d]=%.1f out of [0, %.1f], clamping.", - i, adm_stiffness_(i, i), max_ak_trans); + i, + adm_stiffness_(i, i), + max_ak_trans); adm_stiffness_(i, i) = std::clamp(adm_stiffness_(i, i), 0.0, max_ak_trans); } } for (int i = 3; i < 6; ++i) { if (adm_stiffness_(i, i) < 0.0 || adm_stiffness_(i, i) > max_ak_rot) { - RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100, + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 100, "Admittance rotational stiffness[%d]=%.1f out of [0, %.1f], clamping.", - i, adm_stiffness_(i, i), max_ak_rot); + i, + adm_stiffness_(i, i), + max_ak_rot); adm_stiffness_(i, i) = std::clamp(adm_stiffness_(i, i), 0.0, max_ak_rot); } } @@ -750,13 +795,10 @@ void CartesianAdmittanceController::updateCurrentState(bool initialize) { double dq_meas = state_interfaces_[num_joints + i].get_value(); #endif - q_ref[i] = initialize - ? q_meas - : exponential_moving_average(q_ref[i], q_target[i], params_.filter.q_ref); + q_ref[i] = + initialize ? q_meas : exponential_moving_average(q_ref[i], q_target[i], params_.filter.q_ref); - q[i] = initialize - ? q_meas - : exponential_moving_average(q[i], q_meas, params_.filter.q); + q[i] = initialize ? q_meas : exponential_moving_average(q[i], q_meas, params_.filter.q); if (continous_joint_types.count(joint.shortname())) { q_pin[joint.idx_q()] = std::cos(q[i]); @@ -765,9 +807,7 @@ void CartesianAdmittanceController::updateCurrentState(bool initialize) { q_pin[joint.idx_q()] = q[i]; } - dq[i] = initialize - ? dq_meas - : exponential_moving_average(dq[i], dq_meas, params_.filter.dq); + dq[i] = initialize ? dq_meas : exponential_moving_average(dq[i], dq_meas, params_.filter.dq); q_target[i] = initialize ? q_meas : q_target[i]; } @@ -775,7 +815,6 @@ void CartesianAdmittanceController::updateCurrentState(bool initialize) { CallbackReturn CartesianAdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - // Update the current state with initial measurements (no EMA filtering) updateCurrentState(true); @@ -847,34 +886,54 @@ void CartesianAdmittanceController::parse_target_stiffness_() { } const double max_k_trans = params_.variable_max_impedance_stiffness.translational; const double max_k_rot = params_.variable_max_impedance_stiffness.rotational; - std::array vals = {msg->data[0], msg->data[1], msg->data[2], - msg->data[3], msg->data[4], msg->data[5]}; + 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 impedance stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, vals[i], max_k_trans); + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 100, + "Topic impedance 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 impedance stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, vals[i], max_k_rot); + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 100, + "Topic impedance 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, + RCLCPP_INFO_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 100, "Variable impedance stiffness received: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]", - vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]); + vals[0], + vals[1], + vals[2], + vals[3], + vals[4], + vals[5]); } void CartesianAdmittanceController::parse_ft_sensor_() { auto msg = *ft_sensor_buffer_.readFromRT(); - ft_wrench_ << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, - msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z; + ft_wrench_ << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, msg->wrench.torque.x, + msg->wrench.torque.y, msg->wrench.torque.z; } void CartesianAdmittanceController::parse_target_adm_stiffness_() { @@ -890,28 +949,48 @@ void CartesianAdmittanceController::parse_target_adm_stiffness_() { } const double max_ak_trans = params_.variable_max_admittance_stiffness.translational; const double max_ak_rot = params_.variable_max_admittance_stiffness.rotational; - std::array avals = {msg->data[0], msg->data[1], msg->data[2], - msg->data[3], msg->data[4], msg->data[5]}; + std::array avals = { + 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 (avals[i] < 0.0 || avals[i] > max_ak_trans) { - RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100, - "Topic admittance stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, avals[i], max_ak_trans); + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 100, + "Topic admittance stiffness[%d]=%.1f out of [0, %.1f], clamping.", + i, + avals[i], + max_ak_trans); avals[i] = std::clamp(avals[i], 0.0, max_ak_trans); } } for (int i = 3; i < 6; ++i) { if (avals[i] < 0.0 || avals[i] > max_ak_rot) { - RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100, - "Topic admittance stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, avals[i], max_ak_rot); + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 100, + "Topic admittance stiffness[%d]=%.1f out of [0, %.1f], clamping.", + i, + avals[i], + max_ak_rot); avals[i] = std::clamp(avals[i], 0.0, max_ak_rot); } } topic_adm_stiffness_.setZero(); topic_adm_stiffness_.diagonal() << avals[0], avals[1], avals[2], avals[3], avals[4], avals[5]; use_topic_adm_stiffness_ = true; - RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100, + RCLCPP_INFO_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 100, "Variable admittance stiffness received: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]", - avals[0], avals[1], avals[2], avals[3], avals[4], avals[5]); + avals[0], + avals[1], + avals[2], + avals[3], + avals[4], + avals[5]); } void CartesianAdmittanceController::log_debug_info(const rclcpp::Time & time) { @@ -939,13 +1018,19 @@ void CartesianAdmittanceController::log_debug_info(const rclcpp::Time & time) { RCLCPP_INFO_STREAM_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), 1000, "J: " << J); RCLCPP_INFO_STREAM_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, "inner_SE3 pos: " << inner_SE3_.translation().transpose()); RCLCPP_INFO_STREAM_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, "inner_motion: " << inner_motion_.transpose()); RCLCPP_INFO_STREAM_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, "ft_wrench: " << ft_wrench_.transpose()); } @@ -979,13 +1064,19 @@ void CartesianAdmittanceController::log_debug_info(const rclcpp::Time & time) { 1000, "nullspace_damping: " << nullspace_damping); RCLCPP_INFO_STREAM_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, "adm_mass: " << adm_mass_.diagonal().transpose()); RCLCPP_INFO_STREAM_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, "adm_stiffness: " << adm_stiffness_.diagonal().transpose()); RCLCPP_INFO_STREAM_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, "adm_damping: " << adm_damping_.diagonal().transpose()); } @@ -1067,7 +1158,9 @@ bool CartesianAdmittanceController::check_topic_publisher_count(const std::strin get_node()->get_logger(), *get_node()->get_clock(), 5000, - "Failed to get publisher info for topic '%s': %s", topic_name.c_str(), e.what()); + "Failed to get publisher info for topic '%s': %s", + topic_name.c_str(), + e.what()); return true; // Allow message through if check fails } size_t publisher_count = topic_info.size(); diff --git a/src/cartesian_admittance_controller.yaml b/src/cartesian_admittance_controller.yaml index 85a691b..97c1c34 100644 --- a/src/cartesian_admittance_controller.yaml +++ b/src/cartesian_admittance_controller.yaml @@ -125,25 +125,19 @@ cartesian_admittance_controller: nullspace: stiffness: - type: double - default_value: 1.0 - description: "Stiffness of the nullspace controller." + type: double_array + default_value: [1.0] + description: "Per-joint stiffnesses of the nullspace controller. If this is a single element, applies to all joints." validation: - bounds<>: [0.0, 500.0] + size_gt<>: [0] + element_bounds<>: [0.0, 100000.0] damping: - type: double - default_value: -1.0 - description: "Damping of the nullspace controller. If negative, then 2 sqrt(stiffness) is used." - validation: - bounds<>: [-1.0, 500.0] - weights: - __map_joints: - value: - type: double - default_value: 1.0 - description: "Weights for each joint for the nullspace parameters." - validation: - bounds<>: [0.0, 500.0] + type: double_array + default_value: [-1.0] + description: "Per-joint damping of the nullspace controller. If negative, then 2 sqrt(stiffness) is used. If this is a single element, applies to all joints." + validation: + size_gt<>: [0] + element_bounds<>: [-1.0, 100000.0] regularization: type: double default_value: 0.000001 @@ -157,9 +151,12 @@ cartesian_admittance_controller: validation: one_of<>: [["kinematic", "dynamic", "none"]] max_tau: - type: double - default_value: 5 - description: "Maximal torque value that can be applied in the nullspace. This is used to be sure that we do not interfere with the main task." + type: double_array + default_value: [5.0] + description: "Maximal torque value that can be applied in the nullspace. This is used to be sure that we do not interfere with the main task. If this is a single element, applies to all joints." + validation: + size_gt<>: [0] + lower_element_bounds<>: [0.0] noise: add_random_noise: diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index f1d8bd8..a8dbf27 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include "crisp_controllers/utils/fiters.hpp" #include "crisp_controllers/utils/torque_rate_saturation.hpp" @@ -65,7 +66,6 @@ CartesianController::state_interface_configuration() const { controller_interface::return_type CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - // Update current state information with EMA filtered values updateCurrentState(); @@ -85,7 +85,9 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & if (new_target_stiffness_) { parse_target_stiffness_(); new_target_stiffness_ = false; - setStiffnessAndDamping(); + // An invalid live update must not bring down an active torque controller, so the previous + // gains are kept and the failure is only reported. + (void)setStiffnessAndDamping(); } pinocchio::forwardKinematics(model_, data_, q_pin, dq); @@ -100,8 +102,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & * target_position_);*/ end_effector_pose = data_.oMf[end_effector_frame_id]; - if (!end_effector_pose.translation().allFinite() || - !end_effector_pose.rotation().allFinite()) { + if (!end_effector_pose.translation().allFinite() || !end_effector_pose.rotation().allFinite()) { RCLCPP_ERROR_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), @@ -189,8 +190,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & tau_secondary << nullspace_stiffness * (q_ref - q) + nullspace_damping * (dq_ref - dq); tau_nullspace << nullspace_projection * tau_secondary; - tau_nullspace = - tau_nullspace.cwiseMin(params_.nullspace.max_tau).cwiseMax(-params_.nullspace.max_tau); + tau_nullspace = tau_nullspace.cwiseMin(nullspace_max_tau).cwiseMax(-nullspace_max_tau); tau_friction = params_.use_friction ? get_friction(dq, fp1, fp2, fp3) : Eigen::VectorXd::Zero(model_.nv); @@ -231,7 +231,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & params_listener_->refresh_dynamic_parameters(); if (params_listener_->is_old(params_)) { params_ = params_listener_->get_params(); - setStiffnessAndDamping(); + (void)setStiffnessAndDamping(); } log_debug_info(time); @@ -321,16 +321,16 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta return CallbackReturn::ERROR; } } - + // Preallocate the matrices and vectors that will be used in the control loop if (!model_.existFrame(params_.end_effector_frame)) { RCLCPP_ERROR_STREAM( get_node()->get_logger(), "end_effector_frame '" << params_.end_effector_frame - << "' is not present in the robot model. Refusing to configure: " - "activating with an invalid frame results in undefined behavior " - "(out-of-bounds access into pinocchio::Data, manifesting as a " - "segfault or NaN/Inf in computed torques)."); + << "' is not present in the robot model. Refusing to configure: " + "activating with an invalid frame results in undefined behavior " + "(out-of-bounds access into pinocchio::Data, manifesting as a " + "segfault or NaN/Inf in computed torques)."); return CallbackReturn::ERROR; } end_effector_frame_id = model_.getFrameId(params_.end_effector_frame); @@ -352,8 +352,11 @@ 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_max_tau = Eigen::VectorXd::Zero(model_.nv); - setStiffnessAndDamping(); + if (!setStiffnessAndDamping()) { + return CallbackReturn::ERROR; + } new_target_pose_ = false; new_target_joint_ = false; @@ -402,8 +405,8 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta }; auto target_wrench_callback = - [this, target_wrench_topic]( - const std::shared_ptr msg) -> void { + [this, + target_wrench_topic](const std::shared_ptr msg) -> void { if (!check_topic_publisher_count(target_wrench_topic)) { RCLCPP_WARN_THROTTLE( get_node()->get_logger(), @@ -443,7 +446,9 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta 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", + RCLCPP_INFO( + get_node()->get_logger(), + "Variable stiffness topic: %s", params_.variable_stiffness.topic.c_str()); // Initialize all control vectors with appropriate dimensions @@ -503,7 +508,25 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta return CallbackReturn::SUCCESS; } -void CartesianController::setStiffnessAndDamping() { +bool CartesianController::setStiffnessAndDamping() { + const auto nullspace_stiffness_gains = + expand_nullspace_gains(params_.nullspace.stiffness, model_.nv); + const auto nullspace_damping_gains = expand_nullspace_gains(params_.nullspace.damping, model_.nv); + const auto nullspace_max_tau_gains = expand_nullspace_gains(params_.nullspace.max_tau, model_.nv); + if (!nullspace_stiffness_gains || !nullspace_damping_gains || !nullspace_max_tau_gains) { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "Each nullspace parameter must hold either a single value or one value per degree of " + "freedom (%d), but stiffness has %zu, damping has %zu and max_tau has %zu.", + model_.nv, + params_.nullspace.stiffness.size(), + params_.nullspace.damping.size(), + params_.nullspace.max_tau.size()); + return false; + } + if (use_topic_stiffness_) { stiffness = topic_stiffness_; } else { @@ -517,15 +540,27 @@ void CartesianController::setStiffnessAndDamping() { 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); + 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); + 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); } } @@ -542,19 +577,12 @@ void CartesianController::setStiffnessAndDamping() { nullspace_stiffness.setZero(); nullspace_damping.setZero(); + nullspace_stiffness.diagonal() = *nullspace_stiffness_gains; + nullspace_damping.diagonal() = + resolve_nullspace_damping(*nullspace_damping_gains, *nullspace_stiffness_gains); + nullspace_max_tau = *nullspace_max_tau_gains; - 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; - - // For nullspace, use explicit damping if > 0, otherwise compute from stiffness - if (params_.nullspace.damping > 0) { - nullspace_damping.diagonal() = params_.nullspace.damping * weights; - } else { - nullspace_damping.diagonal() = 2.0 * nullspace_stiffness.diagonal().cwiseSqrt(); - } + return true; } void CartesianController::updateCurrentState(bool initialize) { @@ -571,26 +599,21 @@ void CartesianController::updateCurrentState(bool initialize) { double q_meas = state_interfaces_[i].get_value(); double dq_meas = state_interfaces_[num_joints + i].get_value(); #endif - - q_ref[i] = initialize - ? q_meas - : exponential_moving_average(q_ref[i], q_target[i], params_.filter.q_ref); - - q[i] = 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_ref[i] = + initialize ? q_meas : exponential_moving_average(q_ref[i], q_target[i], params_.filter.q_ref); + + q[i] = 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]); } else { // simple revolute joint case q_pin[joint.idx_q()] = q[i]; } - dq[i] = initialize - ? dq_meas - : exponential_moving_average(dq[i], dq_meas, params_.filter.dq); + dq[i] = initialize ? dq_meas : exponential_moving_average(dq[i], dq_meas, params_.filter.dq); q_target[i] = initialize ? q_meas : q_target[i]; } @@ -598,7 +621,6 @@ void CartesianController::updateCurrentState(bool initialize) { CallbackReturn CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - // Update the current state with initial measurements (no EMA filtering) // to avoid large initial errors updateCurrentState(true); @@ -667,28 +689,48 @@ void CartesianController::parse_target_stiffness_() { } 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]}; + 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); + 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); + 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, + 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]); + 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 01a17c8..8c35c9f 100644 --- a/src/cartesian_controller.yaml +++ b/src/cartesian_controller.yaml @@ -125,25 +125,19 @@ cartesian_controller: nullspace: stiffness: - type: double - default_value: 1.0 - description: "Stiffness of the nullspace controller." + type: double_array + default_value: [1.0] + description: "Per-joint stiffnesses of the nullspace controller. If this is a single element, applies to all joints." validation: - bounds<>: [0.0, 500.0] + size_gt<>: [0] + element_bounds<>: [0.0, 100000.0] damping: - type: double - default_value: -1.0 - description: "Damping of the nullspace controller. If negative, then 2 sqrt(stiffness) is used." + type: double_array + default_value: [-1.0] + description: "Per-joint damping of the nullspace controller. If negative, then 2 sqrt(stiffness) is used. If this is a single element, applies to all joints." validation: - bounds<>: [-1.0, 500.0] - weights: - __map_joints: - value: - type: double - default_value: 1.0 - description: "Weights for each joint for the nullspace parameters." - validation: - bounds<>: [0.0, 500.0] + size_gt<>: [0] + element_bounds<>: [-1.0, 100000.0] regularization: type: double default_value: 0.000001 @@ -157,9 +151,12 @@ cartesian_controller: validation: one_of<>: [["kinematic", "dynamic", "none"]] max_tau: - type: double - default_value: 5 - description: "Maximal torque value that can be applied in the nullspace. This is used to be sure that we do not interfere with the main task." + type: double_array + default_value: [5.0] + description: "Maximal torque value that can be applied in the nullspace. This is used to be sure that we do not interfere with the main task. If this is a single element, applies to all joints." + validation: + size_gt<>: [0] + lower_element_bounds<>: [0.0] noise: add_random_noise: diff --git a/src/torque_feedback_controller.cpp b/src/torque_feedback_controller.cpp index cb55c55..4c71fee 100644 --- a/src/torque_feedback_controller.cpp +++ b/src/torque_feedback_controller.cpp @@ -5,6 +5,7 @@ // NOLINTBEGIN(build/include_order) #include #include +#include #include #include "crisp_controllers/utils/ros2_version.hpp" @@ -76,13 +77,8 @@ controller_interface::return_type TorqueFeedbackController::update( // Compute nullspace control to maintain initial joint positions Eigen::VectorXd q_error = q_ - q_init_; - double nullspace_damping = params_.nullspace.damping > 0 - ? params_.nullspace.damping - : 2.0 * sqrt(params_.nullspace.stiffness); - Eigen::VectorXd tau_secondary = - -params_.nullspace.stiffness * nullspace_weights_.cwiseProduct(q_error) - - nullspace_damping * nullspace_weights_.cwiseProduct(dq_); + -nullspace_stiffness_.cwiseProduct(q_error) - nullspace_damping_.cwiseProduct(dq_); // Compute nullspace projection based on projector type Eigen::MatrixXd Id_nv = Eigen::MatrixXd::Identity(model_.nv, model_.nv); @@ -108,10 +104,7 @@ controller_interface::return_type TorqueFeedbackController::update( Eigen::VectorXd tau_nullspace = nullspace_projection_ * tau_secondary; // Limit nullspace torques - for (int i = 0; i < num_joints_; i++) { - tau_nullspace[i] = - std::max(-params_.nullspace.max_tau, std::min(params_.nullspace.max_tau, tau_nullspace[i])); - } + tau_nullspace = tau_nullspace.cwiseMin(nullspace_max_tau_).cwiseMax(-nullspace_max_tau_); auto tau_d = -params_.k_fb * tau_ext_thresholded - params_.kd * dq_; auto tau_f = get_friction(dq_, friction_fp1_, friction_fp2_, friction_fp3_); @@ -130,10 +123,9 @@ controller_interface::return_type TorqueFeedbackController::update( params_listener_->refresh_dynamic_parameters(); params_ = params_listener_->get_params(); - // Update nullspace weights - for (size_t i = 0; i < joint_names_.size(); ++i) { - nullspace_weights_[i] = params_.nullspace.weights.joints_map.at(joint_names_[i]).value; - } + // An invalid live update must not bring down an active torque controller, so the previous + // gains are kept and the failure is only reported. + (void)setNullspaceGains(); return controller_interface::return_type::OK; } @@ -156,9 +148,11 @@ CallbackReturn TorqueFeedbackController::on_init() { tau_ext_ = Eigen::VectorXd::Zero(num_joints_); - nullspace_weights_ = Eigen::VectorXd::Ones(num_joints_); - for (size_t i = 0; i < joint_names_.size(); ++i) { - nullspace_weights_[i] = params_.nullspace.weights.joints_map.at(joint_names_[i]).value; + nullspace_stiffness_ = Eigen::VectorXd::Zero(num_joints_); + nullspace_damping_ = Eigen::VectorXd::Zero(num_joints_); + nullspace_max_tau_ = Eigen::VectorXd::Zero(num_joints_); + if (!setNullspaceGains()) { + return CallbackReturn::ERROR; } friction_fp1_ = @@ -178,6 +172,31 @@ CallbackReturn TorqueFeedbackController::on_init() { return CallbackReturn::SUCCESS; } +bool TorqueFeedbackController::setNullspaceGains() { + const auto stiffness_gains = expand_nullspace_gains(params_.nullspace.stiffness, num_joints_); + const auto damping_gains = expand_nullspace_gains(params_.nullspace.damping, num_joints_); + const auto max_tau_gains = expand_nullspace_gains(params_.nullspace.max_tau, num_joints_); + if (!stiffness_gains || !damping_gains || !max_tau_gains) { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "Each nullspace parameter must hold either a single value or one value per joint (%d), " + "but stiffness has %zu, damping has %zu and max_tau has %zu.", + num_joints_, + params_.nullspace.stiffness.size(), + params_.nullspace.damping.size(), + params_.nullspace.max_tau.size()); + return false; + } + + nullspace_stiffness_ = *stiffness_gains; + nullspace_damping_ = resolve_nullspace_damping(*damping_gains, *stiffness_gains); + nullspace_max_tau_ = *max_tau_gains; + + return true; +} + CallbackReturn TorqueFeedbackController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { // Get robot description for pinocchio model diff --git a/src/torque_feedback_controller.yaml b/src/torque_feedback_controller.yaml index 3c4342c..472042e 100644 --- a/src/torque_feedback_controller.yaml +++ b/src/torque_feedback_controller.yaml @@ -35,31 +35,26 @@ torque_feedback_controller: nullspace: stiffness: - type: double - default_value: 0.5 - description: "Stiffness of the nullspace controller to maintain joint positions" + type: double_array + default_value: [0.5] + description: "Per-joint stiffnesses of the nullspace controller. If this is a single element, applies to all joints." validation: - bounds<>: [0.0, 50.0] + size_gt<>: [0] + element_bounds<>: [0.0, 100000.0] damping: - type: double - default_value: -1.0 - description: "Damping of the nullspace controller. If negative, then 2*sqrt(stiffness) is used" + type: double_array + default_value: [-1.0] + description: "Per-joint damping of the nullspace controller. If negative, then 2 sqrt(stiffness) is used. If this is a single element, applies to all joints." validation: - bounds<>: [-1.0, 50.0] - weights: - __map_joints: - value: - type: double - default_value: 1.0 - description: "Weights for each joint for the nullspace control" - validation: - bounds<>: [0.0, 10.0] + size_gt<>: [0] + element_bounds<>: [-1.0, 100000.0] max_tau: - type: double - default_value: 2.0 - description: "Maximum torque value that can be applied in the nullspace" + type: double_array + default_value: [2.0] + description: "Maximal torque value that can be applied in the nullspace. This is used to be sure that we do not interfere with the main task. If this is a single element, applies to all joints." validation: - bounds<>: [0.0, 50.0] + size_gt<>: [0] + element_bounds<>: [0.0, 50.0] regularization: type: double default_value: 0.000001 diff --git a/tests/test_nullspace_gains.cpp b/tests/test_nullspace_gains.cpp new file mode 100644 index 0000000..5659466 --- /dev/null +++ b/tests/test_nullspace_gains.cpp @@ -0,0 +1,47 @@ +#include +#include "crisp_controllers/utils/nullspace_gains.hpp" + +using crisp_controllers::expand_nullspace_gains; +using crisp_controllers::resolve_nullspace_damping; + +TEST(ExpandNullspaceGainsTest, SingleValueIsBroadcast) { + auto gains = expand_nullspace_gains({5.0}, 3); + + ASSERT_TRUE(gains.has_value()); + + Eigen::VectorXd expected(3); + expected << 5.0, 5.0, 5.0; + EXPECT_TRUE(gains->isApprox(expected)); +} + +TEST(ExpandNullspaceGainsTest, OneValuePerDegreeOfFreedomIsKept) { + auto gains = expand_nullspace_gains({5.0, 4.0, 3.0}, 3); + + ASSERT_TRUE(gains.has_value()); + + Eigen::VectorXd expected(3); + expected << 5.0, 4.0, 3.0; + EXPECT_TRUE(gains->isApprox(expected)); +} + +TEST(ExpandNullspaceGainsTest, MismatchedSizeIsRejected) { + EXPECT_FALSE(expand_nullspace_gains({5.0, 4.0}, 3).has_value()); + EXPECT_FALSE(expand_nullspace_gains({5.0, 4.0, 3.0, 2.0}, 3).has_value()); + EXPECT_FALSE(expand_nullspace_gains({}, 3).has_value()); +} + +TEST(ResolveNullspaceDampingTest, NegativeEntriesBecomeCriticalDamping) { + Eigen::VectorXd damping(3), stiffness(3); + damping << -1.0, 0.0, 3.0; + stiffness << 25.0, 16.0, 9.0; + + Eigen::VectorXd expected(3); + expected << 10.0, 0.0, 3.0; + + EXPECT_TRUE(resolve_nullspace_damping(damping, stiffness).isApprox(expected)); +} + +int main(int argc, char ** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}