IPDDP development (infeasible) (#81)

* Add control constraint class for IPDDP

* Refactor IPDDP forward pass and update filter point structure for log cost handling

* Infeasible IPDDP without regularization now completed

Author: astomodynamics

include/cddp-cpp/cddp_core/cddp_core.hpp

@@ -44,9 +44,9 @@ struct CDDPOptions {
 
     // Line search method
     int max_line_search_iterations = 11;            // Maximum iterations for line search
-    double backtracking_coeff = 1.0;                // Maximum step size for line search backtracking
-    double backtracking_min = 0.5;                  // Coefficient for line search backtracking
-    double backtracking_factor = std::pow(10, (-3.0/10.0));   // Factor for line search backtracking
+    double backtracking_coeff = 1.0;                // Coefficient for line search backtracking
+    double backtracking_min = 1e-7;                  // Minimum step size for line search
+    double backtracking_factor = 0.5;   // Factor for line search backtracking
     double minimum_reduction_ratio = 1e-6;          // Minimum reduction for line search
 
     // interior-point method
@@ -121,6 +121,7 @@ struct ForwardPassResult {
     std::vector<Eigen::VectorXd> control_sequence;
     std::map<std::string, std::vector<Eigen::VectorXd>> dual_sequence;
     std::map<std::string, std::vector<Eigen::VectorXd>> slack_sequence;
+    std::map<std::string, std::vector<Eigen::VectorXd>>  constraint_sequence;
     double cost;
     double lagrangian;
     double alpha = 1.0;
@@ -129,10 +130,17 @@ struct ForwardPassResult {
 };
 
 struct FilterPoint {
-    double cost;
+    double log_cost;
     double violation;
+    
+    // Default constructor
+    FilterPoint() : log_cost(0.0), violation(0.0) {}
+
+    // Constructor with parameters
+    FilterPoint(double lc, double v) : log_cost(lc), violation(v) {}
+
     bool dominates(const FilterPoint& other) const {
-        return cost <= other.cost && violation <= other.violation;
+        return log_cost <= other.log_cost && violation <= other.violation;
     }
 };
 
@@ -304,6 +312,9 @@ class CDDP {
     CDDPSolution solveFeasibleIPDDP();
     ForwardPassResult solveFeasibleIPDDPForwardPass(double alpha);
     bool solveFeasibleIPDDPBackwardPass();
+    void resetIPDDPFilter();
+    void initialIPDDPRollout();
+    void resetIPDDPRegularization();
 
     // Helper methods
     double computeConstraintViolation(const std::vector<Eigen::VectorXd>& X, const std::vector<Eigen::VectorXd>& U) const;
@@ -343,6 +354,7 @@ class CDDP {
     // Intermediate trajectories
     std::vector<Eigen::VectorXd> X_;                            // State trajectory
     std::vector<Eigen::VectorXd> U_;                            // Control trajectory
+    std::map<std::string, std::vector<Eigen::VectorXd>> G_;    // Constraint trajectory
     std::map<std::string, std::vector<Eigen::VectorXd>> Y_;  // Dual trajectory
     std::map<std::string, std::vector<Eigen::VectorXd>> S_; // Slack trajectory 
 
@@ -359,6 +371,8 @@ class CDDP {
 
     // Log-barrier
     double mu_; // Barrier coefficient
+    std::vector<FilterPoint> filter_; // [logcost, error measure
+    int ipddp_regularization_counter_ = 0; // Regularization counter for IPDDP
     double constraint_violation_; // Current constraint violation measure
     double gamma_; // Small value for filter acceptance
 

include/cddp-cpp/cddp_core/constraint.hpp

@@ -257,6 +257,71 @@ class LinearConstraint : public Constraint {
     double scale_factor_;
 };
 
+class ControlConstraint : public Constraint {
+public:
+    ControlConstraint(const Eigen::VectorXd& upper_bound, 
+                      double scale_factor = 1.0)
+        : Constraint("ControlConstraint"),
+          scale_factor_(scale_factor) {
+            // Rescale the upper bound for this constraint class
+            upper_bound_.resize(2*upper_bound.size());
+            upper_bound_.head(upper_bound.size()) = upper_bound * scale_factor_;
+            upper_bound_.tail(upper_bound.size()) = upper_bound * scale_factor_;
+            dim_ = 2*upper_bound.size();
+          }
+
+    int getDualDim() const override {
+        return dim_;
+    }
+
+    Eigen::VectorXd evaluate(const Eigen::VectorXd& /**/, 
+                             const Eigen::VectorXd& control) const override 
+    {
+        // return [-control; control];
+        Eigen::VectorXd g(2*control.size());
+        g.head(control.size()) = -control;
+        g.tail(control.size()) = control;
+        return g * scale_factor_;
+    }
+
+    Eigen::VectorXd getLowerBound() const override {
+        return Eigen::VectorXd::Constant(upper_bound_.size(), -std::numeric_limits<double>::infinity());
+    }
+
+    Eigen::VectorXd getUpperBound() const override {
+        return upper_bound_;
+    }
+
+    Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, 
+                                     const Eigen::VectorXd& control) const override 
+    {
+        return Eigen::MatrixXd::Zero(dim_, state.size());
+    }
+
+    Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, 
+                                       const Eigen::VectorXd& control) const override 
+    {
+        Eigen::MatrixXd jac(2*control.size(), control.size());
+        jac.topLeftCorner(control.size(), control.size()) = -Eigen::MatrixXd::Identity(control.size(), control.size());
+        jac.bottomRightCorner(control.size(), control.size()) = Eigen::MatrixXd::Identity(control.size(), control.size());
+        return jac;
+    }
+
+    double computeViolation(const Eigen::VectorXd& state, 
+                            const Eigen::VectorXd& control) const override 
+    {
+        Eigen::VectorXd g = evaluate(state, control) - upper_bound_;
+        return computeViolationFromValue(g);
+    }
+
+    double computeViolationFromValue(const Eigen::VectorXd& g) const override {
+        return (g - upper_bound_).cwiseMax(0.0).sum();
+    }
+private:    
+    int dim_;
+    Eigen::VectorXd upper_bound_;
+    double scale_factor_;
+};
 
 class BallConstraint : public Constraint {
 public:

results/tests/dubincs_car_ipddp_test.png

@@ -498,6 +498,11 @@ bool CDDP::solveASCDDPBackwardPass()
         optimality_gap_ = Qu_error;
     }
 
+    if (options_.debug) {
+        std::cout << "Qu_error: " << Qu_error << std::endl;
+        std::cout << "dV: " << dV_.transpose() << std::endl;
+    }
+
     return true;
 }