diff --git a/source/armarx/navigation/core/Trajectory.cpp b/source/armarx/navigation/core/Trajectory.cpp
index de68a8269865b6503e4469b21cf880d68eb17bf8..f68d2942eacd48f8f51f577e2d20af9b6cbea340 100644
--- a/source/armarx/navigation/core/Trajectory.cpp
+++ b/source/armarx/navigation/core/Trajectory.cpp
@@ -445,11 +445,7 @@ namespace armarx::navigation::core
                 << "The resampled trajectory is only allowed to contains no points if it is "
                    "shorter than eps";
 
-            return Trajectory::FromPath(
-                pts.front().waypoint.pose,
-                {}, // no intermediate points
-                pts.back().waypoint.pose,
-                500.F); // TODO remove 0.F!, use proper interpolation! (via projection!)
+            return Trajectory({pts.front(), pts.back()});
         }
 
 
@@ -492,22 +488,33 @@ namespace armarx::navigation::core
             ranges::to_vector;
 
 
-        const auto resampledTrajectory = Trajectory::FromPath(
-            pts.front().waypoint.pose,
-            resampledPath,
-            pts.back().waypoint.pose,
-            500.F); // TODO remove 0.F!, use proper interpolation! (via projection!)
+        auto resampledTrajectory = Trajectory::FromPath(
+            pts.front().waypoint.pose, resampledPath, pts.back().waypoint.pose, 0.F);
+
+        // set velocity based on original trajectory
+        {
+            const auto setVelocityInPlace = [&](TrajectoryPoint& pt)
+            {
+                const auto projection = getProjection(pt.waypoint.pose.translation(),
+                                                      VelocityInterpolation::LinearInterpolation);
+                pt.velocity = projection.projection.velocity;
+            };
+
+            std::for_each(resampledTrajectory.mutablePoints().begin(),
+                          resampledTrajectory.mutablePoints().end(),
+                          setVelocityInPlace);
+        }
 
 
         // sanity check: make sure that resampling was successful
         // number of samples required for source trajectory
-        const float minRequiredSamples = length() / eps;
-        const size_t samples = resampledTrajectory.points().size();
+        // const float minRequiredSamples = length() / eps;
+        // const size_t samples = resampledTrajectory.points().size();
 
-        ARMARX_CHECK_GREATER_EQUAL(samples, minRequiredSamples);
+        // ARMARX_CHECK_GREATER_EQUAL(samples, minRequiredSamples);
 
         // this is a rough approximation of the upper bound
-        ARMARX_CHECK_LESS_EQUAL(samples, 2 * minRequiredSamples);
+        // ARMARX_CHECK_LESS_EQUAL(samples, 2 * minRequiredSamples);
 
         return resampledTrajectory;
     }
@@ -568,6 +575,12 @@ namespace armarx::navigation::core
         return pts;
     }
 
+    std::vector<TrajectoryPoint>&
+    Trajectory::mutablePoints()
+    {
+        return pts;
+    }
+
 
     float
     Trajectory::duration(const core::VelocityInterpolation interpolation) const
diff --git a/source/armarx/navigation/core/Trajectory.h b/source/armarx/navigation/core/Trajectory.h
index 4183d1cc59ce8e1dd4ded356e3f3b3efb6c1cb3e..230ed74620c1315c320bd22c18cf78031ed22b65 100644
--- a/source/armarx/navigation/core/Trajectory.h
+++ b/source/armarx/navigation/core/Trajectory.h
@@ -97,6 +97,8 @@ namespace armarx::navigation::core
 
         const std::vector<TrajectoryPoint>& points() const;
 
+        std::vector<TrajectoryPoint>& mutablePoints();
+
         float duration(core::VelocityInterpolation interpolation) const;
 
     private: