diff --git a/tesseract/tesseract_common/include/tesseract_common/manipulator_info.h b/tesseract/tesseract_common/include/tesseract_common/manipulator_info.h
index 4d72227ff8a..c916d58b18b 100644
--- a/tesseract/tesseract_common/include/tesseract_common/manipulator_info.h
+++ b/tesseract/tesseract_common/include/tesseract_common/manipulator_info.h
@@ -48,9 +48,9 @@ class ToolCenterPoint
   /**
    * @brief Tool Center Point Defined by name
    * @param name The tool center point name
-   * @param external The external TCP is used when the robot is holding the part verus the tool.
-   * @note External should also be set to true your kinematic object which includes say a robot and
-   * positioner, and the positioner has the tool and the robot is holding the part. Basically
+   * @param external The external TCP is used when the robot is holding the part versus the tool.
+   * @note External should also be set to true when your kinematic object includes a robot and
+   * positioner, where the positioner has the tool and the robot is holding the part. Basically
    * anytime the tool is not attached to the tip link of the kinematic chain.
    */
   ToolCenterPoint(const std::string& name, bool external = false);
@@ -58,10 +58,10 @@ class ToolCenterPoint
   /**
    * @brief Tool Center Point Defined by transform
    * @param transform The tool center point transform
-   * @param external The external TCP is used when the robot is holding the part verus the tool.
+   * @param external The external TCP is used when the robot is holding the part versus the tool.
    * @param external_frame If empty assumed relative to world, otherwise the provided tcp is relative to this link.
-   * @note External should also be set to true your kinematic object which includes say a robot and
-   * positioner, and the positioner has the tool and the robot is holding the part. Basically
+   * @note External should also be set to true when your kinematic object includes a robot and
+   * positioner, where the positioner has the tool and the robot is holding the part. Basically
    * anytime the tool is not attached to the tip link of the kinematic chain.
    */
   ToolCenterPoint(const Eigen::Isometry3d& transform, bool external = false, std::string external_frame = "");
@@ -86,9 +86,9 @@ class ToolCenterPoint
 
   /**
    * @brief Check if tool center point is and external tcp which mean it is not defined
-   * @details The external TCP is used when the robot is holding the part verus the tool.
-   * External should also be set to true your kinematic object which includes say a robot and
-   * positioner, and the positioner has the tool and the robot is holding the part. Basically
+   * @details The external TCP is used when the robot is holding the part versus the tool.
+   * External should also be set to true when your kinematic object includes a robot and
+   * positioner, where the positioner has the tool and the robot is holding the part. Basically
    * anytime the tool is not attached to the tip link of the kinematic chain.
    * @return True if and external TCP, otherwise the false
    */
@@ -137,9 +137,9 @@ class ToolCenterPoint
   Eigen::Isometry3d transform_;
 
   /**
-   * @brief The external TCP is used when the robot is holding the part verus the tool.
-   * @details External should also be set to true your kinematic object which includes say a robot and
-   * positioner, and the positioner has the tool and the robot is holding the part. Basically
+   * @brief The external TCP is used when the robot is holding the part versus the tool.
+   * @details External should also be set to true when your kinematic object includes a robot and
+   * positioner, where the positioner has the tool and the robot is holding the part. Basically
    * anytime the tool is not attached to the tip link of the kinematic chain.
    */
   bool external_{ false };
diff --git a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/utils/utils.h b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/utils/utils.h
index b3d147ff4b0..e9aca55dd6b 100644
--- a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/utils/utils.h
+++ b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/utils/utils.h
@@ -67,6 +67,34 @@ const std::vector<std::string>& getJointNames(const Waypoint& waypoint);
  */
 Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, const Waypoint& waypoint);
 
