mayataka / hpipm-cpp Goto Github PK
View Code? Open in Web Editor NEWC++ interface for hpipm, a high-performance interior point MPC solver
License: MIT License
hpipm-cpp's Introduction
hpipm-cpp's People
Contributors
Stargazers
Watchers
Forkers
mfkiwl zhp-ncu-dev ekusiadadus aamanku moreinfoy mmurooka boyali mengxingshifen1218 mr-d-self-driving pac48 martain-liu sashankmodali shbang91 vortexmath vayu-robotics lueluelue12138 chengwei920412hpipm-cpp's Issues
Issue when the constant term in the dynamics equation is involved
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.
Segmentation fault
hello, I try to adjust the horizon in the example_mpc and example_ocp_qp, but I found that if the horizon length is 6, 10, 50 and some other numbers,it will be Segmentation fault. I am confused about it. here is the code
#include "hpipm-cpp/hpipm-cpp.hpp"
#include
#include
#include
#include "Eigen/Core"
int main() {
// setup QP
const int N = 6; // 10 50 core dumped
hpipm::OcpQpDim dim(N);
dim.nx = std::vector(N+1, 12);
dim.nu = std::vector(N, 4);
dim.nbx = std::vector(N+1, 3);
dim.nbx[0] = 12;
dim.nbu = std::vector(N, 4);
dim.ng = std::vector(N+1, 0);
dim.nsbx = std::vector(N+1, 0);
dim.nsbu = std::vector(N, 0);
dim.nsg = std::vector(N+1, 0);
hpipm::OcpQp qp(dim);
// initial state
qp.x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
// dynamics
Eigen::MatrixXd A(12, 12), B(12, 4);
A << 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0. ,
0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0. ,
0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0. ,
0.0488, 0., 0., 1., 0., 0., 0.0016, 0., 0., 0.0992, 0., 0. ,
0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0. ,
0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992,
0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0. ,
0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0. ,
0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0. ,
0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0. ,
0., -0.9734, 0., 0., 0., 0., 0., -0.0488, 0., 0., 0.9846, 0. ,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846;
B << 0., -0.0726, 0., 0.0726,
-0.0726, 0., 0.0726, 0. ,
-0.0152, 0.0152, -0.0152, 0.0152,
-0., -0.0006, -0., 0.0006,
0.0006, 0., -0.0006, 0.0000,
0.0106, 0.0106, 0.0106, 0.0106,
0, -1.4512, 0., 1.4512,
-1.4512, 0., 1.4512, 0. ,
-0.3049, 0.3049, -0.3049, 0.3049,
-0., -0.0236, 0., 0.0236,
0.0236, 0., -0.0236, 0. ,
0.2107, 0.2107, 0.2107, 0.2107;
const Eigen::VectorXd b = Eigen::VectorXd::Zero(12);
for (int i=0; i<N; ++i) {
qp.A[i] = A;
qp.B[i] = B;
qp.b[i] = b;
}
// cost
Eigen::MatrixXd Q(12, 12), S(4, 12), R(4, 4);
Q.setZero(); Q.diagonal() << 0, 0, 10., 10., 10., 10., 0, 0, 0, 5., 5., 5.;
S.setZero();
R.setZero(); R.diagonal() << 0.1, 0.1, 0.1, 0.1;
Eigen::VectorXd x_ref(12);
x_ref << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0;
const Eigen::VectorXd q = - Q * x_ref;
const Eigen::VectorXd r = Eigen::VectorXd::Zero(4);
for (int i=0; i<N; ++i) {
qp.Q[i] = Q;
qp.R[i] = R;
qp.S[i] = S;
qp.q[i] = q;
qp.r[i] = r;
}
qp.Q[N] = Q;
qp.q[N] = q;
// constraints
const bool use_mask_for_one_sided_constraints = true;
for (int i=1; i<=N; ++i) {
constexpr double soft_inf = 1.0e10;
qp.idxbx[i] = {0, 1, 5};
qp.lbx[i] = (Eigen::VectorXd(3) << -M_PI/6.0, -M_PI/6.0, -1.0).finished();
qp.ubx[i] = (Eigen::VectorXd(3) << M_PI/6.0, M_PI/6.0, soft_inf).finished();
if (use_mask_for_one_sided_constraints) {
qp.ubx_mask[i] = (Eigen::VectorXd(3) << 1.0, 1.0, 0.0).finished(); // this mask disables upper bound by ubx[2]
}
}
for (int i=0; i<N; ++i) {
constexpr double u0 = 10.5916;
qp.idxbu[i] = {0, 1, 2, 3};
qp.lbu[i] = (Eigen::VectorXd(4) << 9.6-u0, 9.6-u0, 9.6-u0, 9.6-u0).finished();
qp.ubu[i] = (Eigen::VectorXd(4) << 13.0-u0, 13.0-u0, 13.0-u0, 13.0-u0).finished();
}
hpipm::OcpQpIpmSolverSettings ipm_solver_settings;
ipm_solver_settings.iter_max = 30;
ipm_solver_settings.alpha_min = 1e-8;
ipm_solver_settings.mu0 = 1e2;
ipm_solver_settings.tol_stat = 1e-04;
ipm_solver_settings.tol_eq = 1e-04;
ipm_solver_settings.tol_ineq = 1e-04;
ipm_solver_settings.tol_comp = 1e-04;
ipm_solver_settings.reg_prim = 1e-12;
ipm_solver_settings.warm_start = 1;
ipm_solver_settings.pred_corr = 1;
ipm_solver_settings.ric_alg = 0;
ipm_solver_settings.split_step = 1;
hpipm::OcpQpSolution solution(dim);
hpipm::OcpQpIpmSolver solver(dim, ipm_solver_settings);
Eigen::VectorXd u0(4);
Eigen::VectorXd x(12);
x << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
const int sim_steps = 50;
for (int t=0; t<sim_steps; ++t) {
std::cout << "t: " << t << ", x: " << x.transpose() << std::endl;
qp.x0 = x;
if (solver.solve(qp, solution) != hpipm::HpipmStatus::Success) return 1;
u0 = solution.u[0];
x = A * x + B * u0 + b;
}
std::cout << "t: " << sim_steps << ", x: " << x.transpose() << std::endl;
return 0;
}
Library loading error in test
Thank you for the useful library.
I get the following error about loading the shared library when I build with tests and run them.
$ git clone https://github.com/mayataka/hpipm-cpp
$ cd hpipm-cpp
$ mkdir build && cd build
$ cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=${HOME}/src/install -DBUILD_TESTS=ON
$ make -j4
$ make install
$ cd test
$ ./test/ocp_qp_ipm_solver
./test/ocp_qp_ipm_solver: error while loading shared libraries: libblasfeo.so: cannot open shared object file: No such file or directory
$ ldd ./test/ocp_qp_ipm_solver
linux-vdso.so.1 (0x00007ffed818f000)
libgtk3-nocsd.so.0 => /usr/lib/x86_64-linux-gnu/libgtk3-nocsd.so.0 (0x00007f8bbb317000)
libpthread.so.0 => /lib/x86_64-linux-gnu/libpthread.so.0 (0x00007f8bbb0f8000)
libhpipm.so => /home/mmurooka/src/hpipm-cpp/external/hpipm-install/lib/libhpipm.so (0x00007f8bbadbc000)
libstdc++.so.6 => /usr/lib/x86_64-linux-gnu/libstdc++.so.6 (0x00007f8bbaa33000)
libm.so.6 => /lib/x86_64-linux-gnu/libm.so.6 (0x00007f8bba695000)
libgcc_s.so.1 => /lib/x86_64-linux-gnu/libgcc_s.so.1 (0x00007f8bba47d000)
libc.so.6 => /lib/x86_64-linux-gnu/libc.so.6 (0x00007f8bba08c000)
libdl.so.2 => /lib/x86_64-linux-gnu/libdl.so.2 (0x00007f8bb9e88000)
/lib64/ld-linux-x86-64.so.2 (0x00007f8bbb812000)
libblasfeo.so => not foundI set ${HOME}/src/install for CMAKE_INSTALL_PREFIX, and ${HOME}/src/install/lib is included in LD_LIBRARY_PATH.
If I manually add ${HOME}/src/install/lib/hpipm-cpp to LD_LIBRARY_PATH, this library loading error does not happen, but I feel that it is not so general to impose this on all users. Is there a general solution by modifying CMakeLists.txt?
For reference, I attach the build/install_manifest.txt file.
install_manifest.txt
With the option -DBUILD_SHARED_LIBS=ON, I get the similar error.
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.