Skip to content

Commit a394560

Browse files
christophfroehlichchristophfroehlich
authored
Merge branch 'master' into pid_controller_update_tests
2 parents 65eed40 + 623dc5d commit a394560

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
@@ -47,3 +47,27 @@ test_pid_controller_with_feedforward_gain_dual_interface:
4747
gains:
4848
joint1: {p: 0.5, i: 0.3, d: 0.4, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0}
4949
joint2: {p: 0.5, i: 0.3, d: 0.4, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0}
50+
51+
test_save_i_term_off:
52+
ros__parameters:
53+
dof_names:
54+
- joint1
55+
56+
command_interface: position
57+
58+
reference_and_state_interfaces: ["position"]
59+
60+
gains:
61+
joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: false}
62+
63+
test_save_i_term_on:
64+
ros__parameters:
65+
dof_names:
66+
- joint1
67+
68+
command_interface: position
69+
70+
reference_and_state_interfaces: ["position"]
71+
72+
gains:
73+
joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: true}

‎pid_controller/test/test_pid_controller.cpp

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

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

6769
public:
6870
controller_interface::CallbackReturn on_configure(

0 commit comments

Comments
 (0)