diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h
index abe1e2bfeb1..867ec66c9b0 100644
--- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h
+++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h
@@ -90,6 +90,9 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile
              const std::vector<std::string>& active_links,
              int index) const override;
 
+  bool isFixedCartesian() const override;
+  bool isFixedJoint() const override;
+
   tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
 
 protected:
diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h
index aa49976b82a..265efce236a 100644
--- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h
+++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h
@@ -66,6 +66,9 @@ class TrajOptPlanProfile
                      const std::vector<std::string>& active_links,
                      int index) const = 0;
 
+  virtual bool isFixedCartesian() const = 0;
+  virtual bool isFixedJoint() const = 0;
+
   virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;
 };
 
diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp
index 363805cce1f..5eefe2ba13e 100644
--- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp
+++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp
@@ -211,6 +211,16 @@ void TrajOptDefaultPlanProfile::addConstraintErrorFunctions(trajopt::ProblemCons
   }
 }
 
+bool TrajOptDefaultPlanProfile::isFixedCartesian() const {
+  // If the term type is constraint and all coefficients are non-zero
+  return (term_type == trajopt::TermType::TT_CNT) && (abs(cartesian_coeff.array()) >= std::numeric_limits<double>::epsilon()).all();
+}
+
+bool TrajOptDefaultPlanProfile::isFixedJoint() const {
+  // If the term type is constraint and all coefficients are non-zero
+  return (term_type == trajopt::TermType::TT_CNT) && (abs(joint_coeff.array()) >= std::numeric_limits<double>::epsilon()).all();
+}
+
 tinyxml2::XMLElement* TrajOptDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc) const
 {
   Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " ");
diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp
index ec15dc41bc2..b7667f0a15e 100644
--- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp
+++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp
@@ -274,7 +274,7 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const
       }
 
       // Add to fixed indices
-      if (!cwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */
+      if (!cwp.isToleranced() && cur_plan_profile->isFixedCartesian())
         fixed_steps.push_back(i);
     }
     else if (move_instruction.getWaypoint().isJointWaypoint())
@@ -287,7 +287,7 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const
         cur_plan_profile->apply(*pci, jwp, move_instruction, composite_mi, active_links, i);
 
         // Add to fixed indices
-        if (!jwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */
+        if (!jwp.isToleranced() && cur_plan_profile->isFixedJoint())
           fixed_steps.push_back(i);
       }
 
diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h
index 5eca8d484d8..8c27f5314b0 100644
--- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h
+++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h
@@ -65,6 +65,9 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
              const std::vector<std::string>& active_links,
              int index) const override;
 
+  bool isFixedCartesian() const override;
+  bool isFixedJoint() const override;
+
   tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
 };
 }  // namespace tesseract_planning
diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h
index 3e20d344da8..78202d628f8 100644
--- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h
+++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h
@@ -68,6 +68,9 @@ class TrajOptIfoptPlanProfile
                      const std::vector<std::string>& active_links,
                      int index) const = 0;
 
+  virtual bool isFixedCartesian() const = 0;
+  virtual bool isFixedJoint() const = 0;
+
   virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;
 };
 
diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp
index 62963f892fd..09d4a6fd4ff 100644
--- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp
+++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp
@@ -138,6 +138,16 @@ void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem,
   }
 }
 
+bool TrajOptIfoptDefaultPlanProfile::isFixedCartesian() const {
+  // If the term type is constraint and all coefficients are non-zero
+  return (term_type == TrajOptIfoptTermType::CONSTRAINT) && (abs(cartesian_coeff.array()) >= std::numeric_limits<double>::epsilon()).all();
+}
+
+bool TrajOptIfoptDefaultPlanProfile::isFixedJoint() const {
+  // If the term type is constraint and all coefficients are non-zero
+  return (term_type == TrajOptIfoptTermType::CONSTRAINT) && (abs(joint_coeff.array()) >= std::numeric_limits<double>::epsilon()).all();
+}
+
 tinyxml2::XMLElement* TrajOptIfoptDefaultPlanProfile::toXML(tinyxml2::XMLDocument& /*doc*/) const
 {
   throw std::runtime_error("TrajOptIfoptDefaultPlanProfile::toXML is not implemented!");
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 a28cf4768ce..33c977d437b 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
@@ -290,7 +290,7 @@ std::shared_ptr<TrajOptIfoptProblem> TrajOptIfoptMotionPlanner::createProblem(co
       cur_plan_profile->apply(*problem, cwp, move_instruction, composite_mi, active_links, i);
 
       // Add to fixed indices
-      if (!cwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */
+      if (!cwp.isToleranced() && cur_plan_profile->isFixedCartesian())
         fixed_steps.push_back(i);
     }
     else if (move_instruction.getWaypoint().isJointWaypoint())
@@ -312,7 +312,7 @@ std::shared_ptr<TrajOptIfoptProblem> TrajOptIfoptMotionPlanner::createProblem(co
         cur_plan_profile->apply(*problem, jwp, move_instruction, composite_mi, active_links, i);
 
         // Add to fixed indices
-        if (!jwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */
+        if (!jwp.isToleranced() && cur_plan_profile->isFixedJoint())
           fixed_steps.push_back(i);
       }
     }