Fix totg bug giving invalid accelerations (moveit#1729)

v4hn · Mar 31, 2020 · 14fe2c5 · 14fe2c5
1 parent aace7ce
commit 14fe2c5
Show file tree

Hide file tree

Showing 2 changed files with 83 additions and 0 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
@@ -540,6 +540,12 @@ bool Trajectory::integrateForward(std::list<TrajectoryStep>& trajectory, double
 
     if (next_discontinuity != switching_points.end() && path_pos > next_discontinuity->first)
     {
+      // Avoid having a TrajectoryStep with path_pos near a switching point which will cause an almost identical
+      // TrajectoryStep get added in the next run (https://github.com/ros-planning/moveit/issues/1665)
+      if (path_pos - next_discontinuity->first < EPS)
+      {
+        continue;
+      }
       path_vel = old_path_vel +
                  (next_discontinuity->first - old_path_pos) * (path_vel - old_path_vel) / (path_pos - old_path_pos);
       path_pos = next_discontinuity->first;

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
@@ -150,6 +150,83 @@ TEST(time_optimal_trajectory_generation, test3)
   EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]);
 }
 
+// Test that totg algorithm doesn't give large acceleration
+TEST(time_optimal_trajectory_generation, testLargeAccel)
+{
+  double path_tolerance = 0.1;
+  double resample_dt = 0.1;
+  Eigen::VectorXd waypoint(6);
+  std::list<Eigen::VectorXd> waypoints;
+  Eigen::VectorXd max_velocities(6);
+  Eigen::VectorXd max_accelerations(6);
+
+  // Waypoints
+  // clang-format off
+  waypoint << 1.6113056281076339,
+             -0.21400163389235427,
+             -1.974502599739185,
+              9.9653618690354051e-12,
+             -1.3810916877429624,
+              1.5293902838041467;
+  waypoints.push_back(waypoint);
+
+  waypoint << 1.6088016187976597,
+             -0.21792862470933924,
+             -1.9758628799742952,
+              0.00010424017303217738,
+             -1.3835690515335755,
+              1.5279972853269816;
+  waypoints.push_back(waypoint);
+
+  waypoint << 1.5887695443178671,
+             -0.24934455124521923,
+             -1.9867451218551782,
+              0.00093816147756670078,
+             -1.4033879618584812,
+              1.5168532975096607;
+  waypoints.push_back(waypoint);
+
+  waypoint << 1.1647412393815282,
+             -0.91434018564402375,
+             -2.2170946337498498,
+              0.018590164397622583,
+             -1.8229041212673529,
+              1.2809632867583278;
+  waypoints.push_back(waypoint);
+
+  // Max velocities
+  max_velocities << 0.89535390627300004,
+                    0.89535390627300004,
+                    0.79587013890930003,
+                    0.92022484811399996,
+                    0.82074108075029995,
+                    1.3927727430915;
+  // Max accelerations
+  max_accelerations << 0.82673490883799994,
+                       0.78539816339699997,
+                       0.60883578557700002,
+                       3.2074759432319997,
+                       1.4398966328939999,
+                       4.7292792634680003;
+  // clang-format on
+
+  Trajectory parameterized(Path(waypoints, path_tolerance), max_velocities, max_accelerations, 0.001);
+
+  ASSERT_TRUE(parameterized.isValid());
+
+  size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt);
+  for (size_t sample = 0; sample <= sample_count; ++sample)
+  {
+    // always sample the end of the trajectory as well
+    double t = std::min(parameterized.getDuration(), sample * resample_dt);
+    Eigen::VectorXd acceleration = parameterized.getAcceleration(t);
+
+    ASSERT_EQ(acceleration.size(), 6);
+    for (std::size_t i = 0; i < 6; ++i)
+      EXPECT_NEAR(acceleration(i), 0.0, 100.0) << "Invalid acceleration at position " << sample_count << "\n";
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);