Add isFixed...() methods to plan profiles

Author: rjoomen

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:

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;
 };
 

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, " ", " ");

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);
       }
 

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

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;
 };
 

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!");

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);
       }
     }