diff --git a/tesseract_motion_planners/ompl/CMakeLists.txt b/tesseract_motion_planners/ompl/CMakeLists.txt
index 220864a8eef..1835e305c61 100644
--- a/tesseract_motion_planners/ompl/CMakeLists.txt
+++ b/tesseract_motion_planners/ompl/CMakeLists.txt
@@ -6,15 +6,17 @@ link_directories(BEFORE ${OMPL_LIBRARY_DIRS})
 set(OMPL_SRC
     src/profile/ompl_profile.cpp
     src/profile/ompl_real_vector_plan_profile.cpp
+    src/profile/ompl_constrained_rvss_plan_profile.cpp
     src/compound_state_validator.cpp
-    src/continuous_motion_validator.cpp
     src/discrete_motion_validator.cpp
     src/ompl_motion_planner.cpp
     src/ompl_planner_configurator.cpp
     src/ompl_solver_config.cpp
-    src/state_collision_validator.cpp
     src/utils.cpp
-    src/weighted_real_vector_state_sampler.cpp)
+    src/real_vector_state_space/continuous_motion_validator.cpp
+    src/real_vector_state_space/state_collision_validator.cpp
+    src/real_vector_state_space/weighted_real_vector_state_sampler.cpp
+    src/real_vector_state_space/utils.cpp)
 
 # if(NOT OMPL_VERSION VERSION_LESS "1.4.0") list(APPEND OMPL_SRC src/config/ompl_planner_constrained_config.cpp) endif()
 
diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_constrained_rvss_plan_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_constrained_rvss_plan_profile.h
new file mode 100644
index 00000000000..ff41061c7ef
--- /dev/null
+++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_constrained_rvss_plan_profile.h
@@ -0,0 +1,70 @@
+/**
+ * @file ompl_constrained_rvss_plan_profile.h
+ * @brief Tesseract OMPL constrained real vector state space plan profile
+ *
+ * @author Michael Ripperger
+ * @date December 26, 2024
+ * @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_PROFILE_CONSTRAINED_RVSS_PLAN_PROFILE_H
+#define TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_CONSTRAINED_RVSS_PLAN_PROFILE_H
+
+#ifndef OMPL_LESS_1_4_0
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <vector>
+#include <memory>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h>
+
+namespace ompl::base
+{
+class Constraint;
+using ConstraintPtr = std::shared_ptr<Constraint>;
+}
+
+namespace tesseract_planning
+{
+class OMPLConstrainedRvssPlanProfile : public OMPLRealVectorPlanProfile
+{
+public:
+  OMPLConstrainedRvssPlanProfile();
+
+protected:
+  virtual std::vector<ompl::base::ConstraintPtr> createConstraints(const tesseract_common::ManipulatorInfo& mi,
+                                                                   const std::shared_ptr<const tesseract_environment::Environment>& env) const;
+
+  ompl::base::StateSpacePtr createStateSpace(const tesseract_common::ManipulatorInfo& mi,
+                                             const std::shared_ptr<const tesseract_environment::Environment>& env) const override;
+
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive&, const unsigned int);  // NOLINT
+};
+
+}  // namespace tesseract_planning
+
+BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLConstrainedRvssPlanProfile)
+
+#endif // OMPL_LESS_1_4_0
+
+#endif // TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_CONSTRAINED_RVSS_PLAN_PROFILE_H
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 c4e337e3304..93157054572 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>
@@ -42,9 +40,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_command_language/profile.h>
 
+namespace ompl::base
+{
+class StateSpace;
+using StateSpacePtr = std::shared_ptr<StateSpace>;
+}
+
 namespace ompl::geometric
 {
 class SimpleSetup;
+class PathGeometric;
 }
 
 namespace tesseract_planning
@@ -72,12 +77,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,7 +91,21 @@ 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:
+  /**
+   * @brief Creates an OMPL StateSpace object that con be used internally to construct the SimpleSetup object
+   * @return
+   */
+  ompl::base::StateSpacePtr virtual createStateSpace(const tesseract_common::ManipulatorInfo& composite_mi,
+                                                     const std::shared_ptr<const tesseract_environment::Environment>& env) const = 0;
+
   friend class boost::serialization::access;
   template <class Archive>
   void serialize(Archive&, const unsigned int);  // NOLINT
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 5115e6116ef..f651d5a7445 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
@@ -36,6 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/ompl/profile/ompl_profile.h>
 #include <tesseract_motion_planners/ompl/ompl_solver_config.h>
