diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h
index cf1bce375..532953e70 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h
@@ -223,6 +223,11 @@ class JointTrajectoryController : public controller_interface::Controller<Hardwa
   ActionServerPtr    action_server_;
   ros::ServiceServer query_state_service_;
   StatePublisherPtr  state_publisher_;
+  /**
+   *  @brief Indices of trajectory that is currently being executed
+   *  @details These should usually (always?) be all the same
+   */
+  std::vector<Scalar> trajectory_indices_;
 
   ros::Timer         goal_handle_timer_;
   ros::Time          last_state_publish_time_;
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
index 58be9e348..d758943f5 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
@@ -254,6 +254,7 @@ bool JointTrajectoryController<SegmentImpl, HardwareInterface>::init(HardwareInt
   // Initialize members
   joints_.resize(n_joints);
   angle_wraparound_.resize(n_joints);
+  trajectory_indices_ = std::vector<Scalar>(n_joints, -1);
   for (unsigned int i = 0; i < n_joints; ++i)
   {
     // Joint handle
@@ -382,6 +383,9 @@ update(const ros::Time& time, const ros::Duration& period)
       return;
     }
 
+    // Record the index of the segment we are currently in
+    trajectory_indices_[i] = segment_it - curr_traj[i].begin();
+
     // Get state error for current joint
     state_joint_error_.position[0] = state_error_.position[i];
     state_joint_error_.velocity[0] = state_error_.velocity[i];
@@ -614,6 +618,8 @@ goalCB(GoalHandle gh)
 
   if (update_ok)
   {
+    trajectory_indices_ = std::vector<double>(joint_names_.size(), -1);
+
     // Accept new goal
     preemptActiveGoal();
     gh.setAccepted();
@@ -814,8 +820,8 @@ setActionFeedback()
   current_active_goal->preallocated_feedback_->error.positions       = state_error_.position;
   current_active_goal->preallocated_feedback_->error.velocities      = state_error_.velocity;
   current_active_goal->preallocated_feedback_->error.time_from_start = ros::Duration(state_error_.time_from_start);
+  current_active_goal->preallocated_feedback_->indices               = trajector_indices_;
   current_active_goal->setFeedback( current_active_goal->preallocated_feedback_ );
-
 }
 
 template <class SegmentImpl, class HardwareInterface>