[JTC] Sample at t + dT instead of the beginning of the control cycle

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -112,6 +112,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   // Preallocate variables used in the realtime update() function
   trajectory_msgs::msg::JointTrajectoryPoint state_current_;
   trajectory_msgs::msg::JointTrajectoryPoint command_current_;
+  trajectory_msgs::msg::JointTrajectoryPoint command_next_;
   trajectory_msgs::msg::JointTrajectoryPoint state_desired_;
   trajectory_msgs::msg::JointTrajectoryPoint state_error_;
 
@@ -124,6 +125,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   // Parameters from ROS for joint_trajectory_controller
   std::shared_ptr<ParamListener> param_listener_;
   Params params_;
+  rclcpp::Duration update_period_{0, 0};
 
   trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
   /// Specify interpolation method. Default to splines.

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

@@ -69,8 +69,10 @@ class Trajectory
    * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to
    * be reached at the time defined in the segment.
    *
-   * This function assumes that sampling is only done at monotonically increasing \p sample_time
-   * for any trajectory.
+   * This function by default assumes that sampling is only done at monotonically increasing \p
+   * sample_time for any trajectory. That means, it will only search for a point matching the sample
+   * time after the point it has been called before. If this isn't desired, set \p
+   * progress_search_start_index to false.
    *
    * Specific case returns for start_segment_itr and end_segment_itr:
    * - Sampling before the trajectory start:
@@ -94,13 +96,16 @@ class Trajectory
    *      description above.
    * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See
    *      description above.
+   * \param[in] progress_search_start_index If set to true, the next sample call will start
+   *      searching in the trajectory at the index of this call's result.
    */
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool sample(
     const rclcpp::Time & sample_time,
     const interpolation_methods::InterpolationMethod interpolation_method,
     trajectory_msgs::msg::JointTrajectoryPoint & output_state,
-    TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr);
+    TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr,
+    const bool progress_search_start_index = true);
 
   /**
    * Do interpolation between 2 states given a time in between their respective timestamps

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -18,6 +18,7 @@
 #include <functional>
 #include <memory>
 
+#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -180,6 +181,7 @@ controller_interface::return_type JointTrajectoryController::update(
   if (has_active_trajectory())
   {
     bool first_sample = false;
+    TrajectoryPointConstIter start_segment_itr, end_segment_itr;
     // if sampling the first time, set the point before you sample
     if (!traj_external_point_ptr_->is_sampled_already())
     {
@@ -194,13 +196,20 @@ controller_interface::return_type JointTrajectoryController::update(
         traj_external_point_ptr_->set_point_before_trajectory_msg(
           time, state_current_, joints_angle_wraparound_);
       }
+      // Sample once at the beginning to establish the start time if none was given
+      traj_external_point_ptr_->sample(
+        time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
     }
 
-    // find segment for current timestamp
-    TrajectoryPointConstIter start_segment_itr, end_segment_itr;
-    const bool valid_point = traj_external_point_ptr_->sample(
+    // Sample expected state from the trajectory
+    traj_external_point_ptr_->sample(
       time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
 
+    // Sample setpoint for next control cycle
+    const bool valid_point = traj_external_point_ptr_->sample(
+      time + update_period_, interpolation_method_, command_next_, start_segment_itr,
+      end_segment_itr, false);
+
     if (valid_point)
     {
       const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start();
@@ -276,7 +285,7 @@ controller_interface::return_type JointTrajectoryController::update(
           // Update PIDs
           for (auto i = 0ul; i < dof_; ++i)
           {
-            tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
+            tmp_command_[i] = (command_next_.velocities[i] * ff_velocity_scale_[i]) +
                               pids_[i]->computeCommand(
                                 state_error_.positions[i], state_error_.velocities[i],
                                 (uint64_t)period.nanoseconds());
@@ -286,7 +295,7 @@ controller_interface::return_type JointTrajectoryController::update(
         // set values for next hardware write()
         if (has_position_command_interface_)
         {
-          assign_interface_from_point(joint_command_interface_[0], state_desired_.positions);
+          assign_interface_from_point(joint_command_interface_[0], command_next_.positions);
         }
         if (has_velocity_command_interface_)
         {
@@ -296,20 +305,20 @@ controller_interface::return_type JointTrajectoryController::update(
           }
           else
           {
-            assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities);
+            assign_interface_from_point(joint_command_interface_[1], command_next_.velocities);
           }
         }
         if (has_acceleration_command_interface_)
         {
-          assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations);
+          assign_interface_from_point(joint_command_interface_[2], command_next_.accelerations);
         }
         if (has_effort_command_interface_)
         {
           assign_interface_from_point(joint_command_interface_[3], tmp_command_);
         }
 
         // store the previous command. Used in open-loop control mode
-        last_commanded_state_ = state_desired_;
+        last_commanded_state_ = command_next_;
       }
 
       if (active_goal)
@@ -867,6 +876,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     std::string(get_node()->get_name()) + "/query_state",
     std::bind(&JointTrajectoryController::query_state_service, this, _1, _2));
 
+  if (get_update_rate() == 0)
+  {
+    throw std::runtime_error("Controller's update rate is set to 0. This should not happen!");
+  }
+  update_period_ =
+    rclcpp::Duration(0.0, static_cast<uint32_t>(1.0e9 / static_cast<double>(get_update_rate())));
+
   return CallbackReturn::SUCCESS;
 }
 

joint_trajectory_controller/src/trajectory.cpp

@@ -91,7 +91,8 @@ bool Trajectory::sample(
   const rclcpp::Time & sample_time,
   const interpolation_methods::InterpolationMethod interpolation_method,
   trajectory_msgs::msg::JointTrajectoryPoint & output_state,
-  TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr)
+  TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr,
+  const bool progress_search_start_index)
 {
   THROW_ON_NULLPTR(trajectory_msg_)
 
@@ -176,7 +177,10 @@ bool Trajectory::sample(
       }
       start_segment_itr = begin() + i;
       end_segment_itr = begin() + (i + 1);
-      last_sample_idx_ = i;
+      if (progress_search_start_index)
+      {
+        last_sample_idx_ = i;
+      }
       return true;
     }
   }