+/**
+ * @brief Format the waypoints joint ordered by the provided joint names
+ *
+ * Throws if waypoint does not directly contain that information
+ *
+ * Also this is an expensive call so the motion planners do not leverage this and they expect the order through out
+ * the program all match.
+ *
+ * @param joint_names The joint names defining the order desired
+ * @param waypoint The waypoint to format
+ * @return True if formating was required, otherwise false.
+ */
+bool formatJointPosition(const std::vector<std::string>& joint_names, Waypoint& waypoint);
+
+/**
+ * @brief Check the waypoints joint order against the provided joint names
+ *
+ * Throws if waypoint does not directly contain that information
+ *
+ * Also this is an expensive call so the motion planners do not leverage this and they expect the order through out
+ * the program all match.
+ *
+ * @param joint_names The joint names defining the order desired
+ * @param waypoint The waypoint to check format
+ * @return True if waypoint format is correct, otherwise false.
+ */
+bool checkJointPositionFormat(const std::vector<std::string>& joint_names, const Waypoint& waypoint);
+
 /**
  * @brief Set the joint position for waypoints that contain that information
  * @param waypoint Waypoint to set
diff --git a/tesseract/tesseract_planning/tesseract_command_language/src/utils/utils.cpp b/tesseract/tesseract_planning/tesseract_command_language/src/utils/utils.cpp
index 928b6ce2e64..3e219bfe18e 100644
--- a/tesseract/tesseract_planning/tesseract_command_language/src/utils/utils.cpp
+++ b/tesseract/tesseract_planning/tesseract_command_language/src/utils/utils.cpp
@@ -66,7 +66,7 @@ Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, co
   if (isJointWaypoint(waypoint))
   {
     const auto* jwp = waypoint.cast_const<JointWaypoint>();
-    jv = *jwp;
+    jv = jwp->waypoint;
     jn = jwp->joint_names;
   }
   else if (isStateWaypoint(waypoint))
@@ -80,7 +80,7 @@ Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, co
     throw std::runtime_error("Unsupported waypoint type.");
   }
 
-  if (jn.size() == joint_names.size())
+  if (jn.size() != joint_names.size())
     throw std::runtime_error("Joint name sizes do not match!");
 
   if (joint_names == jn)
@@ -103,6 +103,64 @@ Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, co
   return output;
 }
 
+bool formatJointPosition(const std::vector<std::string>& joint_names, Waypoint& waypoint)
+{
+  Eigen::VectorXd* jv;
+  std::vector<std::string>* jn;
+  if (isJointWaypoint(waypoint))
+  {
+    auto* jwp = waypoint.cast<JointWaypoint>();
+    jv = &(jwp->waypoint);
+    jn = &(jwp->joint_names);
+  }
+  else if (isStateWaypoint(waypoint))
+  {
+    auto* swp = waypoint.cast<StateWaypoint>();
+    jv = &(swp->position);
+    jn = &(swp->joint_names);
+  }
+  else
+  {
+    throw std::runtime_error("Unsupported waypoint type.");
+  }
+
+  if (jn->size() != joint_names.size())
+    throw std::runtime_error("Joint name sizes do not match!");
+
+  if (joint_names == *jn)
+    return false;
+
+  Eigen::VectorXd output = *jv;
+  for (std::size_t i = 0; i < joint_names.size(); ++i)
+  {
+    if (joint_names[i] == (*jn)[i])
+      continue;
+
+    auto it = std::find(jn->begin(), jn->end(), joint_names[i]);
+    if (it == jn->end())
+      throw std::runtime_error("Joint names do not match!");
+
+    long idx = std::distance(jn->begin(), it);
+    output(static_cast<long>(i)) = (*jv)(static_cast<long>(idx));
+  }
+
+  *jn = joint_names;
+  *jv = output;
+
+  return true;
+}
+
+bool checkJointPositionFormat(const std::vector<std::string>& joint_names, const Waypoint& waypoint)
+{
+  if (isJointWaypoint(waypoint))
+    return (joint_names == waypoint.cast_const<JointWaypoint>()->joint_names);
+
+  if (isStateWaypoint(waypoint))
+    return (joint_names == waypoint.cast_const<StateWaypoint>()->joint_names);
+
+  throw std::runtime_error("Unsupported waypoint type.");
+}
+
 bool setJointPosition(Waypoint& waypoint, const Eigen::Ref<const Eigen::VectorXd>& position)
 {
   if (isJointWaypoint(waypoint))
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/utils.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/utils.h
index 67477485ef8..95ecd52b16b 100644
--- a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/utils.h
+++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/utils.h
@@ -183,6 +183,14 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
 CompositeInstruction generateNaiveSeed(const CompositeInstruction& composite_instructions,
                                        const tesseract_environment::Environment& env);
 
+/**
+ * @brief This formats the joint and state waypoints to align with the kinematics object
+ * @param composite_instructions The input program to format
+ * @param env The environment information
+ * @return True if the program required formating.
+ */
+bool formatProgram(CompositeInstruction& composite_instructions, const tesseract_environment::Environment& env);
+
 }  // namespace tesseract_planning
 
 #endif  // TESSERACT_PLANNING_UTILS_H
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp
index f1319dcc8e2..8d26964b80a 100644
--- a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp
+++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp
@@ -139,6 +139,10 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
   auto results_flattened = flattenProgramToPattern(response.results, request.instructions);
   auto instructions_flattened = flattenProgram(request.instructions);
 
+  // Check inverse and forward kinematic objects
+  if (problem->manip_fwd_kin->getJointNames() != problem->manip_inv_kin->getJointNames())
+    throw std::runtime_error("Forward and Inverse Kinematic objects joints are not ordered the same!");
+
   // Loop over the flattened results and add them to response if the input was a plan instruction
   Eigen::Index dof = problem->manip_fwd_kin->numJoints();
   Eigen::Index result_index = 0;
@@ -157,7 +161,9 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
         auto* move_instruction = results_flattened[idx].get().cast<MoveInstruction>();
 
         Eigen::Map<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> temp(solution.data() + dof * result_index++, dof);
-        move_instruction->getWaypoint().cast<StateWaypoint>()->position = temp.template cast<double>();
+        auto* swp = move_instruction->getWaypoint().cast<StateWaypoint>();
+        swp->position = temp.template cast<double>();
+        assert(swp->joint_names == problem->manip_fwd_kin->getJointNames());
       }
       else if (plan_instruction->isLinear())
       {
@@ -168,8 +174,9 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
         {
           Eigen::Map<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> temp(solution.data() + dof * result_index++,
                                                                              dof);
-          instruction.cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>()->position =
-              temp.template cast<double>();
+          auto* swp = instruction.cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>();
+          swp->position = temp.template cast<double>();
+          assert(swp->joint_names == problem->manip_fwd_kin->getJointNames());
         }
       }
       else if (plan_instruction->isFreespace())
@@ -190,8 +197,11 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
 
         assert(temp.cols() == static_cast<long>(move_instructions->size()) + 1);
         for (std::size_t i = 0; i < move_instructions->size(); ++i)
-          (*move_instructions)[i].cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>()->position =
-              temp.col(static_cast<long>(i) + 1);
+        {
+          auto* swp = (*move_instructions)[i].cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>();
+          swp->position = temp.col(static_cast<long>(i) + 1);
+          assert(swp->joint_names == problem->manip_fwd_kin->getJointNames());
+        }
       }
       else
       {
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/problem_generators/default_problem_generator.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/problem_generators/default_problem_generator.h
index d3e4d49e56e..fe5e129461e 100644
--- a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/problem_generators/default_problem_generator.h
+++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/problem_generators/default_problem_generator.h
@@ -134,6 +134,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
   }
   else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
   {
+    assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), start_waypoint));
     const Eigen::VectorXd& position = getJointPosition(start_waypoint);
     cur_plan_profile->apply(*prob, position, *start_instruction, composite_mi, active_links, index);
   }
@@ -187,6 +188,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
           }
           else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
           {
+            assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), start_waypoint));
             const Eigen::VectorXd& position = getJointPosition(start_waypoint);
             if (!prob->manip_fwd_kin->calcFwdKin(prev_pose, position))
               throw std::runtime_error("DescartesMotionPlannerConfig: failed to solve forward kinematics!");
@@ -214,6 +216,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
         }
         else if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
         {
+          assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), plan_instruction->getWaypoint()));
           const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
           Eigen::Isometry3d cur_pose = Eigen::Isometry3d::Identity();
           if (!prob->manip_fwd_kin->calcFwdKin(cur_pose, cur_position))
