diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h
index fd7d501833..5ca4fb75a5 100644
--- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h
+++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h
@@ -34,7 +34,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <mutex>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
-#include <tesseract_motion_planners/ompl/types.h>
 #include <tesseract_collision/core/fwd.h>
 #include <tesseract_kinematics/core/fwd.h>
 #include <tesseract_environment/fwd.h>
@@ -57,8 +56,7 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator
                             ompl::base::StateValidityCheckerPtr state_validator,
                             const tesseract_environment::Environment& env,
                             std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
-                            const tesseract_collision::CollisionCheckConfig& collision_check_config,
-                            OMPLStateExtractor extractor);
+                            const tesseract_collision::CollisionCheckConfig& collision_check_config);
 
   bool checkMotion(const ompl::base::State* s1, const ompl::base::State* s2) const override;
 
@@ -91,9 +89,6 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator
   /** @brief A list of active links */
   std::vector<std::string> links_;
 
-  /** @brief This will extract an Eigen::VectorXd from the OMPL State */
-  OMPLStateExtractor extractor_;
-
   // The items below are to cache the contact manager based on thread ID. Currently ompl is multi
   // threaded but the methods used to implement collision checking are not thread safe. To prevent
   // reconstructing the collision environment for every check this will cache a contact manager
diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h
index c4e337e330..50c8af4d97 100644
--- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h
+++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h
@@ -33,8 +33,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <Eigen/Geometry>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
-#include <tesseract_motion_planners/ompl/types.h>
-
 #include <tesseract_common/fwd.h>
 #include <tesseract_kinematics/core/fwd.h>
 #include <tesseract_environment/fwd.h>
@@ -45,6 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 namespace ompl::geometric
 {
 class SimpleSetup;
+class PathGeometric;
 }
 
 namespace tesseract_planning
@@ -72,12 +71,6 @@ class OMPLPlanProfile : public Profile
    */
   virtual std::unique_ptr<OMPLSolverConfig> createSolverConfig() const = 0;
 
-  /**
-   * @brief Create the state extractor
-   * @return The OMPL state extractor
-   */
-  virtual OMPLStateExtractor createStateExtractor(const tesseract_kinematics::JointGroup& manip) const = 0;
-
   /**
    * @brief Create OMPL Simple Setup
    * @param start_instruction The start instruction
@@ -92,6 +85,13 @@ class OMPLPlanProfile : public Profile
                     const tesseract_common::ManipulatorInfo& composite_mi,
                     const std::shared_ptr<const tesseract_environment::Environment>& env) const = 0;
 
+  /**
+   * @brief Convert an OMPL defined path into a composite instruction that can be returned by the planner
+   */
+  virtual CompositeInstruction convertPath(const ompl::geometric::PathGeometric& path,
+                                           const tesseract_common::ManipulatorInfo& composite_mi,
+                                           const std::shared_ptr<const tesseract_environment::Environment>& env) const = 0;
+
 protected:
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h
index 5115e6116e..477839b528 100644
--- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h
+++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h
@@ -76,14 +76,16 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
 
   std::unique_ptr<OMPLSolverConfig> createSolverConfig() const override;
 
-  OMPLStateExtractor createStateExtractor(const tesseract_kinematics::JointGroup& manip) const override;
-
   std::unique_ptr<ompl::geometric::SimpleSetup>
   createSimpleSetup(const MoveInstructionPoly& start_instruction,
                     const MoveInstructionPoly& end_instruction,
                     const tesseract_common::ManipulatorInfo& composite_mi,
                     const std::shared_ptr<const tesseract_environment::Environment>& env) const override;
 
+  CompositeInstruction convertPath(const ompl::geometric::PathGeometric& path,
+                                   const tesseract_common::ManipulatorInfo& composite_mi,
+                                   const std::shared_ptr<const tesseract_environment::Environment>& env) const override;
+
 protected:
   static void applyGoalStates(ompl::geometric::SimpleSetup& simple_setup,
                               const tesseract_kinematics::KinGroupIKInput& ik_input,
@@ -126,8 +128,7 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
   virtual std::unique_ptr<ompl::base::StateValidityChecker>
   createStateValidator(const ompl::geometric::SimpleSetup& simple_setup,
                        const std::shared_ptr<const tesseract_environment::Environment>& env,
-                       const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
-                       const OMPLStateExtractor& state_extractor) const;
+                       const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip) const;
 
   /**
    * @brief Create collision state validator
@@ -140,8 +141,7 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
   virtual std::unique_ptr<ompl::base::StateValidityChecker>
   createCollisionStateValidator(const ompl::geometric::SimpleSetup& simple_setup,
                                 const std::shared_ptr<const tesseract_environment::Environment>& env,
-                                const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
-                                const OMPLStateExtractor& state_extractor) const;
+                                const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip) const;
 
   /**
    * @brief Create motion validator
@@ -155,7 +155,6 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
   createMotionValidator(const ompl::geometric::SimpleSetup& simple_setup,
                         const std::shared_ptr<const tesseract_environment::Environment>& env,
                         const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
-                        const OMPLStateExtractor& state_extractor,
                         const std::shared_ptr<ompl::base::StateValidityChecker>& svc_without_collision) const;
 
   /**
@@ -169,8 +168,7 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
   virtual std::unique_ptr<ompl::base::OptimizationObjective>
   createOptimizationObjective(const ompl::geometric::SimpleSetup& simple_setup,
                               const std::shared_ptr<const tesseract_environment::Environment>& env,
-                              const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
-                              const OMPLStateExtractor& state_extractor) const;
+                              const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip) const;
 
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h
index a69f6121b9..8cd474dfdb 100644
--- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h
+++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h
@@ -34,8 +34,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <mutex>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
-#include <tesseract_motion_planners/ompl/types.h>
-
 #include <tesseract_environment/fwd.h>
 #include <tesseract_kinematics/core/fwd.h>
 #include <tesseract_collision/core/fwd.h>
@@ -55,8 +53,7 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker
   StateCollisionValidator(const ompl::base::SpaceInformationPtr& space_info,
                           const tesseract_environment::Environment& env,
                           std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
-                          const tesseract_collision::CollisionCheckConfig& collision_check_config,
-                          OMPLStateExtractor extractor);
+                          const tesseract_collision::CollisionCheckConfig& collision_check_config);
 
   bool isValid(const ompl::base::State* state) const override;
 
@@ -70,9 +67,6 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker
   /** @brief A list of active links */
   std::vector<std::string> links_;
 
-  /** @brief This will extract an Eigen::VectorXd from the OMPL State */
-  OMPLStateExtractor extractor_;
-
   // The items below are to cache the contact manager based on thread ID. Currently ompl is multi
   // threaded but the methods used to implement collision checking are not thread safe. To prevent
   // reconstructing the collision environment for every check this will cache a contact manager
diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h
deleted file mode 100644
index 857885d8a1..0000000000
--- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/**
- * @file types.h
- * @brief Tesseract OMPL types
- *
- * @author Levi Armstrong
- * @date June 22, 2020
- * @version TODO
- * @bug No known bugs
- *
- * @copyright Copyright (c) 2020, Southwest Research Institute
- *
- * @par License
- * Software License Agreement (Apache License)
- * @par
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- * http://www.apache.org/licenses/LICENSE-2.0
- * @par
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef TESSERACT_MOTION_PLANNERS_OMPL_TYPES_H
-#define TESSERACT_MOTION_PLANNERS_OMPL_TYPES_H
-
-#include <tesseract_common/macros.h>
-TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
-#include <Eigen/Core>
-TESSERACT_COMMON_IGNORE_WARNINGS_POP
-
-namespace ompl::base
-{
-class State;
-}
-
-namespace tesseract_planning
-{
-using OMPLStateExtractor = std::function<Eigen::Map<Eigen::VectorXd>(const ompl::base::State*)>;
-}
-#endif  // TESSERACT_MOTION_PLANNERS_OMPL_TYPES_H
diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h
index cf40b56186..70c92a8047 100644
--- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h
+++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h
@@ -33,11 +33,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <memory>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
-#include <tesseract_motion_planners/ompl/types.h>
-
 #include <tesseract_common/eigen_types.h>
 #include <tesseract_collision/core/fwd.h>
 #include <tesseract_kinematics/core/fwd.h>
+#include <tesseract_command_language/composite_instruction.h>
 
 namespace ompl::base
 {
@@ -55,21 +54,6 @@ class PathGeometric;
 
 namespace tesseract_planning
 {
-Eigen::Map<Eigen::VectorXd> RealVectorStateSpaceExtractor(const ompl::base::State* s1, unsigned dimension);
-
-#ifndef OMPL_LESS_1_4_0
-Eigen::Map<Eigen::VectorXd> ConstrainedStateSpaceExtractor(const ompl::base::State* s1);
-#endif
-
-/**
- * @brief Convert an ompl path to tesseract TrajArray
- * @param path OMPL Path
- * @param extractor This function understands the type of state space and converts it to an eigen vector.
- * @return Tesseract TrajArray
- */
-tesseract_common::TrajArray toTrajArray(const ompl::geometric::PathGeometric& path,
-                                        const OMPLStateExtractor& extractor);
-
 /**
  * @brief Given longest valid fraction and length it will set the correct information of the state space
  * @param state_space_ptr OMPL State Space
@@ -111,6 +95,29 @@ ompl::base::StateSamplerPtr allocWeightedRealVectorStateSampler(const ompl::base
                                                                 const Eigen::VectorXd& weights,
                                                                 const Eigen::MatrixX2d& limits);
 
+/**
+ * @brief Converts an OMPL state into a vector of doubles
+ * @param s1 OMPL state
+ * @param dimension Size of the state (e.g., number of joints)
+ * @return
+ */
+Eigen::Map<Eigen::VectorXd> fromRealVectorStateSpace(const ompl::base::State* s1, unsigned dimension);
+
+/**
+ * @brief Converts
+ * @param path
+ * @return
+ */
+tesseract_common::TrajArray fromRealVectorStateSpace(const ompl::geometric::PathGeometric& path);
+
+// long assignTrajectory(tesseract_planning::CompositeInstruction& output,
+//                       boost::uuids::uuid start_uuid,
+//                       boost::uuids::uuid end_uuid,
+//                       long start_index,
+//                       const std::vector<std::string>& joint_names,
+//                       const tesseract_common::TrajArray& traj,
+//                       const bool format_result_as_input);
+
 }  // namespace tesseract_planning
 
 #endif  // TESSERACT_MOTION_PLANNERS_OMPL_UTILS_H
diff --git a/tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp b/tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp
index 45c3451e7c..01e5b22ff9 100644
--- a/tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp
+++ b/tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp
@@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/ompl/continuous_motion_validator.h>
+#include <tesseract_motion_planners/ompl/utils.h>
 
 #include <tesseract_environment/environment.h>
 #include <tesseract_kinematics/core/joint_group.h>
@@ -44,13 +45,11 @@ ContinuousMotionValidator::ContinuousMotionValidator(
     ompl::base::StateValidityCheckerPtr state_validator,
     const tesseract_environment::Environment& env,
     std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
-    const tesseract_collision::CollisionCheckConfig& collision_check_config,
-    OMPLStateExtractor extractor)
+    const tesseract_collision::CollisionCheckConfig& collision_check_config)
   : MotionValidator(space_info)
   , state_validator_(std::move(state_validator))
   , manip_(std::move(manip))
   , continuous_contact_manager_(env.getContinuousContactManager())
-  , extractor_(std::move(extractor))
 {
   links_ = manip_->getActiveLinkNames();
 
@@ -139,8 +138,8 @@ bool ContinuousMotionValidator::continuousCollisionCheck(const ompl::base::State
   }
   mutex_.unlock();
 
-  Eigen::Map<Eigen::VectorXd> start_joints = extractor_(s1);
-  Eigen::Map<Eigen::VectorXd> finish_joints = extractor_(s2);
+  Eigen::Map<Eigen::VectorXd> start_joints = fromRealVectorStateSpace(s1, si_->getStateDimension());
+  Eigen::Map<Eigen::VectorXd> finish_joints = fromRealVectorStateSpace(s2, si_->getStateDimension());
 
   tesseract_common::TransformMap state0 = manip_->calcFwdKin(start_joints);
   tesseract_common::TransformMap state1 = manip_->calcFwdKin(finish_joints);
diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp
index db6dc4c5c5..7f090d2bb3 100644
--- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp
+++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp
@@ -38,7 +38,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_motion_planners/ompl/ompl_motion_planner.h>
 #include <tesseract_motion_planners/ompl/ompl_planner_configurator.h>
 #include <tesseract_motion_planners/ompl/ompl_solver_config.h>
-#include <tesseract_motion_planners/ompl/types.h>
 #include <tesseract_motion_planners/ompl/utils.h>
 #include <tesseract_motion_planners/ompl/profile/ompl_profile.h>
 #include <tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h>
@@ -60,43 +59,6 @@ using CachedSimpleSetupsPtr = std::shared_ptr<CachedSimpleSetups>;
 
 namespace tesseract_planning
 {
-bool checkStartState(const ompl::base::ProblemDefinitionPtr& prob_def,
-                     const Eigen::Ref<const Eigen::VectorXd>& state,
-                     const OMPLStateExtractor& extractor)
-{
-  if (!(prob_def->getStartStateCount() >= 1))
-    return false;
-
-  for (unsigned i = 0; i < prob_def->getStartStateCount(); ++i)
-    if (extractor(prob_def->getStartState(i)).isApprox(state, 1e-5))
-      return true;
-
-  return false;
-}
-
-bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def,
-                    const Eigen::Ref<const Eigen::VectorXd>& state,
-                    const OMPLStateExtractor& extractor)
-{
-  ompl::base::GoalPtr goal = prob_def->getGoal();
-  if (goal->getType() == ompl::base::GoalType::GOAL_STATE)
-    return extractor(prob_def->getGoal()->as<ompl::base::GoalState>()->getState()).isApprox(state, 1e-5);
-
-  if (goal->getType() == ompl::base::GoalType::GOAL_STATES)
-  {
-    auto* goal_states = prob_def->getGoal()->as<ompl::base::GoalStates>();
-    for (unsigned i = 0; i < goal_states->getStateCount(); ++i)
-      if (extractor(goal_states->getState(i)).isApprox(state, 1e-5))
-        return true;
-  }
-  else
-  {
-    CONSOLE_BRIDGE_logWarn("checkGoalStates: Unsupported Goal Type!");
-    return true;
-  }
-  return false;
-}
-
 /** @brief Construct a basic planner */
 OMPLMotionPlanner::OMPLMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {}
 
@@ -202,69 +164,6 @@ std::pair<bool, std::string> parallelPlan(ompl::geometric::SimpleSetup& simple_s
   return std::make_pair(true, "SUCCESS");
 }
 
-long OMPLMotionPlanner::assignTrajectory(tesseract_planning::CompositeInstruction& output,
-                                         boost::uuids::uuid start_uuid,
-                                         boost::uuids::uuid end_uuid,
-                                         long start_index,
-                                         const std::vector<std::string>& joint_names,
-                                         const tesseract_common::TrajArray& traj,
-                                         const bool format_result_as_input)
-{
-  bool found{ false };
-  Eigen::Index row{ 0 };
-  auto& ci = output.getInstructions();
-  for (auto it = ci.begin() + static_cast<long>(start_index); it != ci.end(); ++it)
-  {
-    if (it->isMoveInstruction())
-    {
-      auto& mi = it->as<MoveInstructionPoly>();
-      if (mi.getUUID() == start_uuid)
-        found = true;
-
-      if (mi.getUUID() == end_uuid)
-      {
-        std::vector<InstructionPoly> extra;
-        for (; row < traj.rows() - 1; ++row)
-        {
-          MoveInstructionPoly child = mi.createChild();
-          if (format_result_as_input)
-          {
-            JointWaypointPoly jwp = mi.createJointWaypoint();
-            jwp.setIsConstrained(false);
-            jwp.setNames(joint_names);
-            jwp.setPosition(traj.row(row));
-            child.assignJointWaypoint(jwp);
-          }
-          else
-          {
-            StateWaypointPoly swp = mi.createStateWaypoint();
-            swp.setNames(joint_names);
-            swp.setPosition(traj.row(row));
-            child.assignStateWaypoint(swp);
-          }
-
-          extra.emplace_back(child);
-        }
-
-        assignSolution(mi, joint_names, traj.row(row), format_result_as_input);
-
-        if (!extra.empty())
-          ci.insert(it, extra.begin(), extra.end());
-
-        start_index += static_cast<long>(extra.size());
-        break;
-      }
-
-      if (found)
-        assignSolution(mi, joint_names, traj.row(row++), format_result_as_input);
-    }
-
-    ++start_index;
-  }
-
-  return start_index;
-}
-
 PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
 {
   PlannerResponse response;
@@ -280,7 +179,7 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
 
   // Assume all the plan instructions have the same manipulator as the composite
   tesseract_common::ManipulatorInfo composite_mi = request.instructions.getManipulatorInfo();
-  assert(!composite_mi.empty());
+  // assert(!composite_mi.empty());
 
   // Flatten the input for planning
   auto move_instructions = request.instructions.flatten(&moveFilter);
@@ -293,14 +192,9 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
     cached_simple_setups.reserve(move_instructions.size());
 
   // Transform plan instructions into ompl problem
-  unsigned num_output_states = 1;
-  long start_index{ 0 };
-  std::size_t segment{ 1 };
-  std::reference_wrapper<const InstructionPoly> start_instruction = move_instructions.front();
   for (std::size_t i = 1; i < move_instructions.size(); ++i)
   {
-    ++num_output_states;
-
+    std::reference_wrapper<const InstructionPoly> start_instruction = move_instructions[i - 1];
     std::reference_wrapper<const InstructionPoly> end_instruction = move_instructions[i];
     const auto& end_move_instruction = end_instruction.get().as<MoveInstructionPoly>();
     const auto& waypoint = end_move_instruction.getWaypoint();
@@ -318,15 +212,13 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
 
     // Get end state kinematics data
     tesseract_common::ManipulatorInfo end_mi = composite_mi.getCombined(end_move_instruction.getManipulatorInfo());
-    tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(end_mi.manipulator);
 
     // Create problem data
     const auto& start_move_instruction = start_instruction.get().as<MoveInstructionPoly>();
     std::unique_ptr<OMPLSolverConfig> solver_config = cur_plan_profile->createSolverConfig();
-    OMPLStateExtractor extractor = cur_plan_profile->createStateExtractor(*manip);
     std::shared_ptr<ompl::geometric::SimpleSetup> simple_setup;
 
-    if (cached_simple_setups.empty() || segment > cached_simple_setups.size())
+    if (cached_simple_setups.empty() || i > cached_simple_setups.size())
     {
       simple_setup =
           cur_plan_profile->createSimpleSetup(start_move_instruction, end_move_instruction, composite_mi, request.env);
@@ -334,11 +226,11 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
     }
     else
     {
-      simple_setup = cached_simple_setups.at(segment - 1);
+      simple_setup = cached_simple_setups.at(i - 1);
     }
 
     // Parallel Plan problem
-    auto status = parallelPlan(*simple_setup, *solver_config, num_output_states);
+    auto status = parallelPlan(*simple_setup, *solver_config, 2);
     if (!status.first)
     {
       response.successful = false;
@@ -348,33 +240,10 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
     }
 
     // Extract Solution
-    const std::vector<std::string> joint_names = manip->getJointNames();
-    const Eigen::MatrixX2d joint_limits = manip->getLimits().joint_limits;
-    tesseract_common::TrajArray traj = toTrajArray(simple_setup->getSolutionPath(), extractor);
-    assert(checkStartState(simple_setup->getProblemDefinition(), traj.row(0), extractor));
-    assert(checkGoalState(simple_setup->getProblemDefinition(), traj.bottomRows(1).transpose(), extractor));
-    assert(traj.rows() >= num_output_states);
-
-    // Enforce limits
-    for (Eigen::Index i = 0; i < traj.rows(); i++)
-    {
-      assert(tesseract_common::satisfiesLimits<double>(traj.row(i), joint_limits, 1e-4));
-      tesseract_common::enforceLimits<double>(traj.row(i), joint_limits);
-    }
+    CompositeInstruction output = cur_plan_profile->convertPath(simple_setup->getSolutionPath(), composite_mi, request.env);
 
-    // Assign trajectory to results
-    start_index = assignTrajectory(response.results,
-                                   start_move_instruction.getUUID(),
-                                   end_move_instruction.getUUID(),
-                                   start_index,
-                                   joint_names,
-                                   traj,
-                                   request.format_result_as_input);
-
-    // Reset data for next segment
-    start_instruction = end_instruction;
-    num_output_states = 1;
-    ++segment;
+    // Concatenate into results
+    std::copy(response.results.end(), output.begin(), output.end());
   }
 
   response.successful = true;
diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp
index e77b80db7b..61ae63332a 100644
--- a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp
+++ b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp
@@ -38,7 +38,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <boost/serialization/shared_ptr.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
+#include <tesseract_common/kinematic_limits.h>
+
 #include <tesseract_command_language/poly/move_instruction_poly.h>
+#include <tesseract_command_language/move_instruction.h>
+#include <tesseract_command_language/state_waypoint.h>
+#include <tesseract_command_language/poly/state_waypoint_poly.h>
 #include <tesseract_command_language/utils.h>
 
 #include <tesseract_motion_planners/core/types.h>
@@ -71,14 +76,6 @@ std::unique_ptr<OMPLSolverConfig> OMPLRealVectorPlanProfile::createSolverConfig(
   return std::make_unique<OMPLSolverConfig>(solver_config);
 }
 
-OMPLStateExtractor OMPLRealVectorPlanProfile::createStateExtractor(const tesseract_kinematics::JointGroup& manip) const
-{
-  const auto dof = static_cast<unsigned>(manip.numJoints());
-  return [dof](const ompl::base::State* state) -> Eigen::Map<Eigen::VectorXd> {
-    return tesseract_planning::RealVectorStateSpaceExtractor(state, dof);
-  };
-}
-
 std::unique_ptr<ompl::geometric::SimpleSetup>
 OMPLRealVectorPlanProfile::createSimpleSetup(const MoveInstructionPoly& start_instruction,
                                              const MoveInstructionPoly& end_instruction,
@@ -113,31 +110,28 @@ OMPLRealVectorPlanProfile::createSimpleSetup(const MoveInstructionPoly& start_in
   // Create Simple Setup from state space
   auto simple_setup = std::make_unique<ompl::geometric::SimpleSetup>(state_space_ptr);
 
-  // Create state extractor
-  OMPLStateExtractor state_extractor = createStateExtractor(*manip);
-
   // Setup state validators
   auto csvc = std::make_shared<CompoundStateValidator>();
   ompl::base::StateValidityCheckerPtr svc_without_collision =
-      createStateValidator(*simple_setup, env, manip, state_extractor);
+      createStateValidator(*simple_setup, env, manip);
   if (svc_without_collision != nullptr)
     csvc->addStateValidator(svc_without_collision);
 
-  auto svc_collision = createCollisionStateValidator(*simple_setup, env, manip, state_extractor);
+  auto svc_collision = createCollisionStateValidator(*simple_setup, env, manip);
   if (svc_collision != nullptr)
     csvc->addStateValidator(std::move(svc_collision));
 
   simple_setup->setStateValidityChecker(csvc);
 
   // Setup motion validation (i.e. collision checking)
-  auto mv = createMotionValidator(*simple_setup, env, manip, state_extractor, svc_without_collision);
+  auto mv = createMotionValidator(*simple_setup, env, manip, svc_without_collision);
   if (mv != nullptr)
     simple_setup->getSpaceInformation()->setMotionValidator(std::move(mv));
 
   // make sure the planners run until the time limit, and get the best possible solution
   if (solver_config.optimize)
   {
-    auto obj = createOptimizationObjective(*simple_setup, env, manip, state_extractor);
+    auto obj = createOptimizationObjective(*simple_setup, env, manip);
     if (obj != nullptr)
       simple_setup->getProblemDefinition()->setOptimizationObjective(std::move(obj));
   }
@@ -434,8 +428,7 @@ OMPLRealVectorPlanProfile::createStateSamplerAllocator(
 std::unique_ptr<ompl::base::StateValidityChecker> OMPLRealVectorPlanProfile::createStateValidator(
     const ompl::geometric::SimpleSetup& /*simple_setup*/,
     const std::shared_ptr<const tesseract_environment::Environment>& /*env*/,
-    const std::shared_ptr<const tesseract_kinematics::JointGroup>& /*manip*/,
-    const OMPLStateExtractor& /*state_extractor*/) const
+    const std::shared_ptr<const tesseract_kinematics::JointGroup>& /*manip*/) const
 {
   return nullptr;
 }
@@ -443,14 +436,13 @@ std::unique_ptr<ompl::base::StateValidityChecker> OMPLRealVectorPlanProfile::cre
 std::unique_ptr<ompl::base::StateValidityChecker> OMPLRealVectorPlanProfile::createCollisionStateValidator(
     const ompl::geometric::SimpleSetup& simple_setup,
     const std::shared_ptr<const tesseract_environment::Environment>& env,
-    const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
-    const OMPLStateExtractor& state_extractor) const
+    const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip) const
 {
   if (collision_check_config.type == tesseract_collision::CollisionEvaluatorType::DISCRETE ||
       collision_check_config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE)
   {
     return std::make_unique<StateCollisionValidator>(
-        simple_setup.getSpaceInformation(), *env, manip, collision_check_config, state_extractor);
+        simple_setup.getSpaceInformation(), *env, manip, collision_check_config);
   }
 
   return nullptr;
@@ -460,7 +452,6 @@ std::unique_ptr<ompl::base::MotionValidator> OMPLRealVectorPlanProfile::createMo
     const ompl::geometric::SimpleSetup& simple_setup,
     const std::shared_ptr<const tesseract_environment::Environment>& env,
     const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
-    const OMPLStateExtractor& state_extractor,
     const std::shared_ptr<ompl::base::StateValidityChecker>& svc_without_collision) const
 {
   if (collision_check_config.type != tesseract_collision::CollisionEvaluatorType::NONE)
@@ -472,8 +463,7 @@ std::unique_ptr<ompl::base::MotionValidator> OMPLRealVectorPlanProfile::createMo
                                                          svc_without_collision,
                                                          *env,
                                                          manip,
-                                                         collision_check_config,
-                                                         state_extractor);
+                                                         collision_check_config);
     }
 
     // Collision checking is preformed using the state validator which this calls.
@@ -486,12 +476,41 @@ std::unique_ptr<ompl::base::MotionValidator> OMPLRealVectorPlanProfile::createMo
 std::unique_ptr<ompl::base::OptimizationObjective> OMPLRealVectorPlanProfile::createOptimizationObjective(
     const ompl::geometric::SimpleSetup& simple_setup,
     const std::shared_ptr<const tesseract_environment::Environment>& /*env*/,
-    const std::shared_ptr<const tesseract_kinematics::JointGroup>& /*manip*/,
-    const OMPLStateExtractor& /*state_extractor*/) const
+    const std::shared_ptr<const tesseract_kinematics::JointGroup>& /*manip*/) const
 {
   return std::make_unique<ompl::base::PathLengthOptimizationObjective>(simple_setup.getSpaceInformation());
 }
 
+CompositeInstruction OMPLRealVectorPlanProfile::convertPath(const ompl::geometric::PathGeometric& path,
+                                                            const tesseract_common::ManipulatorInfo& composite_mi,
+                                                            const std::shared_ptr<const tesseract_environment::Environment>& env) const
+{
+  tesseract_common::TrajArray traj = fromRealVectorStateSpace(path);
+
+  // Get kinematics
+  tesseract_kinematics::JointGroup::Ptr manip = env->getJointGroup(composite_mi.manipulator);
+  const std::vector<std::string> joint_names = manip->getJointNames();
+  const Eigen::MatrixX2d limits = manip->getLimits().joint_limits;
+
+  CompositeInstruction output;
+  output.reserve(static_cast<std::size_t>(traj.rows()));
+
+  for (Eigen::Index i = 0; i < traj.rows(); i++)
+  {
+    // Enforce limits
+    assert(tesseract_common::satisfiesLimits<double>(traj.row(i), limits, 1e-4));
+    tesseract_common::enforceLimits<double>(traj.row(i), limits);
+
+    // Convert to move instruction
+    MoveInstruction mi(StateWaypointPoly(StateWaypoint(joint_names, traj.row(i))), MoveInstructionType::FREESPACE);
+
+    // Append to composite instruction
+    output.push_back(mi);
+  }
+
+  return output;
+}
+
 template <class Archive>
 void OMPLRealVectorPlanProfile::serialize(Archive& ar, const unsigned int /*version*/)
 {
diff --git a/tesseract_motion_planners/ompl/src/state_collision_validator.cpp b/tesseract_motion_planners/ompl/src/state_collision_validator.cpp
index 00cba11900..e56f64f6bc 100644
--- a/tesseract_motion_planners/ompl/src/state_collision_validator.cpp
+++ b/tesseract_motion_planners/ompl/src/state_collision_validator.cpp
@@ -44,12 +44,10 @@ StateCollisionValidator::StateCollisionValidator(
     const ompl::base::SpaceInformationPtr& space_info,
     const tesseract_environment::Environment& env,
     std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
-    const tesseract_collision::CollisionCheckConfig& collision_check_config,
-    OMPLStateExtractor extractor)
+    const tesseract_collision::CollisionCheckConfig& collision_check_config)
   : StateValidityChecker(space_info)
   , manip_(std::move(manip))
   , contact_manager_(env.getDiscreteContactManager())
