pid -> exp smooth + exp smooth with time constant (#163)

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

lbr_description/ros2_control/lbr_system_interface.xacro

@@ -27,15 +27,10 @@
                     <param name="port_id">${system_config['hardware']['port_id']}</param>
                     <param name="remote_host">${system_config['hardware']['remote_host']}</param>
                     <param name="rt_prio">${system_config['hardware']['rt_prio']}</param>
-                    <param name="pid_p">${system_config['hardware']['pid_p']}</param>
-                    <param name="pid_i">${system_config['hardware']['pid_i']}</param>
-                    <param name="pid_d">${system_config['hardware']['pid_d']}</param>
-                    <param name="pid_i_max">${system_config['hardware']['pid_i_max']}</param>
-                    <param name="pid_i_min">${system_config['hardware']['pid_i_min']}</param>
-                    <param name="pid_antiwindup">${system_config['hardware']['pid_antiwindup']}</param>
+                    <param name="joint_position_tau">${system_config['hardware']['joint_position_tau']}</param>
                     <param name="command_guard_variant">${system_config['hardware']['command_guard_variant']}</param>
-                    <param name="external_torque_cutoff_frequency">${system_config['hardware']['external_torque_cutoff_frequency']}</param>
-                    <param name="measured_torque_cutoff_frequency">${system_config['hardware']['measured_torque_cutoff_frequency']}</param>
+                    <param name="external_torque_tau">${system_config['hardware']['external_torque_tau']}</param>
+                    <param name="measured_torque_tau">${system_config['hardware']['measured_torque_tau']}</param>
                     <param name="open_loop">${system_config['hardware']['open_loop']}</param>
                 </hardware>
             </xacro:if>

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

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<BaseCommandInterface> get_command_interface() {

lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp

@@ -3,57 +3,71 @@
 
 #include <algorithm>
 #include <array>
-#include <chrono>
 #include <cmath>
+#include <limits>
 
-#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 &current, 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<control_toolbox::Pid, N_JNTS>;
-
-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_

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<CommandGuard> command_guard_;
-  JointPIDArray joint_position_pid_;
+  JointExponentialFilterArray joint_position_filter_;
   idl_command_t command_, command_target_;
 };
 } // namespace lbr_fri_ros2

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");
 

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 <atomic>
+#include <cstring>
 #include <string>
 
 #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_; };
 

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");
 

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");
 

lbr_fri_ros2/package.xml

@@ -13,7 +13,6 @@
 
   <build_depend>eigen</build_depend>
 
-  <depend>control_toolbox</depend>
   <depend>fri_client_sdk</depend>
   <depend>geometry_msgs</depend>
   <depend>kdl_parser</depend>

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<PositionCommandInterface>(
-        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<TorqueCommandInterface>(
-        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<WrenchCommandInterface>(
-        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.";