diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index dafb05fead..0473f119c8 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -11,9 +11,9 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) -find_package(angles REQUIRED) find_package(controller_interface REQUIRED) find_package(control_msgs REQUIRED) +find_package(control_toolbox REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -28,10 +28,10 @@ add_library(joint_trajectory_controller SHARED ) target_include_directories(joint_trajectory_controller PRIVATE include) ament_target_dependencies(joint_trajectory_controller - angles builtin_interfaces controller_interface control_msgs + control_toolbox hardware_interface pluginlib rclcpp 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 03b030038c..cf9524e97e 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -24,6 +24,8 @@ #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_msgs/action/follow_joint_trajectory.hpp" +#include "control_toolbox/pid.hpp" + #include "hardware_interface/joint_command_handle.hpp" #include "hardware_interface/joint_state_handle.hpp" #include "hardware_interface/operation_mode_handle.hpp" @@ -95,11 +97,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + rclcpp::Duration get_action_monitor_period() {return action_monitor_period_;} + private: std::vector joint_names_; std::vector write_op_names_; std::vector registered_joint_cmd_handles_; + std::vector registered_joint_eff_cmd_handles_; std::vector registered_joint_state_handles_; std::vector registered_operation_mode_handles_; @@ -108,6 +113,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Subscription::SharedPtr joint_command_subscriber_ = nullptr; + TrajectoryPointConstIter prev_traj_point_ptr_; std::shared_ptr * traj_point_active_ptr_ = nullptr; std::shared_ptr traj_external_point_ptr_ = nullptr; std::shared_ptr traj_home_point_ptr_ = nullptr; @@ -158,6 +164,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); + + typedef std::shared_ptr PidPtr; + std::vector pids_; + std::vector velocity_ff_; }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 73a316c090..7fafbaae90 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -85,6 +85,13 @@ class Trajectory TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void interpolate_between_points( + const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a, + const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b, + const rclcpp::Time & sample_time, + trajectory_msgs::msg::JointTrajectoryPoint & output); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC TrajectoryPointConstIter begin() const; @@ -121,6 +128,34 @@ class Trajectory bool sampled_already_ = false; }; +/** + * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \t2 indices. + * If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated mapping vector is + * "{2, 1}". + */ +template +inline std::vector mapping(const T& t1, const T& t2) +{ + typedef unsigned int SizeType; + + // t1 must be a subset of t2 + if (t1.size() > t2.size()) {return std::vector();} + + std::vector mapping_vector(t1.size()); // Return value + for (typename T::const_iterator t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) + { + typename T::const_iterator t2_it = std::find(t2.begin(), t2.end(), *t1_it); + if (t2.end() == t2_it) {return std::vector();} + else + { + const SizeType t1_dist = std::distance(t1.begin(), t1_it); + const SizeType t2_dist = std::distance(t2.begin(), t2_it); + mapping_vector[t1_dist] = t2_dist; + } + } + return mapping_vector; +} + } // namespace joint_trajectory_controller #endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index a5f5fa53fc..ecc6e34849 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -25,6 +25,7 @@ controller_interface control_msgs + control_toolbox hardware_interface rclcpp rclcpp_lifecycle diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6bbe464ea2..d5859ff3ba 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -14,6 +14,8 @@ #include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include + #include #include #include @@ -21,13 +23,12 @@ #include #include -#include "angles/angles.h" - #include "builtin_interfaces/msg/time.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/time.hpp" + #include "rclcpp_lifecycle/state.hpp" #include "rcutils/logging_macros.h" @@ -97,7 +98,7 @@ JointTrajectoryController::update() point.velocities.resize(size); point.accelerations.resize(size); }; - auto compute_error_for_joint = [](JointTrajectoryPoint & error, int index, + auto compute_error_for_joint = [this](JointTrajectoryPoint & error, int index, const JointTrajectoryPoint & current, const JointTrajectoryPoint & desired) { // error defined as the difference between current and desired @@ -115,6 +116,8 @@ JointTrajectoryController::update() JointTrajectoryPoint state_current, state_desired, state_error; auto joint_num = registered_joint_state_handles_.size(); resize_joint_trajectory_point(state_current, joint_num); + resize_joint_trajectory_point(state_error, joint_num); + resize_joint_trajectory_point(state_desired, joint_num); // current state update for (auto index = 0ul; index < joint_num; ++index) { @@ -153,14 +156,14 @@ JointTrajectoryController::update() if (before_last_point && !check_state_tolerance_per_joint( state_error, index, - default_tolerances_.state_tolerance[index], false)) + default_tolerances_.state_tolerance[index], true)) { abort = 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)) + default_tolerances_.goal_state_tolerance[index], true)) { outside_goal_tolerance = true; break; @@ -209,8 +212,8 @@ JointTrajectoryController::update() rt_active_goal_.reset(); RCLCPP_WARN( lifecycle_node_->get_logger(), - "Aborted due goal_time_tolerance exceeding by %f seconds", - difference); + "Aborted due goal_time_tolerance exceeding by %f seconds (given: %f)", + difference, default_tolerances_.goal_time_tolerance); } } } @@ -219,6 +222,28 @@ JointTrajectoryController::update() set_op_mode(hardware_interface::OperationMode::ACTIVE); } + else + { + state_desired = traj_msg_home_ptr_->points[0]; + + for (auto index = 0ul; index < joint_num; ++index) { + compute_error_for_joint(state_error, index, state_current, state_desired); + } + } + + double period = lifecycle_node_->get_parameter("period").as_double(); + //convert to an effort command buffer via ClosedLoopHardwareInterfaceAdapter::updateCommand + for (auto index = 0ul; index < joint_num; ++index) { + + double f = 0.0; + if (index < state_error.positions.size() && index < state_error.velocities.size() && index < state_desired.velocities.size()) + { + f = pids_[index]->computeCommand(state_error.positions[index], state_error.velocities[index], rclcpp::Duration::from_seconds(period)); + + const double command = (state_desired.velocities[index] * velocity_ff_[index]) + f; + registered_joint_cmd_handles_[index]->set_cmd(command); + } + } publish_state(state_desired, state_current, state_error); return CONTROLLER_INTERFACE_RET_SUCCESS; @@ -275,6 +300,16 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State & previous return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } } + /*registered_joint_eff_cmd_handles_.resize(joint_names_.size()); + for (size_t index = 0; index < joint_names_.size(); ++index) { + auto ret = robot_hardware->get_joint_command_handle( + joint_names_[index].c_str(), ®istered_joint_eff_cmd_handles_[index]); + if (ret != hardware_interface::HW_RET_OK) { + RCLCPP_WARN( + logger, "unable to obtain joint command handle for %s", joint_names_[index].c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + }*/ } else { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -297,10 +332,14 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State & previous traj_msg_home_ptr_->points[0].time_from_start.sec = 0; traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; traj_msg_home_ptr_->points[0].positions.resize(registered_joint_state_handles_.size()); + traj_msg_home_ptr_->points[0].velocities.resize(registered_joint_state_handles_.size()); for (size_t index = 0; index < registered_joint_state_handles_.size(); ++index) { traj_msg_home_ptr_->points[0].positions[index] = registered_joint_state_handles_[index]->get_position(); + traj_msg_home_ptr_->points[0].velocities[index] = + registered_joint_state_handles_[index]->get_velocity(); } + traj_external_point_ptr_ = std::make_shared(); traj_home_point_ptr_ = std::make_shared(); @@ -394,6 +433,34 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State & previous std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1) ); + for (size_t index = 0; index < registered_joint_state_handles_.size(); ++index) { + traj_msg_home_ptr_->points[0].positions[index] = + registered_joint_state_handles_[index]->get_position(); + traj_msg_home_ptr_->points[0].velocities[index] = + registered_joint_state_handles_[index]->get_velocity(); + } + + pids_.resize(joint_names_.size()); + velocity_ff_.resize(joint_names_.size()); + for (uint j=0; jdeclare_parameter("gains." + joint_names_[j] + ".p"); + lifecycle_node_->declare_parameter("gains." + joint_names_[j] + ".d"); + lifecycle_node_->declare_parameter("gains." + joint_names_[j] + ".i"); + lifecycle_node_->declare_parameter("gains." + joint_names_[j] + ".i_clamp");*/ + + double p = lifecycle_node_->get_parameter("gains." + joint_names_[j] + ".p").get_value(); + double d = lifecycle_node_->get_parameter("gains." + joint_names_[j] + ".d").get_value(); + double i = lifecycle_node_->get_parameter("gains." + joint_names_[j] + ".i").get_value(); + double iclamp = lifecycle_node_->get_parameter("gains." + joint_names_[j] + ".i_clamp").get_value(); + + pids_[j].reset(new control_toolbox::Pid()); + pids_[j]->initPid(p, i, d, std::abs(iclamp), -std::abs(iclamp), true); + pids_[j]->reset(); + + velocity_ff_[j] = 0.0; + } + set_op_mode(hardware_interface::OperationMode::INACTIVE); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; @@ -552,6 +619,15 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( "Joints on incoming goal don't match the controller joints."); return rclcpp_action::GoalResponse::REJECT; } + + std::vector mapping_vector = mapping(goal->trajectory.joint_names, joint_names_); + if (mapping_vector.empty()) + { + RCLCPP_ERROR( + lifecycle_node_->get_logger(), + "Joints on incoming goal don't match the controller joints."); + return rclcpp_action::GoalResponse::REJECT; + } } RCLCPP_INFO(lifecycle_node_->get_logger(), "Accepted new action goal"); @@ -590,9 +666,42 @@ void JointTrajectoryController::feedback_setup_callback( // Update new trajectory { std::lock_guard guard(trajectory_mtx_); - preempt_active_goal(); + //preempt_active_goal(); + rt_active_goal_.reset(); auto traj_msg = std::make_shared( goal_handle->get_goal()->trajectory); + + // rearrange all points in the trajectory message based on mapping + std::vector mapping_vector = mapping(traj_msg->joint_names, joint_names_); + auto remap = [](const std::vector& to_remap, const std::vector& mapping) + -> std::vector + { + if (to_remap.size() != mapping.size()) + return to_remap; + std::vector output; + output.resize(mapping.size(), 0.0); + for (auto index = 0ul; indexpoints.size(); ++index) + { + traj_msg->points[index].positions = + remap(traj_msg->points[index].positions, mapping_vector); + + traj_msg->points[index].velocities = + remap(traj_msg->points[index].velocities, mapping_vector); + + traj_msg->points[index].accelerations = + remap(traj_msg->points[index].accelerations, mapping_vector); + + traj_msg->points[index].effort = + remap(traj_msg->points[index].effort, mapping_vector); + } + traj_external_point_ptr_->update(traj_msg); RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 880bb7e298..e6f320cb0d 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -14,6 +14,7 @@ #include "joint_trajectory_controller/trajectory.hpp" +#include #include #include "hardware_interface/macros.hpp" @@ -86,42 +87,6 @@ Trajectory::sample( sampled_already_ = true; } - auto linear_interpolation = [&]( - const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a, - const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b, - const rclcpp::Time & sample_time, - trajectory_msgs::msg::JointTrajectoryPoint & output) - { - rclcpp::Duration duration_so_far = sample_time - time_a; - rclcpp::Duration duration_btwn_points = time_b - time_a; - double percent = duration_so_far.seconds() / duration_btwn_points.seconds(); - percent = percent > 1.0 ? 1.0 : percent; - percent = percent < 0.0 ? 0.0 : percent; - - output.positions.resize(state_a.positions.size()); - for (auto i = 0ul; i < state_a.positions.size(); ++i) { - output.positions[i] = - state_a.positions[i] + percent * (state_b.positions[i] - state_a.positions[i]); - } - - if (!state_a.velocities.empty() && !state_b.velocities.empty()) { - output.velocities.resize(state_b.velocities.size()); - for (auto i = 0ul; i < state_b.velocities.size(); ++i) { - output.velocities[i] = - state_a.velocities[i] + percent * (state_b.velocities[i] - state_a.velocities[i]); - } - } - - if (!state_a.accelerations.empty() && !state_b.accelerations.empty()) { - output.accelerations.resize(state_b.accelerations.size()); - for (auto i = 0ul; i < state_b.accelerations.size(); ++i) { - output.accelerations[i] = - state_a.accelerations[i] + percent * - (state_b.accelerations[i] - state_a.accelerations[i]); - } - } - }; - // current time hasn't reached traj time of the first msg yet const auto & first_point_in_msg = trajectory_msg_->points[0]; rclcpp::Duration offset = first_point_in_msg.time_from_start; @@ -129,7 +94,7 @@ Trajectory::sample( if (sample_time < first_point_timestamp) { rclcpp::Time t0 = time_before_traj_msg_; - linear_interpolation( + interpolate_between_points( t0, state_before_traj_msg_, first_point_timestamp, first_point_in_msg, sample_time, expected_state); start_segment_itr = begin(); // no segments before the first @@ -153,7 +118,7 @@ Trajectory::sample( // Likely a lambda + parameters supplied from the controller would do // do simple linear interpolation for now // reference: https://github.com/ros-controls/ros_controllers/blob/melodic-devel/joint_trajectory_controller/include/trajectory_interface/quintic_spline_segment.h#L84 - linear_interpolation(t0, point, t1, next_point, sample_time, expected_state); + interpolate_between_points(t0, point, t1, next_point, sample_time, expected_state); start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); return true; @@ -167,6 +132,119 @@ Trajectory::sample( return true; } +void Trajectory::interpolate_between_points( + const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a, + const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b, + const rclcpp::Time & sample_time, + trajectory_msgs::msg::JointTrajectoryPoint & output) +{ + rclcpp::Duration duration_so_far = sample_time - time_a; + rclcpp::Duration duration_btwn_points = time_b - time_a; + + auto generate_powers = [](int n, double x, double* powers) + { + powers[0] = 1.0; + for (int i=1; i<=n; ++i) + powers[i] = powers[i-1]*x; + }; + + bool has_velocity = !state_a.velocities.empty() && !state_b.velocities.empty(); + bool has_accel = !state_a.accelerations.empty() && !state_b.accelerations.empty(); + + double t[6]; + generate_powers(5, duration_so_far.seconds(), t); + + if (!has_velocity && !has_accel) + { + double percent = duration_so_far.seconds() / duration_btwn_points.seconds(); + percent = percent > 1.0 ? 1.0 : percent; + percent = percent < 0.0 ? 0.0 : percent; + + // do linear interpolation + output.positions.resize(state_b.positions.size()); + for (auto i = 0ul; i < state_b.positions.size(); ++i) { + output.positions[i] = + state_a.positions[i] + percent * (state_b.positions[i] - state_a.positions[i]); + } + } + else if (has_velocity && !has_accel) + { + // do cubic interpolation + output.positions.resize(state_b.positions.size()); + output.velocities.resize(state_b.velocities.size()); + + double T[4]; + generate_powers(3, duration_btwn_points.seconds(), T); + + for (auto i = 0ul; i < state_b.positions.size(); ++i) { + double start_pos = state_a.positions[i]; + double start_vel = state_a.velocities[i]; + double end_pos = state_b.positions[i]; + double end_vel = state_b.velocities[i]; + + double coefficients[4]; + coefficients[0] = start_pos; + coefficients[1] = start_vel; + coefficients[2] = (-3.0 * start_pos + 3.0 * end_pos - 2.0 * start_vel * T[1] - end_vel * T[1]) / T[2]; + coefficients[3] = (2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] - end_vel * T[1]) / T[3]; + + output.positions[i] = t[0] * coefficients[0] + + t[1] * coefficients[1] + + t[2] * coefficients[2] + + t[3] * coefficients[3]; + output.velocities[i] = t[0] * coefficients[1] + + t[1] * 2.0 * coefficients[2] + + t[2] * 3.0 * coefficients[3]; + } + } + else if (has_velocity && has_accel) + { + // do quintic interpolation + output.positions.resize(state_b.positions.size()); + output.velocities.resize(state_b.velocities.size()); + output.accelerations.resize(state_b.accelerations.size()); + + double T[6]; + generate_powers(5, duration_btwn_points.seconds(), T); + + for (auto i = 0ul; i < state_b.positions.size(); ++i) { + double start_pos = state_a.positions[i]; + double start_vel = state_a.velocities[i]; + double start_acc = state_a.accelerations[i]; + double end_pos = state_b.positions[i]; + double end_vel = state_b.velocities[i]; + double end_acc = state_b.accelerations[i]; + + double coefficients[6]; + coefficients[0] = start_pos; + coefficients[1] = start_vel; + coefficients[2] = 0.5 * start_acc; + coefficients[3] = (-20.0 * start_pos + 20.0 * end_pos - 3.0 * start_acc * T[2] + end_acc * T[2] - + 12.0 * start_vel * T[1] - 8.0 * end_vel * T[1]) / (2.0 * T[3]); + coefficients[4] = (30.0 * start_pos - 30.0 * end_pos + 3.0 * start_acc * T[2] - 2.0 * end_acc*T[2] + + 16.0 * start_vel * T[1] + 14.0 * end_vel *T[1]) / (2.0 * T[4]); + coefficients[5] = (-12.0 * start_pos + 12.0 * end_pos - start_acc * T[2] + end_acc * T[2] - + 6.0 * start_vel * T[1] - 6.0 * end_vel * T[1]) / (2.0 * T[5]); + + output.positions[i] = t[0] * coefficients[0] + + t[1] * coefficients[1] + + t[2] * coefficients[2] + + t[3] * coefficients[3] + + t[4] * coefficients[4] + + t[5] * coefficients[5]; + output.velocities[i] = t[0] * coefficients[1] + + t[1] * 2.0 * coefficients[2] + + t[2] * 3.0 * coefficients[3] + + t[3] * 4.0 * coefficients[4] + + t[4] * 5.0 * coefficients[5]; + output.accelerations[i] = t[0] * 2.0 *coefficients[2] + + t[1] * 6.0 *coefficients[3] + + t[2] * 12.0 * coefficients[4] + + t[3] * 20.0 * coefficients[5]; + } + } +} + TrajectoryPointConstIter Trajectory::begin() const {