[Ready] Port over interpolation formulae, abort if goals tolerance violated

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

@@ -72,7 +72,7 @@ class Trajectory
   *    start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after start_segment_itr
   * - Sampling after entire trajectory:
   *    start_segment_itr = --end(), end_segment_itr = end()
-  * - Sampling empty msg:
+  * - Sampling empty msg or before the time given in set_point_before_trajectory_msg()
   *    return false
   */
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
@@ -83,6 +83,32 @@ class Trajectory
     TrajectoryPointConstIter & start_segment_itr,
     TrajectoryPointConstIter & end_segment_itr);
 
+  /**
+   * Do interpolation between 2 states given a time in between their respective timestamps
+   *
+   * The start and end states need not necessarily be specified all the way to the acceleration level:
+   * - If only \b positions are specified, linear interpolation will be used.
+   * - If \b positions and \b velocities are specified, a cubic spline will be used.
+   * - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be used.
+   *
+   * If start and end states have different specifications
+   * (eg. start is position-only, end is position-velocity), the lowest common specification will be used
+   * (position-only in the example).
+   *
+   * \param[in] time_a Time at which the segment state equals \p state_a.
+   * \param[in] state_a State at \p time_a.
+   * \param[in] time_b Time at which the segment state equals \p state_b.
+   * \param[in] state_b State at time \p time_b.
+   * \param[in] sample_time The time to sample, between time_a and time_b.
+   * \param[out] output The state at \p sample_time.
+   */
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  void interpolate_between_points(
+    const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a,
+    const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b,
+    const rclcpp::Time & sample_time,
+    trajectory_msgs::msg::JointTrajectoryPoint & output);
+
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   TrajectoryPointConstIter
   begin() const;

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -111,12 +111,7 @@ JointTrajectoryController::update()
       // error defined as the difference between current and desired
       error.positions[index] = angles::shortest_angular_distance(
         current.positions[index], desired.positions[index]);
-
-      if (!desired.velocities.empty()) {
-        error.velocities[index] = desired.velocities[index] - current.velocities[index];
-      } else {
-        error.velocities[index] = 0.0;
-      }
+      error.velocities[index] = desired.velocities[index] - current.velocities[index];
       error.accelerations[index] = 0.0;
     };
 
@@ -178,7 +173,6 @@ JointTrajectoryController::update()
             default_tolerances_.goal_state_tolerance[index], false))
         {
           outside_goal_tolerance = true;
-          break;
         }
       }
 
@@ -194,10 +188,16 @@ JointTrajectoryController::update()
         rt_active_goal_->setFeedback(feedback);
 
         // check abort
-        if (abort) {
-          RCLCPP_WARN(lifecycle_node_->get_logger(), "Aborted due to state tolerance violation");
+        if (abort || outside_goal_tolerance) {
           auto result = std::make_shared<FollowJTrajAction::Result>();
-          result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED);
+
+          if (abort) {
+            RCLCPP_WARN(lifecycle_node_->get_logger(), "Aborted due to state tolerance violation");
+            result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED);
+          } else if (outside_goal_tolerance) {
+            RCLCPP_WARN(lifecycle_node_->get_logger(), "Aborted due to goal tolerance violation");
+            result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
+          }
           rt_active_goal_->setAborted(result);
           rt_active_goal_.reset();
         }

joint_trajectory_controller/src/trajectory.cpp