+#include <tesseract_motion_planners/ompl/real_vector_state_space/utils.h>
 
 #include <tesseract_collision/core/fwd.h>
 #include <tesseract_collision/core/types.h>
@@ -46,8 +47,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace ompl::base
 {
+class State;
 class StateSampler;
-class StateSpace;
 class StateValidityChecker;
 class MotionValidator;
 class OptimizationObjective;
@@ -76,15 +77,20 @@ 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:
+  ompl::base::StateSpacePtr createStateSpace(const tesseract_common::ManipulatorInfo& composite_mi,
+                                             const std::shared_ptr<const tesseract_environment::Environment>& env) const override;
+
   static void applyGoalStates(ompl::geometric::SimpleSetup& simple_setup,
                               const tesseract_kinematics::KinGroupIKInput& ik_input,
                               const tesseract_kinematics::KinematicGroup& manip,
@@ -126,8 +132,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 +145,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 +159,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,13 +172,16 @@ 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;
+
+  /** @brief Function used to convert OMPL states into joint states */
+  StateConverterFn state_converter_;
 
   friend class boost::serialization::access;
   template <class Archive>
   void serialize(Archive&, const unsigned int);  // NOLINT
 };
+
 }  // namespace tesseract_planning
 
 BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLRealVectorPlanProfile)
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/real_vector_state_space/continuous_motion_validator.h
similarity index 85%
rename from tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h
rename to tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/continuous_motion_validator.h
index fd7d501833d..814df604d93 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/real_vector_state_space/continuous_motion_validator.h
@@ -23,8 +23,8 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-#ifndef TESSERACT_MOTION_PLANNERS_CONTINUOUS_MOTION_VALIDATOR_H
-#define TESSERACT_MOTION_PLANNERS_CONTINUOUS_MOTION_VALIDATOR_H
+#ifndef TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_CONTINUOUS_MOTION_VALIDATOR_H
+#define TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_CONTINUOUS_MOTION_VALIDATOR_H
 
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
@@ -34,11 +34,12 @@ 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>
 
+#include <tesseract_motion_planners/ompl/real_vector_state_space/utils.h>
+
 namespace ompl::base
 {
 class SpaceInformation;
@@ -58,7 +59,7 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator
                             const tesseract_environment::Environment& env,
                             std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
                             const tesseract_collision::CollisionCheckConfig& collision_check_config,
-                            OMPLStateExtractor extractor);
+                            StateConverterFn state_converter);
 
   bool checkMotion(const ompl::base::State* s1, const ompl::base::State* s2) const override;
 
@@ -91,8 +92,8 @@ 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_;
+  /** @brief Function to convert an OMPL state (typically of type RealVectorStateSpace::StateType or ConstrainedStateSpace::StateType) into a vector of doubles representing a joint state */
+  StateConverterFn state_converter_;
 
   // 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
@@ -108,4 +109,4 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator
 };
 }  // namespace tesseract_planning
 
-#endif  // TESSERACT_MOTION_PLANNERS_CONTINUOUS_MOTION_VALIDATOR_H
+#endif  // TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_CONTINUOUS_MOTION_VALIDATOR_H
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/real_vector_state_space/state_collision_validator.h
similarity index 82%
rename from tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h
rename to tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/state_collision_validator.h
index a69f6121b9b..c3053055de8 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/real_vector_state_space/state_collision_validator.h
@@ -23,8 +23,8 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-#ifndef TESSERACT_MOTION_PLANNERS_OMPL_STATE_COLLISION_VALIDATOR_H
-#define TESSERACT_MOTION_PLANNERS_OMPL_STATE_COLLISION_VALIDATOR_H
+#ifndef TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_STATE_COLLISION_VALIDATOR_H
+#define TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_STATE_COLLISION_VALIDATOR_H
 
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
@@ -34,12 +34,12 @@ 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>
 
