From 3fc0f50c440ad49b2194b2c7dae8c54d6cda7b07 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 15 Nov 2023 20:03:40 +0100 Subject: [PATCH] [JTC] Activate update of dynamic parameters (#761) --- .../joint_trajectory_controller.hpp | 2 + .../src/joint_trajectory_controller.cpp | 95 +++++++++++++------ .../test/test_trajectory_controller.cpp | 36 +++++++ .../test/test_trajectory_controller_utils.hpp | 2 + 4 files changed, 104 insertions(+), 31 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 eaf821aa26..7542aa6951 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 @@ -277,6 +277,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::shared_ptr response); private: + void update_pids(); + bool contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index cf35156541..3936da155f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -115,6 +115,17 @@ controller_interface::return_type JointTrajectoryController::update( { return controller_interface::return_type::OK; } + // update dynamic parameters + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + // use_closed_loop_pid_adapter_ is updated in on_configure only + if (use_closed_loop_pid_adapter_) + { + update_pids(); + default_tolerances_ = get_segment_tolerances(params_); + } + } auto compute_error_for_joint = [&]( JointTrajectoryPoint & error, size_t index, @@ -704,15 +715,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( ff_velocity_scale_.resize(dof_); tmp_command_.resize(dof_, 0.0); - // Init PID gains from ROS parameters - for (size_t i = 0; i < dof_; ++i) - { - const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - pids_[i] = std::make_shared( - gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); - - ff_velocity_scale_[i] = gains.ff_velocity_scale; - } + update_pids(); } // Configure joint position error normalization from ROS parameters (angle_wraparound) @@ -787,8 +790,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(params_.command_interfaces).c_str(), get_interface_list(params_.state_interfaces).c_str()); - default_tolerances_ = get_segment_tolerances(params_); - + // parse remaining parameters const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); interpolation_method_ = interpolation_methods::from_string(interpolation_string); @@ -874,32 +876,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); - if (params_.cmd_timeout > 0.0) - { - if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) - { - cmd_timeout_ = params_.cmd_timeout; - } - else - { - // deactivate timeout - RCLCPP_WARN( - logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)", - params_.cmd_timeout, default_tolerances_.goal_time_tolerance); - cmd_timeout_ = 0.0; - } - } - else - { - cmd_timeout_ = 0.0; - } - return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn JointTrajectoryController::on_activate( const rclcpp_lifecycle::State &) { + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + + // parse remaining parameters + default_tolerances_ = get_segment_tolerances(params_); + // order all joints in the storage for (const auto & interface : params_.command_interfaces) { @@ -974,6 +965,28 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } rt_is_holding_.writeFromNonRT(true); + // parse timeout parameter + if (params_.cmd_timeout > 0.0) + { + if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) + { + cmd_timeout_ = params_.cmd_timeout; + } + else + { + // deactivate timeout + RCLCPP_WARN( + get_node()->get_logger(), + "Command timeout must be higher than goal_time tolerance (%f vs. %f)", params_.cmd_timeout, + default_tolerances_.goal_time_tolerance); + cmd_timeout_ = 0.0; + } + } + else + { + cmd_timeout_ = 0.0; + } + return CallbackReturn::SUCCESS; } @@ -1530,6 +1543,26 @@ bool JointTrajectoryController::has_active_trajectory() const return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); } +void JointTrajectoryController::update_pids() +{ + for (size_t i = 0; i < dof_; ++i) + { + const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + if (pids_[i]) + { + // update PIDs with gains from ROS parameters + pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + } + else + { + // Init PIDs with gains from ROS parameters + pids_[i] = std::make_shared( + gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + } + ff_velocity_scale_[i] = gains.ff_velocity_scale; + } +} + void JointTrajectoryController::init_hold_position_msg() { hold_position_msg_ptr_ = std::make_shared(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 117575be37..7842d50434 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -413,6 +413,42 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) } } +/** + * @brief check if dynamic parameters are updated + */ +TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + updateController(); + auto pids = traj_controller_->get_pids(); + + if (traj_controller_->use_closed_loop_pid_adapter()) + { + EXPECT_EQ(pids.size(), 3); + auto gain_0 = pids.at(0)->getGains(); + EXPECT_EQ(gain_0.p_gain_, 0.0); + + double kp = 1.0; + SetPidParameters(kp); + updateController(); + + pids = traj_controller_->get_pids(); + EXPECT_EQ(pids.size(), 3); + gain_0 = pids.at(0)->getGains(); + EXPECT_EQ(gain_0.p_gain_, kp); + } + else + { + // nothing to check here, skip further test + EXPECT_EQ(pids.size(), 0); + } + + executor.cancel(); +} + /** * @brief check if hold on startup is deactivated */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7118882da1..224265ad83 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -130,6 +130,8 @@ class TestableJointTrajectoryController bool is_open_loop() const { return params_.open_loop_control; } + std::vector get_pids() const { return pids_; } + bool has_active_traj() const { return has_active_trajectory(); } bool has_trivial_traj() const