Optimize costs and constraints

trajopt_common/src/collision_types.cpp

@@ -41,7 +41,7 @@ double CollisionCoeffData::getDefaultCollisionCoeff() const { return default_col
 void CollisionCoeffData::setCollisionCoeff(const std::string& obj1, const std::string& obj2, double collision_coeff)
 {
   auto key = tesseract_common::makeOrderedLinkPair(obj1, obj2);
-  lookup_table_[key] = collision_coeff;
+  lookup_table_.insert_or_assign(key, collision_coeff);
 
   if (tesseract_common::almostEqualRelativeAndAbs(collision_coeff, 0.0))
     zero_coeff_.insert(key);
@@ -72,9 +72,9 @@ const std::set<tesseract_common::LinkNamesPair>& CollisionCoeffData::getPairsWit
 
 bool CollisionCoeffData::operator==(const CollisionCoeffData& rhs) const
 {
-  static auto max_diff = static_cast<double>(std::numeric_limits<float>::epsilon());
+  static constexpr auto max_diff = static_cast<double>(std::numeric_limits<float>::epsilon());
 
-  auto value_eq = [](const double& v1, const double& v2) {
+  static const auto value_eq = [](double v1, double v2) {
     return tesseract_common::almostEqualRelativeAndAbs(v1, v2, max_diff);
   };
 
@@ -147,35 +147,28 @@ double LinkMaxError::getMaxErrorWithBuffer() const
 
 void GradientResultsSet::add(const GradientResults& gradient_result)
 {
-  // Update max error for LinkA and LinkB excluding values at T1
   for (std::size_t i = 0; i < 2; ++i)
   {
-    if (gradient_result.gradients[i].has_gradient &&
-        gradient_result.gradients[i].cc_type != tesseract_collision::ContinuousCollisionType::CCType_Time1)
+    const auto& g = gradient_result.gradients[i];
+    if (!g.has_gradient)
+      continue;
+
+    // Update max error for LinkA and LinkB excluding values at T1
+    if (g.cc_type != tesseract_collision::ContinuousCollisionType::CCType_Time1)
     {
       max_error[i].has_error[0] = true;
-
-      if (gradient_result.error > max_error[i].error[0])
-        max_error[i].error[0] = gradient_result.error;
-
-      if (gradient_result.error_with_buffer > max_error[i].error_with_buffer[0])
-        max_error[i].error_with_buffer[0] = gradient_result.error_with_buffer;
+      max_error[i].error[0] = std::max(max_error[i].error[0], gradient_result.error);
+      max_error[i].error_with_buffer[0] =
+          std::max(max_error[i].error_with_buffer[0], gradient_result.error_with_buffer);
     }
-  }
 
-  // Update max error for LinkA and LinkB excluding values at T0
-  for (std::size_t i = 0; i < 2; ++i)
-  {
-    if (gradient_result.gradients[i].has_gradient &&
-        gradient_result.gradients[i].cc_type != tesseract_collision::ContinuousCollisionType::CCType_Time0)
+    // Update max error for LinkA and LinkB excluding values at T0
+    if (g.cc_type != tesseract_collision::ContinuousCollisionType::CCType_Time0)
     {
       max_error[i].has_error[1] = true;
-
-      if (gradient_result.error > max_error[i].error[1])
-        max_error[i].error[1] = gradient_result.error;
-
-      if (gradient_result.error_with_buffer > max_error[i].error_with_buffer[1])
-        max_error[i].error_with_buffer[1] = gradient_result.error_with_buffer;
+      max_error[i].error[1] = std::max(max_error[i].error[1], gradient_result.error);
+      max_error[i].error_with_buffer[1] =
+          std::max(max_error[i].error_with_buffer[1], gradient_result.error_with_buffer);
     }
   }
 

trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h

@@ -60,6 +60,14 @@ class ContinuousCollisionConstraint : public ifopt::ConstraintSet
                                 bool fixed_sparsity = false,
                                 const std::string& name = "LVSCollision");
 
+  ContinuousCollisionConstraint(std::shared_ptr<ContinuousCollisionEvaluator> collision_evaluator,
+                                std::vector<std::shared_ptr<const Var>> position_vars,
+                                bool vars0_fixed,
+                                bool vars1_fixed,
+                                int max_num_cnt = 1,
+                                bool fixed_sparsity = false,
+                                const std::string& name = "LVSCollision");
+
   /**
    * @brief Returns the values associated with the constraint.
    * @warning Make sure that the values returns are not just the violation but the constraint values.
@@ -101,22 +109,26 @@ class ContinuousCollisionConstraint : public ifopt::ConstraintSet
   /** @brief The number of joints in a single JointPosition */
   long n_dof_;
 
+  /** @brief The per-position_var max constraints */
+  std::size_t max_num_cnt_per_segment_{ 0 };
+
   /** @brief Bounds on the constraint value. Default: std::vector<Bounds>(1, ifopt::BoundSmallerZero) */
   std::vector<ifopt::Bounds> bounds_;
 
   /** @brief Pointers to the vars used by this constraint. */
-  std::array<std::shared_ptr<const Var>, 2> position_vars_;
+  std::vector<std::shared_ptr<const Var>> position_vars_;
   bool vars0_fixed_{ false };
   bool vars1_fixed_{ false };
 
   std::shared_ptr<ContinuousCollisionEvaluator> collision_evaluator_;
 
   /** @brief Used to initialize jacobian because snopt sparsity cannot change */
   bool fixed_sparsity_{ false };
+  mutable std::string var_set_name_;
   mutable std::once_flag init_flag_;
   mutable std::vector<Eigen::Triplet<double>> triplet_list_;
 
-  void initSparsity() const;
+  void init() const;
 };
 }  // namespace trajopt_ifopt
 

trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h

@@ -60,10 +60,10 @@ class ContinuousCollisionEvaluator
 
   ContinuousCollisionEvaluator() = default;
   virtual ~ContinuousCollisionEvaluator() = default;
-  ContinuousCollisionEvaluator(const ContinuousCollisionEvaluator&) = default;
-  ContinuousCollisionEvaluator& operator=(const ContinuousCollisionEvaluator&) = default;
-  ContinuousCollisionEvaluator(ContinuousCollisionEvaluator&&) = default;
-  ContinuousCollisionEvaluator& operator=(ContinuousCollisionEvaluator&&) = default;
+  ContinuousCollisionEvaluator(const ContinuousCollisionEvaluator&) = delete;
+  ContinuousCollisionEvaluator& operator=(const ContinuousCollisionEvaluator&) = delete;
+  ContinuousCollisionEvaluator(ContinuousCollisionEvaluator&&) = delete;
+  ContinuousCollisionEvaluator& operator=(ContinuousCollisionEvaluator&&) = delete;
 
   /**
    * @brief Given joint names and values calculate the collision results for this evaluator

trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h

@@ -47,7 +47,13 @@ class DiscreteCollisionConstraint : public ifopt::ConstraintSet
                               std::shared_ptr<const Var> position_var,
                               int max_num_cnt = 1,
                               bool fixed_sparsity = false,
-                              const std::string& name = "DiscreteCollisionV3");
+                              const std::string& name = "DiscreteCollision");
+
+  DiscreteCollisionConstraint(std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator,
+                              std::vector<std::shared_ptr<const Var>> position_vars,
+                              int max_num_cnt = 1,
+                              bool fixed_sparsity = false,
+                              const std::string& name = "DiscreteCollision");
 
   /**
    * @brief Returns the values associated with the constraint.
@@ -75,21 +81,12 @@ class DiscreteCollisionConstraint : public ifopt::ConstraintSet
    */
   void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const override;
 
-  /** @brief Calculates the values associated with the constraint */
-  Eigen::VectorXd CalcValues(const Eigen::Ref<const Eigen::VectorXd>& joint_vals) const;
-
   /**
    * @brief Sets the bounds on the collision distance
    * @param bounds New bounds that will be set. Should be size 1
    */
   void SetBounds(const std::vector<ifopt::Bounds>& bounds);
 
-  /**
-   * @brief Fills the jacobian block associated with the constraint
-   * @param jac_block Block of the overall jacobian associated with these constraints
-   */
-  void CalcJacobianBlock(const Eigen::Ref<const Eigen::VectorXd>& joint_vals, Jacobian& jac_block) const;
-
   /**
    * @brief Get the collision evaluator. This exposed for plotter callbacks
    * @return The collision evaluator
@@ -98,22 +95,26 @@ class DiscreteCollisionConstraint : public ifopt::ConstraintSet
 
 private:
   /** @brief The number of joints in a single JointPosition */
-  long n_dof_;
+  long n_dof_{ 0 };
+
+  /** @brief The per-position_var max constraints */
+  std::size_t max_num_cnt_per_var_{ 0 };
 
   /** @brief Bounds on the constraint value. Default: std::vector<Bounds>(1, ifopt::BoundSmallerZero) */
   std::vector<ifopt::Bounds> bounds_;
 
   /** @brief Pointers to the vars used by this constraint. */
-  std::shared_ptr<const Var> position_var_;
+  std::vector<std::shared_ptr<const Var>> position_vars_;
 
   std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator_;
 
   /** @brief Used to initialize jacobian because snopt sparsity cannot change */
   bool fixed_sparsity_{ false };
+  mutable std::string var_set_name_;
   mutable std::once_flag init_flag_;
   mutable std::vector<Eigen::Triplet<double>> triplet_list_;
 
-  void initSparsity() const;
+  void init() const;
 };
 
 }  // namespace trajopt_ifopt

trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h

@@ -59,10 +59,10 @@ class DiscreteCollisionEvaluator
 
   DiscreteCollisionEvaluator() = default;
   virtual ~DiscreteCollisionEvaluator() = default;
-  DiscreteCollisionEvaluator(const DiscreteCollisionEvaluator&) = default;
-  DiscreteCollisionEvaluator& operator=(const DiscreteCollisionEvaluator&) = default;
-  DiscreteCollisionEvaluator(DiscreteCollisionEvaluator&&) = default;
-  DiscreteCollisionEvaluator& operator=(DiscreteCollisionEvaluator&&) = default;
+  DiscreteCollisionEvaluator(const DiscreteCollisionEvaluator&) = delete;
+  DiscreteCollisionEvaluator& operator=(const DiscreteCollisionEvaluator&) = delete;
+  DiscreteCollisionEvaluator(DiscreteCollisionEvaluator&&) = delete;
+  DiscreteCollisionEvaluator& operator=(DiscreteCollisionEvaluator&&) = delete;
 
   /**
    * @brief Given joint names and values calculate the collision results for this evaluator