Add dynamic constraint support

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

@@ -32,6 +32,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <trajopt_ifopt/core/constraint_set.h>
+#include <trajopt_ifopt/core/dynamic_constraint_set.h>
 
 namespace trajopt_ifopt
 {
@@ -118,5 +119,74 @@ class DiscreteCollisionConstraint : public ConstraintSet
   void init() const;
 };
 
+class DiscreteCollisionConstraintD : public DynamicConstraintSet
+{
+public:
+  using Ptr = std::shared_ptr<DiscreteCollisionConstraint>;
+  using ConstPtr = std::shared_ptr<const DiscreteCollisionConstraint>;
+
+  DiscreteCollisionConstraintD(std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator,
+                               std::shared_ptr<const Var> position_var,
+                               std::string name = "DiscreteCollision");
+
+  int Update() override;
+
+  /**
+   * @brief Returns the values associated with the constraint.
+   * @warning Make sure that the values returns are not just the violation but the constraint values.
+   * Remember the values are the constant in the quadratic function, so if you only return the
+   * violation then if it is not violating the constraint this would be zero which means it
+   * will always appear to be on the constraint boundary which will cause issue when solving.
+   * If it is not voliating the constraint then return the max negative number.
+   * If no contacts are found return the negative of the collision margin buffer. This is why
+   * it is important to not set the collision margin buffer to zero.
+   * @return The constraint values not the violations
+   */
+  Eigen::VectorXd GetValues() const override;
+
+  /**
+   * @brief  Returns the "bounds" of this constraint. How these are enforced is up to the solver
+   * @return Returns the "bounds" of this constraint
+   */
+  std::vector<Bounds> GetBounds() const override;
+
+  /**
+   * @brief Fills the jacobian block associated with the given var_set.
+   * @param var_set Name of the var_set to which the jac_block is associated
+   * @param jac_block Block of the overall jacobian associated with these constraints and the var_set variable
+   */
+  void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const override;
+
+  /**
+   * @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<Bounds>& bounds);
+
+  /**
+   * @brief Get the collision evaluator. This exposed for plotter callbacks
+   * @return The collision evaluator
+   */
+  std::shared_ptr<DiscreteCollisionEvaluator> GetCollisionEvaluator() const;
+
+private:
+  /** @brief The number of joints in a single JointPosition */
+  long n_dof_{ 0 };
+
+  /** @brief Bounds on the constraint value. Default: std::vector<Bounds>(1, ifopt::BoundSmallerZero) */
+  std::vector<Bounds> bounds_;
+
+  /** @brief Pointers to the vars used by this constraint. */
+  std::shared_ptr<const Var> position_var_;
+
+  std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator_;
+
+  /** @brief Used to initialize */
+  mutable std::string var_set_name_;
+  mutable std::once_flag init_flag_;
+
+  void init() const;
+};
+
 }  // namespace trajopt_ifopt
 #endif

trajopt_ifopt/include/trajopt_ifopt/core/composite.h

@@ -38,11 +38,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <Eigen/Sparse>
 
 #include <trajopt_ifopt/core/bounds.h>
