diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp
index 91c3d3702bd2f9be5e6141eb77be41824f5dc515..6cc4cfbb88d70644d67ca62340ab33920555b7ca 100644
--- a/src/TimedPlanner.cpp
+++ b/src/TimedPlanner.cpp
@@ -114,7 +114,7 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
 
   loadWaypoints();
 
-  while (currentTrajectory.loop) {
+  do {
     for (auto &waypoint : currentTrajectory.waypoints) {
 
       moveit::planning_interface::MoveGroupInterface::Plan plan;
@@ -147,7 +147,9 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
             "Unable to compute " << waypoint.mode << " path to the next waypoint." << std::endl << waypoint);
       }
     }
-  }
+  } while (currentTrajectory.loop);
+
+  currentTrajectory.waypoints.clear(); // the trajectory is completed and can be cleared
 }
 
 void TimedPlanner::newTrajectoryCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg) {