@@ -228,6 +231,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
           }
           else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
           {
+            assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), start_waypoint));
             const Eigen::VectorXd& position = getJointPosition(start_waypoint);
             if (!prob->manip_fwd_kin->calcFwdKin(prev_pose, position))
               throw std::runtime_error("DescartesMotionPlannerConfig: failed to solve forward kinematics!");
@@ -262,6 +266,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
       {
         if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
         {
+          assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), plan_instruction->getWaypoint()));
           const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
 
           // Descartes does not support freespace so it will only include the plan instruction state, then in
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/core/utils.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/core/utils.cpp
index 1e58086b5c7..f054d07cb06 100644
--- a/tesseract/tesseract_planning/tesseract_motion_planners/src/core/utils.cpp
+++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/core/utils.cpp
@@ -508,6 +508,7 @@ void generateNaiveSeedHelper(CompositeInstruction& composite_instructions,
 
       if (isStateWaypoint(base_instruction->getWaypoint()))
       {
+        assert(checkJointPositionFormat(fwd_kin->getJointNames(), base_instruction->getWaypoint()));
         MoveInstruction move_instruction(base_instruction->getWaypoint(), move_type);
         move_instruction.setManipulatorInfo(base_instruction->getManipulatorInfo());
         move_instruction.setDescription(base_instruction->getDescription());
@@ -516,6 +517,7 @@ void generateNaiveSeedHelper(CompositeInstruction& composite_instructions,
       }
       else if (isJointWaypoint(base_instruction->getWaypoint()))
       {
+        assert(checkJointPositionFormat(fwd_kin->getJointNames(), base_instruction->getWaypoint()));
         const auto* jwp = base_instruction->getWaypoint().cast<JointWaypoint>();
         MoveInstruction move_instruction(StateWaypoint(jwp->joint_names, jwp->waypoint), move_type);
         move_instruction.setManipulatorInfo(base_instruction->getManipulatorInfo());
@@ -553,7 +555,7 @@ CompositeInstruction generateNaiveSeed(const CompositeInstruction& composite_ins
   std::string profile;
   if (isPlanInstruction(seed.getStartInstruction()))
   {
-    const auto* pi = seed.getStartInstruction().cast<PlanInstruction>();
+    const auto* pi = seed.getStartInstruction().cast_const<PlanInstruction>();
     wp = pi->getWaypoint();
     base_mi = pi->getManipulatorInfo();
     description = pi->getDescription();
@@ -561,7 +563,7 @@ CompositeInstruction generateNaiveSeed(const CompositeInstruction& composite_ins
   }
   else if (isMoveInstruction(seed.getStartInstruction()))
   {
-    const auto* pi = seed.getStartInstruction().cast<MoveInstruction>();
+    const auto* pi = seed.getStartInstruction().cast_const<MoveInstruction>();
     wp = pi->getWaypoint();
     base_mi = pi->getManipulatorInfo();
     description = pi->getDescription();
@@ -576,6 +578,7 @@ CompositeInstruction generateNaiveSeed(const CompositeInstruction& composite_ins
 
   if (isStateWaypoint(wp))
   {
+    assert(checkJointPositionFormat(fwd_kin->getJointNames(), wp));
     MoveInstruction move_instruction(wp, MoveInstructionType::START);
     move_instruction.setManipulatorInfo(base_mi);
     move_instruction.setDescription(description);
@@ -584,6 +587,7 @@ CompositeInstruction generateNaiveSeed(const CompositeInstruction& composite_ins
   }
   else if (isJointWaypoint(wp))
   {
+    assert(checkJointPositionFormat(fwd_kin->getJointNames(), wp));
     const auto* jwp = wp.cast<JointWaypoint>();
     MoveInstruction move_instruction(StateWaypoint(jwp->joint_names, jwp->waypoint), MoveInstructionType::START);
     move_instruction.setManipulatorInfo(base_mi);
@@ -603,4 +607,91 @@ CompositeInstruction generateNaiveSeed(const CompositeInstruction& composite_ins
   generateNaiveSeedHelper(seed, env, *env_state, mi);
   return seed;
 }
+
+bool formatProgramHelper(CompositeInstruction& composite_instructions,
+                         const tesseract_environment::Environment& env,
+                         const ManipulatorInfo& manip_info)
+{
+  bool format_required = false;
+  for (auto& i : composite_instructions)
+  {
+    if (isCompositeInstruction(i))
+    {
+      if (formatProgramHelper(*(i.cast<CompositeInstruction>()), env, manip_info))
+        format_required = true;
+    }
+    else if (isPlanInstruction(i))
+    {
+      PlanInstruction* base_instruction = i.cast<PlanInstruction>();
+      ManipulatorInfo mi = manip_info.getCombined(base_instruction->getManipulatorInfo());
+
+      ManipulatorInfo combined_mi = mi.getCombined(base_instruction->getManipulatorInfo());
+      auto fwd_kin = env.getManipulatorManager()->getFwdKinematicSolver(combined_mi.manipulator);
+
+      if (isStateWaypoint(base_instruction->getWaypoint()) || isJointWaypoint(base_instruction->getWaypoint()))
+      {
+        if (formatJointPosition(fwd_kin->getJointNames(), base_instruction->getWaypoint()))
+          format_required = true;
+      }
+    }
+    else if (isMoveInstruction(i))
+    {
+      MoveInstruction* base_instruction = i.cast<MoveInstruction>();
+      ManipulatorInfo mi = manip_info.getCombined(base_instruction->getManipulatorInfo());
+
+      ManipulatorInfo combined_mi = mi.getCombined(base_instruction->getManipulatorInfo());
+      auto fwd_kin = env.getManipulatorManager()->getFwdKinematicSolver(combined_mi.manipulator);
+
+      if (isStateWaypoint(base_instruction->getWaypoint()) || isJointWaypoint(base_instruction->getWaypoint()))
+      {
+        if (formatJointPosition(fwd_kin->getJointNames(), base_instruction->getWaypoint()))
+          format_required = true;
+      }
+    }
+  }
+  return format_required;
+}
+
+bool formatProgram(CompositeInstruction& composite_instructions, const tesseract_environment::Environment& env)
+{
+  if (!composite_instructions.hasStartInstruction())
+    throw std::runtime_error("Top most composite instruction is missing start instruction!");
+
+  bool format_required = false;
+  ManipulatorInfo mi = composite_instructions.getManipulatorInfo();
+
+  if (isPlanInstruction(composite_instructions.getStartInstruction()))
+  {
+    auto* pi = composite_instructions.getStartInstruction().cast<PlanInstruction>();
+
+    ManipulatorInfo start_mi = mi.getCombined(pi->getManipulatorInfo());
+    auto fwd_kin = env.getManipulatorManager()->getFwdKinematicSolver(start_mi.manipulator);
+
+    if (isStateWaypoint(pi->getWaypoint()) || isJointWaypoint(pi->getWaypoint()))
+    {
+      if (formatJointPosition(fwd_kin->getJointNames(), pi->getWaypoint()))
+        format_required = true;
+    }
+  }
+  else if (isMoveInstruction(composite_instructions.getStartInstruction()))
+  {
+    auto* pi = composite_instructions.getStartInstruction().cast<MoveInstruction>();
+
+    ManipulatorInfo start_mi = mi.getCombined(pi->getManipulatorInfo());
+    auto fwd_kin = env.getManipulatorManager()->getFwdKinematicSolver(start_mi.manipulator);
+
+    if (isStateWaypoint(pi->getWaypoint()) || isJointWaypoint(pi->getWaypoint()))
+    {
+      if (formatJointPosition(fwd_kin->getJointNames(), pi->getWaypoint()))
+        format_required = true;
+    }
+  }
+  else
+    throw std::runtime_error("Top most composite instruction start instruction has invalid waypoint type!");
+
+  if (formatProgramHelper(composite_instructions, env, mi))
+    format_required = true;
+
+  return format_required;
+}
 };  // namespace tesseract_planning
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/ompl/problem_generators/default_problem_generator.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/ompl/problem_generators/default_problem_generator.cpp
index 727222c9545..e47ee06a44c 100644
--- a/tesseract/tesseract_planning/tesseract_motion_planners/src/ompl/problem_generators/default_problem_generator.cpp
+++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/ompl/problem_generators/default_problem_generator.cpp
@@ -181,6 +181,7 @@ std::vector<OMPLProblem::Ptr> DefaultOMPLProblemGenerator(const std::string& nam
       {
         if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
         {
+          assert(checkJointPositionFormat(manip_fwd_kin_->getJointNames(), plan_instruction->getWaypoint()));
           const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
           cur_plan_profile->applyGoalStates(
               *sub_prob, cur_position, *plan_instruction, composite_mi, active_link_names_, index);
@@ -190,6 +191,7 @@ std::vector<OMPLProblem::Ptr> DefaultOMPLProblemGenerator(const std::string& nam
             ompl::base::ScopedState<> start_state(sub_prob->simple_setup->getStateSpace());
             if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
             {
+              assert(checkJointPositionFormat(manip_fwd_kin_->getJointNames(), start_waypoint));
               const Eigen::VectorXd& prev_position = getJointPosition(start_waypoint);
               cur_plan_profile->applyStartStates(
                   *sub_prob, prev_position, *start_instruction, composite_mi, active_link_names_, index);
@@ -225,6 +227,7 @@ std::vector<OMPLProblem::Ptr> DefaultOMPLProblemGenerator(const std::string& nam
             ompl::base::ScopedState<> start_state(sub_prob->simple_setup->getStateSpace());
             if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
             {
+              assert(checkJointPositionFormat(manip_fwd_kin_->getJointNames(), start_waypoint));
               const Eigen::VectorXd& prev_position = getJointPosition(start_waypoint);
               cur_plan_profile->applyStartStates(
                   *sub_prob, prev_position, *start_instruction, composite_mi, active_link_names_, index);
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/simple_motion_planner.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/simple_motion_planner.cpp
index d3a74f24a5f..e101755bb7b 100644
--- a/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/simple_motion_planner.cpp
+++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/simple_motion_planner.cpp
@@ -161,6 +161,7 @@ SimpleMotionPlanner::getStartInstruction(const PlannerRequest& request,
 
     if (isJointWaypoint(start_waypoint))
     {
+      assert(checkJointPositionFormat(fwd_kin->getJointNames(), start_waypoint));
       const auto* jwp = start_waypoint.cast_const<JointWaypoint>();
       start_instruction_seed.setWaypoint(StateWaypoint(jwp->joint_names, *jwp));
     }
@@ -173,6 +174,7 @@ SimpleMotionPlanner::getStartInstruction(const PlannerRequest& request,
     }
     else if (isStateWaypoint(start_waypoint))
     {
+      assert(checkJointPositionFormat(fwd_kin->getJointNames(), start_waypoint));
       start_instruction_seed.setWaypoint(start_waypoint);
     }
     else
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/fixed_size_assign_position.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/fixed_size_assign_position.cpp
index a5934a168b7..2d1ad80fbd2 100644
--- a/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/fixed_size_assign_position.cpp
+++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/fixed_size_assign_position.cpp
@@ -83,6 +83,14 @@ CompositeInstruction fixedSizeAssignStateWaypoint(const JointWaypoint& start,
   // Joint waypoints should have joint names
   assert(static_cast<long>(start.joint_names.size()) == start.size());
 
+  assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty()));
+
+  assert([=]() {
+    ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());
+    auto fwd_kin = request.env->getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);
+    return checkJointPositionFormat(fwd_kin->getJointNames(), start);
+  }());
+
   return fixedSizeAssignStateWaypoint(start, base_instruction, request, manip_info, steps);
 }
 
@@ -96,6 +104,14 @@ CompositeInstruction fixedSizeAssignStateWaypoint(const CartesianWaypoint& /*sta
   // Joint waypoints should have joint names
   assert(static_cast<long>(end.joint_names.size()) == end.size());
 
+  assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty()));
+
+  assert([=]() {
+    ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());
+    auto fwd_kin = request.env->getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);
+    return checkJointPositionFormat(fwd_kin->getJointNames(), end);
+  }());
+
   return fixedSizeAssignStateWaypoint(end, base_instruction, request, manip_info, steps);
 }
 
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/fixed_size_interpolation.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/fixed_size_interpolation.cpp
index 6722677e31c..4af50eaf82c 100644
--- a/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/fixed_size_interpolation.cpp
+++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/fixed_size_interpolation.cpp
@@ -40,8 +40,8 @@ namespace tesseract_planning
 CompositeInstruction fixedSizeInterpolateStateWaypoint(const JointWaypoint& start,
                                                        const JointWaypoint& end,
                                                        const PlanInstruction& base_instruction,
-                                                       const PlannerRequest& /*request*/,
-                                                       const ManipulatorInfo& /*manip_info*/,
+                                                       const PlannerRequest& request,
+                                                       const ManipulatorInfo& manip_info,
                                                        int steps)
 {
   CompositeInstruction composite;
@@ -50,13 +50,22 @@ CompositeInstruction fixedSizeInterpolateStateWaypoint(const JointWaypoint& star
   assert(static_cast<long>(start.joint_names.size()) == start.size());
   assert(static_cast<long>(end.joint_names.size()) == end.size());
 
+  assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty()));
+  ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());
+
+  // Initialize
+  auto fwd_kin = request.env->getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), start));
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), end));
+
   // Linearly interpolate in joint space
   Eigen::MatrixXd states = interpolate(start, end, steps);
 
   // Convert to MoveInstructions
   for (long i = 1; i < states.cols(); ++i)
   {
-    MoveInstruction move_instruction(StateWaypoint(start.joint_names, states.col(i)), MoveInstructionType::FREESPACE);
+    MoveInstruction move_instruction(StateWaypoint(fwd_kin->getJointNames(), states.col(i)),
+                                     MoveInstructionType::FREESPACE);
     move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
     move_instruction.setDescription(base_instruction.getDescription());
     move_instruction.setProfile(base_instruction.getProfile());
@@ -80,6 +89,10 @@ CompositeInstruction fixedSizeInterpolateStateWaypoint(const JointWaypoint& star
 
   // Initialize
   auto inv_kin = request.env->getManipulatorManager()->getInvKinematicSolver(mi.manipulator);
+  auto fwd_kin = request.env->getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);
+  if (inv_kin->getJointNames() != fwd_kin->getJointNames())
+    throw std::runtime_error("Forward and Inverse Kinematic objects joints are not ordered the same!");
+
   auto world_to_base = request.env_state->link_transforms.at(inv_kin->getBaseLinkName());
   Eigen::Isometry3d tcp = request.env->findTCP(mi);
   assert(start.joint_names.size() == inv_kin->getJointNames().size());
@@ -87,6 +100,7 @@ CompositeInstruction fixedSizeInterpolateStateWaypoint(const JointWaypoint& star
   CompositeInstruction composite;
 
   // Calculate IK for start and end
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), start));
   Eigen::VectorXd j1 = start;
 
   Eigen::Isometry3d p2 = end * tcp.inverse();
@@ -119,7 +133,8 @@ CompositeInstruction fixedSizeInterpolateStateWaypoint(const JointWaypoint& star
   // Convert to MoveInstructions
   for (long i = 1; i < states.cols(); ++i)
   {
-    MoveInstruction move_instruction(StateWaypoint(start.joint_names, states.col(i)), MoveInstructionType::FREESPACE);
+    MoveInstruction move_instruction(StateWaypoint(fwd_kin->getJointNames(), states.col(i)),
+                                     MoveInstructionType::FREESPACE);
     move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
     move_instruction.setDescription(base_instruction.getDescription());
     move_instruction.setProfile(base_instruction.getProfile());
@@ -143,6 +158,10 @@ CompositeInstruction fixedSizeInterpolateStateWaypoint(const CartesianWaypoint&
 
   // Initialize
   auto inv_kin = request.env->getManipulatorManager()->getInvKinematicSolver(mi.manipulator);
+  auto fwd_kin = request.env->getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);
+  if (inv_kin->getJointNames() != fwd_kin->getJointNames())
+    throw std::runtime_error("Forward and Inverse Kinematic objects joints are not ordered the same!");
+
   auto world_to_base = request.env_state->link_transforms.at(inv_kin->getBaseLinkName());
   Eigen::Isometry3d tcp = request.env->findTCP(mi);
   assert(end.joint_names.size() == inv_kin->getJointNames().size());
@@ -156,6 +175,7 @@ CompositeInstruction fixedSizeInterpolateStateWaypoint(const CartesianWaypoint&
   if (!inv_kin->calcInvKin(j1, p1, end))
     throw std::runtime_error("fixedSizeInterpolateStateWaypoint: failed to find inverse kinematics solution!");
 
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), end));
   Eigen::VectorXd j2 = end;
 
   // Find closest solution to the end state
@@ -182,7 +202,8 @@ CompositeInstruction fixedSizeInterpolateStateWaypoint(const CartesianWaypoint&
   // Convert to MoveInstructions
   for (long i = 1; i < states.cols(); ++i)
   {
-    MoveInstruction move_instruction(StateWaypoint(end.joint_names, states.col(i)), MoveInstructionType::FREESPACE);
+    MoveInstruction move_instruction(StateWaypoint(fwd_kin->getJointNames(), states.col(i)),
+                                     MoveInstructionType::FREESPACE);
     move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
     move_instruction.setDescription(base_instruction.getDescription());
     move_instruction.setProfile(base_instruction.getProfile());
@@ -204,6 +225,10 @@ CompositeInstruction fixedSizeInterpolateStateWaypoint(const CartesianWaypoint&
 
   // Initialize
   auto inv_kin = request.env->getManipulatorManager()->getInvKinematicSolver(mi.manipulator);
+  auto fwd_kin = request.env->getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);
+  if (inv_kin->getJointNames() != fwd_kin->getJointNames())
+    throw std::runtime_error("Forward and Inverse Kinematic objects joints are not ordered the same!");
+
   auto world_to_base = request.env_state->link_transforms.at(inv_kin->getBaseLinkName());
   Eigen::Isometry3d tcp = request.env->findTCP(mi);
 
@@ -289,11 +314,13 @@ CompositeInstruction fixedSizeInterpolateCartStateWaypoint(const JointWaypoint&
 
   // Calculate FK for start and end
   Eigen::Isometry3d p1 = Eigen::Isometry3d::Identity();
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), start));
   if (!fwd_kin->calcFwdKin(p1, start))
     throw std::runtime_error("fixedSizeLinearInterpolation: failed to find forward kinematics solution!");
   p1 = world_to_base * p1 * tcp;
 
   Eigen::Isometry3d p2 = Eigen::Isometry3d::Identity();
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), end));
   if (!fwd_kin->calcFwdKin(p2, end))
     throw std::runtime_error("fixedSizeLinearInterpolation: failed to find forward kinematics solution!");
   p2 = world_to_base * p2 * tcp;
@@ -336,6 +363,7 @@ CompositeInstruction fixedSizeInterpolateCartStateWaypoint(const JointWaypoint&
 
   // Calculate FK for start and end
   Eigen::Isometry3d p1 = Eigen::Isometry3d::Identity();
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), start));
   if (!fwd_kin->calcFwdKin(p1, start))
     throw std::runtime_error("fixedSizeLinearInterpolation: failed to find forward kinematics solution!");
   p1 = world_to_base * p1 * tcp;
