Add new Ruckig test, fix termination condition

moveit · Feb 21, 2023 · 915adda · 915adda
1 parent a7684e7
commit 915adda
Show file tree

Hide file tree

Showing 2 changed files with 50 additions and 5 deletions.
diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
@@ -306,14 +306,28 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
       // Run Ruckig
       ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);
 
-      if ((waypoint_idx == num_waypoints - 2) && ruckig_result == ruckig::Result::Finished)
+      // The difference between Result::Working and Result::Finished is that Finished can be reached in one
+      // Ruckig timestep (constructor parameter). Both are acceptable for trajectories.
+      // (The difference is only relevant for streaming mode.)
+
+      // If successful and at the last trajectory segment
+      if ((waypoint_idx == num_waypoints - 2) &&
+          (ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished))
       {
+        trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_output.trajectory.get_duration());
         smoothing_complete = true;
         break;
       }
 
+      // If successful, on to the next waypoint
+      if (ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished)
+      {
+        trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_output.trajectory.get_duration());
+        continue;
+      }
+
       // Extend the trajectory duration if Ruckig could not reach the waypoint successfully
-      if (ruckig_result != ruckig::Result::Finished)
+      if (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished)
       {
         duration_extension_factor *= DURATION_EXTENSION_FRACTION;
         // Reset the trajectory
@@ -339,9 +353,8 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
           }
           target_state->update();
         }
-        ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
         initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input, ruckig_output);
-        // Begin the while() loop again
+        // Begin the for() loop again
         break;
       }
     }
@@ -353,7 +366,7 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
                            "Ruckig extended the trajectory duration to its maximum and still did not find a solution");
   }
 
-  if (ruckig_result != ruckig::Result::Finished)
+  if (ruckig_result != ruckig::Result::Finished && ruckig_result != ruckig::Result::Working)
   {
     ROS_ERROR_STREAM_NAMED(LOGNAME, "Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result);
     return false;

diff --git a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
@@ -103,6 +103,38 @@ TEST_F(RuckigTests, basic_trajectory_with_custom_limits)
   EXPECT_TRUE(smoother_.applySmoothing(*trajectory_, vel_limits, accel_limits, jerk_limits));
 }
 
+TEST_F(RuckigTests, trajectory_duration)
+{
+  // Compare against the OJET online trajectory generator: https://www.trajectorygenerator.com/ojet-online/
+  const double ideal_duration = 0.3662;
+
+  moveit::core::RobotState robot_state(robot_model_);
+  robot_state.setToDefaultValues();
+  // Special attention to Joint 0. It is the only joint to move in this test.
+  // Zero velocities and accelerations at the endpoints
+  robot_state.setVariablePosition("panda_joint1", 0.0);
+  robot_state.update();
+  trajectory_->addSuffixWayPoint(robot_state, 0.0);
+
+  robot_state.setVariablePosition("panda_joint1", 0.1);
+  robot_state.update();
+  trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
+
+  EXPECT_TRUE(
+      smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */));
+
+  // No waypoint durations of zero except the first
+  for (size_t waypoint_idx = 1; waypoint_idx < trajectory_->getWayPointCount() - 1; ++waypoint_idx)
+  {
+    EXPECT_NE(trajectory_->getWayPointDurationFromPrevious(waypoint_idx), 0);
+  }
+
+  // The trajectory duration should be within 10% of the analytical solution since the implementation here extends
+  // the duration by 10% increments
+  EXPECT_GT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 0.9999 * ideal_duration);
+  EXPECT_LT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 1.11 * ideal_duration);
+}
+
 TEST_F(RuckigTests, single_waypoint)
 {
   // With only one waypoint, Ruckig cannot smooth the trajectory.