From 699aa4fba0b49c0e339c731a29607963973a51a9 Mon Sep 17 00:00:00 2001
From: Nicolas Hiillos <nicolashiillos@gmail.com>
Date: Wed, 21 Dec 2022 12:15:27 +0200
Subject: [PATCH] Add start position to motion planning

---
 src/lite6_controller/src/lite6_controller.cpp | 1 +
 1 file changed, 1 insertion(+)

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);