diff --git a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp index 69d510c7f..52cbdd944 100644 --- a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp @@ -37,13 +37,8 @@ #ifndef UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_ #define UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_ -#include #include -#include #include "joint_trajectory_controller/joint_trajectory_controller.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp/duration.hpp" #include "ur_controllers/scaled_joint_trajectory_controller_parameters.hpp" namespace ur_controllers @@ -54,43 +49,11 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join ScaledJointTrajectoryController() = default; ~ScaledJointTrajectoryController() override = default; - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override; - - controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; - CallbackReturn on_init() override; private: - std::atomic scaling_factor_{ 1.0 }; - - std::optional> scaling_state_interface_ = - std::nullopt; - std::shared_ptr scaled_param_listener_; scaled_joint_trajectory_controller::Params scaled_params_; - - // Private methods copied from Upstream JTC - void update_pids(); - - /** - * @brief Assigns the values from a trajectory point interface to a joint interface. - * - * @tparam T The type of the joint interface. - * @param[out] joint_interface The reference_wrapper to assign the values to - * @param[in] trajectory_point_interface Containing the values to assign. - * @todo: Use auto in parameter declaration with c++20 - */ - template - bool assign_interface_from_point(const T& joint_interface, const std::vector& trajectory_point_interface) - { - bool success = true; - for (size_t index = 0; index < num_cmd_joints_; ++index) { - success &= joint_interface[index].get().set_value(trajectory_point_interface[map_cmd_to_joints_[index]]); - } - return success; - } }; } // namespace ur_controllers diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index bdc935b0d..1bd5ca1b1 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -36,344 +36,25 @@ //---------------------------------------------------------------------- #include -#include #include "ur_controllers/scaled_joint_trajectory_controller.hpp" -#include "lifecycle_msgs/msg/state.hpp" - namespace ur_controllers { controller_interface::CallbackReturn ScaledJointTrajectoryController::on_init() { - // Create the parameter listener and get the parameters + // Translate speed scaling state interface parameter scaled_param_listener_ = std::make_shared(get_node()); scaled_params_ = scaled_param_listener_->get_params(); if (!scaled_params_.speed_scaling_interface_name.empty()) { - RCLCPP_INFO(get_node()->get_logger(), "Using scaling state from the hardware from interface %s.", - scaled_params_.speed_scaling_interface_name.c_str()); - } else { - RCLCPP_INFO(get_node()->get_logger(), "No scaling interface set. This controller will not use speed scaling."); + get_node()->declare_parameter("speed_scaling.state_interface", scaled_params_.speed_scaling_interface_name); + get_node()->set_parameter( + rclcpp::Parameter("speed_scaling.state_interface", scaled_params_.speed_scaling_interface_name)); } - return JointTrajectoryController::on_init(); } -controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::state_interface_configuration() const -{ - controller_interface::InterfaceConfiguration conf; - conf = JointTrajectoryController::state_interface_configuration(); - - if (!scaled_params_.speed_scaling_interface_name.empty()) { - conf.names.push_back(scaled_params_.speed_scaling_interface_name); - } - - return conf; -} - -controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) -{ - // Set scaling interfaces - if (!scaled_params_.speed_scaling_interface_name.empty()) { - auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), [&](auto& interface) { - return (interface.get_name() == scaled_params_.speed_scaling_interface_name); - }); - if (it != state_interfaces_.end()) { - scaling_state_interface_ = *it; - } else { - RCLCPP_ERROR(get_node()->get_logger(), "Did not find speed scaling interface in state interfaces."); - } - } - - return JointTrajectoryController::on_activate(state); -} - -controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time, - const rclcpp::Duration& period) -{ - if (scaling_state_interface_.has_value()) { - scaling_factor_ = scaling_state_interface_->get().get_optional().value_or(1.0); - } - - auto logger = this->get_node()->get_logger(); - // update dynamic parameters - if (param_listener_->is_old(params_)) { - params_ = param_listener_->get_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_pids(); - } - } - - // don't update goal after we sampled the trajectory to avoid any racecondition - 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(); - auto new_external_msg = new_trajectory_msg_.readFromRT(); - // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) - if (current_trajectory_msg != *new_external_msg && (rt_has_pending_goal_ && !active_goal) == false) { - fill_partial_goal(*new_external_msg); - sort_to_local_joint_order(*new_external_msg); - // TODO(denis): Add here integration of position and velocity - current_trajectory_->update(*new_external_msg); - } - - // current state update - state_current_.time_from_start.set__sec(0); - read_state_from_state_interfaces(state_current_); - - // currently carrying out a trajectory - if (has_active_trajectory()) { - bool first_sample = false; - joint_trajectory_controller::TrajectoryPointConstIter start_segment_itr, end_segment_itr; - // if sampling the first time, set the point before you sample - if (!current_trajectory_->is_sampled_already()) { - first_sample = true; - if (params_.interpolate_from_desired_state) { - if (std::abs(last_commanded_time_.seconds()) < std::numeric_limits::epsilon()) { - last_commanded_time_ = time; - } - current_trajectory_->set_point_before_trajectory_msg(last_commanded_time_, last_commanded_state_, - joints_angle_wraparound_); - } else { - current_trajectory_->set_point_before_trajectory_msg(time, state_current_, joints_angle_wraparound_); - } - traj_time_ = time; - } else { - traj_time_ += period * scaling_factor_.load(); - } - - // Sample expected state from the trajectory - current_trajectory_->sample(traj_time_, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); - state_desired_.time_from_start = traj_time_ - current_trajectory_->time_from_start(); - - // Sample setpoint for next control cycle - const bool valid_point = current_trajectory_->sample(traj_time_ + update_period_, interpolation_method_, - command_next_, start_segment_itr, end_segment_itr, false); - - state_current_.time_from_start = time - current_trajectory_->time_from_start(); - - if (valid_point) { - const rclcpp::Time traj_start = current_trajectory_->time_from_start(); - // this is the time instance - // - started with the first segment: when the first point will be reached (in the future) - // - later: when the point of the current segment was reached - const rclcpp::Time segment_time_from_start = traj_start + start_segment_itr->time_from_start; - // time_difference is - // - negative until first point is reached - // - counting from zero to time_from_start of next point - double time_difference = traj_time_.seconds() - segment_time_from_start.seconds(); - bool tolerance_violated_while_moving = false; - bool outside_goal_tolerance = false; - bool within_goal_time = true; - const bool before_last_point = end_segment_itr != current_trajectory_->end(); - auto active_tol = active_tolerances_.readFromRT(); - - // have we reached the end, are not holding position, and is a timeout configured? - // Check independently of other tolerances - if (!before_last_point && !rt_is_holding_ && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) { - RCLCPP_WARN(logger, "Aborted due to command timeout"); - - new_trajectory_msg_.reset(); - new_trajectory_msg_.initRT(set_hold_position()); - } - - // Check state/goal tolerance - for (size_t index = 0; index < dof_; ++index) { - compute_error_for_joint(state_error_, index, state_current_, state_desired_); - - // Always check the state tolerance on the first sample in case the first sample - // is the last point - // print output per default, goal will be aborted afterwards - if ((before_last_point || first_sample) && !rt_is_holding_ && - !check_state_tolerance_per_joint(state_error_, index, active_tol->state_tolerance[index], - true /* show_errors */)) { - tolerance_violated_while_moving = true; - } - // past the final point, check that we end up inside goal tolerance - if (!before_last_point && !rt_is_holding_ && - !check_state_tolerance_per_joint(state_error_, index, active_tol->goal_state_tolerance[index], - false /* show_errors */)) { - outside_goal_tolerance = true; - - if (active_tol->goal_time_tolerance != 0.0) { - // if we exceed goal_time_tolerance set it to aborted - if (time_difference > active_tol->goal_time_tolerance) { - within_goal_time = false; - // print once, goal will be aborted afterwards - check_state_tolerance_per_joint(state_error_, index, default_tolerances_.goal_state_tolerance[index], - true /* show_errors */); - } - } - } - } - - // set values for next hardware write() if tolerance is met - if (!tolerance_violated_while_moving && within_goal_time) { - if (use_closed_loop_pid_adapter_) { - // 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); - } - } - - // set values for next hardware write() - if (has_position_command_interface_) { - assign_interface_from_point(joint_command_interface_[0], command_next_.positions); - } - if (has_velocity_command_interface_) { - if (use_closed_loop_pid_adapter_) { - assign_interface_from_point(joint_command_interface_[1], tmp_command_); - } else { - assign_interface_from_point(joint_command_interface_[1], command_next_.velocities); - } - } - if (has_acceleration_command_interface_) { - assign_interface_from_point(joint_command_interface_[2], command_next_.accelerations); - } - if (has_effort_command_interface_) { - if (use_closed_loop_pid_adapter_) { - assign_interface_from_point(joint_command_interface_[3], tmp_command_); - } else { - // If position and effort command interfaces, only pass desired effort - assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); - } - } - - // store the previous command and time used in open-loop control mode - last_commanded_state_ = command_next_; - last_commanded_time_ = time; - } - - if (active_goal) { - // send feedback - auto feedback = std::make_shared(); - feedback->header.stamp = time; - feedback->joint_names = params_.joints; - - feedback->actual = state_current_; - feedback->desired = state_desired_; - feedback->error = state_error_; - active_goal->setFeedback(feedback); - - // check abort - if (tolerance_violated_while_moving) { - auto result = std::make_shared(); - result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); - result->set__error_string("Aborted due to path tolerance violation"); - active_goal->setAborted(result); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - rt_has_pending_goal_ = false; - - RCLCPP_WARN(logger, "Aborted due to state tolerance violation"); - - new_trajectory_msg_.reset(); - new_trajectory_msg_.initRT(set_hold_position()); - } else if (!before_last_point) { - // check goal tolerance - if (!outside_goal_tolerance) { - auto result = std::make_shared(); - result->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); - result->set__error_string("Goal successfully reached!"); - active_goal->setSucceeded(result); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - rt_has_pending_goal_ = false; - - RCLCPP_INFO(logger, "Goal reached, success!"); - - new_trajectory_msg_.reset(); - new_trajectory_msg_.initRT(set_success_trajectory_point()); - } else if (!within_goal_time) { - const std::string error_string = - "Aborted due to goal_time_tolerance exceeding by " + std::to_string(time_difference) + " seconds"; - - auto result = std::make_shared(); - result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); - result->set__error_string(error_string); - active_goal->setAborted(result); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - rt_has_pending_goal_ = false; - - RCLCPP_WARN(logger, "%s", error_string.c_str()); - - new_trajectory_msg_.reset(); - new_trajectory_msg_.initRT(set_hold_position()); - } - } - } else if (tolerance_violated_while_moving && !rt_has_pending_goal_) { - // 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()); - } 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()); - } - // else, run another cycle while waiting for outside_goal_tolerance - // to be satisfied (will stay in this state until new message arrives) - // or outside_goal_tolerance violated within the goal_time_tolerance - } - } - - // TODO(fmauch): Remove once merged upstream - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - if (state_publisher_->trylock()) { - state_publisher_->msg_.speed_scaling_factor = scaling_factor_; - state_publisher_->unlock(); - } -#pragma GCC diagnostic pop - // end remove once merged upstream - - publish_state(time, state_desired_, state_current_, state_error_); - - return controller_interface::return_type::OK; -} - -// Copied from upstream JTC -void ScaledJointTrajectoryController::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; - antiwindup_strat.i_min = -gains.i_clamp; - 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; - } -} - } // namespace ur_controllers #include "pluginlib/class_list_macros.hpp"