Change verbose output

Author: vagtsiats

bindings/python/examples/floating_frame/pendulum_traj_opt.py

@@ -107,7 +107,7 @@
 
 
     minu = min_var.create(f"minu{i}", ocp.stage(i).u, ocp.stage(i).du)
-    minu.setWeight(1e-6 * np.eye(model.nv))
+    minu.setWeight(1e-9 * np.eye(model.nv))
     minus.append(minu)
 
 
@@ -118,7 +118,7 @@
 
     # tau_min
     tau_lim = DynamicsConstraint(ocp.stage(i).model, ocp.stage(i).dx, ocp.stage(i).du)
-    tau_lim.setTorqueLimit([10., 10.])
+    tau_lim.setTorqueLimit([10., 1e-6])
     const.append(tau_lim)
     ocp.stage(i).stack << tau_lim
 
@@ -136,10 +136,11 @@
 solver = swSQP(ocp)
 solver.getOptions().max_iters = 1000
 solver.getOptions().verbose = True
-solver.getOptions().use_line_search = True
-solver.getOptions().beta = 1e-2
+solver.getOptions().line_search_strategy = 1
+solver.getOptions().beta = 1e-4
+solver.getOptions().min_abs_delta_solution = 1e-2
+solver.init()
 print(f"{solver.getOptions().print()}")
-solver.getOptions().min_abs_delta_solution = 1e-3
 print("...solver inited!")
 
 

include/OpenSoT/solvers/swSQP.h

