Update collision and cartesian constraints to use new variable set

Author: Levi-Armstrong

trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h

@@ -36,6 +36,7 @@ TRAJOPT_IGNORE_WARNINGS_POP
 namespace trajopt_ifopt
 {
 class JointPosition;
+class Var;
 
 /** @brief Contains Cartesian pose constraint information */
 struct CartPosInfo
@@ -202,5 +203,124 @@ class CartPosConstraint : public ifopt::ConstraintSet
 
   static thread_local tesseract_common::TransformMap transforms_cache;  // NOLINT
 };
+
+class CartPosConstraint2 : public ifopt::ConstraintSet
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  using Ptr = std::shared_ptr<CartPosConstraint>;
+  using ConstPtr = std::shared_ptr<const CartPosConstraint>;
+  using ErrorFunctionType = std::function<Eigen::VectorXd(const Eigen::Isometry3d&, const Eigen::Isometry3d&)>;
+  using ErrorDiffFunctionType = std::function<Eigen::VectorXd(const Eigen::VectorXd&,
+                                                              const Eigen::Isometry3d&,
+                                                              const Eigen::Isometry3d&,
+                                                              tesseract_common::TransformMap&)>;
+
+  CartPosConstraint2(const CartPosInfo& info,
+                     std::shared_ptr<const Var> position_var,
+                     const std::string& name = "CartPos");
+
+  CartPosConstraint2(CartPosInfo info,
+                     std::shared_ptr<const Var> position_var,
+                     const Eigen::VectorXd& coeffs,
+                     const std::string& name = "CartPos");
+
+  /**
+   * @brief CalcValues Calculates the values associated with the constraint
+   * @param joint_vals Input joint values for which FK is solved
+   * @return Error of FK solution from target, size 6. The first 3 terms are associated with position and the last 3 are
+   * associated with orientation.
+   */
+  Eigen::VectorXd CalcValues(const Eigen::Ref<const Eigen::VectorXd>& joint_vals) const;
+  /**
+   * @brief Returns the values associated with the constraint. In this case that is the concatenated joint values
+   * associated with each of the joint positions should be n_dof_ * n_vars_ long
+   * @return
+   */
+  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<ifopt::Bounds> GetBounds() const override;
+
+  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 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 Gets the Cartesian Pose info used to create this constraint
+   * @return The Cartesian Pose info used to create this constraint
+   */
+  const CartPosInfo& GetInfo() const;
+  CartPosInfo& GetInfo();
+
+  /**
+   * @brief Set the target pose
+   * @param pose
+   */
+  void SetTargetPose(const Eigen::Isometry3d& target_frame_offset);
+
+  /**
+   * @brief Returns the target pose for the constraint
+   * @return The target pose for the constraint
+   */
+  Eigen::Isometry3d GetTargetPose() const;
+
+  /**
+   * @brief Returns the current TCP pose in world frame given the input kinematic info and the current variable values
+   * @return The current TCP pose given the input kinematic info and the current variable values
+   */
+  Eigen::Isometry3d GetCurrentPose() const;
+
+  /** @brief If true, numeric differentiation will be used. Default: true
+   *
+   * Note: While the logic for using the jacobian from KDL will be used if set to false, this has been buggy. Set this
+   * to false at your own risk.
+   */
+  bool use_numeric_differentiation{ true };
+
+private:
+  /** @brief The var_set associated with this constraint */
+  std::string var_set_;
+
+  /** @brief The number of joints in a single JointPosition */
+  long n_dof_;
+
+  /** @brief The constraint coefficients */
+  Eigen::VectorXd coeffs_;
+
+  /** @brief Bounds on the positions of each joint */
+  std::vector<ifopt::Bounds> bounds_;
+
+  /**
+   * @brief Pointers to the vars used by this constraint.
+   * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()
+   */
+  std::shared_ptr<const Var> position_var_;
+
+  /** @brief The kinematic information used when calculating error */
+  CartPosInfo info_;
+
+  /** @brief Error function for calculating the error in the position given the source and target positions */
+  ErrorFunctionType error_function_{ nullptr };
+
+  /** @brief The error function to calculate the error difference used for jacobian calculations */
+  ErrorDiffFunctionType error_diff_function_{ nullptr };
+
+  static thread_local tesseract_common::TransformMap transforms_cache;  // NOLINT
+};
 }  // namespace trajopt_ifopt
 #endif

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

