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 7ec1814f1..69d510c7f 100644 --- a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp @@ -39,9 +39,8 @@ #include #include -#include "angles/angles.h" +#include #include "joint_trajectory_controller/joint_trajectory_controller.hpp" -#include "joint_trajectory_controller/trajectory.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp/duration.hpp" @@ -63,26 +62,35 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join CallbackReturn on_init() override; -protected: - struct TimeData - { - TimeData() : time(0.0), period(rclcpp::Duration::from_nanoseconds(0.0)), uptime(0.0) - { - } - rclcpp::Time time; - rclcpp::Duration period; - rclcpp::Time uptime; - }; - private: - double scaling_factor_{ 1.0 }; - realtime_tools::RealtimeBuffer time_data_; + 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 9ac12d9c8..4a6320fa1 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -74,12 +74,6 @@ controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::st controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) { - TimeData time_data; - time_data.time = get_node()->now(); - time_data.period = rclcpp::Duration::from_nanoseconds(0); - time_data.uptime = get_node()->now(); - time_data_.initRT(time_data); - // Set scaling interfaces if (!scaled_params_.speed_scaling_interface_name.empty()) { auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), [&](auto& interface) { @@ -102,83 +96,66 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const scaling_factor_ = scaling_state_interface_->get().get_optional().value_or(1.0); } - if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - return controller_interface::return_type::OK; - } - - auto compute_error_for_joint = [&](JointTrajectoryPoint& error, size_t index, const JointTrajectoryPoint& current, - const JointTrajectoryPoint& desired) { - // error defined as the difference between current and desired - if (joints_angle_wraparound_[index]) { - // if desired, the shortest_angular_distance is calculated, i.e., the error is - // normalized between -piget_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(); } - if (has_velocity_state_interface_ && (has_velocity_command_interface_ || has_effort_command_interface_)) { - error.velocities[index] = desired.velocities[index] - current.velocities[index]; - } - if (has_acceleration_state_interface_ && has_acceleration_command_interface_) { - error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; - } - }; + } // 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 external message has been received from nonRT threads - auto current_external_msg = current_trajectory_->get_trajectory_msg(); + // 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_external_msg != *new_external_msg && (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) { + if (current_trajectory_msg != *new_external_msg && (*(rt_has_pending_goal_.readFromRT()) && !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); } - // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not - // changed, but its value only? - auto assign_interface_from_point = [&](auto& joint_interface, const std::vector& trajectory_point_interface) { - bool success = true; - for (size_t index = 0; index < dof_; ++index) { - success &= joint_interface[index].get().set_value(trajectory_point_interface[index]); - } - return success; - }; - // 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()) { - // Main Speed scaling difference... - // Adjust time with scaling factor - TimeData time_data; - time_data.time = time; - rcl_duration_value_t t_period = (time_data.time - time_data_.readFromRT()->time).nanoseconds(); - time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * t_period); - time_data.uptime = time_data_.readFromRT()->uptime + time_data.period; - rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(t_period); - time_data_.writeFromNonRT(time_data); - 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_.open_loop_control) { - current_trajectory_->set_point_before_trajectory_msg(traj_time, last_commanded_state_); + if (params_.interpolate_from_desired_state || params_.open_loop_control) { + 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(traj_time, state_current_); + current_trajectory_->set_point_before_trajectory_msg(time, state_current_, joints_angle_wraparound_); } + traj_time_ = time; + } else { + traj_time_ += period * scaling_factor_.load(); } - // find segment for current timestamp - joint_trajectory_controller::TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = current_trajectory_->sample(traj_time, interpolation_method_, state_desired_, - start_segment_itr, end_segment_itr); + // 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(); @@ -189,17 +166,18 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const // time_difference is // - negative until first point is reached // - counting from zero to time_from_start of next point - const double time_difference = time_data.uptime.seconds() - segment_time_from_start.seconds(); + 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_.readFromRT()) == false && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) { - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); + RCLCPP_WARN(logger, "Aborted due to command timeout"); new_trajectory_msg_.reset(); new_trajectory_msg_.initRT(set_hold_position()); @@ -211,21 +189,25 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const // Always check the state tolerance on the first sample in case the first sample // is the last point - if ((before_last_point || first_sample) && - !check_state_tolerance_per_joint(state_error_, index, default_tolerances_.state_tolerance[index], false) && - *(rt_is_holding_.readFromRT()) == false) { + // print output per default, goal will be aborted afterwards + if ((before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && + !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 && - !check_state_tolerance_per_joint(state_error_, index, default_tolerances_.goal_state_tolerance[index], - false) && - *(rt_is_holding_.readFromRT()) == false) { + if (!before_last_point && *(rt_is_holding_.readFromRT()) == false && + !check_state_tolerance_per_joint(state_error_, index, active_tol->goal_state_tolerance[index], + false /* show_errors */)) { outside_goal_tolerance = true; - if (default_tolerances_.goal_time_tolerance != 0.0) { - if (time_difference > default_tolerances_.goal_time_tolerance) { + 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 */); } } } @@ -235,37 +217,44 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const if (!tolerance_violated_while_moving && within_goal_time) { if (use_closed_loop_pid_adapter_) { // Update PIDs - for (auto i = 0ul; i < dof_; ++i) { - tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->compute_command(state_error_.positions[i], state_error_.velocities[i], period); + 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() - bool write_success = true; if (has_position_command_interface_) { - write_success &= assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); + assign_interface_from_point(joint_command_interface_[0], command_next_.positions); } if (has_velocity_command_interface_) { if (use_closed_loop_pid_adapter_) { - write_success &= assign_interface_from_point(joint_command_interface_[1], tmp_command_); + assign_interface_from_point(joint_command_interface_[1], tmp_command_); } else { - write_success &= assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); + assign_interface_from_point(joint_command_interface_[1], command_next_.velocities); } } if (has_acceleration_command_interface_) { - write_success &= assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); + assign_interface_from_point(joint_command_interface_[2], command_next_.accelerations); } if (has_effort_command_interface_) { - write_success &= assign_interface_from_point(joint_command_interface_[3], tmp_command_); - } - if (!write_success) { - RCLCPP_ERROR(get_node()->get_logger(), "Could not write to a command interfaces."); - return controller_interface::return_type::ERROR; + 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. Used in open-loop control mode - last_commanded_state_ = state_desired_; + // store the previous command and time used in open-loop control mode + last_commanded_state_ = command_next_; + last_commanded_time_ = time; } if (active_goal) { @@ -283,41 +272,47 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const 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_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); + 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 + } else if (!before_last_point) { + // check goal tolerance if (!outside_goal_tolerance) { - auto res = std::make_shared(); - res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); - active_goal->setSucceeded(res); + 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_.writeFromNonRT(false); - RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); + RCLCPP_INFO(logger, "Goal reached, success!"); new_trajectory_msg_.reset(); - new_trajectory_msg_.initRT(set_hold_position()); + 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_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", - time_difference); + RCLCPP_WARN(logger, "%s", error_string.c_str()); new_trajectory_msg_.reset(); new_trajectory_msg_.initRT(set_hold_position()); @@ -325,12 +320,12 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const } } else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) { // 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"); + 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_.readFromRT()) == false) { - RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); + RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position..."); new_trajectory_msg_.reset(); new_trajectory_msg_.initRT(set_hold_position()); @@ -341,10 +336,34 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const } } + // TODO(fmauch): Remove once merged upstream + if (state_publisher_->trylock()) { + state_publisher_->msg_.speed_scaling_factor = scaling_factor_; + state_publisher_->unlock(); + } + // 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])); + if (pids_[i]) { + // update PIDs with gains from ROS parameters + pids_[i]->set_gains(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; + } +} + } // namespace ur_controllers #include "pluginlib/class_list_macros.hpp"