@@ -382,6 +410,7 @@ CompositeInstruction fixedSizeInterpolateCartStateWaypoint(const CartesianWaypoi
   Eigen::Isometry3d p1 = start;
 
   Eigen::Isometry3d p2 = Eigen::Isometry3d::Identity();
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), end));
   if (!fwd_kin->calcFwdKin(p2, end))
     throw std::runtime_error("fixedSizeLinearInterpolation: failed to find forward kinematics solution!");
   p2 = world_to_base * p2 * tcp;
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/lvs_interpolation.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/lvs_interpolation.cpp
index 22455cf0e35..7331acadce1 100644
--- a/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/lvs_interpolation.cpp
+++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/lvs_interpolation.cpp
@@ -64,11 +64,13 @@ CompositeInstruction LVSInterpolateStateWaypoint(const JointWaypoint& start,
 
   // Calculate FK for start and end
   Eigen::Isometry3d p1 = Eigen::Isometry3d::Identity();
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), start));
   if (!fwd_kin->calcFwdKin(p1, start))
     throw std::runtime_error("LVSInterpolateStateWaypoint: failed to find forward kinematics solution!");
   p1 = world_to_base * p1 * tcp;
 
   Eigen::Isometry3d p2 = Eigen::Isometry3d::Identity();
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), end));
   if (!fwd_kin->calcFwdKin(p2, end))
     throw std::runtime_error("LVSInterpolateStateWaypoint: failed to find forward kinematics solution!");
   p2 = world_to_base * p2 * tcp;
