[JointTrajectoryController] Add velocity-only command option for JTC with closed loop controller (#239)

* Add velocity pid support.
* Remove incorrect init test for only velocity command interface.
* Add clarification comments for pid aux variables. Adapt update loop.
* Change dt for pid to appropriate measure.
* Improve partial commands for velocity-only mode.
* Extend tests to use velocity-only mode.
* Increase timeout for velocity-only mode parametrized tests.
* add is_same_sign for better refactor
* refactor boolean logic
* set velocity to 0.0 on deactivate

Co-authored-by: Bence Magyar <[email protected]>

Author: livanov93

joint_trajectory_controller/CMakeLists.txt

@@ -14,6 +14,7 @@ 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)
@@ -31,6 +32,7 @@ ament_target_dependencies(joint_trajectory_controller
   builtin_interfaces
   controller_interface
   control_msgs
+  control_toolbox
   hardware_interface
   pluginlib
   rclcpp
@@ -65,7 +67,7 @@ if(BUILD_TESTING)
   ament_add_gtest(test_trajectory_controller
     test/test_trajectory_controller.cpp
     ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml)
-  set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 180)
+  set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220)
   target_include_directories(test_trajectory_controller PRIVATE include)
   target_link_libraries(test_trajectory_controller
     joint_trajectory_controller

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -24,6 +24,7 @@
 
 #include "control_msgs/action/follow_joint_trajectory.hpp"
 #include "control_msgs/msg/joint_trajectory_controller_state.hpp"
+#include "control_toolbox/pid.hpp"
 #include "controller_interface/controller_interface.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "joint_trajectory_controller/tolerances.hpp"
@@ -134,6 +135,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   InterfaceReferences<hardware_interface::LoanedCommandInterface> joint_command_interface_;
   InterfaceReferences<hardware_interface::LoanedStateInterface> joint_state_interface_;
 
+  bool has_position_state_interface_ = false;
   bool has_velocity_state_interface_ = false;
   bool has_acceleration_state_interface_ = false;
   bool has_position_command_interface_ = false;
@@ -145,6 +147,12 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   // There should be PID-approach used as in ROS1:
   // https://github.com/ros-controls/ros_controllers/blob/noetic-devel/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h#L283
   bool use_closed_loop_pid_adapter = false;
+  using PidPtr = std::shared_ptr<control_toolbox::Pid>;
+  std::vector<PidPtr> pids_;
+  /// Feed-forward velocity weight factor when calculating closed loop pid adapter's command
+  std::vector<double> ff_velocity_scale_;
+  /// reserved storage for result of the command when closed loop pid adapter is used
+  std::vector<double> tmp_command_;
 
   // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported
   bool subscriber_is_active_ = false;

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -107,7 +107,7 @@ JointTrajectoryController::state_interface_configuration() const
 }
 
 controller_interface::return_type JointTrajectoryController::update(
-  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
 {
   if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
@@ -197,7 +197,23 @@ controller_interface::return_type JointTrajectoryController::update(
       }
       if (has_velocity_command_interface_)
       {
-        assign_interface_from_point(joint_command_interface_[1], state_desired.velocities);
+        if (!use_closed_loop_pid_adapter)
+        {
+          assign_interface_from_point(joint_command_interface_[1], state_desired.velocities);
+        }
+        else
+        {
+          // Update PIDs
+          for (auto i = 0ul; i < joint_num; ++i)
+          {
+            tmp_command_[i] = (state_desired.velocities[i] * ff_velocity_scale_[i]) +
+                              pids_[i]->computeCommand(
+                                state_desired.positions[i] - state_current.positions[i],
+                                state_desired.velocities[i] - state_current.velocities[i],
+                                (uint64_t)period.nanoseconds());
+          }
+          assign_interface_from_point(joint_command_interface_[1], tmp_command_);
+        }
       }
       if (has_acceleration_command_interface_)
       {
@@ -483,29 +499,19 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S
     }
   }
 
-  if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION))
-  {
-    has_position_command_interface_ = true;
-  }
-  if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY))
-  {
-    has_velocity_command_interface_ = true;
-  }
-  if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION))
-  {
-    has_acceleration_command_interface_ = true;
-  }
+  has_position_command_interface_ =
+    contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION);
+  has_velocity_command_interface_ =
+    contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY);
+  has_acceleration_command_interface_ =
+    contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION);
 
   if (has_velocity_command_interface_)
   {
     // if there is only velocity then use also PID adapter
     if (command_interface_types_.size() == 1)
     {
       use_closed_loop_pid_adapter = true;
-      // TODO(anyone): remove this when implemented
-      RCLCPP_ERROR(logger, "using 'velocity' command interface alone is not yet implemented yet.");
-      return CallbackReturn::FAILURE;
-      // if velocity interface can be used without position if multiple defined
     }
     else if (!has_position_command_interface_)
     {
@@ -526,6 +532,28 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S
     return CallbackReturn::FAILURE;
   }
 
+  // TODO(livanov93): change when other option is implemented
+  if (has_velocity_command_interface_ && use_closed_loop_pid_adapter)
+  {
+    size_t num_joints = joint_names_.size();
+    pids_.resize(num_joints);
+    ff_velocity_scale_.resize(num_joints);
+    tmp_command_.resize(num_joints, 0.0);
+
+    // Init PID gains from ROS parameter server
+    for (size_t i = 0; i < pids_.size(); ++i)
+    {
+      const std::string prefix = "gains." + joint_names_[i];
+      const auto k_p = auto_declare<double>(prefix + ".p", 0.0);
+      const auto k_i = auto_declare<double>(prefix + ".i", 0.0);
+      const auto k_d = auto_declare<double>(prefix + ".d", 0.0);
+      const auto i_clamp = auto_declare<double>(prefix + ".i_clamp", 0.0);
+      ff_velocity_scale_[i] = auto_declare<double>("ff_velocity_scale/" + joint_names_[i], 0.0);
+      // Initialize PID
+      pids_[i] = std::make_shared<control_toolbox::Pid>(k_p, k_i, k_d, i_clamp, -i_clamp);
+    }
+  }
+
   // Read always state interfaces from the parameter because they can be used
   // independently from the controller's type.
   // Specialized, child controllers should set its default value.
@@ -558,14 +586,12 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S
     }
   }
 
