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 83bcdf96710..414c3945bbd 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
@@ -92,6 +92,8 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile
              const std::vector<std::string>& active_links,
              int index) 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 3bfe558b05a..7207d08f5f9 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
@@ -71,6 +71,8 @@ class TrajOptPlanProfile
                      const std::vector<std::string>& active_links,
                      int index) 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 e2cc8a29a64..9ab471a5f68 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
@@ -280,6 +280,13 @@ void TrajOptDefaultPlanProfile::addConstraintErrorFunctions(trajopt::ProblemCons
   }
 }
 
+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
 {
   tinyxml2::XMLElement* xml_planner = doc.NewElement("Planner");
diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp
index be88f59cb34..59564b4d543 100644
--- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp
+++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp
@@ -300,7 +300,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 9676c365dd7..b7bf19244b9 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
@@ -64,6 +64,8 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
              const std::vector<std::string>& active_links,
              int index) 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 35b62e19826..42888800dce 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
@@ -73,6 +73,8 @@ class TrajOptIfoptPlanProfile
                      const std::vector<std::string>& active_links,
                      int index) 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 4283be0dd5e..fa46c4b59ff 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
@@ -127,6 +127,13 @@ void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem,
   }
 }
 
+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 52dc9122803..be9e84a58d4 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
@@ -320,7 +320,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);
       }
     }