@@ -101,7 +103,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const JointWaypoint& start,
   // Convert to MoveInstructions
   for (long i = 1; i < states.cols(); ++i)
   {
-    MoveInstruction move_instruction(StateWaypoint(start.joint_names, states.col(i)), move_type);
+    MoveInstruction move_instruction(StateWaypoint(fwd_kin->getJointNames(), states.col(i)), move_type);
     move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
     move_instruction.setDescription(base_instruction.getDescription());
     move_instruction.setProfile(base_instruction.getProfile());
@@ -130,9 +132,13 @@ CompositeInstruction LVSInterpolateStateWaypoint(const JointWaypoint& start,
   // Initialize
   auto inv_kin = request.env->getManipulatorManager()->getInvKinematicSolver(mi.manipulator);
   auto fwd_kin = request.env->getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);
+  if (inv_kin->getJointNames() != fwd_kin->getJointNames())
+    throw std::runtime_error("Forward and Inverse Kinematic objects joints are not ordered the same!");
+
   auto world_to_base = request.env_state->link_transforms.at(inv_kin->getBaseLinkName());
   const Eigen::Isometry3d& tcp = request.env->findTCP(mi);
 
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), start));
   Eigen::VectorXd j1 = start;
 
   // Calculate p2 in kinematics base frame without tcp for accurate comparison with p1