@@ -34,6 +34,7 @@ TRAJOPT_IGNORE_WARNINGS_POP
 namespace trajopt_ifopt
 {
 class JointPosition;
+class Var;
 class ContinuousCollisionEvaluator;
 
 class ContinuousCollisionConstraint : public ifopt::ConstraintSet
@@ -116,6 +117,90 @@ class ContinuousCollisionConstraint : public ifopt::ConstraintSet
 
   std::shared_ptr<ContinuousCollisionEvaluator> collision_evaluator_;
 };
+
+class ContinuousCollisionConstraint2 : public ifopt::ConstraintSet
+{
+public:
+  using Ptr = std::shared_ptr<ContinuousCollisionConstraint>;
+  using ConstPtr = std::shared_ptr<const ContinuousCollisionConstraint>;
+
+  /**
+   * @brief ContinuousCollisionConstraintIfopt
+   * @param collision_evaluator The continuous collision evaluator to use
+   * @param position_vars The position vars associated with this constraint.
+   * @param position_vars_fixed Indicate if the position var is fixed
+   * @param max_num_cnt The max number of constraits to include
+   * @param fixed_sparsity This is mostly need for snopt which requires sparsity to not change
+   * @param name
+   */
+  ContinuousCollisionConstraint2(std::shared_ptr<ContinuousCollisionEvaluator> collision_evaluator,
+                                 std::array<std::shared_ptr<const Var>, 2> 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.
+   * 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<ifopt::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<ifopt::Bounds>& bounds);
+
+  /**
+   * @brief Get the collision evaluator. This exposed for plotter callbacks
+   * @return The collision evaluator
+   */
+  std::shared_ptr<ContinuousCollisionEvaluator> GetCollisionEvaluator() const;
+
+private:
+  /** @brief The var_set associated with this constraint */
+  std::string var_set_;
+
+  /** @brief The number of joints in a single JointPosition */
+  long n_dof_;
+
+  /** @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.
+   * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()
+   */
+  std::array<std::shared_ptr<const Var>, 2> position_vars_;
+  bool vars0_fixed_{ false };
+  bool vars1_fixed_{ false };
+
+  /** @brief Used to initialize jacobian because snopt sparsity cannot change */
+  std::vector<Eigen::Triplet<double>> triplet_list_;
+
+  std::shared_ptr<ContinuousCollisionEvaluator> collision_evaluator_;
+};
 }  // namespace trajopt_ifopt
 
 #endif  // TRAJOPT_IFOPT_CONTINUOUS_COLLISION_CONSTRAINT_H

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

@@ -34,6 +34,7 @@ TRAJOPT_IGNORE_WARNINGS_POP
 namespace trajopt_ifopt
 {
 class JointPosition;
+class Var;
 class DiscreteCollisionEvaluator;
 
 class DiscreteCollisionConstraint : public ifopt::ConstraintSet
@@ -113,5 +114,87 @@ class DiscreteCollisionConstraint : public ifopt::ConstraintSet
 
   std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator_;
 };
+
+class DiscreteCollisionConstraint2 : public ifopt::ConstraintSet
+{
+public:
+  using Ptr = std::shared_ptr<DiscreteCollisionConstraint>;
+  using ConstPtr = std::shared_ptr<const DiscreteCollisionConstraint>;
+
+  DiscreteCollisionConstraint2(std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator,
+                               std::shared_ptr<const Var> position_var,
+                               int max_num_cnt = 1,
+                               bool fixed_sparsity = false,
+                               const std::string& name = "DiscreteCollisionV3");
+
+  /**
+   * @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<ifopt::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 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
+   */
+  std::shared_ptr<DiscreteCollisionEvaluator> GetCollisionEvaluator() const;
+
+private:
+  /** @brief The var_set associated with this constraint */
+  std::string var_set_;
+
+  /** @brief The number of joints in a single JointPosition */
+  long n_dof_;
+
+  /** @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.
+   * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()
+   */
+  std::shared_ptr<const Var> position_var_;
+
+  /** @brief Used to initialize jacobian because snopt sparsity cannot change */
+  std::vector<Eigen::Triplet<double>> triplet_list_;
+
+  std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator_;
+};
+
 }  // namespace trajopt_ifopt
 #endif