Skip to content

Commit f4943eb

Browse files
mhubiimhubii
committed
redundant pid dt (#171, #187)
1 parent 14365a6 commit f4943eb

File tree

8 files changed

+28
-33
lines changed

8 files changed

+28
-33
lines changed

‎lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp

+4-6
Original file line numberDiff line numberDiff line change
@@ -126,20 +126,18 @@ class JointPIDArray {
126126
using pid_array_t = std::array<control_toolbox::Pid, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
127127

128128
public:
129-
JointPIDArray() = default;
129+
JointPIDArray() = delete;
130+
JointPIDArray(const PIDParameters &pid_parameters);
130131

131132
void compute(const value_array_t &command_target, const value_array_t &state,
132133
const std::chrono::nanoseconds &dt, value_array_t &command);
133134
void compute(const value_array_t &command_target, const double *state,
134135
const std::chrono::nanoseconds &dt, value_array_t &command);
135-
void initialize(const PIDParameters &pid_parameters, const double &dt);
136-
inline const bool &is_initialized() const { return initialized_; };
137-
138136
void log_info() const;
139137

140138
protected:
141-
bool initialized_{false}; /**< True if initialized.*/
142-
pid_array_t pid_controllers_; /**< PID controllers for each joint.*/
139+
PIDParameters pid_parameters_; /**< PID parameters for all joints.*/
140+
pid_array_t pid_controllers_; /**< PID controllers for each joint.*/
143141
};
144142
} // end of namespace lbr_fri_ros2
145143
#endif // LBR_FRI_ROS2__FILTERS_HPP_

‎lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,6 @@ class BaseCommandInterface {
4949

5050
protected:
5151
std::unique_ptr<CommandGuard> command_guard_;
52-
PIDParameters pid_parameters_;
5352
JointPIDArray joint_position_pid_;
5453
idl_command_t command_, command_target_;
5554
};

‎lbr_fri_ros2/src/filters.cpp

+21-7
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,17 @@ void JointExponentialFilterArray::initialize(const double &cutoff_frequency,
4343
initialized_ = true;
4444
}
4545

46+
JointPIDArray::JointPIDArray(const PIDParameters &pid_parameters)
47+
: pid_parameters_(pid_parameters) // keep local copy of parameters since
48+
// controller_toolbox::Pid::getGains is not const correct
49+
// (i.e. can't be called in this->log_info)
50+
{
51+
std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) {
52+
pid.initPid(pid_parameters_.p, pid_parameters_.i, pid_parameters_.d, pid_parameters_.i_max,
53+
pid_parameters_.i_min, pid_parameters_.antiwindup);
54+
});
55+
}
56+
4657
void JointPIDArray::compute(const value_array_t &command_target, const value_array_t &state,
4758
const std::chrono::nanoseconds &dt, value_array_t &command) {
4859
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
5970
});
6071
}
6172

62-
void JointPIDArray::initialize(const PIDParameters &pid_parameters, const double &dt) {
63-
std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) {
64-
pid.initPid(pid_parameters.p * dt, pid_parameters.i * dt, pid_parameters.d * dt,
65-
pid_parameters.i_max * dt, pid_parameters.i_min * dt, pid_parameters.antiwindup);
66-
});
67-
initialized_ = true;
68-
}
73+
void JointPIDArray::log_info() const {
74+
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:");
75+
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* p: %.1f", pid_parameters_.p);
76+
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i: %.1f", pid_parameters_.i);
77+
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* d: %.1f", pid_parameters_.d);
78+
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_max: %.1f", pid_parameters_.i_max);
79+
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_min: %.1f", pid_parameters_.i_min);
80+
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* antiwindup: %s",
81+
pid_parameters_.antiwindup ? "true" : "false");
82+
};
6983
} // end of namespace lbr_fri_ros2

‎lbr_fri_ros2/src/interfaces/base_command.cpp

+2-9
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@ namespace lbr_fri_ros2 {
55
BaseCommandInterface::BaseCommandInterface(const PIDParameters &pid_parameters,
66
const CommandGuardParameters &command_guard_parameters,
77
const std::string &command_guard_variant)
8-
: pid_parameters_(pid_parameters) {
8+
: joint_position_pid_(pid_parameters) {
99
command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant);
1010
};
1111

@@ -18,13 +18,6 @@ void BaseCommandInterface::init_command(const_idl_state_t_ref state) {
1818

1919
void BaseCommandInterface::log_info() const {
2020
command_guard_->log_info();
21-
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*** Parameters:");
22-
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.p: %.1f", pid_parameters_.p);
23-
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i: %.1f", pid_parameters_.i);
24-
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.d: %.1f", pid_parameters_.d);
25-
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i_max: %.1f", pid_parameters_.i_max);
26-
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i_min: %.1f", pid_parameters_.i_min);
27-
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.antiwindup: %s",
28-
pid_parameters_.antiwindup ? "true" : "false");
21+
joint_position_pid_.log_info();
2922
}
3023
} // namespace lbr_fri_ros2

‎lbr_fri_ros2/src/interfaces/position_command.cpp

-3
Original file line numberDiff line numberDiff line change
@@ -41,9 +41,6 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command
4141
}
4242

4343
// PID
44-
if (!joint_position_pid_.is_initialized()) {
45-
joint_position_pid_.initialize(pid_parameters_, state.sample_time);
46-
}
4744
joint_position_pid_.compute(
4845
command_target_.joint_position, state.measured_joint_position,
4946
std::chrono::nanoseconds(static_cast<int64_t>(state.sample_time * 1.e9)),

‎lbr_fri_ros2/src/interfaces/torque_command.cpp

-3
Original file line numberDiff line numberDiff line change
@@ -31,9 +31,6 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
3131
}
3232

3333
// PID
34-
if (!joint_position_pid_.is_initialized()) {
35-
joint_position_pid_.initialize(pid_parameters_, state.sample_time);
36-
}
3734
joint_position_pid_.compute(
3835
command_target_.joint_position, state.measured_joint_position,
3936
std::chrono::nanoseconds(static_cast<int64_t>(state.sample_time * 1.e9)),

‎lbr_fri_ros2/src/interfaces/wrench_command.cpp

-3
Original file line numberDiff line numberDiff line change
@@ -29,9 +29,6 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
2929
}
3030

3131
// PID
32-
if (!joint_position_pid_.is_initialized()) {
33-
joint_position_pid_.initialize(pid_parameters_, state.sample_time);
34-
}
3532
joint_position_pid_.compute(
3633
command_target_.joint_position, state.measured_joint_position,
3734
std::chrono::nanoseconds(static_cast<int64_t>(state.sample_time * 1.e9)),

‎lbr_ros2_control/config/lbr_system_parameters.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@ hardware:
77
port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups
88
remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection
99
rt_prio: 80 # real-time priority for the control loop
10-
pid_p: 10.0 # P gain for the joint position (useful for asynchronous control)
10+
pid_p: 0.1 # P gain for the joint position (useful for asynchronous control)
1111
pid_i: 0.0 # I gain for the joint position command
1212
pid_d: 0.0 # D gain for the joint position command
1313
pid_i_max: 0.0 # max integral value for the joint position command

0 commit comments

Comments
 (0)