Fix joint velocity smoothing

Author: mpowelson

tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h

@@ -257,9 +257,11 @@ class CartesianWaypoint
 
   /** @brief The Cartesian Waypoint */
   Eigen::Isometry3d waypoint;
-  /** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */
+  /** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3
+   * elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */
   Eigen::VectorXd lower_tolerance;
-  /** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/
+  /** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3
+   * elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/
   Eigen::VectorXd upper_tolerance;
 
   bool isToleranced() const

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h

@@ -37,12 +37,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 namespace tesseract_planning
 {
 trajopt::TermInfo::Ptr createCartesianPositionConstraint(const Eigen::Isometry3d& c_wp,
-                                                       int index,
-                                                       std::string working_frame,
-                                                       Eigen::Isometry3d tcp,
-                                                       const Eigen::VectorXd& coeffs,
-                                                       std::string link,
-                                                       trajopt::TermType type);
+                                                         int index,
+                                                         std::string working_frame,
+                                                         Eigen::Isometry3d tcp,
+                                                         const Eigen::VectorXd& coeffs,
+                                                         std::string link,
+                                                         trajopt::TermType type);
 
 trajopt::TermInfo::Ptr createDynamicCartesianWaypointTermInfo(const Eigen::Isometry3d& c_wp,
                                                               int index,
@@ -66,10 +66,9 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
 trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
                                                              const Eigen::VectorXd& lower_tol,
                                                              const Eigen::VectorXd& upper_tol,
-                                                   int index,
-                                                   const Eigen::VectorXd& coeffs,
-                                                   trajopt::TermType type);
-
+                                                             int index,
+                                                             const Eigen::VectorXd& coeffs,
+                                                             trajopt::TermType type);
 
 trajopt::TermInfo::Ptr createCollisionTermInfo(
     int start_index,

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/problem_generators/default_problem_generator.h

@@ -36,10 +36,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_planning
 {
-std::shared_ptr<TrajOptIfoptProblem> DefaultTrajoptProblemGenerator(const std::string& name,
-                                                         const PlannerRequest& request,
-                                                         const TrajOptIfoptPlanProfileMap& plan_profiles,
-                                                         const TrajOptIfoptCompositeProfileMap& composite_profiles);
+std::shared_ptr<TrajOptIfoptProblem>
+DefaultTrajoptProblemGenerator(const std::string& name,
+                               const PlannerRequest& request,
+                               const TrajOptIfoptPlanProfileMap& plan_profiles,
+                               const TrajOptIfoptCompositeProfileMap& composite_profiles);
 
 }  // namespace tesseract_planning
 #endif

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h

@@ -45,8 +45,6 @@ class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile
   TrajOptIfoptDefaultCompositeProfile() = default;
   TrajOptIfoptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element);
 
-  /** @brief The type of contact test to perform: FIRST, CLOSEST, ALL */
-  tesseract_collision::ContactTestType contact_test_type = tesseract_collision::ContactTestType::ALL;
   /** @brief Configuration info for collisions that are modeled as costs */
   CollisionCostConfig collision_cost_config;
   /** @brief Configuration info for collisions that are modeled as constraints */
@@ -63,10 +61,6 @@ class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile
   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 = false;