@@ -16,6 +16,7 @@ class swSQP{
     struct stage_statistics
     {
         double cost;
+        double constraint_violation;
     };
 
     struct statistics
@@ -28,6 +29,7 @@ class swSQP{
         std::vector<stage_statistics> stages_statistics;
         int iters;
         double cost;
+        double constraint_violation;
         double alpha;
         int line_search_iters;
         bool line_search_accepted;
@@ -43,28 +45,34 @@ class swSQP{
             _oss<<"=== swSQP Statistics ==="<<std::endl;
             _oss << "  iter              : " << iters << std::endl;
             _oss << "  cost              : " << cost << std::endl;
+            _oss << "  sum-constr-viol   : " << constraint_violation << std::endl;
             _oss << "  iter time         : " << iter_time << std::endl;
             _oss << "  total time        : " << total_time << std::endl;
             _oss << " === LineSearch Statistics === " << std::endl;
             _oss << "   accepted         : " << line_search_accepted << std::endl;
             _oss << "   alpha            : " << alpha << std::endl;
             _oss << "   ls iters         : " << line_search_iters << std::endl;
-            // TODO constraint violation
 
 
             _oss << "=== swSQP Stage Statistics ===" << std::endl;
-            // Header row
-            _oss << std::setw(15) << "Statistic";
-            for (size_t i = 0; i < stages_statistics.size(); ++i) {
-                _oss << std::setw(12) << ("Stage " + std::to_string(i));
-            }
-            _oss << std::endl;
 
-            _oss << std::setw(15) << "cost";
-            for (const auto& s : stages_statistics) {
-                _oss << std::setw(12) << s.cost;
+            // Column headers
+            _oss << std::setw(10) << "Stage"
+                << std::setw(15) << "Cost"
+                << std::setw(25) << "Constr-Viol"
+                << std::endl;
+
+            // Print a separator line (optional)
+            _oss << std::string(50, '-') << std::endl;
+
+            // Print one row per stage
+            for (size_t i = 0; i < stages_statistics.size(); ++i) {
+                const auto& s = stages_statistics[i];
+                _oss << std::setw(10) << i
+                    << std::setw(15) << s.cost
+                    << std::setw(25) << s.constraint_violation
+                    << std::endl;
             }
-            _oss << std::endl;
 
             return _oss;
         }
@@ -135,7 +143,6 @@ class swSQP{
 
 
     void computeDynamics(const unsigned int i, Eigen::MatrixXd& A, Eigen::MatrixXd& B, Eigen::VectorXd& b);
-    // void computeQuadraticApproximation(const unsigned int i, Eigen::MatrixXd& H, Eigen::VectorXd& g);
     void computeCost(const unsigned int i,
                      Eigen::MatrixXd& Q, Eigen::VectorXd& q,
                      Eigen::MatrixXd& R, Eigen::VectorXd& r,
@@ -145,6 +152,7 @@ class swSQP{
 
 
     void linearize(); // update linearization/quadritization matrices
+    void update_statistics();
     void step(double alpha); //step of the solver
     bool break_criteria(); // sqp solvers breaking criteria
 

src/solvers/hpipmOC.cpp

@@ -6,19 +6,19 @@ hpipmOC::hpipmOC(const unsigned int Ns):
     _Ns(Ns)
 {
     //These are taken from examples in hpipm-cpp
-    _solver_settings.mode = hpipm::HpipmMode::Balance;
-    _solver_settings.iter_max = 30;
-    _solver_settings.alpha_min = 1e-8;
-    _solver_settings.mu0 = 1e2;
-    _solver_settings.tol_stat = 1e-04;
-    _solver_settings.tol_eq = 1e-04;
-    _solver_settings.tol_ineq = 1e-04;
-    _solver_settings.tol_comp = 1e-04;
-    _solver_settings.reg_prim = 1e-12;
-    _solver_settings.warm_start = 0;
-    _solver_settings.pred_corr = 1;
-    _solver_settings.ric_alg = 0;
-    _solver_settings.split_step = 1;
+    _solver_settings.mode = hpipm::HpipmMode::Speed;
+    // _solver_settings.iter_max = 30;
+    // _solver_settings.alpha_min = 1e-8;
+    // _solver_settings.mu0 = 1e2;
+    // _solver_settings.tol_stat = 1e-04;
+    // _solver_settings.tol_eq = 1e-04;
+    //  _solver_settings.tol_ineq = 1e-04;
+    // _solver_settings.tol_comp = 1e-04;
+    // _solver_settings.reg_prim = 1e-12;
+    // _solver_settings.warm_start = 0;
+    // _solver_settings.pred_corr = 1;
+    // _solver_settings.ric_alg = 0;
+    // _solver_settings.split_step = 1;
 
     _qp.resize(Ns);
 

src/solvers/swSQP.cpp

@@ -64,8 +64,6 @@ void swSQP::linearize()
 {
     for(unsigned int k = 0; k < _ocp->getNumberOfNodes(); ++k)
     {
-        _stats.stages_statistics[k].cost = _ocp->stage(k)->stage_cost();
-
         // --- Dynamics (only for k < N) ---
         if(k < _ocp->getNumberOfNodes()-1)
         {
@@ -172,7 +170,7 @@ bool swSQP::solve(const std::vector<Eigen::VectorXd>& x0, const std::vector<Eige
 
         if(_opt.verbose)
         {
-            _stats.cost = _ocp->cost();
+            update_statistics();
             std::chrono::duration<double> iter_elapsed = std::chrono::high_resolution_clock::now() - iter_start;
             _stats.iter_time = iter_elapsed.count();
             std::cout<<_stats.toOSS().str()<<"\n"<<std::endl;
@@ -182,6 +180,7 @@ bool swSQP::solve(const std::vector<Eigen::VectorXd>& x0, const std::vector<Eige
 
     _ocp->update(_x0, _u0);
     _stats.cost = _ocp->cost();
+    _stats.constraint_violation = _ocp->constraint_violation();
 
     std::chrono::duration<double> elapsed = std::chrono::high_resolution_clock::now() - start;
     _stats.total_time = elapsed.count();
@@ -196,10 +195,9 @@ bool swSQP::break_criteria()
     double max_dw = -INFINITY;
     for(unsigned int i = 0; i < _ocp->getNumberOfNodes() ; ++i)
         max_dw = std::max(max_dw, (_x0[i] - _x0_candidate[i]).cwiseAbs().maxCoeff());
-    
-    // std::cout<< max_dw << ","<< _ocp->constraint_violation()<< std::endl;
+        // #TODO: USE THE MANIFOLD OMINUS
 
-    return max_dw <= _opt.min_abs_delta_solution && _ocp->constraint_violation() <= _opt.min_abs_delta_solution;
+    return max_dw <= _opt.min_abs_delta_solution; //&& _ocp->constraint_violation() <= _opt.min_abs_delta_solution;
 
 }
 
@@ -252,6 +250,17 @@ bool swSQP::ls_merit()
     return false;
 }
 
+void swSQP::update_statistics()
+{
+    _stats.cost = _ocp->cost();
+    for (uint i = 0; i < _ocp->getNumberOfNodes(); i++)
+    {
+        _stats.stages_statistics[i].cost = _ocp->stage(i)->stage_cost();
+        _stats.stages_statistics[i].constraint_violation = _ocp->stage(i)->stage_constraint_violation();
+    }
+    
+}
+
 
 bool swSQP::ls_filter()
 {