-  , extractor_(std::move(extractor))
 {
   links_ = manip_->getActiveLinkNames();
 
@@ -75,7 +73,7 @@ bool StateCollisionValidator::isValid(const ompl::base::State* state) const
   }
   mutex_.unlock();
 
-  Eigen::Map<Eigen::VectorXd> finish_joints = extractor_(state);
+  Eigen::Map<Eigen::VectorXd> finish_joints = fromRealVectorStateSpace(state, si_->getStateDimension());
   tesseract_common::TransformMap state1 = manip_->calcFwdKin(finish_joints);
 
   for (const auto& link_name : links_)
diff --git a/tesseract_motion_planners/ompl/src/utils.cpp b/tesseract_motion_planners/ompl/src/utils.cpp
index 36cbde39cf..a24e3cf596 100644
--- a/tesseract_motion_planners/ompl/src/utils.cpp
+++ b/tesseract_motion_planners/ompl/src/utils.cpp
@@ -40,6 +40,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_motion_planners/ompl/utils.h>
 #include <tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h>
 
+#include <tesseract_common/types.h>
+
 #include <tesseract_collision/core/types.h>
 #include <tesseract_collision/core/discrete_contact_manager.h>
 
@@ -47,34 +49,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_planning
 {
-Eigen::Map<Eigen::VectorXd> RealVectorStateSpaceExtractor(const ompl::base::State* s1, unsigned dimension)
-{
-  assert(dynamic_cast<const ompl::base::RealVectorStateSpace::StateType*>(s1) != nullptr);
-  const auto* s = s1->template as<ompl::base::RealVectorStateSpace::StateType>();
-  return Eigen::Map<Eigen::VectorXd>{ s->values, dimension };
-}
-
-#ifndef OMPL_LESS_1_4_0
-Eigen::Map<Eigen::VectorXd> ConstrainedStateSpaceExtractor(const ompl::base::State* s1)
-{
-  assert(dynamic_cast<const ompl::base::ProjectedStateSpace::StateType*>(s1) != nullptr);
-  const Eigen::Map<Eigen::VectorXd>& s = *(s1->template as<ompl::base::ProjectedStateSpace::StateType>());
-  return s;
-}
-#endif
-
-tesseract_common::TrajArray toTrajArray(const ompl::geometric::PathGeometric& path, const OMPLStateExtractor& extractor)
-{
-  const auto n_points = static_cast<long>(path.getStateCount());
-  const auto dof = static_cast<long>(path.getSpaceInformation()->getStateDimension());
-
-  tesseract_common::TrajArray result(n_points, dof);
-  for (long i = 0; i < n_points; ++i)
-    result.row(i) = extractor(path.getState(static_cast<unsigned>(i)));
-
-  return result;
-}
-
 void processLongestValidSegment(const ompl::base::StateSpacePtr& state_space_ptr,
                                 double longest_valid_segment_fraction,
                                 double longest_valid_segment_length)
@@ -129,4 +103,86 @@ ompl::base::StateSamplerPtr allocWeightedRealVectorStateSampler(const ompl::base
   return std::make_shared<WeightedRealVectorStateSampler>(space, weights, limits);
 }
 
+Eigen::Map<Eigen::VectorXd> fromRealVectorStateSpace(const ompl::base::State* s1, unsigned dimension)
+{
+  assert(dynamic_cast<const ompl::base::RealVectorStateSpace::StateType*>(s1) != nullptr);
+  const auto* s = s1->template as<ompl::base::RealVectorStateSpace::StateType>();
+  return Eigen::Map<Eigen::VectorXd>{ s->values, dimension };
+}
+
+tesseract_common::TrajArray fromRealVectorStateSpace(const ompl::geometric::PathGeometric& path)
+{
+  const auto n_points = static_cast<long>(path.getStateCount());
+  const auto dof = static_cast<unsigned>(path.getSpaceInformation()->getStateDimension());
+
+  tesseract_common::TrajArray result(n_points, dof);
+  for (long i = 0; i < n_points; ++i)
+    result.row(i) = fromRealVectorStateSpace(path.getState(static_cast<unsigned>(i)), dof);
+
+  return result;
+}
+
+// long assignTrajectory(tesseract_planning::CompositeInstruction& output,
+//                       boost::uuids::uuid start_uuid,
+//                       boost::uuids::uuid end_uuid,
+//                       long start_index,
+//                       const std::vector<std::string>& joint_names,
+//                       const tesseract_common::TrajArray& traj,
+//                       const bool format_result_as_input)
+// {
+//   bool found{ false };
+//   Eigen::Index row{ 0 };
+//   auto& ci = output.getInstructions();
+//   for (auto it = ci.begin() + static_cast<long>(start_index); it != ci.end(); ++it)
+//   {
+//     if (it->isMoveInstruction())
+//     {
+//       auto& mi = it->as<MoveInstructionPoly>();
+//       if (mi.getUUID() == start_uuid)
+//         found = true;
+
+//       if (mi.getUUID() == end_uuid)
+//       {
+//         std::vector<InstructionPoly> extra;
+//         for (; row < traj.rows() - 1; ++row)
+//         {
+//           MoveInstructionPoly child = mi.createChild();
+//           if (format_result_as_input)
+//           {
+//             JointWaypointPoly jwp = mi.createJointWaypoint();
+//             jwp.setIsConstrained(false);
+//             jwp.setNames(joint_names);
+//             jwp.setPosition(traj.row(row));
+//             child.assignJointWaypoint(jwp);
+//           }
+//           else
+//           {
+//             StateWaypointPoly swp = mi.createStateWaypoint();
+//             swp.setNames(joint_names);
+//             swp.setPosition(traj.row(row));
+//             child.assignStateWaypoint(swp);
+//           }
+
+//           extra.emplace_back(child);
+//         }
+
+//         assignSolution(mi, joint_names, traj.row(row), format_result_as_input);
+
+//         if (!extra.empty())
+//           ci.insert(it, extra.begin(), extra.end());
+
+//         start_index += static_cast<long>(extra.size());
+//         break;
+//       }
+
+//       if (found)
+//         assignSolution(mi, joint_names, traj.row(row++), format_result_as_input);
+//     }
+
+//     ++start_index;
+//   }
+
+//   return start_index;
+// }
+
 }  // namespace tesseract_planning