Issue when the constant term in the dynamics equation is involved #16
Description
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.