[JTC] Enable feed-forward effort trajectories

joint_trajectory_controller/doc/trajectory.rst

@@ -49,6 +49,8 @@ The spline interpolator uses the following interpolation strategies depending on
 
 Trajectories with velocity fields only, velocity and acceleration only, or acceleration fields only can be processed and are accepted, if ``allow_integration_in_goal_trajectories`` is true. Position (and velocity) is then integrated from velocity (or acceleration, respectively) by Heun's method.
 
+Effort trajectories are allowed for controllers that claim the ``effort`` command interface and they are treated as feed-forward effort that is added to the position feedback. Effort is handled separately from position, velocity and acceleration. We use linear interpolation for effort when the ``spline`` interpolation method is selected.
+
 Visualized Examples
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 To visualize the difference of the different interpolation methods and their inputs, different trajectories defined at a 0.5s grid and are sampled at a rate of 10ms.

joint_trajectory_controller/doc/userdoc.rst

@@ -24,12 +24,15 @@ Currently, joints with hardware interface types ``position``, ``velocity``, ``ac
 * ``position``, ``velocity``, ``acceleration``
 * ``velocity``
 * ``effort``
+* ``position``, ``effort``
 
 This means that the joints can have one or more command interfaces, where the following control laws are applied at the same time:
 
 * For command interfaces ``position``, the desired positions are simply forwarded to the joints,
 * For command interfaces ``acceleration``, desired accelerations are simply forwarded to the joints.
 * For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop if it is configured (:ref:`parameters`).
+* For ``effort`` command interface (without ``position`` command interface), if the trajectory contains effort, this will be added to the PID commands as a feed forward effort.
+* For ``position, effort`` command interface, if the trajectory contains effort, this will be passed directly to the ``effort`` interface (PID won't be used) while the positions will be passed to the ``position`` interface.
 
 This leads to the following allowed combinations of command and state interfaces:
 
@@ -38,7 +41,7 @@ This leads to the following allowed combinations of command and state interfaces
 
   * if command interface ``velocity`` is the only one, state interfaces must include  ``position, velocity`` .
 
-* With command interface ``effort``, state interfaces must include  ``position, velocity``.
+* With command interface ``effort`` or ``position, effort``, state interfaces must include  ``position, velocity``.
 
 * With command interface ``acceleration``, state interfaces must include  ``position, velocity``.
 

joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp

@@ -32,7 +32,8 @@ tl::expected<void, std::string> command_interface_type_combinations(
   // Check if command interfaces combination is valid. Valid combinations are:
   // 1. effort
   // 2. velocity
-  // 2. position [velocity, [acceleration]]
+  // 3. position [velocity, [acceleration]]
+  // 4. position, effort
 
   if (
     rsl::contains<std::vector<std::string>>(interface_types, "velocity") &&
@@ -56,9 +57,12 @@ tl::expected<void, std::string> command_interface_type_combinations(
 
   if (
     rsl::contains<std::vector<std::string>>(interface_types, "effort") &&
-    interface_types.size() > 1)
+    !(interface_types.size() == 1 ||
+      (interface_types.size() == 2 &&
+       rsl::contains<std::vector<std::string>>(interface_types, "position"))))
   {
-    return tl::make_unexpected("'effort' command interface has to be used alone");
+    return tl::make_unexpected(
+      "'effort' command interface has to be used alone or with a 'position' interface");
   }
 
   return {};

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -258,12 +258,26 @@ controller_interface::return_type JointTrajectoryController::update(
           // Update PIDs
           for (auto i = 0ul; i < dof_; ++i)
           {
+            // If effort interface only, add desired effort as feed forward
+            // If velocity interface, ignore desired effort
             tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
+                              (has_effort_command_interface_ ? state_desired_.effort[i] : 0.0) +
                               pids_[i]->computeCommand(
                                 state_error_.positions[i], state_error_.velocities[i],
                                 (uint64_t)period.nanoseconds());
           }
         }
+        else
+        {
+          if (has_position_command_interface_ && has_effort_command_interface_)
+          {
+            for (auto i = 0ul; i < dof_; ++i)
+            {
+              // If position and effort command interfaces, only pass desired effort
+              tmp_command_[i] = state_desired_.effort[i];
+            }
+          }
+        }
 
         // set values for next hardware write()
         if (has_position_command_interface_)
@@ -424,6 +438,11 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory
     state.velocities.clear();
     state.accelerations.clear();
   }
+  // No state interface for now, use command interface
+  if (has_effort_command_interface_)
+  {
+    assign_point_from_interface(state.effort, joint_command_interface_[3]);
+  }
 }
 
 bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state)
@@ -492,6 +511,20 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto
     state.accelerations.clear();
   }
 
+  // Effort state always comes from last command
+  if (has_effort_command_interface_)
+  {
+    if (interface_has_values(joint_command_interface_[3]))
+    {
+      assign_point_from_interface(state.effort, joint_command_interface_[3]);
+    }
+    else
+    {
+      state.effort.clear();
+      has_values = false;
+    }
+  }
+
   return has_values;
 }
 
@@ -686,13 +719,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
   use_closed_loop_pid_adapter_ =
     (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 &&
      !params_.open_loop_control) ||
-    has_effort_command_interface_;
+    (has_effort_command_interface_ && params_.command_interfaces.size() == 1);
+
+  tmp_command_.resize(dof_, 0.0);
 
   if (use_closed_loop_pid_adapter_)
   {
     pids_.resize(dof_);
     ff_velocity_scale_.resize(dof_);
-    tmp_command_.resize(dof_, 0.0);
 
     update_pids();
   }
@@ -747,15 +781,17 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     return CallbackReturn::FAILURE;
   }
 
-  // effort is always used alone so no need for size check
+  // effort only or effort and position command interfaces require position and velocity state
   if (
     has_effort_command_interface_ &&
+    (params_.command_interfaces.size() == 1 ||
+     (params_.command_interfaces.size() == 2 && has_position_command_interface_)) &&
     (!has_velocity_state_interface_ || !has_position_state_interface_))
   {
     RCLCPP_ERROR(
       logger,
-      "'effort' command interface can only be used alone if 'velocity' and "
-      "'position' state interfaces are present");
+      "'effort' command interface can only be used alone or with 'position' command interface "
+      "if 'velocity' and 'position' state interfaces are present");
     return CallbackReturn::FAILURE;
   }
 
@@ -1214,6 +1250,10 @@ void JointTrajectoryController::compute_error_for_joint(
   {
     error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
   }
+  if (has_effort_command_interface_)
+  {
+    error.effort[index] = desired.effort[index];
+  }
 }
 
 void JointTrajectoryController::fill_partial_goal(
@@ -1463,10 +1503,12 @@ bool JointTrajectoryController::validate_trajectory_msg(
       return false;
     }
     // reject effort entries
-    if (!points[i].effort.empty())
+    if (!has_effort_command_interface_ && !points[i].effort.empty())
     {
       RCLCPP_ERROR(
-        get_node()->get_logger(), "Trajectories with effort fields are currently not supported.");
+        get_node()->get_logger(),
+        "Trajectories with effort fields are only supported for "
+        "controllers using the 'effort' command interface.");
       return false;
     }
   }
@@ -1537,6 +1579,7 @@ void JointTrajectoryController::resize_joint_trajectory_point(
   {
     point.accelerations.resize(size, 0.0);
   }
+  point.effort.resize(size, 0.0);
 }
 
 void JointTrajectoryController::resize_joint_trajectory_point_command(
@@ -1605,6 +1648,10 @@ void JointTrajectoryController::init_hold_position_msg()
     // add velocity, so that trajectory sampling returns acceleration points in any case
     hold_position_msg_ptr_->points[0].accelerations.resize(dof_, 0.0);
   }
+  if (has_effort_command_interface_)
+  {
+    hold_position_msg_ptr_->points[0].effort.resize(dof_, 0.0);
+  }
 }
 
 }  // namespace joint_trajectory_controller

joint_trajectory_controller/src/trajectory.cpp

@@ -192,6 +192,10 @@ bool Trajectory::sample(
   {
     output_state.accelerations.resize(output_state.positions.size(), 0.0);
   }
+  if (output_state.effort.empty())
+  {
+    output_state.effort.resize(output_state.positions.size(), 0.0);
+  }
   return true;
 }
 
@@ -207,6 +211,7 @@ void Trajectory::interpolate_between_points(
   output.positions.resize(dim, 0.0);
   output.velocities.resize(dim, 0.0);
   output.accelerations.resize(dim, 0.0);
+  output.effort.resize(dim, 0.0);
 
   auto generate_powers = [](int n, double x, double * powers)
   {
@@ -219,6 +224,7 @@ void Trajectory::interpolate_between_points(
 
   bool has_velocity = !state_a.velocities.empty() && !state_b.velocities.empty();
   bool has_accel = !state_a.accelerations.empty() && !state_b.accelerations.empty();
+  bool has_effort = !state_a.effort.empty() && !state_b.effort.empty();
   if (duration_so_far.seconds() < 0.0)
   {
     duration_so_far = rclcpp::Duration::from_seconds(0.0);
@@ -233,6 +239,25 @@ void Trajectory::interpolate_between_points(
   double t[6];
   generate_powers(5, duration_so_far.seconds(), t);
 
+  if (has_effort)
+  {
+    // do linear interpolation
+    for (size_t i = 0; i < dim; ++i)
+    {
+      double start_effort = state_a.effort[i];
+      double end_effort = state_b.effort[i];
+
+      double coefficients[2] = {0.0, 0.0};
+      coefficients[0] = start_effort;
+      if (duration_btwn_points.seconds() != 0.0)
+      {
+        coefficients[1] = (end_effort - start_effort) / duration_btwn_points.seconds();
+      }
+
+      output.effort[i] = t[0] * coefficients[0] + t[1] * coefficients[1];
+    }
+  }
+
   if (!has_velocity && !has_accel)
   {
     // do linear interpolation
@@ -331,6 +356,14 @@ void Trajectory::deduce_from_derivatives(
   trajectory_msgs::msg::JointTrajectoryPoint & first_state,
   trajectory_msgs::msg::JointTrajectoryPoint & second_state, const size_t dim, const double delta_t)
 {
+  if (first_state.effort.empty())
+  {
+    first_state.effort.assign(dim, 0.0);
+  }
+  if (second_state.effort.empty())
+  {
+    second_state.effort.assign(dim, 0.0);
+  }
   if (second_state.positions.empty())
   {
     second_state.positions.resize(dim);