+#include <trajopt_ifopt/core/eigen_types.h>
 
 namespace trajopt_ifopt
 {
-using Jacobian = Eigen::SparseMatrix<double, Eigen::RowMajor>;
-
 /**
  * @brief Interface representing either Variable, Cost or Constraint.
  *
@@ -219,6 +218,9 @@ class Composite : public Component
    */
   const ComponentVec& GetComponents() const;
 
+  /** @brief If no components have been added */
+  bool Empty() const;
+
 private:
   ComponentVec components_;
   bool is_cost_{ false };

trajopt_ifopt/include/trajopt_ifopt/core/eigen_types.h

@@ -0,0 +1,40 @@
+/******************************************************************************
+Copyright (c) 2017, Alexander W Winkler. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation
+and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+contributors may be used to endorse or promote products derived from
+this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#ifndef TRAJOPT_IFOPT_CORE_EIGEN_TYPES_H
+#define TRAJOPT_IFOPT_CORE_EIGEN_TYPES_H
+
+#include <Eigen/Eigen>
+
+namespace trajopt_ifopt
+{
+using Jacobian = Eigen::SparseMatrix<double, Eigen::RowMajor>;
+}  // namespace trajopt_ifopt
+
+#endif  // TRAJOPT_IFOPT_CORE_EIGEN_TYPES_H

trajopt_ifopt/include/trajopt_ifopt/fwd.h

@@ -33,6 +33,9 @@ class NodesVariables;
 class Node;
 class Var;
 
+// Dynamic Constraint Set
+class DynamicConstraintSet;
+
 // cartesian_line_constraint.h
 struct CartLineInfo;
 class CartLineConstraint;

trajopt_ifopt/include/trajopt_ifopt/utils/ifopt_utils.h

@@ -24,15 +24,10 @@
 #ifndef TRAJOPT_IFOPT_IFOPT_UTILS_H
 #define TRAJOPT_IFOPT_IFOPT_UTILS_H
 
-#include <trajopt_common/macros.h>
-TRAJOPT_IGNORE_WARNINGS_PUSH
-#include <Eigen/Eigen>
-TRAJOPT_IGNORE_WARNINGS_POP
+#include <trajopt_ifopt/core/eigen_types.h>
 
 namespace trajopt_ifopt
 {
-using Jacobian = Eigen::SparseMatrix<double, Eigen::RowMajor>;
-
 struct Bounds;
 class Component;
 class ConstraintSet;

trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp

@@ -390,7 +390,7 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen::V
       data->gradient_results_sets.push_back(std::move(kv.second));
   }
 
-  if (data->gradient_results_sets.size() > bounds_size)
+  if (data->gradient_results_sets.size() > bounds_size && bounds_size != 0)
   {
     auto cmp = [vars0_fixed, vars1_fixed](const trajopt_common::GradientResultsSet& a,
                                           const trajopt_common::GradientResultsSet& b) {

trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp

@@ -195,4 +195,111 @@ std::shared_ptr<DiscreteCollisionEvaluator> DiscreteCollisionConstraint::GetColl
   return collision_evaluator_;
 }
 
+DiscreteCollisionConstraintD::DiscreteCollisionConstraintD(
+    std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator,
+    std::shared_ptr<const Var> position_var,
+    std::string name)
+  : DynamicConstraintSet(std::move(name))
+  , position_var_(std::move(position_var))
+  , collision_evaluator_(std::move(collision_evaluator))
+{
+  n_dof_ = position_var_->size();
+  assert(n_dof_ > 0);
+}
+
+int DiscreteCollisionConstraintD::Update()
+{
+  int rows{ 0 };
+  const trajopt_common::CollisionCacheData::ConstPtr collision_data =
+      collision_evaluator_->CalcCollisions(position_var_->value(), 0);
+  for (const auto& gradient_results_set : collision_data->gradient_results_sets)
+    rows += static_cast<int>(gradient_results_set.results.size());
+
+  SetRows(rows);
+  bounds_ = std::vector<Bounds>(static_cast<std::size_t>(rows), BoundSmallerZero);
+
+  std::call_once(init_flag_, &DiscreteCollisionConstraintD::init, this);
+
+  return rows;
+}
+
+Eigen::VectorXd DiscreteCollisionConstraintD::GetValues() const
+{
+  const trajopt_common::CollisionCacheData::ConstPtr collision_data =
+      collision_evaluator_->CalcCollisions(position_var_->value(), 0);
+
+  std::vector<double> values;
+  for (const auto& gradient_results_set : collision_data->gradient_results_sets)
+  {
+    for (const auto& result : gradient_results_set.results)
+      values.emplace_back(result.error * gradient_results_set.coeff);
+  }
+
+  return Eigen::Map<Eigen::VectorXd>(values.data(), static_cast<Eigen::Index>(values.size()));
+}
+
+// Set the limits on the constraint values
+std::vector<Bounds> DiscreteCollisionConstraintD::GetBounds() const { return bounds_; }
+
+void DiscreteCollisionConstraintD::FillJacobianBlock(std::string var_set, Jacobian& jac_block) const
+{
+  // Only modify the jacobian if this constraint uses var_set
+  if (var_set != var_set_name_ || GetRows() == 0)  // NOLINT
+    return;
+
+  Eigen::Index i{ 0 };
+
+  const trajopt_common::CollisionCacheData::ConstPtr collision_data =
+      collision_evaluator_->CalcCollisions(position_var_->value(), 0);
+
+  if (collision_data->gradient_results_sets.empty())
+    return;
+
+  for (const auto& gradient_results_set : collision_data->gradient_results_sets)
+  {
+    for (const auto& result : gradient_results_set.results)
+    {
+      assert(result.gradients[0].has_gradient || result.gradients[1].has_gradient);
+      if (result.gradients[0].has_gradient && result.gradients[1].has_gradient)
+      {
+        // This does work but could be faster
+        for (int j = 0; j < n_dof_; j++)
+          jac_block.coeffRef(i, position_var_->getIndex() + j) =
+              -1.0 * (result.gradients[0].gradient[j] + result.gradients[1].gradient[j]);
+      }
+      else if (result.gradients[0].has_gradient)
+      {
+        // This does work but could be faster
+        for (int j = 0; j < n_dof_; j++)
+          jac_block.coeffRef(i, position_var_->getIndex() + j) = -1.0 * result.gradients[0].gradient[j];
+      }
+      else if (result.gradients[1].has_gradient)
+      {
+        // This does work but could be faster
+        for (int j = 0; j < n_dof_; j++)
+          jac_block.coeffRef(i, position_var_->getIndex() + j) = -1.0 * result.gradients[1].gradient[j];
+      }
+      ++i;
+    }
+  }
+  assert(GetRows() == (i - 1));
+}
+
+void DiscreteCollisionConstraintD::SetBounds(const std::vector<Bounds>& bounds)
+{
+  assert(bounds.size() == GetRows());
+  bounds_ = bounds;
+}
+
+void DiscreteCollisionConstraintD::init() const
+{
+  const auto* parent_set = position_var_->getParent()->getParent();
+  var_set_name_ = parent_set->GetName();
+}
+
+std::shared_ptr<DiscreteCollisionEvaluator> DiscreteCollisionConstraintD::GetCollisionEvaluator() const
+{
+  return collision_evaluator_;
+}
+
 }  // namespace trajopt_ifopt

trajopt_ifopt/src/core/composite.cpp

@@ -197,6 +197,8 @@ std::vector<Bounds> Composite::GetBounds() const
 
 const Composite::ComponentVec& Composite::GetComponents() const { return components_; }
 
+bool Composite::Empty() const { return components_.empty(); }
+
 void Composite::PrintAll() const
 {
   int index = 0;