-  /** @brief Optimization weight associated with kinematic singularity avoidance */
-  double avoid_singularity_coeff = 5.0;
 
   /** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states
    * to be considered valid in post checking of trajectory returned by TrajOptIfopt.
@@ -98,48 +92,6 @@ class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile
              const std::vector<int>& fixed_indices) override;
 
   tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
-
-protected:
-  void addCollisionCost(TrajOptIfoptProblem& problem,
-                        int start_index,
-                        int end_index,
-                        const std::vector<int>& fixed_indices) const;
-
-  void addCollisionConstraint(TrajOptIfoptProblem& problem,
-                              int start_index,
-                              int end_index,
-                              const std::vector<int>& fixed_indices) const;
-
-  void addVelocitySmoothing(TrajOptIfoptProblem& problem,
-                            int start_index,
-                            int end_index,
-                            const std::vector<int>& fixed_indices) const;
-
-  void addAccelerationSmoothing(TrajOptIfoptProblem& problem,
-                                int start_index,
-                                int end_index,
-                                const std::vector<int>& fixed_indices) const;
-
-  void addJerkSmoothing(TrajOptIfoptProblem& problem,
-                        int start_index,
-                        int end_index,
-                        const std::vector<int>& fixed_indices) const;
-
-  void addConstraintErrorFunctions(TrajOptIfoptProblem& problem,
-                                   int start_index,
-                                   int end_index,
-                                   const std::vector<int>& fixed_indices) const;
-
-  void addAvoidSingularity(TrajOptIfoptProblem& problem,
-                           int start_index,
-                           int end_index,
-                           const std::string& link,
-                           const std::vector<int>& fixed_indices) const;
-
-  void smoothMotionTerms(const tinyxml2::XMLElement& xml_element,
-                         bool& enabled,
-                         Eigen::VectorXd& coeff,
-                         std::size_t& length);
 };
 }  // namespace tesseract_planning
 

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h

@@ -38,12 +38,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_planning
 {
-  enum class TrajOptIfoptTermType
-  {
-    CONSTRAINT,
-    SQUARED_COST
-  };
-
 class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
 {
 public:
@@ -57,7 +51,6 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
   Eigen::VectorXd joint_coeff{ Eigen::VectorXd::Constant(1, 1, 5) };
   TrajOptIfoptTermType term_type{ TrajOptIfoptTermType::CONSTRAINT };
 
-
   void apply(TrajOptIfoptProblem& problem,
              const CartesianWaypoint& cartesian_waypoint,
              const Instruction& parent_instruction,
@@ -74,10 +67,11 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
 
   tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
 
-//protected:
-//  void addConstraintErrorFunctions(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps) const;
+  // protected:
+  //  void addConstraintErrorFunctions(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps)
+  //  const;
 
-//  void addAvoidSingularity(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps) const;s
+  //  void addAvoidSingularity(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps) const;s
 };
 }  // namespace tesseract_planning
 

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_collision_config.h

@@ -34,16 +34,20 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_common/utils.h>
+#include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h>
 
 namespace tesseract_planning
 {
 /**
- * @brief Config settings for collision cost terms.
+ * @brief Config settings for collision terms.
  */
-struct CollisionCostConfig
+struct CollisionConfig
 {
-  CollisionCostConfig() = default;
-  CollisionCostConfig(const tinyxml2::XMLElement& xml_element);
+  using Ptr = std::shared_ptr<CollisionConfig>;
+  using ConstPtr = std::shared_ptr<const CollisionConfig>;
+
+  CollisionConfig() = default;
+  CollisionConfig(const tinyxml2::XMLElement& xml_element);
 
   /** @brief If true, a collision cost term will be added to the problem. Default: true*/
   bool enabled = true;
@@ -52,6 +56,8 @@ struct CollisionCostConfig
    * If set to true, it is recommended to start with the coeff set to one
    */
   bool use_weighted_sum = false;
+  /** @brief The type of contact test to perform: FIRST, CLOSEST, ALL */
+  tesseract_collision::ContactTestType contact_test_type = tesseract_collision::ContactTestType::ALL;
   /** @brief The evaluator type that will be used for collision checking. */
   trajopt::CollisionEvaluatorType type = trajopt::CollisionEvaluatorType::CAST_CONTINUOUS;
 
@@ -66,31 +72,38 @@ struct CollisionCostConfig
   tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const;
 };
 
+/**
+ * @brief Config settings for collision cost terms.
+ */
+struct CollisionCostConfig : public CollisionConfig
+{
+  using Ptr = std::shared_ptr<CollisionCostConfig>;
+  using ConstPtr = std::shared_ptr<const CollisionCostConfig>;
+
+  CollisionCostConfig() = default;
+  CollisionCostConfig(const tinyxml2::XMLElement& xml_element);
+
+  tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const;
+
+protected:
+  TrajOptIfoptTermType term_type_ = TrajOptIfoptTermType::SQUARED_COST;
+};
+
 /**
  * @brief Config settings for collision constraint terms.
  */
