[JTC] Implement effort-only command interface (ros-controls#225)

* Fix trajectory tolerance parameters
* Implement effort command interface for JTC
* Use auto_declare for pid params
* Set effort to 0 on deactivate

Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>

Author: Ace314159

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -141,11 +141,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   bool has_position_command_interface_ = false;
   bool has_velocity_command_interface_ = false;
   bool has_acceleration_command_interface_ = false;
+  bool has_effort_command_interface_ = false;
 
   /// If true, a velocity feedforward term plus corrective PID term is used
-  // TODO(anyone): This flag is not used for now
-  // 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_;

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -189,39 +189,48 @@ controller_interface::return_type JointTrajectoryController::update(
       const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end();
 
       // set values for next hardware write()
+      if (use_closed_loop_pid_adapter)
+      {
+        // 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());
+        }
+      }
       if (has_position_command_interface_)
       {
         assign_interface_from_point(joint_command_interface_[0], state_desired.positions);
       }
       if (has_velocity_command_interface_)
       {
-        if (!use_closed_loop_pid_adapter)
+        if (use_closed_loop_pid_adapter)
         {
-          assign_interface_from_point(joint_command_interface_[1], state_desired.velocities);
+          assign_interface_from_point(joint_command_interface_[1], tmp_command_);
         }
         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_);
+          assign_interface_from_point(joint_command_interface_[1], state_desired.velocities);
         }
       }
       if (has_acceleration_command_interface_)
       {
         assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations);
       }
-      // TODO(anyone): Add here "if using_closed_loop_hw_interface_adapter" (see ROS1) - #171
-      //       if (check_if_interface_type_exist(
-      //           command_interface_types_, hardware_interface::HW_IF_EFFORT)) {
-      //         assign_interface_from_point(joint_command_interface_[3], state_desired.effort);
-      //       }
+      if (has_effort_command_interface_)
+      {
+        if (use_closed_loop_pid_adapter)
+        {
+          assign_interface_from_point(joint_command_interface_[3], tmp_command_);
+        }
+        else
+        {
+          assign_interface_from_point(joint_command_interface_[3], state_desired.effort);
+        }
+      }
 
       for (size_t index = 0; index < joint_num; ++index)
       {
@@ -477,32 +486,14 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S
   // 2. velocity
   // 2. position [velocity, [acceleration]]
 
-  // effort can't be combined with other interfaces
-  if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT))
-  {
-    if (command_interface_types_.size() == 1)
-    {
-      // TODO(anyone): This flag is not used for now
-      // 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
-      use_closed_loop_pid_adapter = true;
-      // TODO(anyone): remove the next two lines when implemented
-      RCLCPP_ERROR(logger, "using 'effort' command interface alone is not yet implemented yet.");
-      return CallbackReturn::FAILURE;
-    }
-    else
-    {
-      RCLCPP_ERROR(logger, "'effort' command interface has to be used alone.");
-      return CallbackReturn::FAILURE;
-    }
-  }
-
   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);