@@ -141,7 +147,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const JointWaypoint& start,
 
   // Calculate FK for start
   Eigen::Isometry3d p1 = Eigen::Isometry3d::Identity();
-  if (!fwd_kin->calcFwdKin(p1, start))
+  if (!fwd_kin->calcFwdKin(p1, j1))
     throw std::runtime_error("LVSInterpolateStateWaypoint: failed to find forward kinematics solution!");
 
   // Calculate steps based on cartesian information
@@ -199,7 +205,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const JointWaypoint& start,
     // Convert to MoveInstructions
     for (long i = 1; i < states.cols(); ++i)
     {
-      MoveInstruction move_instruction(StateWaypoint(start.joint_names, states.col(i)), move_type);
+      MoveInstruction move_instruction(StateWaypoint(fwd_kin->getJointNames(), states.col(i)), move_type);
       move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
       move_instruction.setDescription(base_instruction.getDescription());
       move_instruction.setProfile(base_instruction.getProfile());
@@ -209,7 +215,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const JointWaypoint& start,
   else
   {
     // Convert to MoveInstructions
-    StateWaypoint swp(start.joint_names, j1);
+    StateWaypoint swp(fwd_kin->getJointNames(), j1);
     for (long i = 1; i < steps; ++i)
     {
       MoveInstruction move_instruction(swp, move_type);
@@ -242,6 +248,9 @@ CompositeInstruction LVSInterpolateStateWaypoint(const CartesianWaypoint& start,
   // Initialize
   auto inv_kin = request.env->getManipulatorManager()->getInvKinematicSolver(mi.manipulator);
   auto fwd_kin = request.env->getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);
+  if (inv_kin->getJointNames() != fwd_kin->getJointNames())
+    throw std::runtime_error("Forward and Inverse Kinematic objects joints are not ordered the same!");
+
   auto world_to_base = request.env_state->link_transforms.at(inv_kin->getBaseLinkName());
   const Eigen::Isometry3d& tcp = request.env->findTCP(mi);
 
@@ -251,7 +260,9 @@ CompositeInstruction LVSInterpolateStateWaypoint(const CartesianWaypoint& start,
 
   // Calculate FK for end state
   Eigen::Isometry3d p2 = Eigen::Isometry3d::Identity();
-  if (!fwd_kin->calcFwdKin(p2, end))
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), end));
+  Eigen::VectorXd j2 = end;
+  if (!fwd_kin->calcFwdKin(p2, j2))
     throw std::runtime_error("LVSInterpolateStateWaypoint: failed to find forward kinematics solution!");
 
   double trans_dist = (p2.translation() - p1.translation()).norm();
@@ -261,7 +272,6 @@ CompositeInstruction LVSInterpolateStateWaypoint(const CartesianWaypoint& start,
   int steps = std::max(trans_steps, rot_steps);
 
   Eigen::VectorXd j1, j1_final;
-  Eigen::VectorXd j2 = end;
   CompositeInstruction composite;
 
   bool j1_found = inv_kin->calcInvKin(j1, p1, j2);
@@ -309,7 +319,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const CartesianWaypoint& start,
     // Convert to MoveInstructions
     for (long i = 1; i < states.cols(); ++i)
     {
-      MoveInstruction move_instruction(StateWaypoint(end.joint_names, states.col(i)), move_type);
+      MoveInstruction move_instruction(StateWaypoint(fwd_kin->getJointNames(), states.col(i)), move_type);
       move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
       move_instruction.setDescription(base_instruction.getDescription());
       move_instruction.setProfile(base_instruction.getProfile());
@@ -319,7 +329,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const CartesianWaypoint& start,
   else
   {
     // Convert to MoveInstructions
-    StateWaypoint swp(end.joint_names, j2);
+    StateWaypoint swp(fwd_kin->getJointNames(), j2);
     for (long i = 1; i < steps; ++i)
     {
       MoveInstruction move_instruction(swp, move_type);
@@ -347,6 +357,10 @@ CompositeInstruction LVSInterpolateStateWaypoint(const CartesianWaypoint& start,
   ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());
 
   auto inv_kin = request.env->getManipulatorManager()->getInvKinematicSolver(mi.manipulator);
+  auto fwd_kin = request.env->getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);
+  if (inv_kin->getJointNames() != fwd_kin->getJointNames())
+    throw std::runtime_error("Forward and Inverse Kinematic objects joints are not ordered the same!");
+
   auto world_to_base = request.env_state->link_transforms.at(inv_kin->getBaseLinkName());
   const Eigen::Isometry3d& tcp = request.env->findTCP(mi);
 
@@ -532,11 +546,13 @@ CompositeInstruction LVSInterpolateCartStateWaypoint(const JointWaypoint& start,
 
   // Calculate FK for start and end
   Eigen::Isometry3d p1 = Eigen::Isometry3d::Identity();
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), start));
   if (!fwd_kin->calcFwdKin(p1, start))
     throw std::runtime_error("fixedSizeLinearInterpolation: failed to find forward kinematics solution!");
   p1 = world_to_base * p1 * tcp;
 
   Eigen::Isometry3d p2 = Eigen::Isometry3d::Identity();
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), end));
   if (!fwd_kin->calcFwdKin(p2, end))
     throw std::runtime_error("fixedSizeLinearInterpolation: failed to find forward kinematics solution!");
   p2 = world_to_base * p2 * tcp;
