diff --git a/tesseract_planning_server/src/tesseract_planning_server.cpp b/tesseract_planning_server/src/tesseract_planning_server.cpp
index ae408470..678403f4 100644
--- a/tesseract_planning_server/src/tesseract_planning_server.cpp
+++ b/tesseract_planning_server/src/tesseract_planning_server.cpp
@@ -242,6 +242,10 @@ void TesseractPlanningServer::onMotionPlanningCallback(
   env->applyCommands(tesseract_rosutils::fromMsg(goal->request.commands));
   env->setState(env_state.joints);
 
+  // Store the initial state in the response for publishing trajectories
+  tesseract_scene_graph::SceneState initial_state = env->getState();
+  tesseract_rosutils::toMsg(result->response.initial_state, initial_state.joints);
+
   // Create solve data storage
   auto data = std::make_unique<TaskComposerDataStorage>();
   data->setData("planning_input", std::move(planning_input));
@@ -255,10 +259,6 @@ void TesseractPlanningServer::onMotionPlanningCallback(
   if (!composite_profile_remapping.empty())
     data->setData("composite_profile_remapping", composite_profile_remapping);
 
-  // Store the initial state in the response for publishing trajectories
-  tesseract_scene_graph::SceneState initial_state = env->getState();
-  tesseract_rosutils::toMsg(result->response.initial_state, initial_state.joints);
-
   // Solve
   tesseract_common::Timer timer;
   timer.start();