Timestep bug

moveit · Apr 4, 2023 · b25f9b8 · b25f9b8
1 parent d8fee48
commit b25f9b8
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 9 deletions.
diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
@@ -868,14 +868,14 @@ Eigen::VectorXd Trajectory::getAcceleration(double time) const
   const double acceleration =
       2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);
 
-  time_step = time - previous->time_;
   const double path_pos =
       previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration;
   const double path_vel = previous->path_vel_ + time_step * acceleration;
   Eigen::VectorXd path_acc =
       (path_.getTangent(path_pos) * path_vel - path_.getTangent(previous->path_pos_) * previous->path_vel_);
   if (time_step > 0.0)
     path_acc /= time_step;
+  std::cerr << "Getting acceleration: " << acceleration << " vector: " << path_acc[0] << std::endl;
   return path_acc;
 }
 

diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
@@ -380,20 +380,17 @@ TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity)
   // Start matches
   EXPECT_DOUBLE_EQ(start_position, trajectory.getPosition(0.0)[0]);
 
-  std::stringstream buffer;
-  // Redirect std::cerr to buffer
-  std::streambuf* cerr_buf = std::cerr.rdbuf(buffer.rdbuf());
+  std::cerr << "Getting accel at 0.01: " << std::endl;
+  trajectory.getAcceleration(0.01)[0];
+  std::cerr << "Getting accel at 0.02: " << std::endl;
+  trajectory.getAcceleration(0.02)[0];
 
   // Check vels and accels at all points
   for (double time = 0; time < traj_duration; time += 0.01)
   {
     // This trajectory has a single switching point
     double t_switch = 0.1603407;
-    if (time == 0)
-    {
-      EXPECT_NEAR(trajectory.getAcceleration(time)[0], 0, 1e-3) << "Time: " << time;
-    }
-    else if (time > 0 && time < t_switch)
+    if (time < t_switch)
     {
       EXPECT_NEAR(trajectory.getAcceleration(time)[0], max_accelerations[0], 1e-3) << "Time: " << time;
     }
@@ -403,6 +400,9 @@ TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity)
     }
   }
 
+  std::stringstream buffer;
+  // Redirect std::cerr to buffer
+  std::streambuf* cerr_buf = std::cerr.rdbuf(buffer.rdbuf());
   std::cerr.rdbuf(cerr_buf);
 }