diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index dc02d4c57dd881d4f2a1132d8b8797c8d7c72639..de0514f6e23f8aea568adb86499f07b59e41773d 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -98,8 +98,8 @@ namespace armarx::navigation::local_planning
         const core::Pose currentPose{scene.robot->getGlobalPose()};
 
         // prune global trajectory
-        const auto& [prunedGoal, planToDest] =
-            goal.getSubTrajectory(currentPose.translation(), params.cfg.planning_distance * 1000);
+        const auto& [prunedGoal, planToDest] = goal.getSubTrajectory(
+            currentPose.translation(), params.cfg.planning_distance * 1000); // [m] to [mm]
 
         const teb_local_planner::TimedElasticBand globalPath = conv::toRos(prunedGoal);
         teb_globalPath = globalPath.poses();
diff --git a/source/armarx/navigation/local_planning/ros_conversions.cpp b/source/armarx/navigation/local_planning/ros_conversions.cpp
index 3ad81d9c24bf20736e15b5473689a1f585b421ac..c23adf1af93c6eaa9196f8398e6c5f5f8e764ba3 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.cpp
+++ b/source/armarx/navigation/local_planning/ros_conversions.cpp
@@ -18,7 +18,7 @@ namespace armarx::navigation::conv
     Eigen::Vector2d
     toRos(const Eigen::Vector3f& vec)
     {
-        return conv::to2D(vec).cast<double>() / 1000;
+        return conv::to2D(vec).cast<double>() / 1000; // [mm] to [m]
     }