+#include <tesseract_motion_planners/ompl/real_vector_state_space/utils.h>
+
 namespace ompl::base
 {
 class SpaceInformation;
@@ -56,7 +56,7 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker
                           const tesseract_environment::Environment& env,
                           std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
                           const tesseract_collision::CollisionCheckConfig& collision_check_config,
-                          OMPLStateExtractor extractor);
+                          StateConverterFn state_converter);
 
   bool isValid(const ompl::base::State* state) const override;
 
@@ -70,8 +70,8 @@ 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_;
+  /** @brief Function to convert an OMPL state (typically of type RealVectorStateSpace::StateType or ConstrainedStateSpace::StateType) into a vector of doubles representing a joint state */
+  StateConverterFn state_converter_;
 
   // 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
@@ -86,4 +86,4 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker
 };
 
 }  // namespace tesseract_planning
-#endif  // TESSERACT_MOTION_PLANNERS_OMPL_STATE_COLLISION_VALIDATOR_H
+#endif  // TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_STATE_COLLISION_VALIDATOR_H
diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/utils.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/utils.h
new file mode 100644
index 00000000000..7c8abd93f2e
--- /dev/null
+++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/utils.h
@@ -0,0 +1,79 @@
+/**
+ * @file utils.h
+ * @brief Tesseract OMPL real vector state space utility functions
+ *
+ * @author Levi Armstrong
+ * @date February 17, 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_REAL_VECTOR_STATE_SPACE_UTILS_H
+#define TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_UTILS_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <Eigen/Core>
+#include <functional>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_common/eigen_types.h>
+
+namespace ompl::base
+{
+class State;
+}  // namespace ompl::base
+
+namespace ompl::geometric
+{
+class PathGeometric;
+}
+
+namespace tesseract_planning
+{
+/**
+ * @brief Typedef for a function that can convert an OMPL base state object into a vector of contiguous double values
+ */
+using StateConverterFn = std::function<Eigen::Map<Eigen::VectorXd>(const ompl::base::State*, unsigned dim)>;
+
+/**
+ * @brief Implementation of StateConverterFn that converts an OMPL state into a vector of doubles by casting through a RealVectorStateSpace::StateType OMPL state
+ * @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);
+
+#ifndef OMPL_LESS_1_4_0
+/**
+ * @brief Implementation of StateConverterFn that converts an OMPL state into a vector of doubles by casting through a ProjectedStateSpace::StateType OMPL state
+ * @param s1 OMPL state
+ * @param dimension Size of the state (e.g., number of joints)
+ * @return
+ */
+Eigen::Map<Eigen::VectorXd> fromConstrainedRealVectorStateSpace(const ompl::base::State* s1, unsigned dimension);
+#endif
+
+/**
+ * @brief Converts an OMPL path into a trajectory array by using the input state converter to convert the path waypoints into joint state vectors
+ */
+tesseract_common::TrajArray fromOMPL(const ompl::geometric::PathGeometric& path, StateConverterFn state_converter);
+
+}  // namespace tesseract_planning
+
+#endif  // TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_UTILS_H
diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/weighted_real_vector_state_sampler.h
similarity index 94%
rename from tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h
rename to tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/weighted_real_vector_state_sampler.h
index cc7999943f8..a5dc7161a19 100644
--- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h
+++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/weighted_real_vector_state_sampler.h
@@ -26,8 +26,8 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-#ifndef TESSERACT_MOTION_PLANNERS_WEIGHTED_REAL_VECTOR_STATE_SAMPLER_H
-#define TESSERACT_MOTION_PLANNERS_WEIGHTED_REAL_VECTOR_STATE_SAMPLER_H
+#ifndef TESSERACT_MOTION_PLANNERS_OMPL_WEIGHTED_REAL_VECTOR_STATE_SAMPLER_H
+#define TESSERACT_MOTION_PLANNERS_OMPL_WEIGHTED_REAL_VECTOR_STATE_SAMPLER_H
 
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
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 857885d8a10..00000000000
--- 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 cf40b56186a..9904020c43e 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,15 +33,13 @@ 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
 {
-class State;
 class StateSpace;
 using StateSpacePtr = std::shared_ptr<StateSpace>;
 class StateSampler;
@@ -55,21 +53,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
@@ -101,15 +84,13 @@ bool checkStateInCollision(tesseract_collision::ContactResultMap& contact_map,
                            const tesseract_kinematics::JointGroup& manip,
                            const Eigen::VectorXd& state);
 
-/**
- * @brief Default State sampler which uses the weights information to scale the sampled state. This is use full
- * when you state space has mixed units like meters and radian.
- * @param space The ompl state space.
- * @return OMPL state sampler shared pointer
- */
-ompl::base::StateSamplerPtr allocWeightedRealVectorStateSampler(const ompl::base::StateSpace* space,
-                                                                const Eigen::VectorXd& weights,
-                                                                const Eigen::MatrixX2d& limits);
+// 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
 
diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp
index db6dc4c5c54..7f090d2bb36 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_constrained_rvss_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_constrained_rvss_plan_profile.cpp
new file mode 100644
index 00000000000..efe9900aa80
--- /dev/null
+++ b/tesseract_motion_planners/ompl/src/profile/ompl_constrained_rvss_plan_profile.cpp
@@ -0,0 +1,79 @@
+/**
+ * @file ompl_constrained_rvss_plan_profile.cpp
+ * @brief
+ *
+ * @author Michael Ripperger
+ * @date December 26, 2024
+ * @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 OMPL_LESS_1_4_0
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <ompl/base/spaces/RealVectorStateSpace.h>
+#include <ompl/base/spaces/constraint/ProjectedStateSpace.h>
+#include <boost/serialization/base_object.hpp>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_motion_planners/ompl/profile/ompl_constrained_rvss_plan_profile.h>
+
+#include <tesseract_environment/environment.h>
+
+namespace tesseract_planning
+{
+OMPLConstrainedRvssPlanProfile::OMPLConstrainedRvssPlanProfile()
+  : OMPLRealVectorPlanProfile()
+{
+  // Update the state converter function
+  state_converter_ = &fromConstrainedRealVectorStateSpace;
+}
+
+std::vector<ompl::base::ConstraintPtr> OMPLConstrainedRvssPlanProfile::createConstraints(const tesseract_common::ManipulatorInfo& /*mi*/,
+                                                                                         const std::shared_ptr<const tesseract_environment::Environment>& /*env*/) const
+{
+  return {};
+}
+
+ompl::base::StateSpacePtr OMPLConstrainedRvssPlanProfile::createStateSpace(const tesseract_common::ManipulatorInfo& mi,
+                                                                           const std::shared_ptr<const tesseract_environment::Environment>& env) const
+{
+  // Create the ambient real vector state space
+  ompl::base::StateSpacePtr rvss = OMPLRealVectorPlanProfile::createStateSpace(mi, env);
+
+  // Create the constraints
+  auto constraints = std::make_shared<ompl::base::ConstraintIntersection>(rvss->getDimension(), createConstraints(mi, env));
+
+  // Create the projected state space
+  return std::make_shared<ompl::base::ProjectedStateSpace>(rvss, constraints);
+}
+
+template <class Archive>
+void OMPLConstrainedRvssPlanProfile::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLRealVectorPlanProfile);
+}
+
+}  // namespace tesseract_planning
+
+#include <tesseract_common/serialization.h>
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::OMPLConstrainedRvssPlanProfile)
+BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::OMPLConstrainedRvssPlanProfile)
+
+#endif  // OMPL_LESS_1_4_0
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 e77b80db7b0..bfdf9009a62 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,18 +38,25 @@ 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>
+
 #include <tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h>
 #include <tesseract_motion_planners/ompl/utils.h>
