redundant pid dt (#171, #187)

lbr-stack · Jun 11, 2024 · f4943eb · f4943eb
1 parent 14365a6
commit f4943eb
Show file tree

Hide file tree

Showing 8 changed files with 28 additions and 33 deletions.
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp
@@ -126,20 +126,18 @@ class JointPIDArray {
   using pid_array_t = std::array<control_toolbox::Pid, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
 
 public:
-  JointPIDArray() = default;
+  JointPIDArray() = delete;
+  JointPIDArray(const PIDParameters &pid_parameters);
 
   void compute(const value_array_t &command_target, const value_array_t &state,
                const std::chrono::nanoseconds &dt, value_array_t &command);
   void compute(const value_array_t &command_target, const double *state,
                const std::chrono::nanoseconds &dt, value_array_t &command);
-  void initialize(const PIDParameters &pid_parameters, const double &dt);
-  inline const bool &is_initialized() const { return initialized_; };
-
   void log_info() const;
 
 protected:
-  bool initialized_{false};     /**< True if initialized.*/
-  pid_array_t pid_controllers_; /**< PID controllers for each joint.*/
+  PIDParameters pid_parameters_; /**< PID parameters for all joints.*/
+  pid_array_t pid_controllers_;  /**< PID controllers for each joint.*/
 };
 } // end of 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
@@ -49,7 +49,6 @@ class BaseCommandInterface {
 
 protected:
   std::unique_ptr<CommandGuard> command_guard_;
-  PIDParameters pid_parameters_;
   JointPIDArray joint_position_pid_;
   idl_command_t command_, command_target_;
 };

diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp
@@ -43,6 +43,17 @@ void JointExponentialFilterArray::initialize(const double &cutoff_frequency,
   initialized_ = true;
 }
 
+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 value_array_t &command_target, const value_array_t &state,
                             const std::chrono::nanoseconds &dt, value_array_t &command) {
   std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable {
@@ -59,11 +70,14 @@ void JointPIDArray::compute(const value_array_t &command_target, const double *s
   });
 }
 
-void JointPIDArray::initialize(const PIDParameters &pid_parameters, const double &dt) {
-  std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) {
-    pid.initPid(pid_parameters.p * dt, pid_parameters.i * dt, pid_parameters.d * dt,
-                pid_parameters.i_max * dt, pid_parameters.i_min * dt, pid_parameters.antiwindup);
-  });
-  initialized_ = true;
-}
+void JointPIDArray::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");
+};
 } // end of namespace lbr_fri_ros2
diff --git a/lbr_fri_ros2/src/interfaces/base_command.cpp b/lbr_fri_ros2/src/interfaces/base_command.cpp
@@ -5,7 +5,7 @@ namespace lbr_fri_ros2 {
 BaseCommandInterface::BaseCommandInterface(const PIDParameters &pid_parameters,
                                            const CommandGuardParameters &command_guard_parameters,
                                            const std::string &command_guard_variant)
-    : pid_parameters_(pid_parameters) {
+    : joint_position_pid_(pid_parameters) {
   command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant);
 };
 
@@ -18,13 +18,6 @@ void BaseCommandInterface::init_command(const_idl_state_t_ref state) {
 
 void BaseCommandInterface::log_info() const {
   command_guard_->log_info();
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*** Parameters:");
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*   pid.p: %.1f", pid_parameters_.p);
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*   pid.i: %.1f", pid_parameters_.i);
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*   pid.d: %.1f", pid_parameters_.d);
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*   pid.i_max: %.1f", pid_parameters_.i_max);
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*   pid.i_min: %.1f", pid_parameters_.i_min);
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*   pid.antiwindup: %s",
-              pid_parameters_.antiwindup ? "true" : "false");
+  joint_position_pid_.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
@@ -41,9 +41,6 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command
   }
 
   // PID
-  if (!joint_position_pid_.is_initialized()) {
-    joint_position_pid_.initialize(pid_parameters_, state.sample_time);
-  }
   joint_position_pid_.compute(
       command_target_.joint_position, state.measured_joint_position,
       std::chrono::nanoseconds(static_cast<int64_t>(state.sample_time * 1.e9)),

diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp
@@ -31,9 +31,6 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
   }
 
   // PID
-  if (!joint_position_pid_.is_initialized()) {
-    joint_position_pid_.initialize(pid_parameters_, state.sample_time);
-  }
   joint_position_pid_.compute(
       command_target_.joint_position, state.measured_joint_position,
       std::chrono::nanoseconds(static_cast<int64_t>(state.sample_time * 1.e9)),

diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp
@@ -29,9 +29,6 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
   }
 
   // PID
-  if (!joint_position_pid_.is_initialized()) {
-    joint_position_pid_.initialize(pid_parameters_, state.sample_time);
-  }
   joint_position_pid_.compute(
       command_target_.joint_position, state.measured_joint_position,
       std::chrono::nanoseconds(static_cast<int64_t>(state.sample_time * 1.e9)),

diff --git a/lbr_ros2_control/config/lbr_system_parameters.yaml b/lbr_ros2_control/config/lbr_system_parameters.yaml
@@ -7,7 +7,7 @@ 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: 10.0 # P gain for the joint position  (useful for asynchronous control)
+  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