Skip to content

Commit 623dc5d

Browse files
christophfroehlichchristophfroehlich
authored
Reset PID controllers on activation and add save_i_term parameter (#1507)
1 parent c9ca6f7 commit 623dc5d

File tree

6 files changed

+172
-0
lines changed

6 files changed

+172
-0
lines changed

‎doc/release_notes.rst

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,7 @@ mecanum_drive_controller
6060
pid_controller
6161
************************
6262
* 🚀 The PID controller was added 🎉 (`#434 <https://github.com/ros-controls/ros2_controllers/pull/434>`_).
63+
* Add ``save_i_term`` parameter to control retention of integral state after re-activation (`#1507 <https://github.com/ros-controls/ros2_controllers/pull/1507>`_).
6364

6465
steering_controllers_library
6566
********************************

‎pid_controller/src/pid_controller.cpp

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -442,6 +442,11 @@ controller_interface::CallbackReturn PidController::on_activate(
442442
measured_state_values_.assign(
443443
measured_state_values_.size(), std::numeric_limits<double>::quiet_NaN());
444444

445+
// prefixed save_i_term parameter is read from ROS parameters
446+
for (auto & pid : pids_)
447+
{
448+
pid->reset();
449+
}
445450
return controller_interface::CallbackReturn::SUCCESS;
446451
}
447452

‎pid_controller/src/pid_controller.yaml

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -89,3 +89,8 @@ pid_controller:
8989
description: "For joints that wrap around (i.e., are continuous).
9090
Normalizes position-error to -pi to pi."
9191
}
92+
save_i_term: {
93+
type: bool,
94+
default_value: true,
95+
description: "Indicating if integral term is retained after re-activation"
96+
}

‎pid_controller/test/pid_controller_params.yaml

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,30 @@ test_pid_controller:
1010
gains:
1111
joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0}
1212

13+
test_save_i_term_off:
14+
ros__parameters:
15+
dof_names:
16+
- joint1
17+
18+
command_interface: position
19+
20+
reference_and_state_interfaces: ["position"]
21+
22+
gains:
23+
joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: false}
24+
25+
26+
test_save_i_term_on:
27+
ros__parameters:
28+
dof_names:
29+
- joint1
30+
31+
command_interface: position
32+
33+
reference_and_state_interfaces: ["position"]
34+
35+
gains:
36+
joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: true}
1337

1438
test_pid_controller_with_feedforward_gain:
1539
ros__parameters:

‎pid_controller/test/test_pid_controller.cpp

Lines changed: 135 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -673,6 +673,141 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain)
673673
ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value);
674674
}
675675