-
 #include <tesseract_motion_planners/ompl/ompl_planner_configurator.h>
-#include <tesseract_motion_planners/ompl/continuous_motion_validator.h>
 #include <tesseract_motion_planners/ompl/discrete_motion_validator.h>
-#include <tesseract_motion_planners/ompl/state_collision_validator.h>
 #include <tesseract_motion_planners/ompl/compound_state_validator.h>
+#include <tesseract_motion_planners/ompl/real_vector_state_space/continuous_motion_validator.h>
+#include <tesseract_motion_planners/ompl/real_vector_state_space/state_collision_validator.h>
+#include <tesseract_motion_planners/ompl/real_vector_state_space/weighted_real_vector_state_sampler.h>
+#include <tesseract_motion_planners/ompl/real_vector_state_space/utils.h>
 
 #include <tesseract_kinematics/core/utils.h>
 #include <tesseract_kinematics/core/joint_group.h>
@@ -61,6 +68,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 namespace tesseract_planning
 {
 OMPLRealVectorPlanProfile::OMPLRealVectorPlanProfile()
+    : state_converter_(&fromRealVectorStateSpace)
 {
   solver_config.planners = { std::make_shared<const RRTConnectConfigurator>(),
                              std::make_shared<const RRTConnectConfigurator>() };
@@ -71,12 +79,26 @@ std::unique_ptr<OMPLSolverConfig> OMPLRealVectorPlanProfile::createSolverConfig(
   return std::make_unique<OMPLSolverConfig>(solver_config);
 }
 
-OMPLStateExtractor OMPLRealVectorPlanProfile::createStateExtractor(const tesseract_kinematics::JointGroup& manip) const
+ompl::base::StateSpacePtr OMPLRealVectorPlanProfile::createStateSpace(const tesseract_common::ManipulatorInfo& mi,
+                                                                      const std::shared_ptr<const tesseract_environment::Environment>& env) 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);
-  };
+  // Get kinematics
+  tesseract_kinematics::JointGroup::Ptr manip = env->getJointGroup(mi.manipulator);
+  const auto dof = static_cast<unsigned>(manip->numJoints());
+  const std::vector<std::string> joint_names = manip->getJointNames();
+  const Eigen::MatrixX2d limits = manip->getLimits().joint_limits;
+
+  // Construct the OMPL state space for this manipulator
+  auto rvss = std::make_shared<ompl::base::RealVectorStateSpace>();
+  for (unsigned i = 0; i < dof; ++i)
+    rvss->addDimension(joint_names[i], limits(i, 0), limits(i, 1));
+
+  rvss->setStateSamplerAllocator(createStateSamplerAllocator(env, manip));
+
+  // Setup Longest Valid Segment
+  processLongestValidSegment(rvss, collision_check_config);
+
+  return rvss;
 }
 
 std::unique_ptr<ompl::geometric::SimpleSetup>
@@ -92,52 +114,32 @@ OMPLRealVectorPlanProfile::createSimpleSetup(const MoveInstructionPoly& start_in
 
   // Get kinematics
   tesseract_kinematics::JointGroup::Ptr manip = env->getJointGroup(end_mi.manipulator);
-  const auto dof = static_cast<unsigned>(manip->numJoints());
-  const std::vector<std::string> joint_names = manip->getJointNames();
-  const Eigen::MatrixX2d limits = manip->getLimits().joint_limits;
-
-  // Construct the OMPL state space for this manipulator
-  ompl::base::StateSpacePtr state_space_ptr;
-
-  auto rss = std::make_shared<ompl::base::RealVectorStateSpace>();
-  for (unsigned i = 0; i < dof; ++i)
-    rss->addDimension(joint_names[i], limits(i, 0), limits(i, 1));
-
-  rss->setStateSamplerAllocator(createStateSamplerAllocator(env, manip));
-
-  state_space_ptr = rss;
-
-  // Setup Longest Valid Segment
-  processLongestValidSegment(state_space_ptr, collision_check_config);
 
   // 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);
+  auto simple_setup = std::make_unique<ompl::geometric::SimpleSetup>(createStateSpace(end_mi, env));
 
   // 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));
   }
@@ -427,15 +429,14 @@ OMPLRealVectorPlanProfile::createStateSamplerAllocator(
   Eigen::MatrixX2d limits = manip->getLimits().joint_limits;
   Eigen::VectorXd weights = Eigen::VectorXd::Ones(manip->numJoints());
   return [weights, limits](const ompl::base::StateSpace* state_space) -> ompl::base::StateSamplerPtr {
-    return allocWeightedRealVectorStateSampler(state_space, weights, limits);
+    return std::make_shared<WeightedRealVectorStateSampler>(state_space, weights, limits);
   };
 }
 
 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 +444,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, state_converter_);
   }
 
   return nullptr;
@@ -460,7 +460,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)
@@ -473,7 +472,7 @@ std::unique_ptr<ompl::base::MotionValidator> OMPLRealVectorPlanProfile::createMo
                                                          *env,
                                                          manip,
                                                          collision_check_config,
-                                                         state_extractor);
+                                                         state_converter_);
     }
 
     // Collision checking is preformed using the state validator which this calls.
@@ -486,12 +485,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 = fromOMPL(path, state_converter_);
+
+  // 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/continuous_motion_validator.cpp b/tesseract_motion_planners/ompl/src/real_vector_state_space/continuous_motion_validator.cpp
similarity index 93%
rename from tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp
rename to tesseract_motion_planners/ompl/src/real_vector_state_space/continuous_motion_validator.cpp
index 45c3451e7ce..12e859be78f 100644
--- a/tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp
+++ b/tesseract_motion_planners/ompl/src/real_vector_state_space/continuous_motion_validator.cpp
@@ -30,7 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <thread>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
-#include <tesseract_motion_planners/ompl/continuous_motion_validator.h>
+#include <tesseract_motion_planners/ompl/real_vector_state_space/continuous_motion_validator.h>
 
 #include <tesseract_environment/environment.h>
 #include <tesseract_kinematics/core/joint_group.h>
@@ -45,12 +45,12 @@ ContinuousMotionValidator::ContinuousMotionValidator(
     const tesseract_environment::Environment& env,
     std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
     const tesseract_collision::CollisionCheckConfig& collision_check_config,
-    OMPLStateExtractor extractor)
+    StateConverterFn state_converter)
   : MotionValidator(space_info)
   , state_validator_(std::move(state_validator))
   , manip_(std::move(manip))
   , continuous_contact_manager_(env.getContinuousContactManager())