-struct CollisionConstraintConfig
+struct CollisionConstraintConfig : public CollisionConfig
 {
+  using Ptr = std::shared_ptr<CollisionConstraintConfig>;
+  using ConstPtr = std::shared_ptr<const CollisionConstraintConfig>;
+
   CollisionConstraintConfig() = default;
   CollisionConstraintConfig(const tinyxml2::XMLElement& xml_element);
 
-  /** @brief If true, a collision cost term will be added to the problem. Default: true*/
-  bool enabled = true;
-  /**
-   * @brief Use the weighted sum for each link pair. This reduces the number equations added to the problem
-   * If set to true, it is recommended to start with the coeff set to one.
-   */
-  bool use_weighted_sum = false;
-  /** @brief The evaluator type that will be used for collision checking. */
-  trajopt::CollisionEvaluatorType type = trajopt::CollisionEvaluatorType::CAST_CONTINUOUS;
-  /** @brief Max distance in which collision constraints will be evaluated. */
-  double safety_margin = 0.01;
-  /** @brief Distance beyond safety_margin in which collision optimization will be evaluated. */
-  double safety_margin_buffer = 0.05;
-  /** @brief The collision coeff/weight */
-  double coeff = 20;
-
   tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const;
+
+protected:
+  TrajOptIfoptTermType term_type_ = TrajOptIfoptTermType::CONSTRAINT;
 };
 }  // namespace tesseract_planning
 

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h

@@ -38,10 +38,11 @@ namespace tesseract_planning
 {
 class TrajOptIfoptMotionPlannerStatusCategory;
 
-using TrajOptIfoptProblemGeneratorFn = std::function<std::shared_ptr<ifopt::Problem>(const std::string&,
-                                                                          const PlannerRequest&,
-                                                                          const TrajOptIfoptPlanProfileMap&,
-                                                                          const TrajOptIfoptCompositeProfileMap&)>;
+using TrajOptIfoptProblemGeneratorFn =
+    std::function<std::shared_ptr<ifopt::Problem>(const std::string&,
+                                                  const PlannerRequest&,
+                                                  const TrajOptIfoptPlanProfileMap&,
+                                                  const TrajOptIfoptCompositeProfileMap&)>;
 
 class TrajOptIfoptMotionPlanner : public MotionPlanner
 {
@@ -75,10 +76,10 @@ class TrajOptIfoptMotionPlanner : public MotionPlanner
   TrajOptIfoptPlanProfileMap plan_profiles;
 
   /** @brief Optimization parameters to be used (Optional) */
-//  sco::BasicTrustRegionSQPParameters params;
+  //  sco::BasicTrustRegionSQPParameters params;
 
   /** @brief Callback functions called on each iteration of the optimization (Optional) */
-//  std::vector<sco::Optimizer::Callback> callbacks;
+  //  std::vector<sco::Optimizer::Callback> callbacks;
 
   /**
    * @brief Sets up the opimizer and solves a SQP problem read from json with no callbacks and dafault parameterss
@@ -99,7 +100,8 @@ class TrajOptIfoptMotionPlanner : public MotionPlanner
   void clear() override;
 
 protected:
-  std::shared_ptr<const TrajOptIfoptMotionPlannerStatusCategory> status_category_; /** @brief The planners status codes */
+  std::shared_ptr<const TrajOptIfoptMotionPlannerStatusCategory> status_category_; /** @brief The planners status codes
+                                                                                    */
 };
 
 class TrajOptIfoptMotionPlannerStatusCategory : public tesseract_common::StatusCategory

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h

@@ -39,6 +39,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_planning
 {
+enum class TrajOptIfoptTermType
+{
+  CONSTRAINT,
+  SQUARED_COST
+};
+
 struct TrajOptIfoptProblem
 {
   // These are required for Tesseract to configure Descartes