Skip to content

Commit cf8b96e

Browse files
authored
[pid_controller] Fix logic for feedforward_mode with single reference interface (#1520)
1 parent 0d4b7e8 commit cf8b96e

File tree

4 files changed

+161
-8
lines changed

4 files changed

+161
-8
lines changed

pid_controller/src/pid_controller.cpp

Lines changed: 15 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -500,20 +500,27 @@ controller_interface::return_type PidController::update_and_write_commands(
500500

501501
for (size_t i = 0; i < dof_; ++i)
502502
{
503-
double tmp_command = std::numeric_limits<double>::quiet_NaN();
503+
double tmp_command = 0.0;
504504

505-
// Using feedforward
506505
if (!std::isnan(reference_interfaces_[i]) && !std::isnan(measured_state_values_[i]))
507506
{
508507
// calculate feed-forward
509508
if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON)
510509
{
511-
tmp_command = reference_interfaces_[dof_ + i] *
512-
params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain;
513-
}
514-
else
515-
{
516-
tmp_command = 0.0;
510+
// two interfaces
511+
if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_)
512+
{
513+
if (std::isfinite(reference_interfaces_[dof_ + i]))
514+
{
515+
tmp_command = reference_interfaces_[dof_ + i] *
516+
params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain;
517+
}
518+
}
519+
else // one interface
520+
{
521+
tmp_command = reference_interfaces_[i] *
522+
params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain;
523+
}
517524
}
518525

519526
double error = reference_interfaces_[i] - measured_state_values_[i];

pid_controller/test/pid_controller_params.yaml

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,3 +9,16 @@ test_pid_controller:
99

1010
gains:
1111
joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0}
12+
13+
14+
test_pid_controller_with_feedforward_gain:
15+
ros__parameters:
16+
dof_names:
17+
- joint1
18+
19+
command_interface: position
20+
21+
reference_and_state_interfaces: ["position"]
22+
23+
gains:
24+
joint1: {p: 0.5, i: 0.0, d: 0.0, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0}

pid_controller/test/test_pid_controller.cpp

Lines changed: 131 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -542,6 +542,137 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status)
542542
}
543543
}
544544

545+
/**
546+
* @brief check chained pid controller with feedforward and gain as non-zero, single interface
547+
*/
548+
TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain)
549+
{
550+
// state interface value is 1.1 as defined in test fixture
551+
// with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95
552+
// with feedforward gain 1.0, the command value should be 1.95 + 1.0 * 5.0 = 6.95
553+
const double target_value = 5.0;
554+
const double expected_command_value = 6.95;
555+
556+
SetUpController("test_pid_controller_with_feedforward_gain");
557+
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
558+
559+
// check on interfaces & pid gain parameters
560+
for (const auto & dof_name : dof_names_)
561+
{
562+
ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5);
563+
ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0);
564+
}
565+
ASSERT_EQ(controller_->params_.command_interface, command_interface_);
566+
EXPECT_THAT(
567+
controller_->params_.reference_and_state_interfaces,
568+
testing::ElementsAreArray(state_interfaces_));
569+
ASSERT_FALSE(controller_->params_.use_external_measured_states);
570+
571+
// setup executor
572+
rclcpp::executors::MultiThreadedExecutor executor;
573+
executor.add_node(controller_->get_node()->get_node_base_interface());
574+
executor.add_node(service_caller_node_->get_node_base_interface());
575+
576+
controller_->set_chained_mode(true);
577+
578+
// activate controller
579+
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
580+
ASSERT_TRUE(controller_->is_in_chained_mode());
581+
582+
// turn on feedforward
583+
controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON);
584+
ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON);
585+
586+
// send a message to update reference interface
587+
std::shared_ptr<ControllerCommandMsg> msg = std::make_shared<ControllerCommandMsg>();
588+
msg->dof_names = controller_->params_.dof_names;
589+
msg->values.resize(msg->dof_names.size(), 0.0);
590+
for (size_t i = 0; i < msg->dof_names.size(); ++i)
591+
{
592+
msg->values[i] = target_value;
593+
}
594+
msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits<double>::quiet_NaN());
595+
controller_->input_ref_.writeFromNonRT(msg);
596+
ASSERT_EQ(
597+
controller_->update_reference_from_subscribers(
598+
rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
599+
controller_interface::return_type::OK);
600+
601+
// run update
602+
ASSERT_EQ(
603+
controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
604+
controller_interface::return_type::OK);
605+
606+
// check on result from update
607+
ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value);
608+
}
609+
610+
/**
611+
* @brief check chained pid controller with feedforward OFF and gain as non-zero, single interface
612+
*/
613+
TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain)
614+
{
615+
// state interface value is 1.1 as defined in test fixture
616+
// given target value 5.0
617+
// with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95
618+
// with feedforward off, the command value should be still 1.95 even though feedforward gain
619+
// is 1.0
620+
const double target_value = 5.0;
621+
const double expected_command_value = 1.95;
622+
623+
SetUpController("test_pid_controller_with_feedforward_gain");
624+
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
625+
626+
// check on interfaces & pid gain parameters
627+
for (const auto & dof_name : dof_names_)
628+
{
629+
ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5);
630+
ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0);
631+
}
632+
ASSERT_EQ(controller_->params_.command_interface, command_interface_);
633+
EXPECT_THAT(
634+
controller_->params_.reference_and_state_interfaces,
635+
testing::ElementsAreArray(state_interfaces_));
636+
ASSERT_FALSE(controller_->params_.use_external_measured_states);
637+
638+
// setup executor
639+
rclcpp::executors::MultiThreadedExecutor executor;
640+
executor.add_node(controller_->get_node()->get_node_base_interface());
641+
executor.add_node(service_caller_node_->get_node_base_interface());
642+
643+
controller_->set_chained_mode(true);
644+
645+
// activate controller
646+
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
647+
ASSERT_TRUE(controller_->is_in_chained_mode());
648+
649+
// feedforward by default is OFF
650+
ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF);
651+
652+
// send a message to update reference interface
653+
std::shared_ptr<ControllerCommandMsg> msg = std::make_shared<ControllerCommandMsg>();
654+
msg->dof_names = controller_->params_.dof_names;
655+
msg->values.resize(msg->dof_names.size(), 0.0);
656+
for (size_t i = 0; i < msg->dof_names.size(); ++i)
657+
{
658+
msg->values[i] = target_value;
659+
}
660+
msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits<double>::quiet_NaN());
661+
controller_->input_ref_.writeFromNonRT(msg);
662+
ASSERT_EQ(
663+
controller_->update_reference_from_subscribers(
664+
rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
665+
controller_interface::return_type::OK);
666+
667+
// run update
668+
ASSERT_EQ(
669+
controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
670+
controller_interface::return_type::OK);
671+
672+
// check on result from update
673+
ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value);
674+
}
675+
545676
int main(int argc, char ** argv)
546677
{
547678
::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
@@ -59,6 +59,8 @@ class TestablePidController : public pid_controller::PidController
5959
FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_on);
6060
FRIEND_TEST(PidControllerTest, subscribe_and_get_messages_success);
6161
FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status);
62+
FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_with_gain);
63+
FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_off_with_gain);
6264

6365
public:
6466
controller_interface::CallbackReturn on_configure(

0 commit comments

Comments
 (0)