diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 4b0ca82df8..f9b1e2073a 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -12,6 +12,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface generate_parameter_library hardware_interface + joint_trajectory_controller_plugins pluginlib rclcpp rclcpp_lifecycle @@ -107,4 +108,5 @@ install(TARGETS ament_export_targets(export_joint_trajectory_controller HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + ament_package() 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 ae370df0a6..5c47515580 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 @@ -24,13 +24,9 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_msgs/srv/query_trajectory_state.hpp" -#include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "joint_trajectory_controller/interpolation_methods.hpp" -#include "joint_trajectory_controller/tolerances.hpp" -#include "joint_trajectory_controller/trajectory.hpp" -#include "joint_trajectory_controller/visibility_control.h" +#include "pluginlib/class_loader.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" @@ -45,8 +41,12 @@ #include "trajectory_msgs/msg/joint_trajectory_point.hpp" #include "urdf/model.h" -// auto-generated by generate_parameter_library -#include "joint_trajectory_controller_parameters.hpp" +#include "joint_trajectory_controller/interpolation_methods.hpp" +#include "joint_trajectory_controller/tolerances.hpp" +#include "joint_trajectory_controller/trajectory.hpp" +#include "joint_trajectory_controller/visibility_control.h" +#include "joint_trajectory_controller_parameters.hpp" // auto-generated by generate_parameter_library +#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp" using namespace std::chrono_literals; // NOLINT @@ -123,6 +123,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Storing command joint names for interfaces std::vector command_joint_names_; + // TODO(anyone) remove this if there is another way to lock command_joints parameter + rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackHandle::SharedPtr lock_cmd_joint_names; // Parameters from ROS for joint_trajectory_controller std::shared_ptr param_listener_; @@ -151,11 +153,13 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool has_effort_command_interface_ = false; /// If true, a velocity feedforward term plus corrective PID term is used - bool use_closed_loop_pid_adapter_ = false; - using PidPtr = std::shared_ptr; - std::vector pids_; - // Feed-forward velocity weight factor when calculating closed loop pid adapter's command - std::vector ff_velocity_scale_; + bool use_external_control_law_ = false; + // class loader for actual trajectory controller + std::shared_ptr< + pluginlib::ClassLoader> + traj_controller_loader_; + // The actual trajectory controller + std::shared_ptr traj_contr_; // Configuration for every joint if it wraps around (ie. is continuous, position error is // normalized) std::vector joints_angle_wraparound_; @@ -237,7 +241,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void add_new_trajectory_msg( + void add_new_trajectory_msg_nonRT( + const std::shared_ptr & traj_msg); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void add_new_trajectory_msg_RT( const std::shared_ptr & traj_msg); JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_point_field( @@ -288,8 +295,6 @@ 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/package.xml b/joint_trajectory_controller/package.xml index 8457c02aeb..ddc7c6962f 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -13,14 +13,15 @@ angles backward_ros - controller_interface control_msgs control_toolbox + controller_interface generate_parameter_library hardware_interface + joint_trajectory_controller_plugins pluginlib - rclcpp rclcpp_lifecycle + rclcpp realtime_tools rsl tl_expected diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5e3d9d1c7d..b98b8da4ea 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -27,7 +27,6 @@ #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "joint_trajectory_controller/trajectory.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" @@ -37,10 +36,18 @@ #include "rclcpp_lifecycle/state.hpp" #include "urdf/model.h" +#include "joint_trajectory_controller/trajectory.hpp" + namespace joint_trajectory_controller { JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), dof_(0) +: controller_interface::ControllerInterface(), + dof_(0), + traj_controller_loader_( + std::make_shared< + pluginlib::ClassLoader>( + "joint_trajectory_controller_plugins", + "joint_trajectory_controller_plugins::TrajectoryControllerBase")) { } @@ -131,11 +138,14 @@ controller_interface::return_type JointTrajectoryController::update( { params_ = param_listener_->get_params(); default_tolerances_ = get_segment_tolerances(params_); - // update the PID gains - // variable use_closed_loop_pid_adapter_ is updated in on_configure only - if (use_closed_loop_pid_adapter_) + // update gains of controller + if (traj_contr_) { - update_pids(); + if (traj_contr_->updateGainsRT() == false) + { + RCLCPP_ERROR(get_node()->get_logger(), "Could not update gains of controller"); + return controller_interface::return_type::ERROR; + } } } @@ -145,10 +155,13 @@ controller_interface::return_type JointTrajectoryController::update( // Check if a new external message has been received from nonRT threads auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); - // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) + // Discard, + // if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) + // and if traj_contr_: wait until control law is computed by the traj_contr_ if ( current_external_msg != *new_external_msg && - (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) + (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false && + (traj_contr_ == nullptr || traj_contr_->is_ready())) { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); @@ -178,6 +191,11 @@ controller_interface::return_type JointTrajectoryController::update( traj_external_point_ptr_->set_point_before_trajectory_msg( time, state_current_, joints_angle_wraparound_); } + if (traj_contr_) + { + // switch RT buffer of traj_contr_ + traj_contr_->start(); + } } // find segment for current timestamp @@ -209,8 +227,7 @@ controller_interface::return_type JointTrajectoryController::update( { RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } // Check state/goal tolerance @@ -255,16 +272,11 @@ controller_interface::return_type JointTrajectoryController::update( // set values for next hardware write() if tolerance is met if (!tolerance_violated_while_moving && within_goal_time) { - if (use_closed_loop_pid_adapter_) + if (traj_contr_) { - // Update PIDs - for (auto i = 0ul; i < dof_; ++i) - { - tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - state_error_.positions[i], state_error_.velocities[i], - (uint64_t)period.nanoseconds()); - } + traj_contr_->computeCommands( + tmp_command_, state_current_, state_error_, state_desired_, + time - traj_external_point_ptr_->time_from_start(), period); } // set values for next hardware write() @@ -274,7 +286,7 @@ controller_interface::return_type JointTrajectoryController::update( } if (has_velocity_command_interface_) { - if (use_closed_loop_pid_adapter_) + if (use_external_control_law_) { assign_interface_from_point(joint_command_interface_[1], tmp_command_); } @@ -322,8 +334,7 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } // check goal tolerance else if (!before_last_point) @@ -341,8 +352,7 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); + add_new_trajectory_msg_RT(set_success_trajectory_point()); } else if (!within_goal_time) { @@ -360,8 +370,7 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_WARN(get_node()->get_logger(), error_string.c_str()); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } } } @@ -370,16 +379,14 @@ controller_interface::return_type JointTrajectoryController::update( // we need to ensure that there is no pending goal -> we get a race condition otherwise RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } else if ( !before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) { RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } // else, run another cycle while waiting for outside_goal_tolerance // to be satisfied (will stay in this state until new message arrives) @@ -658,6 +665,36 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( command_joint_names_ = params_.joints; RCLCPP_INFO( logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); + + // set the parameter for the controller plugin + auto result = + get_node()->set_parameter(rclcpp::Parameter("command_joints", command_joint_names_)); + if (result.successful == false) + { + RCLCPP_ERROR(logger, "Failed to set 'command_joints' parameter"); + return CallbackReturn::FAILURE; + } + // TODO(christophfroehlich) how to lock the parameter (set read_only to false)? + // Setting it to read_only but override is not supported + // https://github.com/ros2/rclcpp/issues/1762 get_node()->undeclare_parameter("command_joints"); + // rcl_interfaces::msg::ParameterDescriptor parameter_descriptor; + // parameter_descriptor.read_only = true; + // get_node()->declare_parameter("command_joints", + // rclcpp::ParameterValue(command_joint_names_), parameter_descriptor); + lock_cmd_joint_names = get_node()->add_pre_set_parameters_callback( + [this](std::vector & parameters) + { + for (auto & parameter : parameters) + { + if (parameter.get_name() == "command_joints") + { + RCLCPP_ERROR( + get_node()->get_logger(), + "The parameter 'command_joints' is read-only. You can't change it."); + parameter = rclcpp::Parameter("command_joints", command_joint_names_); + } + } + }); } else if (command_joint_names_.size() != params_.joints.size()) { @@ -686,19 +723,53 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT); // if there is only velocity or if there is effort command interface - // then use also PID adapter - use_closed_loop_pid_adapter_ = + // then use external control law + use_external_control_law_ = (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && !params_.open_loop_control) || has_effort_command_interface_; - if (use_closed_loop_pid_adapter_) + if (use_external_control_law_) { - pids_.resize(dof_); - ff_velocity_scale_.resize(dof_); - tmp_command_.resize(dof_, 0.0); + try + { + traj_contr_ = + traj_controller_loader_->createSharedInstance(params_.controller_plugin.c_str()); + } + catch (pluginlib::PluginlibException & ex) + { + RCLCPP_FATAL( + logger, "The trajectory controller plugin `%s` failed to load for some reason. Error: %s\n", + params_.controller_plugin.c_str(), ex.what()); + return CallbackReturn::FAILURE; + } + if (traj_contr_->initialize(get_node()) == false) + { + RCLCPP_FATAL( + logger, + "The trajectory controller plugin `%s` failed to initialize for some reason. Aborting.", + params_.controller_plugin.c_str()); + return CallbackReturn::FAILURE; + } + else + { + if (traj_contr_->configure() == false) + { + RCLCPP_FATAL( + logger, + "The trajectory controller plugin `%s` failed to configure for some reason. Aborting.", + params_.controller_plugin.c_str()); + return CallbackReturn::FAILURE; + } + else + { + RCLCPP_INFO( + logger, "The trajectory controller plugin `%s` was loaded and configured.", + params_.controller_plugin.c_str()); + } + } - update_pids(); + tmp_command_.resize(dof_, 0.0); } // Configure joint position error normalization (angle_wraparound) @@ -947,8 +1018,15 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( read_state_from_state_interfaces(last_commanded_state_); } + // activate traj_contr_, e.g., update gains + if (traj_contr_ && traj_contr_->activate() == false) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation."); + return CallbackReturn::ERROR; + } + // The controller should start by holding position at the beginning of active state - add_new_trajectory_msg(set_hold_position()); + add_new_trajectory_msg_nonRT(set_hold_position()); rt_is_holding_.writeFromNonRT(true); // parse timeout parameter @@ -1050,12 +1128,9 @@ bool JointTrajectoryController::reset() subscriber_is_active_ = false; joint_command_subscriber_.reset(); - for (const auto & pid : pids_) + if (traj_contr_) { - if (pid) - { - pid->reset(); - } + traj_contr_->reset(); } traj_external_point_ptr_.reset(); @@ -1113,7 +1188,7 @@ void JointTrajectoryController::topic_callback( // always replace old msg with new one for now if (subscriber_is_active_) { - add_new_trajectory_msg(msg); + add_new_trajectory_msg_nonRT(msg); rt_is_holding_.writeFromNonRT(false); } }; @@ -1159,7 +1234,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); // Enter hold current position mode - add_new_trajectory_msg(set_hold_position()); + add_new_trajectory_msg_nonRT(set_hold_position()); } return rclcpp_action::CancelResponse::ACCEPT; } @@ -1176,7 +1251,7 @@ void JointTrajectoryController::goal_accepted_callback( auto traj_msg = std::make_shared(goal_handle->get_goal()->trajectory); - add_new_trajectory_msg(traj_msg); + add_new_trajectory_msg_nonRT(traj_msg); rt_is_holding_.writeFromNonRT(false); } @@ -1483,10 +1558,39 @@ bool JointTrajectoryController::validate_trajectory_msg( return true; } -void JointTrajectoryController::add_new_trajectory_msg( +void JointTrajectoryController::add_new_trajectory_msg_nonRT( const std::shared_ptr & traj_msg) { traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); + + // compute control law + if (traj_contr_) + { + // this can take some time; trajectory won't start until control law is computed + if (traj_contr_->computeControlLawNonRT(traj_msg) == false) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory."); + } + } +} + +void JointTrajectoryController::add_new_trajectory_msg_RT( + const std::shared_ptr & traj_msg) +{ + // TODO(christophfroehlich): Need a lock-free write here + // See https://github.com/ros-controls/ros2_controllers/issues/168 + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(traj_msg); + + // compute control law + if (traj_contr_) + { + // this is used for set_hold_position() only -> this should (and must) not take a long time + if (traj_contr_->computeControlLawRT(traj_msg) == false) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory."); + } + } } void JointTrajectoryController::preempt_active_goal() @@ -1575,26 +1679,6 @@ 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/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index b17229cab8..6183732602 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -11,8 +11,8 @@ joint_trajectory_controller: command_joints: { type: string_array, default_value: [], - description: "Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names.", - read_only: true, + description: "Names of the commanding joints used by the controller.\nThis parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names, or if the controller has less command_joints than DOF.", + read_only: false, # should be set to true after configuration of the controller validation: { unique<>: null, } @@ -95,33 +95,14 @@ joint_trajectory_controller: ``cmd_timeout`` must be greater than ``constraints.goal_time``, otherwise ignored. If zero, timeout is deactivated", } + controller_plugin: { + type: string, + default_value: "joint_trajectory_controller_plugins::PidTrajectoryPlugin", + description: "Type of the plugin for the trajectory controller", + read_only: true, + } gains: __map_joints: - p: { - type: double, - default_value: 0.0, - description: "Proportional gain :math:`k_p` for PID" - } - i: { - type: double, - default_value: 0.0, - description: "Integral gain :math:`k_i` for PID" - } - d: { - type: double, - default_value: 0.0, - description: "Derivative gain :math:`k_d` for PID" - } - i_clamp: { - type: double, - default_value: 0.0, - description: "Integral clamp. Symmetrical in both positive and negative direction." - } - ff_velocity_scale: { - type: double, - default_value: 0.0, - description: "Feed-forward scaling :math:`k_{ff}` of velocity" - } angle_wraparound: { type: bool, default_value: false, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f5e9cb7260..d4adf2c8cc 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -357,30 +357,38 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) { rclcpp::executors::MultiThreadedExecutor executor; + // with kp = 0.0 SetUpAndActivateTrajectoryController(executor); updateControllerAsync(); - auto pids = traj_controller_->get_pids(); + auto pids = traj_controller_->get_traj_contr(); - if (traj_controller_->use_closed_loop_pid_adapter()) + if (traj_controller_->use_external_control_law()) { - EXPECT_EQ(pids.size(), 3); - auto gain_0 = pids.at(0)->getGains(); - EXPECT_EQ(gain_0.p_gain_, 0.0); + std::vector tmp_command{0.0, 0.0, 0.0}; + trajectory_msgs::msg::JointTrajectoryPoint error; + error.positions = {1.0, 0.0, 0.0}; + error.velocities = {0.0, 0.0, 0.0}; + trajectory_msgs::msg::JointTrajectoryPoint current; + trajectory_msgs::msg::JointTrajectoryPoint desired; + desired.velocities = {0.0, 0.0, 0.0}; + rclcpp::Duration duration_since_start(std::chrono::milliseconds(250)); + rclcpp::Duration period(std::chrono::milliseconds(100)); + + pids->computeCommands(tmp_command, current, error, desired, duration_since_start, period); + EXPECT_EQ(tmp_command.at(0), 0.0); double kp = 1.0; SetPidParameters(kp); updateControllerAsync(); - pids = traj_controller_->get_pids(); - EXPECT_EQ(pids.size(), 3); - gain_0 = pids.at(0)->getGains(); - EXPECT_EQ(gain_0.p_gain_, kp); + pids->computeCommands(tmp_command, current, error, desired, duration_since_start, period); + EXPECT_EQ(tmp_command.at(0), 1.0); } else { // nothing to check here, skip further test - EXPECT_EQ(pids.size(), 0); + EXPECT_EQ(pids, nullptr); } executor.cancel(); @@ -697,8 +705,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun if (traj_controller_->has_velocity_command_interface()) { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) + // use_external_control_law_ + if (traj_controller_->use_external_control_law()) { // we expect u = k_p * (s_d-s) for positions EXPECT_NEAR( @@ -801,8 +809,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) if (traj_controller_->has_velocity_command_interface()) { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) + // use_external_control_law_ + if (traj_controller_->use_external_control_law()) { // we expect u = k_p * (s_d-s) for joint0 and joint1 EXPECT_NEAR( @@ -983,9 +991,9 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) } /** - * @brief check if use_closed_loop_pid is active + * @brief check if use_external_control_law is set */ -TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) +TEST_P(TrajectoryControllerTestParameterized, set_external_control_law) { rclcpp::executors::MultiThreadedExecutor executor; @@ -999,7 +1007,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) !traj_controller_->is_open_loop()) || traj_controller_->has_effort_command_interface()) { - EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter()); + EXPECT_TRUE(traj_controller_->use_external_control_law()); } } @@ -1056,7 +1064,7 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) } // is the velocity error correct? if ( - traj_controller_->use_closed_loop_pid_adapter() // always needed for PID controller + traj_controller_->use_external_control_law() // always needed for PID controller || (traj_controller_->has_velocity_state_interface() && traj_controller_->has_velocity_command_interface())) { @@ -1121,8 +1129,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) if (traj_controller_->has_velocity_command_interface()) { - // if use_closed_loop_pid_adapter_==false: we expect desired velocities from direct sampling - // if use_closed_loop_pid_adapter_==true: we expect desired velocities, because we use PID with + // if use_external_control_law_==false: we expect desired velocities from direct sampling + // if use_external_control_law_==true: we expect desired velocities, because we use PID with // feedforward term only EXPECT_GT(0.0, joint_vel_[0]); EXPECT_GT(0.0, joint_vel_[1]); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index baa4111239..99f08eae2d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -87,26 +87,11 @@ class TestableJointTrajectoryController return success; } - void set_joint_names(const std::vector & joint_names) - { - params_.joints = joint_names; - } - void set_command_joint_names(const std::vector & command_joint_names) { command_joint_names_ = command_joint_names; } - void set_command_interfaces(const std::vector & command_interfaces) - { - params_.command_interfaces = command_interfaces; - } - - void set_state_interfaces(const std::vector & state_interfaces) - { - params_.state_interfaces = state_interfaces; - } - void trigger_declare_parameters() { param_listener_->declare_params(); } void testable_compute_error_for_joint( @@ -120,6 +105,7 @@ class TestableJointTrajectoryController { return last_commanded_state_; } + bool has_position_state_interface() const { return has_position_state_interface_; } bool has_velocity_state_interface() const { return has_velocity_state_interface_; } @@ -134,11 +120,15 @@ class TestableJointTrajectoryController bool has_effort_command_interface() const { return has_effort_command_interface_; } - bool use_closed_loop_pid_adapter() const { return use_closed_loop_pid_adapter_; } + bool use_external_control_law() const { return use_external_control_law_; } bool is_open_loop() const { return params_.open_loop_control; } - std::vector get_pids() const { return pids_; } + std::shared_ptr get_traj_contr() + const + { + return traj_contr_; + } joint_trajectory_controller::SegmentTolerances get_tolerances() const { @@ -245,6 +235,10 @@ class TrajectoryControllerTest : public ::testing::Test controller_name_, urdf, 0, "", traj_controller_->define_custom_node_options()); } + /** + * @brief set PIDs for every entry in joint_names_ + * be aware to update if PIDs should be configured for different command_joints than joint_names + */ void SetPidParameters(double p_value = 0.0, double ff_value = 1.0) { traj_controller_->trigger_declare_parameters(); @@ -287,10 +281,15 @@ class TrajectoryControllerTest : public ::testing::Test // read-only parameters have to be set before init -> won't be read otherwise SetUpTrajectoryController(executor, parameters_local, urdf); - // set pid parameters before configure - SetPidParameters(k_p, ff); traj_controller_->get_node()->configure(); + // set pid parameters before activate. The PID plugin has to be loaded already, otherwise + // parameters are not declared yet + if (traj_controller_->use_external_control_law()) + { + SetPidParameters(k_p, ff); + } + ActivateTrajectoryController( separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints, initial_eff_joints); @@ -556,7 +555,7 @@ class TrajectoryControllerTest : public ::testing::Test // i.e., active but trivial trajectory (one point only) EXPECT_TRUE(traj_controller_->has_trivial_traj()); - if (traj_controller_->use_closed_loop_pid_adapter() == false) + if (traj_controller_->use_external_control_law() == false) { if (traj_controller_->has_position_command_interface()) { diff --git a/joint_trajectory_controller_plugins/CMakeLists.txt b/joint_trajectory_controller_plugins/CMakeLists.txt new file mode 100644 index 0000000000..5ef16e7b54 --- /dev/null +++ b/joint_trajectory_controller_plugins/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.8) +project(joint_trajectory_controller_plugins LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wconversion) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_toolbox + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + trajectory_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(pid_trajectory_plugin_parameters + src/pid_trajectory_plugin_parameters.yaml +) + +add_library(pid_trajectory_plugin SHARED src/pid_trajectory_plugin.cpp) +add_library(joint_trajectory_controller_plugins::pid_trajectory_plugin ALIAS pid_trajectory_plugin) +target_compile_features(pid_trajectory_plugin PUBLIC cxx_std_17) +target_include_directories(pid_trajectory_plugin PUBLIC + $ + $) +ament_target_dependencies(pid_trajectory_plugin PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(pid_trajectory_plugin PUBLIC + pid_trajectory_plugin_parameters +) +pluginlib_export_plugin_description_file(joint_trajectory_controller_plugins plugins.xml) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pid_trajectory_plugin PRIVATE "JOINT_TRAJECTORY_CONTROLLER_PLUGINS_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) +install( + TARGETS pid_trajectory_plugin pid_trajectory_plugin_parameters + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + ament_add_gmock(test_load_pid_trajectory test/test_load_pid_trajectory.cpp) + target_link_libraries(test_load_pid_trajectory pid_trajectory_plugin pid_trajectory_plugin_parameters) + ament_target_dependencies(test_load_pid_trajectory ${THIS_PACKAGE_INCLUDE_DEPENDS}) + + ament_add_gmock(test_pid_trajectory test/test_pid_trajectory.cpp) + target_link_libraries(test_pid_trajectory pid_trajectory_plugin pid_trajectory_plugin_parameters) + ament_target_dependencies(test_pid_trajectory ${THIS_PACKAGE_INCLUDE_DEPENDS}) +endif() + +ament_export_include_directories( + "include/${PROJECT_NAME}" +) +ament_export_libraries( + pid_trajectory_plugin +) +ament_export_targets( + export_${PROJECT_NAME} +) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp new file mode 100644 index 0000000000..3718f04847 --- /dev/null +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp @@ -0,0 +1,91 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_ + +#include +#include +#include + +#include "control_toolbox/pid.hpp" + +#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp" +#include "joint_trajectory_controller_plugins/visibility_control.h" +#include "pid_trajectory_plugin_parameters.hpp" // auto-generated by generate_parameter_library + +using PidPtr = std::shared_ptr; + +namespace joint_trajectory_controller_plugins +{ + +class PidTrajectoryPlugin : public TrajectoryControllerBase +{ +public: + bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; + + bool configure() override; + + bool activate() override; + + bool computeControlLawNonRT_impl( + const std::shared_ptr & /*trajectory*/) override + { + // nothing to do + return true; + } + + bool computeControlLawRT_impl( + const std::shared_ptr & /*trajectory*/) override + { + // nothing to do + return true; + } + + bool updateGainsRT() override; + + void computeCommands( + std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, + const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) override; + + void reset() override; + + void start() override + { + // nothing to do + } + +protected: + /** + * @brief parse PID gains from parameter struct + */ + void parseGains(); + + // number of command joints + size_t num_cmd_joints_; + // PID controllers + std::vector pids_; + // Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector ff_velocity_scale_; + + // Parameters from ROS for joint_trajectory_controller_plugins + std::shared_ptr param_listener_; + Params params_; +}; + +} // namespace joint_trajectory_controller_plugins + +#endif // JOINT_TRAJECTORY_CONTROLLER_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_ diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp new file mode 100644 index 0000000000..9814ddfeb6 --- /dev/null +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp @@ -0,0 +1,177 @@ +// Copyright (c) 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +#include "joint_trajectory_controller_plugins/visibility_control.h" + +namespace joint_trajectory_controller_plugins +{ +class TrajectoryControllerBase +{ +public: + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + TrajectoryControllerBase() = default; + + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual ~TrajectoryControllerBase() = default; + + /** + * @brief initialize the controller plugin. + * declare parameters + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; + + /** + * @brief configure the controller plugin. + * parse read-only parameters, pre-allocate memory for the controller + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual bool configure() = 0; + + /** + * @brief activate the controller plugin. + * parse parameters + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual bool activate() = 0; + + /** + * @brief compute the control law for the given trajectory + * + * this method can take more time to compute the control law. Hence, it will block the execution + * of the trajectory until it finishes + * + * this method is not virtual, any overrides won't be called by JTC. Instead, override + * computeControlLawNonRT_impl for your implementation + * + * @return true if the gains were computed, false otherwise + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + bool computeControlLawNonRT( + const std::shared_ptr & trajectory) + { + rt_control_law_ready_.writeFromNonRT(false); + auto ret = computeControlLawNonRT_impl(trajectory); + rt_control_law_ready_.writeFromNonRT(true); + return ret; + } + + /** + * @brief compute the control law for the given trajectory + * + * this method must finish quickly (within one controller-update rate) + * + * this method is not virtual, any overrides won't be called by JTC. Instead, override + * computeControlLawRT_impl for your implementation + * + * @return true if the gains were computed, false otherwise + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + bool computeControlLawRT( + const std::shared_ptr & trajectory) + { + // TODO(christophfroehlich): Need a lock-free write here + rt_control_law_ready_.writeFromNonRT(false); + auto ret = computeControlLawRT_impl(trajectory); + rt_control_law_ready_.writeFromNonRT(true); + return ret; + } + + /** + * @brief called when the current trajectory started in update loop + * + * use this to implement switching of real-time buffers for updating the control law + */ + virtual void start(void) = 0; + + /** + * @brief update the gains from a RT thread + * + * this method must finish quickly (within one controller-update rate) + * + * @return true if the gains were updated, false otherwise + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual bool updateGainsRT(void) = 0; + + /** + * @brief compute the commands with the precalculated control law + * + * @param[out] tmp_command the output command + * @param[in] current the current state + * @param[in] error the error between the current state and the desired state + * @param[in] desired the desired state + * @param[in] duration_since_start the duration since the start of the trajectory + * can be negative if the trajectory-start is in the future + * @param[in] period the period since the last update + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual void computeCommands( + std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, + const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) = 0; + + /** + * @brief reset internal states + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual void reset() = 0; + + /** + * @return true if the control law is ready (updated with the trajectory) + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + bool is_ready() { return rt_control_law_ready_.readFromRT(); } + +protected: + // the node handle for parameter handling + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + // Are we computing the control law or is it valid? + realtime_tools::RealtimeBuffer rt_control_law_ready_; + + /** + * @brief compute the control law from the given trajectory (in the non-RT loop) + * @return true if the gains were computed, false otherwise + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual bool computeControlLawNonRT_impl( + const std::shared_ptr & trajectory) = 0; + + /** + * @brief compute the control law for a single point (in the RT loop) + * @return true if the gains were computed, false otherwise + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual bool computeControlLawRT_impl( + const std::shared_ptr & trajectory) = 0; +}; + +} // namespace joint_trajectory_controller_plugins + +#endif // JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h new file mode 100644 index 0000000000..71b56114c6 --- /dev/null +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__VISIBILITY_CONTROL_H_ +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT __attribute__((dllexport)) +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT __attribute__((dllimport)) +#else +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT __declspec(dllexport) +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_TRAJECTORY_CONTROLLER_PLUGINS_BUILDING_LIBRARY +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT +#else +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT +#endif +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC_TYPE JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_LOCAL +#else +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT __attribute__((visibility("default"))) +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT +#if __GNUC__ >= 4 +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC __attribute__((visibility("default"))) +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_LOCAL +#endif +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC_TYPE +#endif + +#endif // JOINT_TRAJECTORY_CONTROLLER_PLUGINS__VISIBILITY_CONTROL_H_ diff --git a/joint_trajectory_controller_plugins/package.xml b/joint_trajectory_controller_plugins/package.xml new file mode 100644 index 0000000000..b0812ed8f8 --- /dev/null +++ b/joint_trajectory_controller_plugins/package.xml @@ -0,0 +1,23 @@ + + + + joint_trajectory_controller_plugins + 0.0.0 + TODO: Package description + vscode + Apache-2.0 + + ament_cmake_ros + + control_toolbox + pluginlib + trajectory_msgs + generate_parameter_library + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/joint_trajectory_controller_plugins/plugins.xml b/joint_trajectory_controller_plugins/plugins.xml new file mode 100644 index 0000000000..af91db2a73 --- /dev/null +++ b/joint_trajectory_controller_plugins/plugins.xml @@ -0,0 +1,6 @@ + + + A trajectory controller consisting of a PID controller per joint. + + diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp new file mode 100644 index 0000000000..7ca9b8b4a9 --- /dev/null +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp @@ -0,0 +1,137 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp" + +namespace joint_trajectory_controller_plugins +{ + +bool PidTrajectoryPlugin::initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) +{ + node_ = node; + + try + { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(node_); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return false; + } + + return true; +} + +bool PidTrajectoryPlugin::configure() +{ + try + { + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what()); + return false; + } + + // parse read-only params + num_cmd_joints_ = params_.command_joints.size(); + if (num_cmd_joints_ == 0) + { + RCLCPP_ERROR(node_->get_logger(), "[PidTrajectoryPlugin] No command joints specified."); + return false; + } + pids_.resize(num_cmd_joints_); + ff_velocity_scale_.resize(num_cmd_joints_); + + return true; +}; + +bool PidTrajectoryPlugin::activate() +{ + params_ = param_listener_->get_params(); + parseGains(); + return true; +}; + +bool PidTrajectoryPlugin::updateGainsRT() +{ + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + parseGains(); + } + + return true; +} + +void PidTrajectoryPlugin::parseGains() +{ + for (size_t i = 0; i < num_cmd_joints_; ++i) + { + RCLCPP_DEBUG( + node_->get_logger(), "[PidTrajectoryPlugin] params_.command_joints %lu : %s", i, + params_.command_joints[i].c_str()); + + const auto & gains = params_.gains.command_joints_map.at(params_.command_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; + + RCLCPP_DEBUG(node_->get_logger(), "[PidTrajectoryPlugin] gains.p: %f", gains.p); + RCLCPP_DEBUG( + node_->get_logger(), "[PidTrajectoryPlugin] ff_velocity_scale_: %f", ff_velocity_scale_[i]); + } + + RCLCPP_INFO( + node_->get_logger(), + "[PidTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joint(s).", + num_cmd_joints_); +} + +void PidTrajectoryPlugin::computeCommands( + std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint /*current*/, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, + const rclcpp::Duration & /*duration_since_start*/, const rclcpp::Duration & period) +{ + // Update PIDs + for (auto i = 0ul; i < num_cmd_joints_; ++i) + { + tmp_command[i] = (desired.velocities[i] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + error.positions[i], error.velocities[i], (uint64_t)period.nanoseconds()); + } +} + +void PidTrajectoryPlugin::reset() +{ + for (const auto & pid : pids_) + { + if (pid) + { + pid->reset(); + } + } +} + +} // namespace joint_trajectory_controller_plugins + +#include + +PLUGINLIB_EXPORT_CLASS( + joint_trajectory_controller_plugins::PidTrajectoryPlugin, + joint_trajectory_controller_plugins::TrajectoryControllerBase) diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml new file mode 100644 index 0000000000..10d028c86b --- /dev/null +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml @@ -0,0 +1,37 @@ +joint_trajectory_controller_plugins: + command_joints: { + type: string_array, + default_value: [], + description: "Names of joints used by the controller plugin", + read_only: true, + validation: { + size_gt<>: [0], + } + } + gains: + __map_command_joints: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain :math:`k_p` for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Integral gain :math:`k_i` for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain :math:`k_d` for PID" + } + i_clamp: { + type: double, + default_value: 0.0, + description: "Integral clamp. Symmetrical in both positive and negative direction." + } + ff_velocity_scale: { + type: double, + default_value: 0.0, + description: "Feed-forward scaling :math:`k_{ff}` of velocity" + } diff --git a/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp new file mode 100644 index 0000000000..c53c35c64a --- /dev/null +++ b/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp @@ -0,0 +1,49 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "gmock/gmock.h" + +#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +TEST(TestLoadPidController, load_controller) +{ + rclcpp::init(0, nullptr); + + pluginlib::ClassLoader + traj_controller_loader( + "joint_trajectory_controller_plugins", + "joint_trajectory_controller_plugins::TrajectoryControllerBase"); + + std::shared_ptr traj_contr; + + auto available_classes = traj_controller_loader.getDeclaredClasses(); + + std::cout << "available plugins:" << std::endl; + for (const auto & available_class : available_classes) + { + std::cout << " " << available_class << std::endl; + } + + std::string controller_type = "joint_trajectory_controller_plugins::PidTrajectoryPlugin"; + ASSERT_TRUE(traj_controller_loader.isClassAvailable(controller_type)); + ASSERT_NO_THROW(traj_contr = traj_controller_loader.createSharedInstance(controller_type)); + + rclcpp::shutdown(); +} diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp new file mode 100644 index 0000000000..ef8227a332 --- /dev/null +++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp @@ -0,0 +1,107 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_pid_trajectory.hpp" + +TEST_F(PidTrajectoryTest, TestEmptySetup) +{ + std::shared_ptr traj_contr = + std::make_shared(); + + ASSERT_FALSE(traj_contr->initialize(node_)); +} + +TEST_F(PidTrajectoryTest, TestSingleJoint) +{ + std::shared_ptr traj_contr = + std::make_shared(); + + std::vector joint_names = {"joint1"}; + auto joint_names_paramv = rclcpp::ParameterValue(joint_names); + + // override parameter + node_->declare_parameter("command_joints", joint_names_paramv); + + ASSERT_TRUE(traj_contr->initialize(node_)); + node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0)); + ASSERT_TRUE(traj_contr->configure()); + ASSERT_TRUE(traj_contr->activate()); + ASSERT_TRUE( + traj_contr->computeControlLawNonRT(std::make_shared())); + ASSERT_TRUE(traj_contr->updateGainsRT()); + + trajectory_msgs::msg::JointTrajectoryPoint traj_msg; + traj_msg.positions.push_back(0.0); + traj_msg.velocities.push_back(0.0); + std::vector tmp_command(1, 0.0); + const rclcpp::Duration time_since_start(1, 0); + const rclcpp::Duration period(1, 0); + + ASSERT_NO_THROW(traj_contr->computeCommands( + tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period)); +} + +TEST_F(PidTrajectoryTest, TestMultipleJoints) +{ + std::shared_ptr traj_contr = + std::make_shared(); + + std::vector joint_names = {"joint1", "joint2", "joint3"}; + auto joint_names_paramv = rclcpp::ParameterValue(joint_names); + + // override parameter + node_->declare_parameter("command_joints", joint_names_paramv); + + ASSERT_TRUE(traj_contr->initialize(node_)); + // set dynamic parameters + node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0)); + node_->set_parameter(rclcpp::Parameter("gains.joint2.p", 2.0)); + node_->set_parameter(rclcpp::Parameter("gains.joint3.p", 3.0)); + ASSERT_TRUE(traj_contr->configure()); + ASSERT_TRUE(traj_contr->activate()); + ASSERT_TRUE( + traj_contr->computeControlLawNonRT(std::make_shared())); + ASSERT_TRUE(traj_contr->updateGainsRT()); + + ASSERT_TRUE( + traj_contr->computeControlLawNonRT(std::make_shared())); + ASSERT_TRUE(traj_contr->updateGainsRT()); + + trajectory_msgs::msg::JointTrajectoryPoint traj_msg; + traj_msg.positions.push_back(1.0); + traj_msg.positions.push_back(1.0); + traj_msg.positions.push_back(1.0); + traj_msg.velocities.push_back(0.0); + traj_msg.velocities.push_back(0.0); + traj_msg.velocities.push_back(0.0); + std::vector tmp_command(3, 0.0); + const rclcpp::Duration time_since_start(1, 0); + const rclcpp::Duration period(1, 0); + + ASSERT_NO_THROW(traj_contr->computeCommands( + tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period)); + + EXPECT_EQ(tmp_command[0], 1.0); + EXPECT_EQ(tmp_command[1], 2.0); + EXPECT_EQ(tmp_command[2], 3.0); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp new file mode 100644 index 0000000000..ebe2b95a98 --- /dev/null +++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp @@ -0,0 +1,72 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_PID_TRAJECTORY_HPP_ +#define TEST_PID_TRAJECTORY_HPP_ + +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_pid_trajectory"); +} // namespace + +class TestableJointTrajectoryControllerPlugin +: public joint_trajectory_controller_plugins::PidTrajectoryPlugin +{ +public: + // updates mapped parameters, i.e., should be called after setting the joint names + void trigger_declare_parameters() { param_listener_->declare_params(); } +}; + +class PidTrajectoryTest : public ::testing::Test +{ +public: + void SetUp() override + { + auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + node_ = std::make_shared(testname); + executor_->add_node(node_->get_node_base_interface()); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + PidTrajectoryTest() { executor_ = std::make_shared(); } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + node_.reset(); + } + +protected: + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + std::thread executor_thread_; +}; + +#endif // TEST_PID_TRAJECTORY_HPP_