@@ -43,6 +43,7 @@ Trajectory::Trajectory(
   trajectory_start_time_(static_cast<rclcpp::Time>(joint_trajectory->header.stamp))
 {
   set_point_before_trajectory_msg(current_time, current_point);
+  update(joint_trajectory);
 }
 
 void
@@ -70,6 +71,7 @@ Trajectory::sample(
   TrajectoryPointConstIter & end_segment_itr)
 {
   THROW_ON_NULLPTR(trajectory_msg_)
+  expected_state = trajectory_msgs::msg::JointTrajectoryPoint();
 
   if (trajectory_msg_->points.empty()) {
     start_segment_itr = end();
@@ -86,49 +88,19 @@ Trajectory::sample(
     sampled_already_ = true;
   }
 
-  auto linear_interpolation = [&](
-    const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a,
-    const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b,
-    const rclcpp::Time & sample_time,
-    trajectory_msgs::msg::JointTrajectoryPoint & output)
-    {
-      rclcpp::Duration duration_so_far = sample_time - time_a;
-      rclcpp::Duration duration_btwn_points = time_b - time_a;
-      double percent = duration_so_far.seconds() / duration_btwn_points.seconds();
-      percent = rcppmath::clamp(percent, 0.0, 1.0);
-
-      output.positions.resize(state_a.positions.size());
-      for (auto i = 0ul; i < state_a.positions.size(); ++i) {
-        output.positions[i] =
-          state_a.positions[i] + percent * (state_b.positions[i] - state_a.positions[i]);
-      }
-
-      if (!state_a.velocities.empty() && !state_b.velocities.empty()) {
-        output.velocities.resize(state_b.velocities.size());
-        for (auto i = 0ul; i < state_b.velocities.size(); ++i) {
-          output.velocities[i] =
-            state_a.velocities[i] + percent * (state_b.velocities[i] - state_a.velocities[i]);
-        }
-      }
-
-      if (!state_a.accelerations.empty() && !state_b.accelerations.empty()) {
-        output.accelerations.resize(state_b.accelerations.size());
-        for (auto i = 0ul; i < state_b.accelerations.size(); ++i) {
-          output.accelerations[i] =
-            state_a.accelerations[i] + percent *
-            (state_b.accelerations[i] - state_a.accelerations[i]);
-        }
-      }
-    };
+  // sampling before the current point
+  if (sample_time < time_before_traj_msg_) {
+    return false;
+  }
 
-  // current time hasn't reached traj time of the first msg yet
+  // current time hasn't reached traj time of the first point in the msg yet
   const auto & first_point_in_msg = trajectory_msg_->points[0];
   const rclcpp::Duration offset = first_point_in_msg.time_from_start;
   const rclcpp::Time first_point_timestamp = trajectory_start_time_ + offset;
   if (sample_time < first_point_timestamp) {
     const rclcpp::Time t0 = time_before_traj_msg_;
 
-    linear_interpolation(
+    interpolate_between_points(
       t0, state_before_traj_msg_, first_point_timestamp, first_point_in_msg,
       sample_time, expected_state);
     start_segment_itr = begin();  // no segments before the first
@@ -148,11 +120,7 @@ Trajectory::sample(
     const rclcpp::Time t1 = trajectory_start_time_ + t1_offset;
 
     if (sample_time >= t0 && sample_time < t1) {
-      // TODO(ddengster): Find a way to add custom interpolation implementations.
-      // Likely a lambda + parameters supplied from the controller would do
-      // do simple linear interpolation for now
-      // reference: https://github.com/ros-controls/ros_controllers/blob/melodic-devel/joint_trajectory_controller/include/trajectory_interface/quintic_spline_segment.h#L84
-      linear_interpolation(t0, point, t1, next_point, sample_time, expected_state);
+      interpolate_between_points(t0, point, t1, next_point, sample_time, expected_state);
       start_segment_itr = begin() + i;
       end_segment_itr = begin() + (i + 1);
       return true;
@@ -163,9 +131,146 @@ Trajectory::sample(
   start_segment_itr = --end();
   end_segment_itr = end();
   expected_state = (*start_segment_itr);
+  // the trajectories in msg may have empty velocities/accel, so resize them
+  if (expected_state.velocities.empty()) {
+    expected_state.velocities.resize(expected_state.positions.size(), 0.0);
+  }
+  if (expected_state.accelerations.empty()) {
+    expected_state.accelerations.resize(expected_state.positions.size(), 0.0);
+  }
   return true;
 }
 
+void Trajectory::interpolate_between_points(
+  const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a,
+  const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b,
+  const rclcpp::Time & sample_time,
+  trajectory_msgs::msg::JointTrajectoryPoint & output)
+{
+  rclcpp::Duration duration_so_far = sample_time - time_a;
+  rclcpp::Duration duration_btwn_points = time_b - time_a;
+
+  const size_t dim = state_a.positions.size();
+  output.positions.resize(dim, 0.0);
+  output.velocities.resize(dim, 0.0);
+  output.accelerations.resize(dim, 0.0);
+
+  auto generate_powers = [](int n, double x, double * powers)
+    {
+      powers[0] = 1.0;
+      for (int i = 1; i <= n; ++i) {
+        powers[i] = powers[i - 1] * x;
+      }
+    };
+
+  bool has_velocity = !state_a.velocities.empty() && !state_b.velocities.empty();
+  bool has_accel = !state_a.accelerations.empty() && !state_b.accelerations.empty();
+  if (duration_so_far.seconds() < 0.0) {
+    duration_so_far = rclcpp::Duration::from_seconds(0.0);
+    has_velocity = has_accel = false;
+  }
+  if (duration_so_far.seconds() > duration_btwn_points.seconds()) {
+    duration_so_far = duration_btwn_points;
+    has_velocity = has_accel = false;
+  }
+
+  double t[6];
+  generate_powers(5, duration_so_far.seconds(), t);
+
+  if (!has_velocity && !has_accel) {
+    // do linear interpolation
+    for (size_t i = 0; i < dim; ++i) {
+      double start_pos = state_a.positions[i];
+      double end_pos = state_b.positions[i];
+
+      double coefficients[2] = {0.0, 0.0};
+      coefficients[0] = start_pos;
+      if (duration_btwn_points.seconds() != 0.0) {
+        coefficients[1] = (end_pos - start_pos) / duration_btwn_points.seconds();
+      }
+
+      output.positions[i] = t[0] * coefficients[0] +
+        t[1] * coefficients[1];
+      output.velocities[i] = t[0] * coefficients[1];
+    }
+  } else if (has_velocity && !has_accel) {
+    // do cubic interpolation
+    double T[4];
+    generate_powers(3, duration_btwn_points.seconds(), T);
+
+    for (size_t i = 0; i < dim; ++i) {
+      double start_pos = state_a.positions[i];
+      double start_vel = state_a.velocities[i];
+      double end_pos = state_b.positions[i];
+      double end_vel = state_b.velocities[i];
+
+      double coefficients[4] = {0.0, 0.0, 0.0, 0.0};
+      coefficients[0] = start_pos;
+      coefficients[1] = start_vel;
+      if (duration_btwn_points.seconds() != 0.0) {
+        coefficients[2] =
+          (-3.0 * start_pos + 3.0 * end_pos - 2.0 * start_vel * T[1] - end_vel * T[1]) / T[2];
+        coefficients[3] =
+          ( 2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] + end_vel * T[1]) / T[3];
+      }
+
+      output.positions[i] = t[0] * coefficients[0] +
+        t[1] * coefficients[1] +
+        t[2] * coefficients[2] +
+        t[3] * coefficients[3];
+      output.velocities[i] = t[0] * coefficients[1] +
+        t[1] * 2.0 * coefficients[2] +
+        t[2] * 3.0 * coefficients[3];
+      output.accelerations[i] = t[0] * 2.0 * coefficients[2] +
+        t[1] * 6.0 * coefficients[3];
+    }
+  } else if (has_velocity && has_accel) {
+    // do quintic interpolation
+    double T[6];
+    generate_powers(5, duration_btwn_points.seconds(), T);
+
+    for (size_t i = 0; i < dim; ++i) {
+      double start_pos = state_a.positions[i];
+      double start_vel = state_a.velocities[i];
+      double start_acc = state_a.accelerations[i];
+      double end_pos = state_b.positions[i];
+      double end_vel = state_b.velocities[i];
+      double end_acc = state_b.accelerations[i];
+
+      double coefficients[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+      coefficients[0] = start_pos;
+      coefficients[1] = start_vel;
+      coefficients[2] = 0.5 * start_acc;
+      if (duration_btwn_points.seconds() != 0.0) {
+        coefficients[3] =
+          (-20.0 * start_pos + 20.0 * end_pos - 3.0 * start_acc * T[2] + end_acc * T[2] -
+          12.0 * start_vel * T[1] - 8.0 * end_vel * T[1]) / (2.0 * T[3]);
+        coefficients[4] =
+          (30.0 * start_pos - 30.0 * end_pos + 3.0 * start_acc * T[2] - 2.0 * end_acc * T[2] +
+          16.0 * start_vel * T[1] + 14.0 * end_vel * T[1]) / (2.0 * T[4]);
+        coefficients[5] = (-12.0 * start_pos + 12.0 * end_pos - start_acc * T[2] + end_acc * T[2] -
+          6.0 * start_vel * T[1] - 6.0 * end_vel * T[1]) / (2.0 * T[5]);
+      }
+
+      output.positions[i] = t[0] * coefficients[0] +
+        t[1] * coefficients[1] +
+        t[2] * coefficients[2] +
+        t[3] * coefficients[3] +
+        t[4] * coefficients[4] +
+        t[5] * coefficients[5];
+      output.velocities[i] = t[0] * coefficients[1] +
+        t[1] * 2.0 * coefficients[2] +
+        t[2] * 3.0 * coefficients[3] +
+        t[3] * 4.0 * coefficients[4] +
+        t[4] * 5.0 * coefficients[5];
+      output.accelerations[i] = t[0] * 2.0 * coefficients[2] +
+        t[1] * 6.0 * coefficients[3] +
+        t[2] * 12.0 * coefficients[4] +
+        t[3] * 20.0 * coefficients[5];
+    }
+  }
+}
+
 TrajectoryPointConstIter
 Trajectory::begin() const
 {