@@ -589,6 +605,7 @@ CompositeInstruction LVSInterpolateCartStateWaypoint(const JointWaypoint& start,
 
   // Calculate FK for start and end
   Eigen::Isometry3d p1 = Eigen::Isometry3d::Identity();
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), start));
   if (!fwd_kin->calcFwdKin(p1, start))
     throw std::runtime_error("fixedSizeLinearInterpolation: failed to find forward kinematics solution!");
   p1 = world_to_base * p1 * tcp;
@@ -645,6 +662,7 @@ CompositeInstruction LVSInterpolateCartStateWaypoint(const CartesianWaypoint& st
   Eigen::Isometry3d p1 = start;
 
   Eigen::Isometry3d p2 = Eigen::Isometry3d::Identity();
+  assert(checkJointPositionFormat(fwd_kin->getJointNames(), end));
   if (!fwd_kin->calcFwdKin(p2, end))
     throw std::runtime_error("fixedSizeLinearInterpolation: failed to find forward kinematics solution!");
   p2 = world_to_base * p2 * tcp;
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp
index d344491af76..b95980a9c7c 100644
--- a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp
+++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp
@@ -128,6 +128,7 @@ DefaultTrajoptProblemGenerator(const std::string& name,
   assert(request.seed.hasStartInstruction());
   assert(isMoveInstruction(request.seed.getStartInstruction()));
   const auto* seed_instruction = request.seed.getStartInstruction().cast_const<MoveInstruction>();
+  assert(checkJointPositionFormat(pci->kin->getJointNames(), seed_instruction->getWaypoint()));
   seed_states.push_back(getJointPosition(seed_instruction->getWaypoint()));
 
   // Add start waypoint
@@ -138,6 +139,7 @@ DefaultTrajoptProblemGenerator(const std::string& name,
   }
   else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
   {
+    assert(checkJointPositionFormat(pci->kin->getJointNames(), start_waypoint));
     const Eigen::VectorXd& position = getJointPosition(start_waypoint);
     start_plan_profile->apply(*pci, position, *start_instruction, composite_mi, active_links, index);
 
@@ -191,6 +193,7 @@ DefaultTrajoptProblemGenerator(const std::string& name,
           }
           else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
           {
+            assert(checkJointPositionFormat(pci->kin->getJointNames(), start_waypoint));
             const Eigen::VectorXd& position = getJointPosition(start_waypoint);
             if (!pci->kin->calcFwdKin(prev_pose, position))
               throw std::runtime_error("TrajOptPlannerUniversalConfig: failed to solve forward kinematics!");
@@ -211,6 +214,7 @@ DefaultTrajoptProblemGenerator(const std::string& name,
             // Add seed state
             assert(isMoveInstruction(seed_composite->at(p)));
             const auto* seed_instruction = seed_composite->at(p).cast_const<MoveInstruction>();
+            assert(checkJointPositionFormat(pci->kin->getJointNames(), seed_instruction->getWaypoint()));
             seed_states.push_back(getJointPosition(seed_instruction->getWaypoint()));
 
             ++index;
@@ -222,12 +226,14 @@ DefaultTrajoptProblemGenerator(const std::string& name,
           // Add seed state
           assert(isMoveInstruction(seed_composite->back()));
           const auto* seed_instruction = seed_composite->back().cast_const<tesseract_planning::MoveInstruction>();
+          assert(checkJointPositionFormat(pci->kin->getJointNames(), seed_instruction->getWaypoint()));
           seed_states.push_back(getJointPosition(seed_instruction->getWaypoint()));
 
           ++index;
         }
         else if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
         {
+          assert(checkJointPositionFormat(pci->kin->getJointNames(), plan_instruction->getWaypoint()));
           const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
           Eigen::Isometry3d cur_pose = Eigen::Isometry3d::Identity();
           if (!pci->kin->calcFwdKin(cur_pose, cur_position))
@@ -242,6 +248,7 @@ DefaultTrajoptProblemGenerator(const std::string& name,
           }
           else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
           {
+            assert(checkJointPositionFormat(pci->kin->getJointNames(), start_waypoint));
             const Eigen::VectorXd& position = getJointPosition(start_waypoint);
             if (!pci->kin->calcFwdKin(prev_pose, position))
               throw std::runtime_error("TrajOptPlannerUniversalConfig: failed to solve forward kinematics!");
@@ -262,6 +269,7 @@ DefaultTrajoptProblemGenerator(const std::string& name,
             // Add seed state
             assert(isMoveInstruction(seed_composite->at(p)));
             const auto* seed_instruction = seed_composite->at(p).cast_const<MoveInstruction>();
+            assert(checkJointPositionFormat(pci->kin->getJointNames(), seed_instruction->getWaypoint()));
             seed_states.push_back(getJointPosition(seed_instruction->getWaypoint()));
 
             ++index;
@@ -276,6 +284,7 @@ DefaultTrajoptProblemGenerator(const std::string& name,
           // Add seed state
           assert(isMoveInstruction(seed_composite->back()));
           const auto* seed_instruction = seed_composite->back().cast_const<MoveInstruction>();
+          assert(checkJointPositionFormat(pci->kin->getJointNames(), seed_instruction->getWaypoint()));
           seed_states.push_back(getJointPosition(seed_instruction->getWaypoint()));
 
           ++index;
@@ -289,6 +298,7 @@ DefaultTrajoptProblemGenerator(const std::string& name,
       {
         if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
         {
+          assert(checkJointPositionFormat(pci->kin->getJointNames(), plan_instruction->getWaypoint()));
           const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
 
           // Add intermediate points with path costs and constraints
@@ -297,6 +307,7 @@ DefaultTrajoptProblemGenerator(const std::string& name,
             // Add seed state
             assert(isMoveInstruction(seed_composite->at(s)));
             const auto* seed_instruction = seed_composite->at(s).cast_const<MoveInstruction>();
+            assert(checkJointPositionFormat(pci->kin->getJointNames(), seed_instruction->getWaypoint()));
             seed_states.push_back(getJointPosition(seed_instruction->getWaypoint()));
 
             ++index;
@@ -312,6 +323,7 @@ DefaultTrajoptProblemGenerator(const std::string& name,
           // Add seed state
           assert(isMoveInstruction(seed_composite->back()));
           const auto* seed_instruction = seed_composite->back().cast_const<MoveInstruction>();
+          assert(checkJointPositionFormat(pci->kin->getJointNames(), seed_instruction->getWaypoint()));
           seed_states.push_back(getJointPosition(seed_instruction->getWaypoint()));
         }
         else if (isCartesianWaypoint(plan_instruction->getWaypoint()))
@@ -324,6 +336,7 @@ DefaultTrajoptProblemGenerator(const std::string& name,
             // Add seed state
             assert(isMoveInstruction(seed_composite->at(s)));
             const auto* seed_instruction = seed_composite->at(s).cast_const<MoveInstruction>();
+            assert(checkJointPositionFormat(pci->kin->getJointNames(), seed_instruction->getWaypoint()));
             seed_states.push_back(getJointPosition(seed_instruction->getWaypoint()));
 
             ++index;
@@ -336,6 +349,7 @@ DefaultTrajoptProblemGenerator(const std::string& name,
           // Add seed state
           assert(isMoveInstruction(seed_composite->back()));
           const auto* seed_instruction = seed_composite->back().cast_const<MoveInstruction>();
+          assert(checkJointPositionFormat(pci->kin->getJointNames(), seed_instruction->getWaypoint()));
           seed_states.push_back(getJointPosition(seed_instruction->getWaypoint()));
         }
         else
diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_planning_server.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_planning_server.cpp
index d74cfdfea75..f94f33c4f3e 100644
--- a/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_planning_server.cpp
+++ b/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_planning_server.cpp
@@ -38,6 +38,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_motion_planners/ompl/profile/ompl_profile.h>
 #include <tesseract_motion_planners/descartes/profile/descartes_profile.h>
 #include <tesseract_motion_planners/simple/profile/simple_planner_profile.h>
+#include <tesseract_motion_planners/core/utils.h>
 
 #include <tesseract_command_language/utils/utils.h>
 
@@ -115,7 +116,7 @@ ProcessPlanningFuture ProcessPlanningServer::run(const ProcessPlanningRequest& r
       std::make_unique<const PlannerProfileRemapping>(request.composite_profile_remapping);
 
   response.input = std::make_unique<Instruction>(request.instructions);
-  const auto* composite_program = response.input->cast_const<CompositeInstruction>();
+  auto* composite_program = response.input->cast<CompositeInstruction>();
   ManipulatorInfo mi = composite_program->getManipulatorInfo();
   response.global_manip_info = std::make_unique<const ManipulatorInfo>(mi);
 
@@ -143,6 +144,12 @@ ProcessPlanningFuture ProcessPlanningServer::run(const ProcessPlanningRequest& r
   if (request.env_state != nullptr)
     tc->setState(request.env_state->joints);
 
+  // This makes sure the Joint and State Waypoints match the same order as the kinematics
+  if (formatProgram(*composite_program, *tc))
+  {
+    CONSOLE_BRIDGE_logInform("Tesseract Planning Server: Input program required formating!");
+  }
+
   if (!request.commands.empty() && !tc->applyCommands(request.commands))
   {
     CONSOLE_BRIDGE_logInform("Tesseract Planning Server Finished Request!");