-  , extractor_(std::move(extractor))
+  , state_converter_(state_converter)
 {
   links_ = manip_->getActiveLinkNames();
 
@@ -139,8 +139,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 = state_converter_(s1, si_->getStateDimension());
+  Eigen::Map<Eigen::VectorXd> finish_joints = state_converter_(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/state_collision_validator.cpp b/tesseract_motion_planners/ompl/src/real_vector_state_space/state_collision_validator.cpp
similarity index 91%
rename from tesseract_motion_planners/ompl/src/state_collision_validator.cpp
rename to tesseract_motion_planners/ompl/src/real_vector_state_space/state_collision_validator.cpp
index 00cba119007..bd50347397e 100644
--- a/tesseract_motion_planners/ompl/src/state_collision_validator.cpp
+++ b/tesseract_motion_planners/ompl/src/real_vector_state_space/state_collision_validator.cpp
@@ -30,8 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <Eigen/Core>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
-#include <tesseract_motion_planners/ompl/utils.h>
-#include <tesseract_motion_planners/ompl/state_collision_validator.h>
+#include <tesseract_motion_planners/ompl/real_vector_state_space/state_collision_validator.h>
 
 #include <tesseract_kinematics/core/joint_group.h>
 #include <tesseract_collision/core/discrete_contact_manager.h>
@@ -45,11 +44,11 @@ StateCollisionValidator::StateCollisionValidator(
     const tesseract_environment::Environment& env,
     std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
     const tesseract_collision::CollisionCheckConfig& collision_check_config,
-    OMPLStateExtractor extractor)
+    StateConverterFn state_converter)
   : StateValidityChecker(space_info)
   , manip_(std::move(manip))
   , contact_manager_(env.getDiscreteContactManager())
-  , extractor_(std::move(extractor))
+  , state_converter_(state_converter)
 {
   links_ = manip_->getActiveLinkNames();
 
@@ -75,7 +74,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 = state_converter_(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/real_vector_state_space/utils.cpp b/tesseract_motion_planners/ompl/src/real_vector_state_space/utils.cpp
new file mode 100644
index 00000000000..71f0858377d
--- /dev/null
+++ b/tesseract_motion_planners/ompl/src/real_vector_state_space/utils.cpp
@@ -0,0 +1,72 @@
+/**
+ * @file utils.cpp
+ * @brief Tesseract OMPL real vector state space utility functions
+ *
+ * @author Levi Armstrong
+ * @date February 17, 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.
+ */
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <ompl/base/spaces/RealVectorStateSpace.h>
+#include <ompl/base/State.h>
+#include <ompl/geometric/PathGeometric.h>
+
+#ifndef OMPL_LESS_1_4_0
+#include <ompl/base/spaces/constraint/ProjectedStateSpace.h>
+#endif
+
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_motion_planners/ompl/real_vector_state_space/utils.h>
+
+namespace tesseract_planning
+{
+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 };
+}
+
+#ifndef OMPL_LESS_1_4_0
+Eigen::Map<Eigen::VectorXd> fromConstrainedRealVectorStateSpace(const ompl::base::State* s1, unsigned /*dimension*/)
+{
+  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 fromRealVectorStateSpace(const ompl::geometric::PathGeometric& path,
+                                                     StateConverterFn state_converter)
+{
+  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) = state_converter(path.getState(static_cast<unsigned>(i)), dof);
+
+  return result;
+}
+
+}  // namespace tesseract_planning
diff --git a/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp b/tesseract_motion_planners/ompl/src/real_vector_state_space/weighted_real_vector_state_sampler.cpp
similarity index 97%
rename from tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp
rename to tesseract_motion_planners/ompl/src/real_vector_state_space/weighted_real_vector_state_sampler.cpp
index 9694ffcddd9..6b4ad84f661 100644
--- a/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp
+++ b/tesseract_motion_planners/ompl/src/real_vector_state_space/weighted_real_vector_state_sampler.cpp
@@ -33,7 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <ompl/base/ScopedState.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
-#include <tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h>
+#include <tesseract_motion_planners/ompl/real_vector_state_space/weighted_real_vector_state_sampler.h>
 
 namespace tesseract_planning
 {
diff --git a/tesseract_motion_planners/ompl/src/utils.cpp b/tesseract_motion_planners/ompl/src/utils.cpp
index 36cbde39cf2..dd5f51c5782 100644
--- a/tesseract_motion_planners/ompl/src/utils.cpp
+++ b/tesseract_motion_planners/ompl/src/utils.cpp
@@ -31,14 +31,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <ompl/geometric/PathGeometric.h>
 #include <memory>
 
-#ifndef OMPL_LESS_1_4_0
-#include <ompl/base/spaces/constraint/ProjectedStateSpace.h>
-#endif
-
 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 +44,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)
@@ -122,11 +91,67 @@ bool checkStateInCollision(tesseract_collision::ContactResultMap& contact_map,
   return (!contact_map.empty());
 }
 
-ompl::base::StateSamplerPtr allocWeightedRealVectorStateSampler(const ompl::base::StateSpace* space,
-                                                                const Eigen::VectorXd& weights,
-                                                                const Eigen::MatrixX2d& limits)
-{
-  return std::make_shared<WeightedRealVectorStateSampler>(space, weights, limits);
-}
+// 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