+  has_effort_command_interface_ =
+    contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT);
 
   if (has_velocity_command_interface_)
   {
@@ -530,8 +521,21 @@ 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)
+  // effort can't be combined with other interfaces
+  if (has_effort_command_interface_)
+  {
+    if (command_interface_types_.size() == 1)
+    {
+      use_closed_loop_pid_adapter = true;
+    }
+    else
+    {
+      RCLCPP_ERROR(logger, "'effort' command interface has to be used alone.");
+      return CallbackReturn::FAILURE;
+    }
+  }
+
+  if (use_closed_loop_pid_adapter)
   {
     size_t num_joints = joint_names_.size();
     pids_.resize(num_joints);
@@ -593,7 +597,7 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S
 
   if (has_velocity_state_interface_)
   {
-    if (!contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION))
+    if (!has_position_state_interface_)
     {
       RCLCPP_ERROR(
         logger,
@@ -602,13 +606,33 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S
       return CallbackReturn::FAILURE;
     }
   }
-  else if (has_acceleration_state_interface_)
+  else
   {
-    RCLCPP_ERROR(
-      logger,
-      "'acceleration' state interface cannot be used if 'position' and 'velocity' "
-      "interfaces are not present.");
-    return CallbackReturn::FAILURE;
+    if (has_acceleration_state_interface_)
+    {
+      RCLCPP_ERROR(
+        logger,
+        "'acceleration' state interface cannot be used if 'position' and 'velocity' "
+        "interfaces are not present.");
+      return CallbackReturn::FAILURE;
+    }
+    if (has_velocity_command_interface_ && command_interface_types_.size() == 1)
+    {
+      RCLCPP_ERROR(
+        logger,
+        "'velocity' command interface can only be used alone if 'velocity' and "
+        "'position' state interfaces are present");
+      return CallbackReturn::FAILURE;
+    }
+    // effort is always used alone so no need for size check
+    if (has_effort_command_interface_)
+    {
+      RCLCPP_ERROR(
+        logger,
+        "'effort' command interface can only be used alone if 'velocity' and "
+        "'position' state interfaces are present");
+      return CallbackReturn::FAILURE;
+    }
   }
 
   auto get_interface_list = [](const std::vector<std::string> & interface_types) {
@@ -808,6 +832,11 @@ CallbackReturn JointTrajectoryController::on_deactivate(const rclcpp_lifecycle::
     {
       joint_command_interface_[1][index].get().set_value(0.0);
     }
+
+    if (has_effort_command_interface_)
+    {
+      joint_command_interface_[3][index].get().set_value(0.0);
+    }
   }
 
   for (size_t index = 0; index < allowed_interface_types_.size(); ++index)
@@ -845,6 +874,11 @@ bool JointTrajectoryController::reset()
   subscriber_is_active_ = false;
   joint_command_subscriber_.reset();
 
+  for (const auto & pid : pids_)
+  {
+    pid->reset();
+  }
+
   // iterator has no default value
   // prev_traj_point_ptr_;
   traj_point_active_ptr_ = nullptr;

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -323,6 +323,13 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
     EXPECT_LE(0.0, joint_vel_[2]);
   }
 
+  if (traj_controller_->has_effort_command_interface())
+  {
+    EXPECT_LE(0.0, joint_eff_[0]);
+    EXPECT_LE(0.0, joint_eff_[1]);
+    EXPECT_LE(0.0, joint_eff_[2]);
+  }
+
   // cleanup
   state = traj_controller_->get_node()->cleanup();
 
@@ -469,6 +476,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order)
       traj_msg.points[0].velocities[2] = -0.1;
     }
 
+    if (traj_controller_->has_effort_command_interface())
+    {
+      traj_msg.points[0].effort.resize(3);
+      traj_msg.points[0].effort[0] = -0.1;
+      traj_msg.points[0].effort[1] = -0.1;
+      traj_msg.points[0].effort[2] = -0.1;
+    }
+
     trajectory_publisher_->publish(traj_msg);
   }
 
@@ -491,6 +506,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order)
     EXPECT_GT(0.0, joint_vel_[1]);
     EXPECT_GT(0.0, joint_vel_[2]);
   }