-  if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY))
-  {
-    has_velocity_state_interface_ = true;
-  }
-  if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION))
-  {
-    has_acceleration_state_interface_ = true;
-  }
+  has_position_state_interface_ =
+    contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION);
+  has_velocity_state_interface_ =
+    contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY);
+  has_acceleration_state_interface_ =
+    contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION);
 
   if (has_velocity_state_interface_)
   {
@@ -774,8 +800,16 @@ CallbackReturn JointTrajectoryController::on_deactivate(const rclcpp_lifecycle::
   // TODO(anyone): How to halt when using effort commands?
   for (size_t index = 0; index < joint_names_.size(); ++index)
   {
-    joint_command_interface_[0][index].get().set_value(
-      joint_command_interface_[0][index].get().get_value());
+    if (has_position_command_interface_)
+    {
+      joint_command_interface_[0][index].get().set_value(
+        joint_command_interface_[0][index].get().get_value());
+    }
+
+    if (has_velocity_command_interface_)
+    {
+      joint_command_interface_[1][index].get().set_value(0.0);
+    }
   }
 
   for (size_t index = 0; index < allowed_interface_types_.size(); ++index)
@@ -820,6 +854,12 @@ bool JointTrajectoryController::reset()
   traj_home_point_ptr_.reset();
   traj_msg_home_ptr_.reset();
 
+  // reset pids
+  for (const auto & pid : pids_)
+  {
+    pid->reset();
+  }
+
   return true;
 }
 
@@ -965,7 +1005,19 @@ void JointTrajectoryController::fill_partial_goal(
       for (auto & it : trajectory_msg->points)
       {
         // Assume hold position with 0 velocity and acceleration for missing joints
-        it.positions.push_back(joint_command_interface_[0][index].get().get_value());
+        if (!it.positions.empty())
+        {
+          if (has_position_command_interface_)
+          {
+            // copy last command if cmd interface exists
+            it.positions.push_back(joint_command_interface_[0][index].get().get_value());
+          }
+          else if (has_position_state_interface_)
+          {
+            // copy current state if state interface exists
+            it.positions.push_back(joint_state_interface_[0][index].get().get_value());
+          }
+        }
         if (!it.velocities.empty())
         {
           it.velocities.push_back(0.0);