diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 19ac350e10..a414fddd6b 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -20,6 +20,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tl_expected trajectory_msgs urdf + joint_trajectory_controller_plugins ) find_package(ament_cmake REQUIRED) @@ -50,6 +51,7 @@ target_link_libraries(joint_trajectory_controller PUBLIC control_toolbox::control_toolbox controller_interface::controller_interface hardware_interface::hardware_interface + joint_trajectory_controller_plugins::pid_trajectory_plugin pluginlib::pluginlib rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle @@ -125,4 +127,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 5a3816d462..d706f8a393 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 @@ -25,13 +25,14 @@ #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_msgs/msg/speed_scaling_factor.hpp" #include "control_msgs/srv/query_trajectory_state.hpp" -#include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/loaned_command_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_plugins/trajectory_controller_base.hpp" +#include "pluginlib/class_loader.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" @@ -41,6 +42,7 @@ #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "realtime_tools/realtime_server_goal_handle.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" @@ -102,7 +104,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Storing command joint names for interfaces std::vector command_joint_names_; - +#if RCLCPP_VERSION_MAJOR >= 17 + // TODO(anyone) remove this if there is another way to lock command_joints parameter + rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackHandle::SharedPtr lock_cmd_joint_names; +#endif // Parameters from ROS for joint_trajectory_controller std::shared_ptr param_listener_; Params params_; @@ -129,6 +134,11 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa scaling_state_interface_; std::optional> scaling_command_interface_; + std::vector traj_ctr_state_interface_names_; + std::vector> + traj_ctr_state_interfaces_; + std::vector traj_ctr_state_interfaces_values_; + realtime_tools::RealtimeThreadSafeBox> traj_ctr_state_interfaces_box_; bool has_position_state_interface_ = false; bool has_velocity_state_interface_ = false; @@ -139,11 +149,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_; @@ -221,7 +233,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void sort_to_local_joint_order( std::shared_ptr trajectory_msg) const; bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; - void add_new_trajectory_msg( + void add_new_trajectory_msg_nonRT( + const std::shared_ptr & traj_msg); + void add_new_trajectory_msg_RT( const std::shared_ptr & traj_msg); bool validate_trajectory_point_field( size_t joint_names_size, const std::vector & vector_field, @@ -268,8 +282,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 bc5c4ab0c5..9859772ea8 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -34,6 +34,7 @@ tl_expected trajectory_msgs urdf + joint_trajectory_controller_plugins ament_cmake_gmock controller_manager diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4b74930b7a..aa4082901e 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -25,7 +25,6 @@ #include "angles/angles.h" #include "controller_interface/helpers.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" @@ -41,10 +40,19 @@ #include "urdf/model.h" #endif +#include "joint_trajectory_controller/trajectory.hpp" + namespace joint_trajectory_controller { JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), dof_(0), num_cmd_joints_(0) +: controller_interface::ControllerInterface(), + dof_(0), + num_cmd_joints_(0), + traj_controller_loader_( + std::make_shared< + pluginlib::ClassLoader>( + "joint_trajectory_controller_plugins", + "joint_trajectory_controller_plugins::TrajectoryControllerBase")) { } @@ -125,7 +133,9 @@ JointTrajectoryController::state_interface_configuration() const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(dof_ * params_.state_interfaces.size()); + + conf.names.reserve( + dof_ * params_.state_interfaces.size() + traj_ctr_state_interface_names_.size()); for (const auto & joint_name : params_.joints) { for (const auto & interface_type : params_.state_interfaces) @@ -137,6 +147,9 @@ JointTrajectoryController::state_interface_configuration() const { conf.names.push_back(params_.speed_scaling.state_interface); } + conf.names.insert( + conf.names.end(), traj_ctr_state_interface_names_.begin(), + traj_ctr_state_interface_names_.end()); return conf; } @@ -167,23 +180,29 @@ controller_interface::return_type JointTrajectoryController::update( if (param_listener_->try_update_params(params_)) { default_tolerances_ = get_segment_tolerances(logger, 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_->update_gains_rt() == false) + { + RCLCPP_ERROR(get_node()->get_logger(), "Could not update gains of controller"); + return controller_interface::return_type::ERROR; + } } } // don't update goal after we sampled the trajectory to avoid any race condition const auto active_goal = *rt_active_goal_.readFromRT(); - // Check if a new trajectory message has been received from Non-RT threads - const auto current_trajectory_msg = current_trajectory_->get_trajectory_msg(); + // Check if a new external message has been received from nonRT threads + auto current_external_msg = current_trajectory_->get_trajectory_msg(); auto new_external_msg = new_trajectory_msg_.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_trajectory_msg != *new_external_msg && (rt_has_pending_goal_ && !active_goal) == false) + current_external_msg != *new_external_msg && (rt_has_pending_goal_ && !active_goal) == false && + (traj_contr_ == nullptr || traj_contr_->is_ready())) { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); @@ -219,6 +238,11 @@ controller_interface::return_type JointTrajectoryController::update( current_trajectory_->set_point_before_trajectory_msg( time, state_current_, joints_angle_wraparound_); } + if (traj_contr_) + { + // switch RT buffer of traj_contr_ + traj_contr_->start(); + } traj_time_ = time; } else @@ -265,8 +289,7 @@ controller_interface::return_type JointTrajectoryController::update( { RCLCPP_WARN(logger, "Aborted due to command timeout"); - new_trajectory_msg_.reset(); - new_trajectory_msg_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } // Check state/goal tolerance @@ -310,21 +333,14 @@ 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 < num_cmd_joints_; ++i) - { - // If effort interface only, add desired effort as feed forward - // If velocity interface, ignore desired effort - size_t index_cmd_joint = map_cmd_to_joints_[i]; - tmp_command_[index_cmd_joint] = - (command_next_.velocities[index_cmd_joint] * ff_velocity_scale_[i]) + - (has_effort_command_interface_ ? command_next_.effort[index_cmd_joint] : 0.0) + - pids_[i]->compute_command( - state_error_.positions[index_cmd_joint], state_error_.velocities[index_cmd_joint], - period); - } + double scaling_fact = scaling_factor_.load(); + traj_contr_->compute_commands( + tmp_command_, scaling_fact, state_current_, state_error_, command_next_, + traj_ctr_state_interfaces_values_, time - current_trajectory_->time_from_start(), + period); + scaling_factor_.store(scaling_fact); } // set values for next hardware write() @@ -334,7 +350,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_); } @@ -349,7 +365,7 @@ controller_interface::return_type JointTrajectoryController::update( } if (has_effort_command_interface_) { - if (use_closed_loop_pid_adapter_) + if (use_external_control_law_) { assign_interface_from_point(joint_command_interface_[3], tmp_command_); } @@ -392,8 +408,7 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_WARN(logger, "Aborted due to state tolerance violation"); - new_trajectory_msg_.reset(); - new_trajectory_msg_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } // check goal tolerance else if (!before_last_point) @@ -411,8 +426,7 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_INFO(logger, "Goal reached, success!"); - new_trajectory_msg_.reset(); - new_trajectory_msg_.initRT(set_success_trajectory_point()); + add_new_trajectory_msg_RT(set_success_trajectory_point()); } else if (!within_goal_time) { @@ -430,8 +444,7 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_WARN(logger, "%s", error_string.c_str()); - new_trajectory_msg_.reset(); - new_trajectory_msg_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } } } @@ -440,15 +453,13 @@ controller_interface::return_type JointTrajectoryController::update( // we need to ensure that there is no pending goal -> we get a race condition otherwise RCLCPP_ERROR(logger, "Holding position due to state tolerance violation"); - new_trajectory_msg_.reset(); - new_trajectory_msg_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } else if (!before_last_point && !within_goal_time && !rt_has_pending_goal_) { RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position..."); - new_trajectory_msg_.reset(); - new_trajectory_msg_.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) @@ -531,6 +542,22 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory { assign_point_from_command_interface(state.effort, joint_command_interface_[3]); } + + // Optional traj_ctrl_ state interfaces + for (size_t index = 0; index < traj_ctr_state_interfaces_.size(); ++index) + { + const auto state_interface_value_op = traj_ctr_state_interfaces_[index].get().get_optional(); + if (!state_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve state interface value for joint at index %zu", index); + } + else + { + traj_ctr_state_interfaces_values_[index] = state_interface_value_op.value(); + } + } + traj_ctr_state_interfaces_box_.try_set(traj_ctr_state_interfaces_values_); } bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state) @@ -789,6 +816,38 @@ 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; + } +#if RCLCPP_VERSION_MAJOR >= 17 + // 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_); + } + } + }); +#endif } num_cmd_joints_ = command_joint_names_.size(); @@ -849,19 +908,57 @@ 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) || (has_effort_command_interface_ && params_.command_interfaces.size() == 1); - tmp_command_.resize(dof_, 0.0); - - if (use_closed_loop_pid_adapter_) + if (use_external_control_law_) { - pids_.resize(num_cmd_joints_); - ff_velocity_scale_.resize(num_cmd_joints_); + 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(), map_cmd_to_joints_) == 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(); + traj_ctr_state_interface_names_ = + traj_contr_ ? traj_contr_->state_interface_configuration() : std::vector{}; + traj_ctr_state_interfaces_.reserve(traj_ctr_state_interface_names_.size()); + traj_ctr_state_interfaces_values_.resize(traj_ctr_state_interface_names_.size(), 0.0); + traj_ctr_state_interfaces_box_.set(traj_ctr_state_interfaces_values_); + tmp_command_.resize(dof_, 0.0); } if (params_.state_interfaces.empty()) @@ -1147,6 +1244,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( return CallbackReturn::ERROR; } } + if (!controller_interface::get_ordered_interfaces( + state_interfaces_, traj_ctr_state_interface_names_, "", traj_ctr_state_interfaces_)) + { + RCLCPP_ERROR( + logger, "Expected %zu state interfaces, got %zu.", traj_ctr_state_interface_names_.size(), + traj_ctr_state_interfaces_.size()); + return CallbackReturn::ERROR; + } current_trajectory_ = std::make_shared(); new_trajectory_msg_.writeFromNonRT(std::shared_ptr()); @@ -1175,15 +1280,17 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( read_state_from_state_interfaces(state_current_); read_state_from_state_interfaces(last_commanded_state_); } - // reset/zero out all of the PID's (The integral term is not retained and reset to zero) - for (auto & pid : pids_) + last_commanded_time_ = rclcpp::Time(); + + // activate traj_contr_, e.g., update gains + if (traj_contr_ && traj_contr_->activate() == false) { - pid->reset(); + RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation."); + return CallbackReturn::ERROR; } - last_commanded_time_ = rclcpp::Time(); // 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_ = true; // parse timeout parameter @@ -1270,11 +1377,18 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( joint_command_interface_[index].clear(); joint_state_interface_[index].clear(); } + traj_ctr_state_interfaces_.clear(); subscriber_is_active_ = false; current_trajectory_.reset(); + // e.g., reset integral states + if (traj_contr_) + { + traj_contr_->reset(); + } + return CallbackReturn::SUCCESS; } @@ -1293,14 +1407,6 @@ bool JointTrajectoryController::reset() subscriber_is_active_ = false; joint_command_subscriber_.reset(); - for (const auto & pid : pids_) - { - if (pid) - { - pid->reset(); - } - } - current_trajectory_.reset(); return true; @@ -1349,10 +1455,10 @@ 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_ = false; } -}; +} rclcpp_action::GoalResponse JointTrajectoryController::goal_received_callback( const rclcpp_action::GoalUUID &, std::shared_ptr goal) @@ -1395,7 +1501,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; } @@ -1412,7 +1518,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_ = false; } @@ -1760,10 +1866,42 @@ 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) { new_trajectory_msg_.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_->compute_control_law_non_rt(traj_msg, traj_ctr_state_interfaces_box_.get()) == + 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 + new_trajectory_msg_.reset(); + new_trajectory_msg_.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_->compute_control_law_rt(traj_msg, traj_ctr_state_interfaces_box_.get()) == false) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory."); + } + } } void JointTrajectoryController::preempt_active_goal() @@ -1771,6 +1909,7 @@ void JointTrajectoryController::preempt_active_goal() const auto active_goal = *rt_active_goal_.readFromNonRT(); if (active_goal) { + add_new_trajectory_msg_nonRT(set_hold_position()); auto action_res = std::make_shared(); action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); action_res->set__error_string("Current goal cancelled due to new incoming action."); @@ -1887,33 +2026,6 @@ bool JointTrajectoryController::has_active_trajectory() const return current_trajectory_ != nullptr && current_trajectory_->has_trajectory_msg(); } -void JointTrajectoryController::update_pids() -{ - for (size_t i = 0; i < num_cmd_joints_; ++i) - { - const auto & gains = params_.gains.joints_map.at(params_.joints.at(map_cmd_to_joints_[i])); - control_toolbox::AntiWindupStrategy antiwindup_strat; - antiwindup_strat.set_type(gains.antiwindup_strategy); - antiwindup_strat.i_max = gains.i_clamp_max; - antiwindup_strat.i_min = gains.i_clamp_min; - antiwindup_strat.error_deadband = gains.error_deadband; - antiwindup_strat.tracking_time_constant = gains.tracking_time_constant; - if (pids_[i]) - { - // update PIDs with gains from ROS parameters - pids_[i]->set_gains( - gains.p, gains.i, gains.d, gains.u_clamp_max, gains.u_clamp_min, antiwindup_strat); - } - else - { - // Init PIDs with gains from ROS parameters - pids_[i] = std::make_shared( - gains.p, gains.i, gains.d, gains.u_clamp_max, gains.u_clamp_min, antiwindup_strat); - } - 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 5e7947d15d..7ee15fada3 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -17,7 +17,7 @@ joint_trajectory_controller: * JTC is used in a controller chain where command and state interfaces don't have same names. * If the number of command joints is smaller than the degrees-of-freedom. For example to track the state and error of passive joints. ``command_joints`` must then be a subset of ``joints``.", - read_only: true, + read_only: false, # should be set to true after configuration of the controller validation: { unique<>: null, } @@ -133,73 +133,12 @@ joint_trajectory_controller: ``cmd_timeout`` must be greater than ``constraints.goal_time``, otherwise ignored. If zero, timeout is deactivated", } - 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" - } - ff_velocity_scale: { - type: double, - default_value: 0.0, - description: "Feed-forward scaling :math:`k_{ff}` of velocity" - } - u_clamp_max: { - type: double, - default_value: .inf, - description: "Upper output clamp." - } - u_clamp_min: { - type: double, - default_value: -.inf, - description: "Lower output clamp." - } - i_clamp_max: { - type: double, - default_value: .inf, - description: "Upper integral clamp." - } - i_clamp_min: { - type: double, - default_value: -.inf, - description: "Lower integral clamp." - } - antiwindup_strategy: { - type: string, - default_value: "none", - description: "Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration' or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior.", - validation: { - one_of<>: [[ - "back_calculation", - "conditional_integration", - "none" - ]] - } - } - tracking_time_constant: { - type: double, - default_value: 0.0, - description: "Specifies the tracking time constant for the 'back_calculation' strategy. If - set to 0.0 when this strategy is selected, a recommended default value will be applied." - } - error_deadband: { - type: double, - default_value: 0.0, - description: "Is used to stop integration when the error is within the given range." - } + controller_plugin: { + type: string, + default_value: "joint_trajectory_controller_plugins::PidTrajectoryPlugin", + description: "Type of the plugin for the trajectory controller", + read_only: true, + } constraints: stopped_velocity_tolerance: { type: double, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 07b2432275..8f8865acd6 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -467,30 +467,43 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) { rclcpp::executors::MultiThreadedExecutor executor; + // with kp = 0.0 SetUpAndActivateTrajectoryController(executor); updateControllerAsync(); - 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)->get_gains(); - EXPECT_EQ(gain_0.p_gain_, 0.0); + auto pids = traj_controller_->get_traj_contr(); + + if (traj_controller_->use_external_control_law()) + { + 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)); + + double scaling_fact = 1.0; + pids->compute_commands( + tmp_command, scaling_fact, current, error, desired, {}, duration_since_start, period); + EXPECT_EQ(tmp_command.at(0), 0.0); + EXPECT_EQ(scaling_fact, 1.0); double kp = 1.0; SetPidParameters(kp); updateControllerAsync(); - pids = traj_controller_->get_pids(); - EXPECT_EQ(pids.size(), 3); - gain_0 = pids.at(0)->get_gains(); - EXPECT_EQ(gain_0.p_gain_, kp); + pids->compute_commands( + tmp_command, scaling_fact, current, error, desired, {}, duration_since_start, period); + EXPECT_EQ(tmp_command.at(0), 1.0); + EXPECT_EQ(scaling_fact, 1.0); } else { // nothing to check here, skip further test - EXPECT_EQ(pids.size(), 0); + EXPECT_EQ(pids, nullptr); } executor.cancel(); @@ -813,8 +826,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( @@ -940,8 +953,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( @@ -1078,8 +1091,7 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le EXPECT_LT(0.0, joint_vel_[1]); EXPECT_TRUE(std::isnan(current_command.velocities[2])); - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) + if (traj_controller_->use_external_control_law()) { // we expect u = k_p * (s_d-s) EXPECT_NEAR( @@ -1192,8 +1204,7 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le EXPECT_LT(0.0, joint_vel_[1]); EXPECT_TRUE(std::isnan(current_command.velocities[2])); - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) + if (traj_controller_->use_external_control_law()) { // we expect u = k_p * (s_d-s) EXPECT_NEAR( @@ -1361,9 +1372,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; @@ -1377,7 +1388,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) (traj_controller_->has_effort_command_interface() && !traj_controller_->has_position_command_interface())) { - EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter()); + EXPECT_TRUE(traj_controller_->use_external_control_law()); } } @@ -1434,9 +1445,9 @@ 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_->has_velocity_state_interface() && - traj_controller_->has_velocity_command_interface())) + traj_controller_->use_external_control_law() || // always needed for PID controller + (traj_controller_->has_velocity_state_interface() && + traj_controller_->has_velocity_command_interface())) { // don't check against a value, because spline interpolation might overshoot depending on // interface combinations @@ -1507,8 +1518,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 ef3c8b8054..24a34c8ccb 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -119,11 +119,6 @@ class TestableJointTrajectoryController } } - 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; @@ -166,15 +161,19 @@ 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_; } + + std::shared_ptr get_traj_contr() + const + { + return traj_contr_; + } joint_trajectory_controller::SegmentTolerances get_active_tolerances() { return *(active_tolerances_.readFromRT()); } - std::vector get_pids() const { return pids_; } - joint_trajectory_controller::SegmentTolerances get_tolerances() const { return default_tolerances_; @@ -288,12 +287,17 @@ class TrajectoryControllerTest : public ::testing::Test controller_name_, urdf, 100, "", traj_controller_->define_custom_node_options()); } + /** + * @brief set PIDs for every entry in joint_names_ + */ void SetPidParameters(double p_value = 0.0, double ff_value = 1.0) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); - for (size_t i = 0; i < joint_names_.size(); ++i) + // if command_joints were not set manually, it was done in the on_configure() method + auto command_joint_names = node->get_parameter("command_joints").as_string_array(); + for (size_t i = 0; i < command_joint_names.size(); ++i) { const std::string prefix = "gains." + joint_names_[i]; const rclcpp::Parameter k_p(prefix + ".p", p_value); @@ -328,10 +332,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_->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); @@ -654,7 +663,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()) { @@ -684,7 +693,7 @@ class TrajectoryControllerTest : public ::testing::Test EXPECT_EQ(effort.at(2), joint_eff_[2]); } } - else // traj_controller_->use_closed_loop_pid_adapter() == true + else // traj_controller_->use_external_control_law() == true { // velocity or effort PID? // --> set kp > 0.0 in test @@ -694,9 +703,9 @@ class TrajectoryControllerTest : public ::testing::Test { EXPECT_TRUE(is_same_sign_or_zero( position.at(i) - pos_state_interfaces_[i]->get_optional().value(), joint_vel_[i])) - << "test position point " << position.at(i) << ", position state is " - << pos_state_interfaces_[i]->get_optional().value() << ", velocity command is " - << joint_vel_[i]; + << std::fixed << std::setprecision(2) << "test position point " << position.at(i) + << ", position state is " << pos_state_interfaces_[i]->get_optional().value() + << ", velocity command is " << joint_vel_[i]; } } if (traj_controller_->has_effort_command_interface()) @@ -706,9 +715,9 @@ class TrajectoryControllerTest : public ::testing::Test EXPECT_TRUE(is_same_sign_or_zero( position.at(i) - pos_state_interfaces_[i]->get_optional().value() + effort.at(i), joint_eff_[i])) - << "test position point " << position.at(i) << ", position state is " - << pos_state_interfaces_[i]->get_optional().value() << ", effort command is " - << joint_eff_[i]; + << std::fixed << std::setprecision(2) << "test position point " << position.at(i) + << ", position state is " << pos_state_interfaces_[i]->get_optional().value() + << ", effort command is " << joint_eff_[i]; } } } diff --git a/joint_trajectory_controller_plugins/CMakeLists.txt b/joint_trajectory_controller_plugins/CMakeLists.txt new file mode 100644 index 0000000000..654b697302 --- /dev/null +++ b/joint_trajectory_controller_plugins/CMakeLists.txt @@ -0,0 +1,75 @@ +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 + $ + $) +target_link_libraries(pid_trajectory_plugin PUBLIC + pid_trajectory_plugin_parameters + control_toolbox::control_toolbox + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${trajectory_msgs_TARGETS} +) +pluginlib_export_plugin_description_file(joint_trajectory_controller_plugins plugins.xml) + +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) + + ament_add_gmock(test_pid_trajectory test/test_pid_trajectory.cpp) + target_link_libraries(test_pid_trajectory pid_trajectory_plugin) +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..8342168d2a --- /dev/null +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp @@ -0,0 +1,102 @@ +// 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/pid_trajectory_plugin_parameters.hpp" // auto-generated by generate_parameter_library +#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp" + +using PidPtr = std::shared_ptr; + +namespace joint_trajectory_controller_plugins +{ + +class PidTrajectoryPlugin : public TrajectoryControllerBase +{ +public: + bool update_gains_rt() override; + + void compute_commands( + std::vector & tmp_command, double & scaling_fact, + const trajectory_msgs::msg::JointTrajectoryPoint current, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, + const std::vector & opt_state_interfaces_values, + const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) override; + + void reset() override; + + void start() override + { + // nothing to do + } + +protected: + // Parameters from ROS for joint_trajectory_controller_plugins + std::shared_ptr param_listener_; + + bool on_initialize(void) override; + + rclcpp::Logger set_logger() const override + { + return node_->get_logger().get_child("PidTrajectoryPlugin"); + } + + bool on_configure() override; + + bool on_activate() override; + + bool on_compute_control_law_non_rt( + const std::shared_ptr & /*trajectory*/, + const std::vector & /*opt_state_interfaces_values*/) override + { + // nothing to do + return true; + } + + bool on_compute_control_law_rt( + const std::shared_ptr & /*trajectory*/, + const std::vector & /*opt_state_interfaces_values*/) override + { + // nothing to do + return true; + } + +private: + /** + * @brief parse PID gains from parameter struct + */ + void parse_gains(); + + // 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 + 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..182031d59a --- /dev/null +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp @@ -0,0 +1,221 @@ +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace joint_trajectory_controller_plugins +{ +class TrajectoryControllerBase +{ +public: + TrajectoryControllerBase() = default; + + virtual ~TrajectoryControllerBase() = default; + + /** + * @brief get additional state interfaces required by this controller plugin + */ + virtual std::vector state_interface_configuration() const { return {}; } + + /** + * @brief initialize the controller plugin. + * @param node the node handle to use for parameter handling + * @param map_cmd_to_joints a mapping from the joint names in the trajectory messages to the + * command joints + */ + bool initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector map_cmd_to_joints) + { + node_ = node; + map_cmd_to_joints_ = map_cmd_to_joints; + logger_ = this->set_logger(); + return on_initialize(); + }; + + /** + * @brief configure the controller plugin. + */ + bool configure() { return on_configure(); } + + /** + * @brief activate the controller plugin. + */ + bool activate() { return on_activate(); } + + /** + * @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 + * on_compute_control_law_non_rt for your implementation + * + * @return true if the gains were computed, false otherwise + */ + bool compute_control_law_non_rt( + const std::shared_ptr & trajectory, + const std::vector & opt_state_interfaces_values) + { + rt_control_law_ready_ = false; + auto ret = on_compute_control_law_non_rt(trajectory, opt_state_interfaces_values); + rt_control_law_ready_ = 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 + * on_compute_control_law_rt for your implementation + * + * @return true if the gains were computed, false otherwise + */ + bool compute_control_law_rt( + const std::shared_ptr & trajectory, + const std::vector & opt_state_interfaces_values) + { + rt_control_law_ready_ = false; + auto ret = on_compute_control_law_rt(trajectory, opt_state_interfaces_values); + rt_control_law_ready_ = 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 + */ + virtual bool update_gains_rt(void) = 0; + + /** + * @brief compute the commands with the precalculated control law + * + * @param[out] tmp_command the output command + * @param[out] scaling_fact scaling factor if ctrl is not following reference + * @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] opt_state_interfaces_values optional state interface values, \ref + * state_interface_configuration + * @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 + */ + virtual void compute_commands( + std::vector & tmp_command, double & scaling_fact, + const trajectory_msgs::msg::JointTrajectoryPoint current, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, + const std::vector & opt_state_interfaces_values, + const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) = 0; + + /** + * @brief reset internal states + * + * is called in on_deactivate() of JTC + */ + virtual void reset() = 0; + + /** + * @return true if the control law is ready (updated with the trajectory) + */ + bool is_ready() { return rt_control_law_ready_; } + +protected: + // the node handle for parameter handling + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + // map from joints in the message to command joints + std::vector map_cmd_to_joints_; + + /** + * @brief Get the logger for this plugin + */ + rclcpp::Logger get_logger() const { return logger_; } + /** + * @brief Get the logger for this plugin + */ + virtual rclcpp::Logger set_logger() const = 0; + + /** + * @brief compute the control law from the given trajectory (in the non-RT loop) + * @return true if the gains were computed, false otherwise + */ + virtual bool on_compute_control_law_non_rt( + const std::shared_ptr & trajectory, + const std::vector & opt_state_interfaces_values) = 0; + + /** + * @brief compute the control law for a single point (in the RT loop) + * @return true if the gains were computed, false otherwise + */ + virtual bool on_compute_control_law_rt( + const std::shared_ptr & trajectory, + const std::vector & opt_state_interfaces_values) = 0; + + /** + * @brief initialize the controller plugin. + * + * declare parameters + */ + virtual bool on_initialize(void) = 0; + + /** + * @brief configure the controller plugin. + * + * parse read-only parameters, pre-allocate memory for the controller + */ + virtual bool on_configure() = 0; + + /** + * @brief activate the controller plugin. + * + * parse parameters + */ + virtual bool on_activate() = 0; + +private: + // child logger for this plugin + rclcpp::Logger logger_ = rclcpp::get_logger("joint_trajectory_controller_plugins"); + // Are we computing the control law or is it valid? + std::atomic rt_control_law_ready_; +}; + +} // namespace joint_trajectory_controller_plugins + +#endif // JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ diff --git a/joint_trajectory_controller_plugins/package.xml b/joint_trajectory_controller_plugins/package.xml new file mode 100644 index 0000000000..2d09f4beb7 --- /dev/null +++ b/joint_trajectory_controller_plugins/package.xml @@ -0,0 +1,25 @@ + + + + joint_trajectory_controller_plugins + 0.0.0 + TODO: Package description + vscode + Apache-2.0 + + ament_cmake_ros + + backward_ros + control_toolbox + generate_parameter_library + pluginlib + trajectory_msgs + + ament_cmake_gmock + 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..9351cb331d --- /dev/null +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp @@ -0,0 +1,154 @@ +// 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::on_initialize() +{ + 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::on_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(get_logger(), "No command joints specified."); + return false; + } + if (num_cmd_joints_ != map_cmd_to_joints_.size()) + { + RCLCPP_ERROR(get_logger(), "map_cmd_to_joints has to be of size num_cmd_joints."); + return false; + } + pids_.resize(num_cmd_joints_); // memory for the shared pointers, will be nullptr + // create the objects with default values + for (size_t i = 0; i < num_cmd_joints_; ++i) + { + pids_[i] = std::make_shared(); + } + ff_velocity_scale_.resize(num_cmd_joints_, 0.0); + + return true; +} + +bool PidTrajectoryPlugin::on_activate() +{ + params_ = param_listener_->get_params(); + parse_gains(); + return true; +} + +bool PidTrajectoryPlugin::update_gains_rt() +{ + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + parse_gains(); + } + + return true; +} + +void PidTrajectoryPlugin::parse_gains() +{ + for (size_t i = 0; i < num_cmd_joints_; ++i) + { + RCLCPP_DEBUG( + get_logger(), "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]); + control_toolbox::AntiWindupStrategy antiwindup_strat; + antiwindup_strat.set_type(gains.antiwindup_strategy); + antiwindup_strat.i_max = gains.i_clamp_max; + antiwindup_strat.i_min = gains.i_clamp_min; + antiwindup_strat.error_deadband = gains.error_deadband; + antiwindup_strat.tracking_time_constant = gains.tracking_time_constant; + pids_[i]->set_gains( + gains.p, gains.i, gains.d, gains.u_clamp_max, gains.u_clamp_min, antiwindup_strat); + ff_velocity_scale_[i] = gains.ff_velocity_scale; + + RCLCPP_DEBUG(get_logger(), "gains.p: %f", gains.p); + RCLCPP_DEBUG(get_logger(), "ff_velocity_scale_: %f", ff_velocity_scale_[i]); + } + + RCLCPP_INFO( + get_logger(), "Loaded PID gains from ROS parameters for %lu joint(s).", num_cmd_joints_); +} + +void PidTrajectoryPlugin::compute_commands( + std::vector & tmp_command, double & /*scaling_fact*/, + const trajectory_msgs::msg::JointTrajectoryPoint /*current*/, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, + const std::vector & /* opt_state_interfaces_values*/, + const rclcpp::Duration & /*duration_since_start*/, const rclcpp::Duration & period) +{ + // if effort field is present, otherwise it would have been rejected + auto has_effort_command_interface = !desired.effort.empty(); + // Update PIDs + for (auto i = 0ul; i < num_cmd_joints_; ++i) + { + const auto map_cmd_to_joint = map_cmd_to_joints_[i]; + tmp_command[map_cmd_to_joint] = + (desired.velocities[map_cmd_to_joint] * ff_velocity_scale_[i]) + + (has_effort_command_interface ? desired.effort[i] : 0.0) + + pids_[i]->compute_command( + error.positions[map_cmd_to_joint], error.velocities[map_cmd_to_joint], period); + } +} + +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..0219786813 --- /dev/null +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml @@ -0,0 +1,77 @@ +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" + } + ff_velocity_scale: { + type: double, + default_value: 0.0, + description: "Feed-forward scaling :math:`k_{ff}` of velocity" + } + u_clamp_max: { + type: double, + default_value: .inf, + description: "Upper output clamp." + } + u_clamp_min: { + type: double, + default_value: -.inf, + description: "Lower output clamp." + } + i_clamp_max: { + type: double, + default_value: .inf, + description: "Upper integral clamp." + } + i_clamp_min: { + type: double, + default_value: -.inf, + description: "Lower integral clamp." + } + antiwindup_strategy: { + type: string, + default_value: "none", + description: "Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration' or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior.", + validation: { + one_of<>: [[ + "back_calculation", + "conditional_integration", + "none" + ]] + } + } + tracking_time_constant: { + type: double, + default_value: 0.0, + description: "Specifies the tracking time constant for the 'back_calculation' strategy. If + set to 0.0 when this strategy is selected, a recommended default value will be applied." + } + error_deadband: { + type: double, + default_value: 0.0, + description: "Is used to stop integration when the error is within the given range." + } 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..a1faae5137 --- /dev/null +++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp @@ -0,0 +1,114 @@ +// 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(); + + std::vector map_cmd_to_joints{}; + ASSERT_FALSE(traj_contr->initialize(node_, map_cmd_to_joints)); +} + +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); + + std::vector map_cmd_to_joints{0}; + ASSERT_TRUE(traj_contr->initialize(node_, map_cmd_to_joints)); + node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0)); + ASSERT_TRUE(traj_contr->configure()); + ASSERT_TRUE(traj_contr->activate()); + ASSERT_TRUE(traj_contr->compute_control_law_non_rt( + std::make_shared(), {})); + ASSERT_TRUE(traj_contr->update_gains_rt()); + + 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); + + double scaling_fact = 1.0; + ASSERT_NO_THROW(traj_contr->compute_commands( + tmp_command, scaling_fact, traj_msg, traj_msg, traj_msg, {}, time_since_start, period)); + EXPECT_EQ(scaling_fact, 1.0); +} + +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); + + std::vector map_cmd_to_joints{0, 1, 2}; + ASSERT_TRUE(traj_contr->initialize(node_, map_cmd_to_joints)); + // 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->compute_control_law_non_rt( + std::make_shared(), {})); + ASSERT_TRUE(traj_contr->update_gains_rt()); + + ASSERT_TRUE(traj_contr->compute_control_law_non_rt( + std::make_shared(), {})); + ASSERT_TRUE(traj_contr->update_gains_rt()); + + 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); + + double scaling_fact = 1.0; + ASSERT_NO_THROW(traj_contr->compute_commands( + tmp_command, scaling_fact, traj_msg, traj_msg, traj_msg, {}, time_since_start, period)); + EXPECT_EQ(scaling_fact, 1.0); + + 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_ diff --git a/ros2_controllers.cmake b/ros2_controllers.cmake new file mode 100644 index 0000000000..eceacdf4d7 --- /dev/null +++ b/ros2_controllers.cmake @@ -0,0 +1,44 @@ +# Copyright 2025 AIT - Austrian Institute of Technology GmbH +# +# 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. + +# set compiler options depending on detected compiler +macro(set_compiler_options) + if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format + -Werror=missing-braces) + message(STATUS "Compiler warnings enabled for ${CMAKE_CXX_COMPILER_ID}") + + # Extract major version if g++ is used + if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + + string(REPLACE "." ";" VERSION_LIST ${CMAKE_CXX_COMPILER_VERSION}) + list(GET VERSION_LIST 0 GCC_MAJOR_VERSION) + list(GET VERSION_LIST 1 GCC_MINOR_VERSION) + + message(STATUS "Detected GCC Version: ${CMAKE_CXX_COMPILER_VERSION} (Major: ${GCC_MAJOR_VERSION}, Minor: ${GCC_MINOR_VERSION})") + + if (GCC_MAJOR_VERSION GREATER 10) + # GCC 11 introduced -Werror=range-loop-construct + add_compile_options(-Werror=range-loop-construct) + endif() + endif() + endif() +endmacro() + +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +macro(export_windows_symbols) + set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) +endmacro()