From 54509970861c86f413959f92d1a2da49adc0bd94 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 28 Dec 2022 14:39:55 +0000 Subject: [PATCH 1/6] Use the same variables for state feedback and PID loop --- .../src/joint_trajectory_controller.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 96110adcb2..323605e9fc 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -243,8 +243,7 @@ controller_interface::return_type JointTrajectoryController::update( { tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + pids_[i]->computeCommand( - state_desired_.positions[i] - state_current_.positions[i], - state_desired_.velocities[i] - state_current_.velocities[i], + state_error_.positions[i], state_error_.velocities[i], (uint64_t)period.nanoseconds()); } } From 65dde4e7e4cf37abf9bc6543f0af90b2af81b050 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 28 Dec 2022 17:47:55 +0000 Subject: [PATCH 2/6] Make joint_error normalization configurable --- .../joint_trajectory_controller.hpp | 2 + .../src/joint_trajectory_controller.cpp | 21 ++- ...oint_trajectory_controller_parameters.yaml | 5 + .../test/test_trajectory_controller.cpp | 170 ++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 9 +- 5 files changed, 201 insertions(+), 6 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index a229489422..3eff7f17b1 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -166,6 +166,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::vector pids_; // Feed-forward velocity weight factor when calculating closed loop pid adapter's command std::vector ff_velocity_scale_; + // Configuration for every joint, if position error is normalized + std::vector normalize_joint_error_; // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 323605e9fc..d9f0008577 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -125,8 +125,17 @@ controller_interface::return_type JointTrajectoryController::update( const JointTrajectoryPoint & desired) { // error defined as the difference between current and desired - error.positions[index] = - angles::shortest_angular_distance(current.positions[index], desired.positions[index]); + if (normalize_joint_error_[index]) + { + // if desired, the shortest_angular_distance is calculated, i.e., the error is + // normalized between -pierror.accelerations.empty() || state->error.accelerations == zeros); } +// Floating-point value comparison threshold +const double EPS = 1e-6; +/** + * @brief check if position error of revolute joints are normalized if not configured so +*/ +TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {}, true); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 9.9}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + const auto allowed_delta = 0.1; + + // no update of state_interface + EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->desired.positions.size()); + EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + + // are the correct desired positions used? + EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + + // no normalization of position error + EXPECT_NEAR( + state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR( + state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_msg->error.positions[2], state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2], EPS); + + if (traj_controller_->has_position_command_interface()) + { + std::cout << "has_position_command_interface\n"; + // check command interface + EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); + EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); + EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); + } + + if (traj_controller_->has_velocity_command_interface()) + { + std::cout << "has_velocity command_interface: " << joint_vel_[0] << " " << joint_vel_[1] << " " + << joint_vel_[2] << "\n"; + // check command interface + EXPECT_LE(0.0, joint_vel_[0]); + EXPECT_LE(0.0, joint_vel_[1]); + EXPECT_LE(0.0, joint_vel_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + std::cout << "has_position_command_interface\n"; + // check command interface + EXPECT_LE(0.0, joint_eff_[0]); + EXPECT_LE(0.0, joint_eff_[1]); + EXPECT_LE(0.0, joint_eff_[2]); + } + + executor.cancel(); +} + +/** + * @brief check if position error of revolute joints are normalized if configured so +*/ +TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {}, true, true); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 9.9}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + const auto allowed_delta = 0.1; + + // no update of state_interface + EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->desired.positions.size()); + EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + + // are the correct desired positions used? + EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + + // no normalization of position error + EXPECT_NEAR( + state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR( + state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + // normalization of position error + EXPECT_NEAR( + state_msg->error.positions[2], + state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); + EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); + EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_vel_[0]); + EXPECT_LE(0.0, joint_vel_[1]); + EXPECT_LE(0.0, joint_vel_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_eff_[0]); + EXPECT_LE(0.0, joint_eff_[1]); + EXPECT_LE(0.0, joint_eff_[2]); + } + + executor.cancel(); +} + void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) { rclcpp::Parameter state_publish_rate_param( diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 0d523cc88d..875e744509 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -171,7 +171,7 @@ class TrajectoryControllerTest : public ::testing::Test node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params}); } - void SetPidParameters() + void SetPidParameters(bool normalize_error_default = false) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -184,19 +184,20 @@ class TrajectoryControllerTest : public ::testing::Test const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", 1.0); - node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); + const rclcpp::Parameter normalize_error(prefix + ".normalize_error", normalize_error_default); + node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, normalize_error}); } } void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, bool use_local_parameters = true, const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false) + bool separate_cmd_and_state_values = false, bool normalize_error_default = false) { SetUpTrajectoryController(executor, use_local_parameters); // set pid parameters before configure - SetPidParameters(); + SetPidParameters(normalize_error_default); for (const auto & param : parameters) { traj_controller_->get_node()->set_parameter(param); From 8c3a17c52ee8e21c6abd3795c98bd94ab0938cc8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 28 Dec 2022 18:32:48 +0000 Subject: [PATCH 3/6] Activate test for only velocity controller --- .../test/test_trajectory_controller.cpp | 29 ++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 89bdf17c7f..8348ac3c97 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -421,9 +421,13 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param traj_controller_->update( rclcpp::Time(static_cast(0.35 * 1e9)), rclcpp::Duration::from_seconds(0.1)); - EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); - EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); - EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + if (traj_controller_->has_position_command_interface()) + { + // otherwise this won't be updated + EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); + EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); + EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + } // state = traj_controller_->get_node()->configure(); // ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -1537,16 +1541,15 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"position", "velocity", "acceleration"}), std::vector({"position", "velocity", "acceleration"})))); -// // only velocity controller -// INSTANTIATE_TEST_SUITE_P( -// OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, -// ::testing::Values( -// std::make_tuple( -// std::vector({"velocity"}), -// std::vector({"position", "velocity"})), -// std::make_tuple( -// std::vector({"velocity"}), -// std::vector({"position", "velocity", "acceleration"})))); +// only velocity controller +INSTANTIATE_TEST_SUITE_P( + OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"velocity"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"velocity"}), + std::vector({"position", "velocity", "acceleration"})))); // // only effort controller // INSTANTIATE_TEST_SUITE_P( From 6b2563936806484e75ef1859a175b3b3ca3d798b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 28 Dec 2022 18:58:00 +0000 Subject: [PATCH 4/6] Allow ff_velocity_scale=0 without deprecated warning --- .../src/joint_trajectory_controller.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index d9f0008577..e7ce3dfdeb 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -573,14 +573,18 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // TODO(destogl): remove this in ROS2 Iron // Check deprecated style for "ff_velocity_scale" parameter definition. - if (gains.ff_velocity_scale == 0.0) + double ff_velocity_scale_tmp = + auto_declare("ff_velocity_scale/" + params_.joints[i], -1.0); + if (ff_velocity_scale_tmp != -1.0) { RCLCPP_WARN( get_node()->get_logger(), - "'ff_velocity_scale' parameters is not defined under 'gains..' structure. " - "Maybe you are using deprecated format 'ff_velocity_scale/'!"); + "You are using deprecated format 'ff_velocity_scale/%s: %f'! " + "'ff_velocity_scale' parameters have to be defined under 'gains..' " + "structure. ", + command_joint_names_[i].c_str(), ff_velocity_scale_tmp); - ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + params_.joints[i], 0.0); + ff_velocity_scale_[i] = ff_velocity_scale_tmp; } else { From 7e1e0064a7dfbafbc7b32cc80e7b65ed3a40781a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 28 Dec 2022 19:57:04 +0000 Subject: [PATCH 5/6] Add test for setpoint due to normalized position error --- .../test/test_trajectory_controller.cpp | 53 +++++++++++++++---- .../test/test_trajectory_controller_utils.hpp | 14 +++-- 2 files changed, 53 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 8348ac3c97..d114e28446 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -495,7 +495,8 @@ const double EPS = 1e-6; TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}, true); + constexpr double k_p = 10.0; + SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, false); subscribeToState(); size_t n_joints = joint_names_.size(); @@ -505,7 +506,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; // *INDENT-OFF* std::vector> points{ - {{3.3, 4.4, 9.9}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; std::vector> points_velocities{ {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; // *INDENT-ON* @@ -545,7 +546,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) if (traj_controller_->has_position_command_interface()) { - std::cout << "has_position_command_interface\n"; // check command interface EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); @@ -554,17 +554,30 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) if (traj_controller_->has_velocity_command_interface()) { - std::cout << "has_velocity command_interface: " << joint_vel_[0] << " " << joint_vel_[1] << " " - << joint_vel_[2] << "\n"; // check command interface EXPECT_LE(0.0, joint_vel_[0]); EXPECT_LE(0.0, joint_vel_[1]); EXPECT_LE(0.0, joint_vel_[2]); + + // use_closed_loop_pid_adapter_? + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) + EXPECT_NEAR( + k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * allowed_delta); + // no normalization of position error + EXPECT_NEAR( + k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * allowed_delta); + } } if (traj_controller_->has_effort_command_interface()) { - std::cout << "has_position_command_interface\n"; // check command interface EXPECT_LE(0.0, joint_eff_[0]); EXPECT_LE(0.0, joint_eff_[1]); @@ -580,7 +593,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}, true, true); + constexpr double k_p = 10.0; + SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, true); subscribeToState(); size_t n_joints = joint_names_.size(); @@ -590,7 +604,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; // *INDENT-OFF* std::vector> points{ - {{3.3, 4.4, 9.9}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; std::vector> points_velocities{ {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; // *INDENT-ON* @@ -643,7 +657,28 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) // check command interface EXPECT_LE(0.0, joint_vel_[0]); EXPECT_LE(0.0, joint_vel_[1]); - EXPECT_LE(0.0, joint_vel_[2]); + + // use_closed_loop_pid_adapter_? + if (traj_controller_->use_closed_loop_pid_adapter()) + { + EXPECT_GE(0.0, joint_vel_[2]); + // we expect u = k_p * (s_d-s) + EXPECT_NEAR( + k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * allowed_delta); + // normalization of position error + EXPECT_NEAR( + k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], + k_p * allowed_delta); + } + else + { + // interpolated points_velocities only + EXPECT_LE(0.0, joint_vel_[2]); + } } if (traj_controller_->has_effort_command_interface()) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 875e744509..ca58b124a5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -112,6 +112,8 @@ class TestableJointTrajectoryController bool has_effort_command_interface() { return has_effort_command_interface_; } + bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -171,7 +173,8 @@ class TrajectoryControllerTest : public ::testing::Test node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params}); } - void SetPidParameters(bool normalize_error_default = false) + void SetPidParameters( + double p_default = 0.0, double ff_default = 1.0, bool normalize_error_default = false) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -179,11 +182,11 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < joint_names_.size(); ++i) { const std::string prefix = "gains." + joint_names_[i]; - const rclcpp::Parameter k_p(prefix + ".p", 0.0); + const rclcpp::Parameter k_p(prefix + ".p", p_default); const rclcpp::Parameter k_i(prefix + ".i", 0.0); const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); - const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", 1.0); + const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_default); const rclcpp::Parameter normalize_error(prefix + ".normalize_error", normalize_error_default); node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, normalize_error}); } @@ -192,12 +195,13 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, bool use_local_parameters = true, const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false, bool normalize_error_default = false) + bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, + bool normalize_error = false) { SetUpTrajectoryController(executor, use_local_parameters); // set pid parameters before configure - SetPidParameters(normalize_error_default); + SetPidParameters(k_p, ff, normalize_error); for (const auto & param : parameters) { traj_controller_->get_node()->set_parameter(param); From 85ddcbf81f061738466ed62583a7bd1a33c961be Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 9 Jan 2023 22:08:02 +0000 Subject: [PATCH 6/6] Update comments in test --- .../test/test_trajectory_controller.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index d114e28446..511ae0869e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -559,7 +559,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) EXPECT_LE(0.0, joint_vel_[1]); EXPECT_LE(0.0, joint_vel_[2]); - // use_closed_loop_pid_adapter_? + // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) { // we expect u = k_p * (s_d-s) @@ -634,12 +634,11 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); - // no normalization of position error + // is error.positions[2] normalized? EXPECT_NEAR( state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); EXPECT_NEAR( state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); - // normalization of position error EXPECT_NEAR( state_msg->error.positions[2], state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); @@ -658,18 +657,18 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) EXPECT_LE(0.0, joint_vel_[0]); EXPECT_LE(0.0, joint_vel_[1]); - // use_closed_loop_pid_adapter_? + // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) { EXPECT_GE(0.0, joint_vel_[2]); - // we expect u = k_p * (s_d-s) + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] EXPECT_NEAR( k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], k_p * allowed_delta); EXPECT_NEAR( k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], k_p * allowed_delta); - // normalization of position error + // is error of positions[2] normalized? EXPECT_NEAR( k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], k_p * allowed_delta);