+
+  if (traj_controller_->has_effort_command_interface())
+  {
+    EXPECT_GT(0.0, joint_eff_[0]);
+    EXPECT_GT(0.0, joint_eff_[1]);
+    EXPECT_GT(0.0, joint_eff_[2]);
+  }
 }
 
 /**
@@ -552,6 +574,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list)
     EXPECT_NEAR(0.0, joint_vel_[2], threshold)
       << "Joint 3 velocity should be 0.0 since it's not in the goal";
   }
+
+  if (
+    std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") !=
+    command_interface_types_.end())
+  {
+    // estimate the sign of the velocity
+    // joint rotates forward
+    EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0]));
+    EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1]));
+    EXPECT_NEAR(0.0, joint_eff_[2], threshold)
+      << "Joint 3 effort should be 0.0 since it's not in the goal";
+  }
   // TODO(anyone): add here ckecks for acceleration commands
 
   executor.cancel();
@@ -1180,6 +1214,16 @@ INSTANTIATE_TEST_SUITE_P(
       std::vector<std::string>({"velocity"}),
       std::vector<std::string>({"position", "velocity", "acceleration"}))));
 
+// only effort controller
+INSTANTIATE_TEST_CASE_P(
+  OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized,
+  ::testing::Values(
+    std::make_tuple(
+      std::vector<std::string>({"effort"}), std::vector<std::string>({"position", "velocity"})),
+    std::make_tuple(
+      std::vector<std::string>({"effort"}),
+      std::vector<std::string>({"position", "velocity", "acceleration"}))));
+
 TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters)
 {
   auto set_parameter_and_check_result = [&]() {
@@ -1199,10 +1243,6 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame
   command_interface_types_ = {"bad_name"};
   set_parameter_and_check_result();
 
-  // command interfaces: effort not yet implemented
-  command_interface_types_ = {"effort"};
-  set_parameter_and_check_result();
-
   // command interfaces: effort has to be only
   command_interface_types_ = {"effort", "position"};
   set_parameter_and_check_result();
@@ -1236,4 +1276,18 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame
   // state interfaces: acceleration without position and velocity
   state_interface_types_ = {"acceleration"};
   set_parameter_and_check_result();
+
+  // velocity-only command interface: position - velocity not present
+  command_interface_types_ = {"velocity"};
+  state_interface_types_ = {"position"};
+  set_parameter_and_check_result();
+  state_interface_types_ = {"velocity"};
+  set_parameter_and_check_result();
+
+  // effort-only command interface: position - velocity not present
+  command_interface_types_ = {"effort"};
+  state_interface_types_ = {"position"};
+  set_parameter_and_check_result();
+  state_interface_types_ = {"velocity"};
+  set_parameter_and_check_result();
 }

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -36,6 +36,7 @@ const std::vector<double> INITIAL_POS_JOINTS = {
   INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3};
 const std::vector<double> INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0};
 const std::vector<double> INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0};
+const std::vector<double> INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0};
 }  // namespace
 
 namespace test_trajectory_controllers
@@ -98,6 +99,8 @@ class TestableJointTrajectoryController
 
   bool has_velocity_command_interface() { return has_velocity_command_interface_; }
 
+  bool has_effort_command_interface() { return has_effort_command_interface_; }
+
   rclcpp::WaitSet joint_cmd_sub_wait_set_;
 };
 
@@ -117,6 +120,7 @@ class TrajectoryControllerTest : public ::testing::Test
     joint_state_vel_.resize(joint_names_.size(), 0.0);
     joint_acc_.resize(joint_names_.size(), 0.0);
     joint_state_acc_.resize(joint_names_.size(), 0.0);
+    joint_eff_.resize(joint_names_.size(), 0.0);
     // Default interface values - they will be overwritten by parameterized tests
     command_interface_types_ = {"position"};
     state_interface_types_ = {"position", "velocity"};
@@ -199,6 +203,7 @@ class TrajectoryControllerTest : public ::testing::Test
     pos_cmd_interfaces_.reserve(joint_names_.size());
     vel_cmd_interfaces_.reserve(joint_names_.size());
     acc_cmd_interfaces_.reserve(joint_names_.size());
+    eff_cmd_interfaces_.reserve(joint_names_.size());
     pos_state_interfaces_.reserve(joint_names_.size());
     vel_state_interfaces_.reserve(joint_names_.size());
     acc_state_interfaces_.reserve(joint_names_.size());
@@ -210,6 +215,8 @@ class TrajectoryControllerTest : public ::testing::Test
         joint_names_[i], hardware_interface::HW_IF_VELOCITY, &joint_vel_[i]));
       acc_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface(
         joint_names_[i], hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i]));
+      eff_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface(
+        joint_names_[i], hardware_interface::HW_IF_EFFORT, &joint_eff_[i]));
 
       pos_state_interfaces_.emplace_back(hardware_interface::StateInterface(
         joint_names_[i], hardware_interface::HW_IF_POSITION,
@@ -228,6 +235,8 @@ class TrajectoryControllerTest : public ::testing::Test
       cmd_interfaces.back().set_value(INITIAL_VEL_JOINTS[i]);
       cmd_interfaces.emplace_back(acc_cmd_interfaces_.back());
       cmd_interfaces.back().set_value(INITIAL_ACC_JOINTS[i]);
+      cmd_interfaces.emplace_back(eff_cmd_interfaces_.back());
+      cmd_interfaces.back().set_value(INITIAL_EFF_JOINTS[i]);
       joint_state_pos_[i] = INITIAL_POS_JOINTS[i];
       joint_state_vel_[i] = INITIAL_VEL_JOINTS[i];
       joint_state_acc_[i] = INITIAL_ACC_JOINTS[i];
@@ -399,12 +408,14 @@ class TrajectoryControllerTest : public ::testing::Test
   std::vector<double> joint_pos_;
   std::vector<double> joint_vel_;
   std::vector<double> joint_acc_;
+  std::vector<double> joint_eff_;
   std::vector<double> joint_state_pos_;
   std::vector<double> joint_state_vel_;
   std::vector<double> joint_state_acc_;
   std::vector<hardware_interface::CommandInterface> pos_cmd_interfaces_;
   std::vector<hardware_interface::CommandInterface> vel_cmd_interfaces_;
   std::vector<hardware_interface::CommandInterface> acc_cmd_interfaces_;
+  std::vector<hardware_interface::CommandInterface> eff_cmd_interfaces_;
   std::vector<hardware_interface::StateInterface> pos_state_interfaces_;
   std::vector<hardware_interface::StateInterface> vel_state_interfaces_;
   std::vector<hardware_interface::StateInterface> acc_state_interfaces_;