Added avoid singularity to TrajOpt motion planner utils and default configuration

Author: marip8

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/config/trajopt_planner_default_config.h

@@ -110,6 +110,10 @@ struct TrajOptPlannerDefaultConfig : public TrajOptPlannerConfig
   bool smooth_jerks = true;
   /** @brief This default to all ones, but allows you to weight different joints */
   Eigen::VectorXd jerk_coeff;
+  /** @brief If true, applies a cost to avoid kinematic singularities */
+  bool avoid_singularity = true;
+  /** @brief Optimization weight associated with kinematic singularity avoidance */
+  double avoid_singularity_coeff = 5.0;
 
   /** @brief Error function that is set as a constraint for each timestep.
    *

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/config/utils.h

@@ -114,6 +114,11 @@ trajopt::TermInfo::Ptr createUserDefinedTermInfo(int n_steps,
                                                  sco::MatrixOfVector::func jacobian_function,
                                                  const std::string& name = "user_defined");
 
+trajopt::TermInfo::Ptr createAvoidSingularityTermInfo(const int n_steps,
+                                                      const std::string& link,
+                                                      const double coeff = 5.0,
+                                                      const std::string& name = "avoid_singularity");
+
 }  // namespace tesseract_motion_planners
 
 #endif  // TESSERACT_MOTION_PLANNERS_TRAJOPT_CONFIG_UTILS_H

tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/config/trajopt_planner_default_config.cpp

@@ -258,6 +258,13 @@ std::shared_ptr<trajopt::ProblemConstructionInfo> TrajOptPlannerDefaultConfig::g
     }
   }
 
+  // Avoid singularity
+  if (avoid_singularity)
+  {
+    trajopt::TermInfo::Ptr ti = createAvoidSingularityTermInfo(pci.basic_info.n_steps, link, avoid_singularity_coeff);
+    pci.cost_infos.push_back(ti);
+  }
+
   return std::make_shared<trajopt::ProblemConstructionInfo>(pci);
 }
 

tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/config/utils.cpp

@@ -334,4 +334,18 @@ trajopt::TermInfo::Ptr createUserDefinedTermInfo(int n_steps,
   return ef;
 }
 
+trajopt::TermInfo::Ptr
+createAvoidSingularityTermInfo(const int n_steps, const std::string& link, const double coeff, const std::string& name)
+{
+  auto as = std::make_shared<trajopt::AvoidSingularityTermInfo>();
+  as->term_type = trajopt::TT_COST;
+  as->link = link;
+  as->first_step = 0;
+  as->last_step = n_steps - 1;
+  as->coeffs = std::vector<double>(1, coeff);
+  as->name = name;
+
+  return as;
+}
+
 }  // namespace tesseract_motion_planners