diff --git a/src/lite6_controller/src/lite6_controller.cpp b/src/lite6_controller/src/lite6_controller.cpp
index 766b651..1c0f313 100644
--- a/src/lite6_controller/src/lite6_controller.cpp
+++ b/src/lite6_controller/src/lite6_controller.cpp
@@ -53,6 +53,7 @@ public:
 
     std::vector<geometry_msgs::msg::Pose> waypoints;
 
+    waypoints.push_back(move_group.getCurrentPose().pose);
     for (auto p : goal->motion.path)
       waypoints.push_back(p.pose);