diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp
index 54f785a070..d9ba3f0b4c 100644
--- a/joint_trajectory_controller/src/trajectory.cpp
+++ b/joint_trajectory_controller/src/trajectory.cpp
@@ -119,80 +119,70 @@ bool Trajectory::sample(
   }
 
   output_state = trajectory_msgs::msg::JointTrajectoryPoint();
-  auto & first_point_in_msg = trajectory_msg_->points[0];
-  const rclcpp::Time first_point_timestamp =
-    trajectory_start_time_ + first_point_in_msg.time_from_start;
 
-  // current time hasn't reached traj time of the first point in the msg yet
-  if (sample_time < first_point_timestamp)
+  // Find the first trajectory point later than the current sample_time
+  const auto next_traj_point_it = std::upper_bound(
+    trajectory_msg_->points.begin(), trajectory_msg_->points.end(), sample_time,
+    [this](const rclcpp::Time & t, const auto & point)
+    {
+      // time_from_start + trajectory time is the expected arrival time of trajectory
+      return t < (trajectory_start_time_ + point.time_from_start);
+    });
+
+  if (next_traj_point_it != trajectory_msg_->points.end())
   {
-    // If interpolation is disabled, just forward the next waypoint
+    auto & next_traj_point = *next_traj_point_it;
+    const auto t1 = trajectory_start_time_ + next_traj_point.time_from_start;
+
+    // Sample by interpolating between last_- and next_traj_point:
+    // - If there is no point before sample_time in the trajectory, use before_first_point as
+    //   last_traj_point
+    // - Otherwise, use the last point before sample_time instead
+    const bool before_first_point = next_traj_point_it == trajectory_msg_->points.begin();
+    auto & last_traj_point =
+      before_first_point ? state_before_traj_msg_ : *std::prev(next_traj_point_it);
+    const auto t0 = before_first_point ? time_before_traj_msg_
+                                       : trajectory_start_time_ + last_traj_point.time_from_start;
+
     if (interpolation_method == interpolation_methods::InterpolationMethod::NONE)
     {
-      output_state = state_before_traj_msg_;
+      output_state = before_first_point ? last_traj_point : next_traj_point;
     }
+    // Do interpolation
     else
     {
       // it changes points only if position and velocity do not exist, but their derivatives
       deduce_from_derivatives(
-        state_before_traj_msg_, first_point_in_msg, state_before_traj_msg_.positions.size(),
-        (first_point_timestamp - time_before_traj_msg_).seconds());
+        last_traj_point, next_traj_point, state_before_traj_msg_.positions.size(),
+        (t1 - t0).seconds());
 
       interpolate_between_points(
-        time_before_traj_msg_, state_before_traj_msg_, first_point_timestamp, first_point_in_msg,
-        sample_time, output_state);
+        t0, last_traj_point, t1, next_traj_point, sample_time, output_state);
     }
-    start_segment_itr = begin();  // no segments before the first
-    end_segment_itr = begin();
+
+    // If sample_time is before the first trajectory point, there is no segment and
+    // start_segment_itr = end_segment_itr
+    start_segment_itr = before_first_point ? begin() : std::prev(next_traj_point_it);
+    end_segment_itr = next_traj_point_it;
     return true;
   }
-
-  // time_from_start + trajectory time is the expected arrival time of trajectory
-  const auto last_idx = trajectory_msg_->points.size() - 1;
-  for (size_t i = 0; i < last_idx; ++i)
+  // whole animation has played out
+  else
   {
-    auto & point = trajectory_msg_->points[i];
-    auto & next_point = trajectory_msg_->points[i + 1];
-
-    const rclcpp::Time t0 = trajectory_start_time_ + point.time_from_start;
-    const rclcpp::Time t1 = trajectory_start_time_ + next_point.time_from_start;
-
-    if (sample_time >= t0 && sample_time < t1)
+    start_segment_itr = --end();
+    end_segment_itr = end();
+    output_state = (*start_segment_itr);
+    // the trajectories in msg may have empty velocities/accel, so resize them
+    if (output_state.velocities.empty())
     {
-      // If interpolation is disabled, just forward the next waypoint
-      if (interpolation_method == interpolation_methods::InterpolationMethod::NONE)
-      {
-        output_state = next_point;
-      }
-      // Do interpolation
-      else
-      {
-        // it changes points only if position and velocity do not exist, but their derivatives
-        deduce_from_derivatives(
-          point, next_point, state_before_traj_msg_.positions.size(), (t1 - t0).seconds());
-
-        interpolate_between_points(t0, point, t1, next_point, sample_time, output_state);
-      }
-      start_segment_itr = begin() + i;
-      end_segment_itr = begin() + (i + 1);
-      return true;
+      output_state.velocities.resize(output_state.positions.size(), 0.0);
     }
+    if (output_state.accelerations.empty())
+    {
+      output_state.accelerations.resize(output_state.positions.size(), 0.0);
+    }
+    return true;
   }
-
-  // whole animation has played out
-  start_segment_itr = --end();
-  end_segment_itr = end();
-  output_state = (*start_segment_itr);
-  // the trajectories in msg may have empty velocities/accel, so resize them
-  if (output_state.velocities.empty())
-  {
-    output_state.velocities.resize(output_state.positions.size(), 0.0);
-  }
-  if (output_state.accelerations.empty())
-  {
-    output_state.accelerations.resize(output_state.positions.size(), 0.0);
-  }
-  return true;
 }
 
 void Trajectory::interpolate_between_points(