Removed fixed cartesian logic

tesseract-robotics · Jan 8, 2024 · cca499e · cca499e
1 parent b09ef53
commit cca499e
Show file tree

Hide file tree

Showing 8 changed files with 2 additions and 24 deletions.
diff --git a/.../trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h b/.../trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h
@@ -90,7 +90,6 @@ 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;

diff --git a/...tion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h b/...tion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h
@@ -66,7 +66,6 @@ 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
@@ -211,13 +211,6 @@ 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

diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp
@@ -273,9 +273,7 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const
         seed_states.push_back(request.env_state.getJointValues(joint_names));
       }
 
-      // Add to fixed indices
-      if (!cwp.isToleranced() && cur_plan_profile->isFixedCartesian())
-        fixed_steps.push_back(i);
+      /** @todo If fixed cartesian and not term_type cost add as fixed */
     }
     else if (move_instruction.getWaypoint().isJointWaypoint())
     {

diff --git a/...lude/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h b/...lude/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h
@@ -65,7 +65,6 @@ 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;

diff --git a/...opt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h b/...opt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h
@@ -68,7 +68,6 @@ 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
@@ -138,13 +138,6 @@ 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

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
@@ -289,9 +289,7 @@ std::shared_ptr<TrajOptIfoptProblem> TrajOptIfoptMotionPlanner::createProblem(co
       // Apply profile
       cur_plan_profile->apply(*problem, cwp, move_instruction, composite_mi, active_links, i);
 
-      // Add to fixed indices
-      if (!cwp.isToleranced() && cur_plan_profile->isFixedCartesian())
-        fixed_steps.push_back(i);
+      /** @todo If fixed cartesian and not term_type cost add as fixed */
     }
     else if (move_instruction.getWaypoint().isJointWaypoint())
     {