diff --git a/lbr_description/ros2_control/lbr_system_config.yaml b/lbr_description/ros2_control/lbr_system_config.yaml index 6e110244..8f96cf0d 100644 --- a/lbr_description/ros2_control/lbr_system_config.yaml +++ b/lbr_description/ros2_control/lbr_system_config.yaml @@ -7,15 +7,19 @@ hardware: port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection rt_prio: 80 # real-time priority for the control loop - pid_p: 0.1 # P gain for the joint position (useful for asynchronous control) - pid_i: 0.0 # I gain for the joint position command - pid_d: 0.0 # D gain for the joint position command - pid_i_max: 0.0 # max integral value for the joint position command - pid_i_min: 0.0 # min integral value for the joint position command - pid_antiwindup: false # enable antiwindup for the joint position command + # exponential moving average time constant for joint position commands [s]: + # Set tau > robot sample time for more smoothing on the commands + # Set tau << robot sample time for no smoothing on the commands + joint_position_tau: 0.04 command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] - external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz] - measured_torque_cutoff_frequency: 10 # low-pass filter for the joint torque measurements [Hz] + # exponential moving average time constant for external joint torque measurements [s]: + # Set tau > robot sample time for more smoothing on the external torque measurements + # Set tau << robot sample time for more smoothing on the external torque measurements + external_torque_tau: 0.04 + # exponential moving average time constant for joint torque measurements [s]: + # Set tau > robot sample time for more smoothing on the raw torque measurements + # Set tau << robot sample time for more smoothing on the raw torque measurements + measured_torque_tau: 0.04 open_loop: true # KUKA works the best in open_loop control mode estimated_ft_sensor: # estimates the external force-torque from the external joint torque values diff --git a/lbr_description/ros2_control/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro index fbe30cc6..f4a4452f 100644 --- a/lbr_description/ros2_control/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -27,15 +27,10 @@ ${system_config['hardware']['port_id']} ${system_config['hardware']['remote_host']} ${system_config['hardware']['rt_prio']} - ${system_config['hardware']['pid_p']} - ${system_config['hardware']['pid_i']} - ${system_config['hardware']['pid_d']} - ${system_config['hardware']['pid_i_max']} - ${system_config['hardware']['pid_i_min']} - ${system_config['hardware']['pid_antiwindup']} + ${system_config['hardware']['joint_position_tau']} ${system_config['hardware']['command_guard_variant']} - ${system_config['hardware']['external_torque_cutoff_frequency']} - ${system_config['hardware']['measured_torque_cutoff_frequency']} + ${system_config['hardware']['external_torque_tau']} + ${system_config['hardware']['measured_torque_tau']} ${system_config['hardware']['open_loop']} diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 1dc8a943..a26f445b 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -11,7 +11,6 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(control_toolbox REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) find_package(FRIClient REQUIRED) @@ -48,7 +47,6 @@ target_include_directories(lbr_fri_ros2 ) ament_target_dependencies(lbr_fri_ros2 - control_toolbox Eigen3 geometry_msgs kdl_parser @@ -64,7 +62,6 @@ target_link_libraries(lbr_fri_ros2 ament_export_targets(lbr_fri_ros2_export HAS_LIBRARY_TARGET) ament_export_dependencies( - control_toolbox eigen3_cmake_module Eigen3 FRIClient diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp index db194729..f44f33a8 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp @@ -11,7 +11,6 @@ #include "friClientVersion.h" #include "friLBRClient.h" -#include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/interfaces/base_command.hpp" #include "lbr_fri_ros2/interfaces/position_command.hpp" @@ -27,10 +26,10 @@ class AsyncClient : public KUKA::FRI::LBRClient { public: AsyncClient() = delete; AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mode, - const PIDParameters &pid_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant, - const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}, + const StateInterfaceParameters &state_interface_parameters = {0.04, 0.04}, const bool &open_loop = true); inline std::shared_ptr get_command_interface() { diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index 6f62bdd6..3773be57 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -3,57 +3,71 @@ #include #include -#include #include +#include -#include "control_toolbox/filters.hpp" -#include "control_toolbox/pid_ros.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" -#include "friLBRClient.h" - -#include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/types.hpp" namespace lbr_fri_ros2 { class ExponentialFilter { public: /** - * @brief Construct a new Exponential Filter object. Performs exponential smoothing with a - * #cutoff_frequency_ according to - * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. + * @brief Construct a new ExponentialFilter object. * */ ExponentialFilter(); /** - * @brief Construct a new Exponential Filter object. Performs exponential smoothing with a - * #cutoff_frequency_ according to - * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. + * @brief Construct a new ExponentialFilter object. + * + * @param[in] tau Time constant in seconds. + */ + ExponentialFilter(const double &tau); + + /** + * @brief Initialize the filter from members. Computes the new #alpha_ following + * alpha = 1.0 - exp(-sample_time / tau). + * + * If tau << sample_time => alpha -> 1 (very fast response, no smoothing) + * If tau >> sample_time => alpha -> 0 (very slow response, heavy smoothing) * - * @param[in] cutoff_frequency Frequency in Hz. * @param[in] sample_time Sample time in seconds. */ - ExponentialFilter(const double &cutoff_frequency, const double &sample_time); + void initialize(const double &sample_time); /** - * @brief Compute the exponential smoothing using the control_toolbox - * https://github.com/ros-controls/control_toolbox. + * @brief Initialize the filter. Computes the new #alpha_ following + * alpha = 1.0 - exp(-sample_time / tau). + * + * If tau << sample_time => alpha -> 1 (very fast response, no smoothing) + * If tau >> sample_time => alpha -> 0 (very slow response, heavy smoothing) + * + * @param[in] tau Time constant in seconds. + * @param[in] sample_time Sample time in seconds. + */ + void initialize(const double &tau, const double &sample_time); + + /** + * @brief Compute the exponential smoothing. Internally computes the new smoothed value following + * smoothed = alpha * current + (1 - alpha) * previous. * * @param[in] current The current value. * @param[in] previous The previous smoothed value. * @return double The returned smoothed value. */ inline double compute(const double ¤t, const double &previous) { - return filters::exponentialSmoothing(current, previous, alpha_); + return alpha_ * current + (1.0 - alpha_) * previous; }; /** - * @brief Set the cutoff frequency object. Internally computes the new #alpha_. + * @brief Get the time constant #tau_. * - * @param[in] cutoff_frequency Frequency in Hz. - * @param[in] sample_time Sample time in seconds. + * @return const double& */ - void set_cutoff_frequency(const double &cutoff_frequency, const double &sample_time); + inline const double &get_tau() const { return tau_; }; /** * @brief Get #sample_time_. @@ -70,16 +84,6 @@ class ExponentialFilter { inline const double &get_alpha() const { return alpha_; }; protected: - /** - * @brief Compute alpha given the cutoff frequency and the sample time. - * - * @param[in] cutoff_frequency Frequency in Hz. - * @param[in] sample_time Sample time in seconds. - * @return double Alpha based on - * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. - */ - double compute_alpha_(const double &cutoff_frequency, const double &sample_time); - /** * @brief Validate alpha in [0, 1]. * @@ -89,53 +93,30 @@ class ExponentialFilter { */ bool validate_alpha_(const double &alpha); - double cutoff_frequency_; /**< Frequency in Hz.*/ - double sample_time_; /**< Sample time in seconds.*/ - double - alpha_; /**< Alpha parameter based on - https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency.*/ + double tau_; /**< Time constant in seconds.*/ + double sample_time_; /**< Sample time in seconds.*/ + double alpha_; /**< Smoothing parameter in [0, 1].*/ }; class JointExponentialFilterArray { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointExponentialFilterArray"; + public: JointExponentialFilterArray() = default; + JointExponentialFilterArray(const double &tau); void compute(const double *const current, jnt_array_t_ref previous); - void initialize(const double &cutoff_frequency, const double &sample_time); + void compute(const_jnt_array_t_ref current, jnt_array_t_ref previous); + void initialize(const double &sample_time); + void initialize(const double &tau, const double &sample_time); inline const bool &is_initialized() const { return initialized_; }; -protected: - bool initialized_{false}; /**< True if initialized.*/ - ExponentialFilter exponential_filter_; /**< Exponential filter applied to all joints.*/ -}; - -struct PIDParameters { - double p{0.0}; /**< Proportional gain.*/ - double i{0.0}; /**< Integral gain.*/ - double d{0.0}; /**< Derivative gain.*/ - double i_max{0.0}; /**< Maximum integral value.*/ - double i_min{0.0}; /**< Minimum integral value.*/ - bool antiwindup{false}; /**< Antiwindup enabled.*/ -}; - -class JointPIDArray { -protected: - static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointPIDArray"; - using pid_array_t = std::array; - -public: - JointPIDArray() = delete; - JointPIDArray(const PIDParameters &pid_parameters); - - void compute(const_jnt_array_t_ref command_target, const_jnt_array_t_ref state, - const std::chrono::nanoseconds &dt, jnt_array_t_ref command); - void compute(const_jnt_array_t_ref command_target, const double *state, - const std::chrono::nanoseconds &dt, jnt_array_t_ref command); void log_info() const; protected: - PIDParameters pid_parameters_; /**< PID parameters for all joints.*/ - pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ + bool initialized_{false}; /**< True if initialized.*/ + ExponentialFilter exponential_filter_; /**< Exponential filter applied to all joints.*/ }; } // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__FILTERS_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp index bb46d0ae..4aa3c582 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp @@ -24,7 +24,7 @@ class BaseCommandInterface { public: BaseCommandInterface() = delete; - BaseCommandInterface(const PIDParameters &pid_parameters, + BaseCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); @@ -39,7 +39,7 @@ class BaseCommandInterface { protected: std::unique_ptr command_guard_; - JointPIDArray joint_position_pid_; + JointExponentialFilterArray joint_position_filter_; idl_command_t command_, command_target_; }; } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp index 24e8a324..399beed7 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp @@ -12,7 +12,7 @@ class PositionCommandInterface : public BaseCommandInterface { public: PositionCommandInterface() = delete; - PositionCommandInterface(const PIDParameters &pid_parameters, + PositionCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp index 23457637..ca0a6068 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp @@ -1,6 +1,8 @@ #ifndef LBR_FRI_ROS2__INTERFACES__STATE_HPP_ #define LBR_FRI_ROS2__INTERFACES__STATE_HPP_ + #include +#include #include #include "rclcpp/logger.hpp" @@ -14,8 +16,8 @@ namespace lbr_fri_ros2 { struct StateInterfaceParameters { - double external_torque_cutoff_frequency; /*Hz*/ - double measured_torque_cutoff_frequency; /*Hz*/ + double external_torque_tau; /*seconds*/ + double measured_torque_tau; /*seconds*/ }; class StateInterface { @@ -24,7 +26,7 @@ class StateInterface { public: StateInterface() = delete; - StateInterface(const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}); + StateInterface(const StateInterfaceParameters &state_interface_parameters = {0.04, 0.04}); inline const_idl_state_t_ref get_state() const { return state_; }; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp index e4ec7874..70536bee 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp @@ -12,7 +12,7 @@ class TorqueCommandInterface : public BaseCommandInterface { public: TorqueCommandInterface() = delete; - TorqueCommandInterface(const PIDParameters &pid_parameters, + TorqueCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp index 0c846ee4..4e68fae0 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp @@ -12,7 +12,7 @@ class WrenchCommandInterface : public BaseCommandInterface { public: WrenchCommandInterface() = delete; - WrenchCommandInterface(const PIDParameters &pid_parameters, + WrenchCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index d9cc4bf1..a13fe491 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -13,7 +13,6 @@ eigen - control_toolbox fri_client_sdk geometry_msgs kdl_parser diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 14ad96bf..7d11414c 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -2,7 +2,7 @@ namespace lbr_fri_ros2 { AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mode, - const PIDParameters &pid_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant, const StateInterfaceParameters &state_interface_parameters, @@ -26,16 +26,16 @@ AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mod #endif { command_interface_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, command_guard_variant); + joint_position_tau, command_guard_parameters, command_guard_variant); break; } case KUKA::FRI::EClientCommandMode::TORQUE: command_interface_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, command_guard_variant); + joint_position_tau, command_guard_parameters, command_guard_variant); break; case KUKA::FRI::EClientCommandMode::WRENCH: command_interface_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, command_guard_variant); + joint_position_tau, command_guard_parameters, command_guard_variant); break; default: std::string err = "Unsupported client command mode."; diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp index eb842ccc..2bf0ed2c 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -1,34 +1,41 @@ #include "lbr_fri_ros2/filters.hpp" namespace lbr_fri_ros2 { -ExponentialFilter::ExponentialFilter() : ExponentialFilter::ExponentialFilter(0, 0.0) {} +ExponentialFilter::ExponentialFilter() + : tau_(std::numeric_limits::quiet_NaN()), + sample_time_(std::numeric_limits::quiet_NaN()), + alpha_(std::numeric_limits::quiet_NaN()) {} -ExponentialFilter::ExponentialFilter(const double &cutoff_frequency, const double &sample_time) { - set_cutoff_frequency(cutoff_frequency, sample_time); +ExponentialFilter::ExponentialFilter(const double &tau) : tau_(tau) {} + +void ExponentialFilter::initialize(const double &sample_time) { + if (std::isnan(tau_)) { + throw std::runtime_error("Time constant must be set before initializing."); + } + return initialize(tau_, sample_time); } -void ExponentialFilter::set_cutoff_frequency(const double &cutoff_frequency, - const double &sample_time) { - cutoff_frequency_ = cutoff_frequency; - if (cutoff_frequency_ > (1. / sample_time)) { - cutoff_frequency_ = (1. / sample_time); +void ExponentialFilter::initialize(const double &tau, const double &sample_time) { + if (tau <= 0.0) { + throw std::runtime_error("Time constant must be positive and greater zero."); } - sample_time_ = sample_time; - alpha_ = compute_alpha_(cutoff_frequency, sample_time); - if (!validate_alpha_(alpha_)) { + if (sample_time < 0.0) { + throw std::runtime_error("Sample time must be positive."); + } + double alpha = 1.0 - std::exp(-sample_time / tau); + if (!validate_alpha_(alpha)) { throw std::runtime_error("Alpha is not within [0, 1]"); } -} - -double ExponentialFilter::compute_alpha_(const double &cutoff_frequency, - const double &sample_time) { - double omega_3db = 2.0 * M_PI * sample_time * cutoff_frequency; - return std::cos(omega_3db) - 1 + - std::sqrt(std::pow(std::cos(omega_3db), 2) - 4 * std::cos(omega_3db) + 3); + tau_ = tau; + sample_time_ = sample_time; + alpha_ = alpha; } bool ExponentialFilter::validate_alpha_(const double &alpha) { return alpha <= 1. && alpha >= 0.; } +JointExponentialFilterArray::JointExponentialFilterArray(const double &tau) + : exponential_filter_(tau) {} + void JointExponentialFilterArray::compute(const double *const current, jnt_array_t_ref previous) { std::for_each(current, current + N_JNTS, [&, i = 0](const auto ¤t_i) mutable { previous[i] = exponential_filter_.compute(current_i, previous[i]); @@ -36,47 +43,22 @@ void JointExponentialFilterArray::compute(const double *const current, jnt_array }); } -void JointExponentialFilterArray::initialize(const double &cutoff_frequency, - const double &sample_time) { - exponential_filter_.set_cutoff_frequency(cutoff_frequency, sample_time); - initialized_ = true; +void JointExponentialFilterArray::compute(const_jnt_array_t_ref current, jnt_array_t_ref previous) { + compute(current.data(), previous); } -JointPIDArray::JointPIDArray(const PIDParameters &pid_parameters) - : pid_parameters_(pid_parameters) // keep local copy of parameters since - // controller_toolbox::Pid::getGains is not const correct - // (i.e. can't be called in this->log_info) -{ - std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) { - pid.initPid(pid_parameters_.p, pid_parameters_.i, pid_parameters_.d, pid_parameters_.i_max, - pid_parameters_.i_min, pid_parameters_.antiwindup); - }); -} - -void JointPIDArray::compute(const_jnt_array_t_ref command_target, const_jnt_array_t_ref state, - const std::chrono::nanoseconds &dt, jnt_array_t_ref command) { - std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable { - command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt.count()); - ++i; - }); +void JointExponentialFilterArray::initialize(const double &sample_time) { + exponential_filter_.initialize(sample_time); + initialized_ = true; } -void JointPIDArray::compute(const_jnt_array_t_ref command_target, const double *state, - const std::chrono::nanoseconds &dt, jnt_array_t_ref command) { - std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable { - command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt.count()); - ++i; - }); +void JointExponentialFilterArray::initialize(const double &tau, const double &sample_time) { + exponential_filter_.initialize(tau, sample_time); + initialized_ = true; } -void JointPIDArray::log_info() const { +void JointExponentialFilterArray::log_info() const { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* p: %.1f", pid_parameters_.p); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i: %.1f", pid_parameters_.i); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* d: %.1f", pid_parameters_.d); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_max: %.1f", pid_parameters_.i_max); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_min: %.1f", pid_parameters_.i_min); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* antiwindup: %s", - pid_parameters_.antiwindup ? "true" : "false"); -}; + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* tau: %.5f s", exponential_filter_.get_tau()); +} } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/base_command.cpp b/lbr_fri_ros2/src/interfaces/base_command.cpp index f53bddeb..e43111ea 100644 --- a/lbr_fri_ros2/src/interfaces/base_command.cpp +++ b/lbr_fri_ros2/src/interfaces/base_command.cpp @@ -2,10 +2,10 @@ namespace lbr_fri_ros2 { -BaseCommandInterface::BaseCommandInterface(const PIDParameters &pid_parameters, +BaseCommandInterface::BaseCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : joint_position_pid_(pid_parameters) { + : joint_position_filter_(joint_position_tau) { command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); }; @@ -18,6 +18,6 @@ void BaseCommandInterface::init_command(const_idl_state_t_ref state) { void BaseCommandInterface::log_info() const { command_guard_->log_info(); - joint_position_pid_.log_info(); + joint_position_filter_.log_info(); } } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index b80530f3..95533981 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -2,9 +2,9 @@ namespace lbr_fri_ros2 { PositionCommandInterface::PositionCommandInterface( - const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + : BaseCommandInterface(joint_position_tau, command_guard_parameters, command_guard_variant) {} void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) { @@ -40,11 +40,11 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command throw std::runtime_error(err); } - // PID - joint_position_pid_.compute( - command_target_.joint_position, state.measured_joint_position, - std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), - command_.joint_position); + // exponential smooth + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); + } + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/src/interfaces/state.cpp b/lbr_fri_ros2/src/interfaces/state.cpp index 3054bf55..d34b457e 100644 --- a/lbr_fri_ros2/src/interfaces/state.cpp +++ b/lbr_fri_ros2/src/interfaces/state.cpp @@ -15,7 +15,6 @@ void StateInterface::set_state(const_fri_state_t_ref state) { state_.connection_quality = state.getConnectionQuality(); state_.control_mode = state.getControlMode(); state_.drive_state = state.getDriveState(); - external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); if (state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_WAIT || state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { std::memcpy(state_.ipo_joint_position.data(), state.getIpoJointPosition(), @@ -23,7 +22,6 @@ void StateInterface::set_state(const_fri_state_t_ref state) { } std::memcpy(state_.measured_joint_position.data(), state.getMeasuredJointPosition(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); - measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); state_.operation_mode = state.getOperationMode(); state_.overlay_type = state.getOverlayType(); state_.safety_state = state.getSafetyState(); @@ -34,9 +32,14 @@ void StateInterface::set_state(const_fri_state_t_ref state) { state_.tracking_performance = state.getTrackingPerformance(); if (!external_torque_filter_.is_initialized() || !measured_torque_filter_.is_initialized()) { - // initialize once state_ is available + // initialize state_.sample_time is available init_filters_(); } + + // only compute after state_.sample_time is available + external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); + measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); + state_initialized_ = true; }; @@ -52,7 +55,6 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, state_.connection_quality = state.getConnectionQuality(); state_.control_mode = state.getControlMode(); state_.drive_state = state.getDriveState(); - external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); if (state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_WAIT || state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { std::memcpy(state_.ipo_joint_position.data(), state.getIpoJointPosition(), @@ -60,7 +62,6 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, } std::memcpy(state_.measured_joint_position.data(), joint_position.data(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); - measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); state_.operation_mode = state.getOperationMode(); state_.overlay_type = state.getOverlayType(); state_.safety_state = state.getSafetyState(); @@ -71,24 +72,27 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, state_.tracking_performance = state.getTrackingPerformance(); if (!external_torque_filter_.is_initialized() || !measured_torque_filter_.is_initialized()) { - // initialize once state_ is available + // initialize state_.sample_time is available init_filters_(); } + + // only compute after state_.sample_time is available + external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); + measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); + state_initialized_ = true; } void StateInterface::init_filters_() { - external_torque_filter_.initialize(parameters_.external_torque_cutoff_frequency, - state_.sample_time); - measured_torque_filter_.initialize(parameters_.measured_torque_cutoff_frequency, - state_.sample_time); + external_torque_filter_.initialize(parameters_.external_torque_tau, state_.sample_time); + measured_torque_filter_.initialize(parameters_.measured_torque_tau, state_.sample_time); } void StateInterface::log_info() const { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* external_torque_cutoff_frequency: %.1f Hz", - parameters_.external_torque_cutoff_frequency); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* measured_torque_cutoff_frequency: %.1f Hz", - parameters_.measured_torque_cutoff_frequency); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* external_torque_tau: %.5f s", + parameters_.external_torque_tau); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* measured_torque_tau: %.5f s", + parameters_.measured_torque_tau); } } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index b658b266..54f53a2b 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -2,9 +2,9 @@ namespace lbr_fri_ros2 { TorqueCommandInterface::TorqueCommandInterface( - const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + : BaseCommandInterface(joint_position_tau, command_guard_parameters, command_guard_variant) {} void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) { @@ -30,12 +30,11 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, throw std::runtime_error(err); } - // PID - joint_position_pid_.compute( - command_target_.joint_position, state.measured_joint_position, - std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), - command_.joint_position); - command_.torque = command_target_.torque; + // exponential smooth + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); + } + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index 9dd9618a..1a1c0ab7 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -2,9 +2,9 @@ namespace lbr_fri_ros2 { WrenchCommandInterface::WrenchCommandInterface( - const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + : BaseCommandInterface(joint_position_tau, command_guard_parameters, command_guard_variant) {} void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) { @@ -28,12 +28,11 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, throw std::runtime_error(err); } - // PID - joint_position_pid_.compute( - command_target_.joint_position, state.measured_joint_position, - std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), - command_.joint_position); - command_.wrench = command_target_.wrench; + // exponential smooth + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); + } + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp index 127fd2ee..d662643a 100644 --- a/lbr_fri_ros2/test/test_command_interfaces.cpp +++ b/lbr_fri_ros2/test/test_command_interfaces.cpp @@ -18,7 +18,6 @@ class TestCommandInterfaces : public ::testing::Test { public: TestCommandInterfaces() : random_engine_(std::random_device{}()) { - pid_params_ = lbr_fri_ros2::PIDParameters(); cmd_guard_params_ = lbr_fri_ros2::CommandGuardParameters(); state_interface_params_ = lbr_fri_ros2::StateInterfaceParameters(); @@ -40,17 +39,17 @@ class TestCommandInterfaces : public ::testing::Test { case KUKA::FRI::EClientCommandMode::JOINT_POSITION: #endif { - command_interface_ = - std::make_shared(pid_params_, cmd_guard_params_); + command_interface_ = std::make_shared( + joint_position_tau_, cmd_guard_params_); break; } case KUKA::FRI::EClientCommandMode::TORQUE: - command_interface_ = - std::make_shared(pid_params_, cmd_guard_params_); + command_interface_ = std::make_shared( + joint_position_tau_, cmd_guard_params_); break; case KUKA::FRI::EClientCommandMode::WRENCH: - command_interface_ = - std::make_shared(pid_params_, cmd_guard_params_); + command_interface_ = std::make_shared( + joint_position_tau_, cmd_guard_params_); break; default: throw std::runtime_error("Unsupported client command mode."); @@ -129,7 +128,7 @@ class TestCommandInterfaces : public ::testing::Test { std::default_random_engine random_engine_; std::uniform_real_distribution uniform_real_dist_{-1.0, 1.0}; - lbr_fri_ros2::PIDParameters pid_params_; + double joint_position_tau_{0.04}; lbr_fri_ros2::CommandGuardParameters cmd_guard_params_; lbr_fri_ros2::StateInterfaceParameters state_interface_params_; diff --git a/lbr_fri_ros2/test/test_position_command.cpp b/lbr_fri_ros2/test/test_position_command.cpp index e2f7830d..7da064aa 100644 --- a/lbr_fri_ros2/test/test_position_command.cpp +++ b/lbr_fri_ros2/test/test_position_command.cpp @@ -8,12 +8,11 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" int main() { rclcpp::init(0, nullptr); - lbr_fri_ros2::PIDParameters pid_params; + double joint_position_tau = 0.04; lbr_fri_ros2::CommandGuardParameters cmd_guard_params; lbr_fri_ros2::StateInterfaceParameters state_interface_params; @@ -22,8 +21,6 @@ int main() { // 1. read this info!!!! from robot description - pid_params.p = 1.0; - cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., @@ -39,9 +36,9 @@ int main() { 200., 200., 200., 200., 200., 200., 200., }; - auto client = std::make_shared(KUKA::FRI::EClientCommandMode::POSITION, - pid_params, cmd_guard_params, "default", - state_interface_params, true); + auto client = std::make_shared( + KUKA::FRI::EClientCommandMode::POSITION, joint_position_tau, cmd_guard_params, "default", + state_interface_params, true); lbr_fri_ros2::App app(client); app.open_udp_socket(); diff --git a/lbr_fri_ros2/test/test_torque_command.cpp b/lbr_fri_ros2/test/test_torque_command.cpp index 9c580828..45472578 100644 --- a/lbr_fri_ros2/test/test_torque_command.cpp +++ b/lbr_fri_ros2/test/test_torque_command.cpp @@ -8,12 +8,11 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" int main() { rclcpp::init(0, nullptr); - lbr_fri_ros2::PIDParameters pid_params; + double joint_position_tau = 0.04; lbr_fri_ros2::CommandGuardParameters cmd_guard_params; lbr_fri_ros2::StateInterfaceParameters state_interface_params; @@ -22,8 +21,6 @@ int main() { // 1. read this info!!!! from robot description - pid_params.p = 10.0; - cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., @@ -39,9 +36,9 @@ int main() { 200., 200., 200., 200., 200., 200., 200., }; - auto client = std::make_shared(KUKA::FRI::EClientCommandMode::TORQUE, - pid_params, cmd_guard_params, "default", - state_interface_params, true); + auto client = std::make_shared( + KUKA::FRI::EClientCommandMode::TORQUE, joint_position_tau, cmd_guard_params, "default", + state_interface_params, true); lbr_fri_ros2::App app(client); app.open_udp_socket(); diff --git a/lbr_fri_ros2/test/test_wrench_command.cpp b/lbr_fri_ros2/test/test_wrench_command.cpp index 9ad8b315..e353476b 100644 --- a/lbr_fri_ros2/test/test_wrench_command.cpp +++ b/lbr_fri_ros2/test/test_wrench_command.cpp @@ -8,12 +8,11 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" int main() { rclcpp::init(0, nullptr); - lbr_fri_ros2::PIDParameters pid_params; + double joint_position_tau = 0.04; lbr_fri_ros2::CommandGuardParameters cmd_guard_params; lbr_fri_ros2::StateInterfaceParameters state_interface_params; @@ -22,8 +21,6 @@ int main() { // 1. read this info!!!! from robot description - pid_params.p = 10.0; - cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., @@ -39,9 +36,9 @@ int main() { 200., 200., 200., 200., 200., 200., 200., }; - auto client = std::make_shared(KUKA::FRI::EClientCommandMode::WRENCH, - pid_params, cmd_guard_params, "default", - state_interface_params, true); + auto client = std::make_shared( + KUKA::FRI::EClientCommandMode::WRENCH, joint_position_tau, cmd_guard_params, "default", + state_interface_params, true); lbr_fri_ros2::App app(client); app.open_udp_socket(); diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index 9a43948b..a7448a3c 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -23,7 +23,6 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/ft_estimator.hpp" #include "lbr_fri_ros2/interfaces/state.hpp" @@ -44,15 +43,10 @@ struct SystemInterfaceParameters { const char *remote_host{nullptr}; int32_t rt_prio{80}; bool open_loop{true}; - double pid_p{0.0}; - double pid_i{0.0}; - double pid_d{0.0}; - double pid_i_max{0.0}; - double pid_i_min{0.0}; - double pid_antiwindup{0.0}; + double joint_position_tau{0.04}; std::string command_guard_variant{"default"}; - double external_torque_cutoff_frequency{10.0}; - double measured_torque_cutoff_frequency{10.0}; + double external_torque_tau{0.04}; + double measured_torque_tau{0.04}; }; struct EstimatedFTSensorParameters { diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 91cbf710..ebab71e4 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -18,15 +18,8 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { } // setup driver - lbr_fri_ros2::PIDParameters pid_parameters; lbr_fri_ros2::CommandGuardParameters command_guard_parameters; lbr_fri_ros2::StateInterfaceParameters state_interface_parameters; - pid_parameters.p = parameters_.pid_p; - pid_parameters.i = parameters_.pid_i; - pid_parameters.d = parameters_.pid_d; - pid_parameters.i_max = parameters_.pid_i_max; - pid_parameters.i_min = parameters_.pid_i_min; - pid_parameters.antiwindup = parameters_.pid_antiwindup; for (std::size_t idx = 0; idx < system_info.joints.size(); ++idx) { command_guard_parameters.joint_names[idx] = system_info.joints[idx].name; command_guard_parameters.max_positions[idx] = @@ -38,14 +31,12 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { command_guard_parameters.max_torques[idx] = std::stod(system_info.joints[idx].parameters.at("max_torque")); } - state_interface_parameters.external_torque_cutoff_frequency = - parameters_.external_torque_cutoff_frequency; - state_interface_parameters.measured_torque_cutoff_frequency = - parameters_.measured_torque_cutoff_frequency; + state_interface_parameters.external_torque_tau = parameters_.external_torque_tau; + state_interface_parameters.measured_torque_tau = parameters_.measured_torque_tau; try { async_client_ptr_ = std::make_shared( - parameters_.client_command_mode, pid_parameters, command_guard_parameters, + parameters_.client_command_mode, parameters_.joint_position_tau, command_guard_parameters, parameters_.command_guard_variant, state_interface_parameters, parameters_.open_loop); app_ptr_ = std::make_unique(async_client_ptr_); } catch (const std::exception &e) { @@ -412,17 +403,10 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo & info_.hardware_parameters["pid_antiwindup"].end(), info_.hardware_parameters["pid_antiwindup"].begin(), ::tolower); // convert to lower case - parameters_.pid_p = std::stod(info_.hardware_parameters["pid_p"]); - parameters_.pid_i = std::stod(info_.hardware_parameters["pid_i"]); - parameters_.pid_d = std::stod(info_.hardware_parameters["pid_d"]); - parameters_.pid_i_max = std::stod(info_.hardware_parameters["pid_i_max"]); - parameters_.pid_i_min = std::stod(info_.hardware_parameters["pid_i_min"]); - parameters_.pid_antiwindup = info_.hardware_parameters["pid_antiwindup"] == "true"; + parameters_.joint_position_tau = std::stod(info_.hardware_parameters["joint_position_tau"]); parameters_.command_guard_variant = system_info.hardware_parameters.at("command_guard_variant"); - parameters_.external_torque_cutoff_frequency = - std::stod(info_.hardware_parameters["external_torque_cutoff_frequency"]); - parameters_.measured_torque_cutoff_frequency = - std::stod(info_.hardware_parameters["measured_torque_cutoff_frequency"]); + parameters_.external_torque_tau = std::stod(info_.hardware_parameters["external_torque_tau"]); + parameters_.measured_torque_tau = std::stod(info_.hardware_parameters["measured_torque_tau"]); } catch (const std::out_of_range &e) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR