[Reference Only/Draft] joint_trajectory_controller convert trajectories to effort values

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

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<std::string> joint_names_;
   std::vector<std::string> write_op_names_;
 
   std::vector<hardware_interface::JointCommandHandle *> registered_joint_cmd_handles_;
+  std::vector<hardware_interface::JointCommandHandle *> registered_joint_eff_cmd_handles_;
   std::vector<const hardware_interface::JointStateHandle *> registered_joint_state_handles_;
   std::vector<hardware_interface::OperationModeHandle *> registered_operation_mode_handles_;
 
@@ -108,6 +113,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>::SharedPtr
     joint_command_subscriber_ = nullptr;
 
+  TrajectoryPointConstIter prev_traj_point_ptr_;
   std::shared_ptr<Trajectory> * traj_point_active_ptr_ = nullptr;
   std::shared_ptr<Trajectory> traj_external_point_ptr_ = nullptr;
   std::shared_ptr<Trajectory> 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<control_toolbox::Pid> PidPtr;
+  std::vector<PidPtr> pids_;
+  std::vector<double> velocity_ff_;
 };
 
 }  // namespace joint_trajectory_controller

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 <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated mapping vector is
+ * <tt>"{2, 1}"</tt>.
+ */
+template <class T>
+inline std::vector<unsigned int> 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<SizeType>();}
+
+  std::vector<SizeType> 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<SizeType>();}
+    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_

joint_trajectory_controller/package.xml

@@ -25,6 +25,7 @@
 
   <build_export_depend>controller_interface</build_export_depend>
   <build_export_depend>control_msgs</build_export_depend>
+  <build_export_depend>control_toolbox</build_export_depend>
   <build_export_depend>hardware_interface</build_export_depend>
   <build_export_depend>rclcpp</build_export_depend>
   <build_export_depend>rclcpp_lifecycle</build_export_depend>

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -14,20 +14,21 @@
 
 #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
 
+#include <angles/angles.h>
+
 #include <cassert>
 #include <chrono>
 #include <iterator>
 #include <string>
 #include <memory>
 #include <vector>
 
-#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(), &registered_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<Trajectory>();
   traj_home_point_ptr_ = std::make_shared<Trajectory>();
@@ -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; j<pids_.size(); ++j)
+  {
+    /*lifecycle_node_->declare_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>();
+    double d = lifecycle_node_->get_parameter("gains." + joint_names_[j] + ".d").get_value<double>();
+    double i = lifecycle_node_->get_parameter("gains." + joint_names_[j] + ".i").get_value<double>();
+    double iclamp = lifecycle_node_->get_parameter("gains." + joint_names_[j] + ".i_clamp").get_value<double>();
+
+    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<unsigned int> 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<std::mutex> guard(trajectory_mtx_);
-    preempt_active_goal();
+    //preempt_active_goal();
+    rt_active_goal_.reset();
     auto traj_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(
       goal_handle->get_goal()->trajectory);
+
+    // rearrange all points in the trajectory message based on mapping
+    std::vector<unsigned int> mapping_vector = mapping(traj_msg->joint_names, joint_names_);
+    auto remap = [](const std::vector<double>& to_remap, const std::vector<unsigned int>& mapping)
+      -> std::vector<double>
+    {
+      if (to_remap.size() != mapping.size())
+        return to_remap;
+      std::vector<double> output;
+      output.resize(mapping.size(), 0.0);
+      for (auto index = 0ul; index<mapping.size(); ++index)
+      {
+        unsigned int map_index = mapping[index];
+        output[map_index] = to_remap[index];
+      }
+      return output;
+    };
+    for (auto index = 0ul; index < traj_msg->points.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<RealtimeGoalHandle>(goal_handle);