676+
/**
677+
* @brief Test if retention of the integral state is deactivated
678+
*
679+
*/
680+
681+
TEST_F(PidControllerTest, test_save_i_term_off)
682+
{
683+
SetUpController("test_save_i_term_off");
684+
rclcpp::executors::MultiThreadedExecutor executor;
685+
executor.add_node(controller_->get_node()->get_node_base_interface());
686+
executor.add_node(service_caller_node_->get_node_base_interface());
687+
688+
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
689+
controller_->set_chained_mode(false);
690+
for (const auto & dof_name : dof_names_)
691+
{
692+
ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_name].save_i_term);
693+
}
694+
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
695+
ASSERT_FALSE(controller_->is_in_chained_mode());
696+
697+
std::shared_ptr<ControllerCommandMsg> msg = std::make_shared<ControllerCommandMsg>();
698+
msg->dof_names = dof_names_;
699+
msg->values.resize(dof_names_.size(), 0.0);
700+
for (size_t i = 0; i < dof_command_values_.size(); ++i)
701+
{
702+
msg->values[i] = dof_command_values_[i];
703+
}
704+
msg->values_dot.resize(dof_names_.size(), std::numeric_limits<double>::quiet_NaN());
705+
controller_->input_ref_.writeFromNonRT(msg);
706+
707+
ASSERT_EQ(
708+
controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
709+
controller_interface::return_type::OK);
710+
711+
// check the command value
712+
// error = ref - state = 100.001, error_dot = error/ds = 10000.1,
713+
// p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3
714+
// feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3
715+
auto expected_command_value = 30102.30102;
716+
717+
double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
718+
EXPECT_NEAR(actual_value, expected_command_value, 1e-5);
719+
720+
// deactivate the controller and set command=state
721+
ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
722+
723+
msg = std::make_shared<ControllerCommandMsg>();
724+
msg->dof_names = dof_names_;
725+
msg->values.resize(dof_names_.size(), 0.0);
726+
for (size_t i = 0; i < dof_command_values_.size(); ++i)
727+
{
728+
msg->values[i] = dof_state_values_[i];
729+
}
730+
msg->values_dot.resize(dof_names_.size(), std::numeric_limits<double>::quiet_NaN());
731+
controller_->input_ref_.writeFromNonRT(msg);
732+
733+
// reactivate the controller, the integral term should NOT be saved
734+
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
735+
736+
ASSERT_EQ(
737+
controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
738+
controller_interface::return_type::OK);
739+
actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
740+
EXPECT_NEAR(actual_value, 0.0, 1e-5);
741+
}
742+
743+
/**
744+
* @brief Test if retention of the integral state is working
745+
*
746+
*/
747+
748+
TEST_F(PidControllerTest, test_save_i_term_on)
749+
{
750+
SetUpController("test_save_i_term_on");
751+
rclcpp::executors::MultiThreadedExecutor executor;
752+
executor.add_node(controller_->get_node()->get_node_base_interface());
753+
executor.add_node(service_caller_node_->get_node_base_interface());
754+
755+
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
756+
for (const auto & dof_name : dof_names_)
757+
{
758+
ASSERT_TRUE(controller_->params_.gains.dof_names_map[dof_name].save_i_term);
759+
}
760+
controller_->set_chained_mode(false);
761+
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
762+
ASSERT_FALSE(controller_->is_in_chained_mode());
763+
764+
std::shared_ptr<ControllerCommandMsg> msg = std::make_shared<ControllerCommandMsg>();
765+
msg->dof_names = dof_names_;
766+
msg->values.resize(dof_names_.size(), 0.0);
767+
for (size_t i = 0; i < dof_command_values_.size(); ++i)
768+
{
769+
msg->values[i] = dof_command_values_[i];
770+
}
771+
msg->values_dot.resize(dof_names_.size(), std::numeric_limits<double>::quiet_NaN());
772+
controller_->input_ref_.writeFromNonRT(msg);
773+
774+
ASSERT_EQ(
775+
controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
776+
controller_interface::return_type::OK);
777+
778+
// check the command value
779+
// error = ref - state = 100.001, error_dot = error/ds = 10000.1,
780+
// p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3
781+
// feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3
782+
auto expected_command_value = 30102.30102;
783+
784+
double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
785+
EXPECT_NEAR(actual_value, expected_command_value, 1e-5);
786+
787+
// deactivate the controller and set command=state
788+
ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
789+
790+
msg = std::make_shared<ControllerCommandMsg>();
791+
msg->dof_names = dof_names_;
792+
msg->values.resize(dof_names_.size(), 0.0);
793+
for (size_t i = 0; i < dof_command_values_.size(); ++i)
794+
{
795+
msg->values[i] = dof_state_values_[i];
796+
}
797+
msg->values_dot.resize(dof_names_.size(), std::numeric_limits<double>::quiet_NaN());
798+
controller_->input_ref_.writeFromNonRT(msg);
799+
800+
// reactivate the controller, the integral term should be saved
801+
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
802+
803+
ASSERT_EQ(
804+
controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
805+
controller_interface::return_type::OK);
806+
expected_command_value = 2.00002; // i_term from above
807+
actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
808+
EXPECT_NEAR(actual_value, expected_command_value, 1e-5);
809+
}
810+
676811
int main(int argc, char ** argv)
677812
{
678813
::testing::InitGoogleTest(&argc, argv);

‎pid_controller/test/test_pid_controller.hpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,8 @@ class TestablePidController : public pid_controller::PidController
6161
FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status);
6262
FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_with_gain);
6363
FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_off_with_gain);
64+
FRIEND_TEST(PidControllerTest, test_save_i_term_on);
65+
FRIEND_TEST(PidControllerTest, test_save_i_term_off);
6466

6567
public:
6668
controller_interface::CallbackReturn on_configure(

0 commit comments

Comments
 (0)