diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp
index 13905c9d6d..ec15dc41bc 100644
--- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp
+++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp
@@ -273,7 +273,9 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const
         seed_states.push_back(request.env_state.getJointValues(joint_names));
       }
 
-      /** @todo If fixed cartesian and not term_type cost add as fixed */
+      // Add to fixed indices
+      if (!cwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */
+        fixed_steps.push_back(i);
     }
     else if (move_instruction.getWaypoint().isJointWaypoint())
     {
diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp
index 302c0daa76..a28cf4768c 100644
--- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp
+++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp
@@ -289,7 +289,9 @@ std::shared_ptr<TrajOptIfoptProblem> TrajOptIfoptMotionPlanner::createProblem(co
       // Apply profile
       cur_plan_profile->apply(*problem, cwp, move_instruction, composite_mi, active_links, i);
 
-      /** @todo If fixed cartesian and not term_type cost add as fixed */
+      // Add to fixed indices
+      if (!cwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */
+        fixed_steps.push_back(i);
     }
     else if (move_instruction.getWaypoint().isJointWaypoint())
     {