From 8903b39ae97152cbc527f534b4da6efed2f70235 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= <denis@stoglrobotics.de>
Date: Thu, 7 Dec 2023 19:32:17 +0100
Subject: [PATCH] Revert "Cleanup rucking code and add simple joint limiter."

This reverts commit 916817daca9233468b38c5417fde0b19ae355c2f.
---
 .../src/trajectory.cpp                        | 57 +++++++------------
 1 file changed, 20 insertions(+), 37 deletions(-)

diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp
index 7615eb7c5a..c55098f3b3 100644
--- a/joint_trajectory_controller/src/trajectory.cpp
+++ b/joint_trajectory_controller/src/trajectory.cpp
@@ -97,7 +97,6 @@ bool Trajectory::sample(
 
   // TODO(anyone): this shouldn't be initialized at runtime
   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;
@@ -172,6 +171,7 @@ bool Trajectory::sample(
 
   // whole animation has played out - but still continue s interpolating and smoothing
   auto & last_point = trajectory_msg_->points[trajectory_msg_->points.size() - 1];
+  const rclcpp::Time t0 = trajectory_start_time_ + last_point.time_from_start;
 
   // FIXME(destogl): this is from backport - check if needed
   //   // whole animation has played out
@@ -219,26 +219,10 @@ bool Trajectory::sample(
 
   if (joint_limiter)
   {
-    // When running Joint Limiter we might not get to the last_point in time - so SplineIt!
-    interpolate_between_points(
-      sample_time - period, previous_state_, sample_time, last_point, sample_time, output_state);
-
-    // if limits are enforced time of the second point is in the future
-    if (joint_limiter->enforce(previous_state_, output_state, period))
-    {
-      // TODO(destogl): spline it again to avoid oscillations in output from the filter
-      // interpolate_between_points(
-      //   sample_time-period, previous_state_, sample_time + period, output_state,
-      //   sample_time, output_state);
-    }
-    previous_state_ = output_state;
-  }
-  else
-  {
-    const rclcpp::Time t0 = trajectory_start_time_ + last_point.time_from_start;
-    // do not do splines when trajectory has finished because the time is achieved
-    interpolate_between_points(t0, last_point, t0, last_point, sample_time, output_state);
+    // TODO(destogl): use here output state
+    joint_limiter->enforce(previous_state_, output_state, period);
   }
+  previous_state_ = output_state;
 
   return true;
 }
@@ -252,7 +236,6 @@ void Trajectory::interpolate_between_points(
   rclcpp::Duration duration_btwn_points = time_b - time_a;
 
   const size_t dim = state_a.positions.size();
-  // TODO(anyone): this shouldn't be resized at runtime
   output.positions.resize(dim, 0.0);
   output.velocities.resize(dim, 0.0);
   output.accelerations.resize(dim, 0.0);
@@ -306,6 +289,7 @@ void Trajectory::interpolate_between_points(
     // 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];
@@ -412,24 +396,23 @@ void Trajectory::deduce_from_derivatives(
   {
     for (size_t i = 0; i < dim; ++i)
     {
-      // reset velocities always to 0 if it is empty or NaN
-      double first_state_velocity =
-        first_state.velocities.empty() ? 0.0 : first_state.velocities[i];
-      if (std::isnan(first_state_velocity))
-      {
-        first_state.velocities[i] = 0.0;
-        first_state_velocity = 0.0;
-      }
-      double second_state_velocity =
-        second_state.velocities.empty() ? 0.0 : second_state.velocities[i];
-      if (std::isnan(second_state_velocity))
-      {
-        second_state.velocities[i] = 0.0;
-        second_state_velocity = 0.0;
-      }
-
       if (std::isnan(second_state.positions[i]))
       {
+        double first_state_velocity =
+          first_state.velocities.empty() ? 0.0 : first_state.velocities[i];
+        if (std::isnan(first_state_velocity))
+        {
+          first_state.velocities[i] = 0.0;
+          first_state_velocity = 0.0;
+        }
+        double second_state_velocity =
+          second_state.velocities.empty() ? 0.0 : second_state.velocities[i];
+        if (std::isnan(second_state_velocity))
+        {
+          second_state.velocities[i] = 0.0;
+          second_state_velocity = 0.0;
+        }
+
         second_state.positions[i] =
           first_state.positions[i] + (first_state_velocity + second_state_velocity) * 0.5 * delta_t;
       }