Issue when the constant term in the dynamics equation is involved

[shbang91]

Hello,

Thanks for maintaining this nice repo!
While using the HPIPM solver for my QP formulation, I encountered an issue related to the contact term (symbol b in the HPIPM paper) in the dynamics equation. In order to replicate my issue for a simple case, here, I use the 1D double integrator. The first scenario is that I commanded the double integrator to go to x_goal = (x, x_dot) = (-5, 0) from x_initial = (x, x_dot) = (5, 0) with only the cost and dynamics. As expected, it succeeded in stabilizing the double integrator with the HPIPM QP solver as shown below:

However, when adding the gravity term with the double integrator and trying to stabilize it, it diverges around the final node like below:

Here, I just added the gravity term with -g * dt as a constant term (b) in the dynamics equation (x_{k+1} = A x_k + B u_k + b) and commanded it to stay at the initial state. Also, the plots represent the solution trajectories (x and u) of the optimal control problem over 100 nodes with the dt = 0.01. As I played with the other higher dimensional examples, I noticed that only when I introduced some constant terms in the dynamics equation the HPIPM solver gives the diverging solution around the final node.
Have you ever seen this issue or do you know how to resolve this issue?

Here I attached an example code to replicate the above plot.

#include "hpipm-cpp/hpipm-cpp.hpp"                                                                                                                                                                                                                                                        
                                                                                                                                                                                                                                                                                          
#include "Eigen/Core"                                                                                                                                                                                                                                                                     
#include <iostream>                                                                                                                                                                                                                                                                       
#include <vector>                                                                                                                                                                                                                                                                         
                                                                                                                                                                                                                                                                                          
int main() {                                                                                                                                                                                                                                                                              
  int N = 100; // horizon lenght                                                                                                                                                                                                                                                          
  double dt = 0.01;                                                                                                                                                                                                                                                                       
                                                                                                                                                                                                                                                                                          
  const Eigen::MatrixXd A =                                                                                                                                                                                                                                                               
      (Eigen::MatrixXd(2, 2) << 1.0, 1.0 * dt, 0.0, 1.0).finished();                                                                                                                                                                                                                      
  const Eigen::MatrixXd B = (Eigen::MatrixXd(2, 1) << 0.0, 1.0 * dt).finished();                                                                                                                                                                                                          
  const Eigen::VectorXd b = (Eigen::VectorXd(2) << 0.0, -9.81 * dt).finished();                                                                                                                                                                                                           
                                                                                                                                                                                                                                                                                          
  const Eigen::MatrixXd Q =                                                                                                                                                                                                                                                               
      (Eigen::MatrixXd(2, 2) << 10000.0, 0.0, 0.0, 100.0).finished();                                                                                                                                                                                                                     
  const Eigen::MatrixXd R = (Eigen::MatrixXd(1, 1) << 1.0).finished();                                                                                                                                                                                                                    
  const Eigen::MatrixXd S = (Eigen::MatrixXd(1, 2) << 0.0, 0.0).finished();                                                                                                                                                                                                               
  const Eigen::VectorXd q =                                                                                                                                                                                                                                                               
      (Eigen::VectorXd(2) << -10000.0, 0.0).finished(); // x_ref = (1.0, 0.0)                                                                                                                                                                                                             
  const Eigen::VectorXd r = (Eigen::VectorXd(1) << 0.0).finished();                                                                                                                                                                                                                       
                                                                                                                                                                                                                                                                                          
  const Eigen::VectorXd x0 = (Eigen::VectorXd(2) << 1.0, 0.0).finished();                                                                                                                                                                                                                 
                                                                                                                                                                                                                                                                                          
  std::vector<hpipm::OcpQp> qp(N + 1);                                                                                                                                                                                                                                                    
  for (int i = 0; i < N; ++i) {                                                                                                                                                                                                                                                           
    qp[i].A = A;                                                                                                                                                                                                                                                                          
    qp[i].B = B;                                                                                                                                                                                                                                                                          
    qp[i].b = b;                                                                                                                                                                                                                                                                          
  }                                                                                                                                                                                                                                                                                       
  // cost                                                                                                                                                                                                                                                                                 
  for (int i = 0; i < N; ++i) {                                                                                                                                                                                                                                                           
    qp[i].Q = Q;                                                                                                                                                                                                                                                                          
    qp[i].R = R;                                                                                                                                                                                                                                                                          
    qp[i].S = S;                                                                                                                                                                                                                                                                          
    qp[i].q = q;                                                                                                                                                                                                                                                                          
    qp[i].r = r;                                                                                                                                                                                                                                                                          
  }                                                                                                                                                                                                                                                                                       
  qp[N].Q = Q;                                                                                                                                                                                                                                                                            
  qp[N].q = q;                                                                                                                                                                                                                                                                            
                                                                                                                                                                                                                                                                                          
  hpipm::OcpQpIpmSolverSettings solver_settings;                                                                                                                                                                                                                                          
  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;                                                                                                                                                                                                                                                         
                                                                                                                                                                                                                                                                                          
  std::vector<hpipm::OcpQpSolution> solution(N + 1);                                                                                                                                                                                                                                      
  hpipm::OcpQpIpmSolver solver(qp, solver_settings);                                                                                                                                                                                                                                      
                                                                                                                                                                                                                                                                                          
  const auto res = solver.solve(x0, qp, solution);                                                                                                                                                                                                                                        
  std::cout << "QP result: " << res << std::endl;                                                                                                                                                                                                                                         
                                                                                                                                                                                                                                                                                          
  std::cout << "OCP QP primal solution: " << std::endl;                                                                                                                                                                                                                                   
  for (int i = 0; i <= N; ++i) {                                                                                                                                                                                                                                                          
    std::cout << "x[" << i << "]: " << solution[i].x.transpose() << std::endl;                                                                                                                                                                                                            
  }                                                                                                                                                                                                                                                                                       
  for (int i = 0; i < N; ++i) {                                                                                                                                                                                                                                                           
    std::cout << "u[" << i << "]: " << solution[i].u.transpose() << std::endl;                                                                                                                                                                                                            
  }                       

Thank you.