Commit 030d7e2e authored by roangel's avatar roangel
Browse files

Started adding input constraints

parent 5289bce5
......@@ -38,4 +38,4 @@ void funQbar(Eigen::MatrixXd Q, Eigen::MatrixXd P, int N, Eigen::MatrixXd &Qbar)
void funRbar(Eigen::MatrixXd R, int N, Eigen::MatrixXd &Rbar);
Eigen::VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, Eigen::MatrixXd Q, Eigen::MatrixXd R, Eigen::MatrixXd P, int N, VectorXtype x0, Eigen::VectorXd X_ref, Eigen::VectorXd U_ref, float ft_min, float ft_max, float angle_constraint);
Eigen::VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F);
Eigen::VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b);
......@@ -310,12 +310,23 @@ VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<Matr
// // std::cout << Su_test<< std::endl;
// std::cout << H.format(CleanFmt) << std::endl;
Eigen::VectorXd U_0 = solve_QP(H, F);
// ft_min and ft_max constraints:
RowVectorXd pattern;
pattern << 0, 0, 0, 1;
MatrixXd Au_kernel(2*N, N);
Au_kernel.topRows(N) = MatrixXd::Identity(N,N);
Au_kernel.bottomRows(N) = (-1)*MatrixXd::Identity(N,N);
MatrixXd Au = kroneckerProduct(Au_kernel, pattern);
VectorXd bu(2*N);
bu.topRows(N) = VectorXd::Ones(N)*ft_max;
bu.bottomRows(N) = VectorXd::Ones(N)*(-ft_min);
Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu);
return U_0;
VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F)
VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b)
// H is assumed to be symmetric. There is NO 0.5 factor anywhere, with this we solve:
// min(x'Hx + F'x)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment