diff --git a/include/quadruped-reactive-walking/QPWBC.hpp b/include/quadruped-reactive-walking/QPWBC.hpp
index 1cd25493f0f0535b7e4b4884b19a5bb9e03ea95c..553dfbb1f1de4c2413cfebd2d3d4ac6319223155 100644
--- a/include/quadruped-reactive-walking/QPWBC.hpp
+++ b/include/quadruped-reactive-walking/QPWBC.hpp
@@ -17,7 +17,8 @@
#include "osqp_folder/include/osqp_configure.h"
#include "other/st_to_cc.hpp"
-#include "eiquadprog/eiquadprog-rt.hpp"
+// #include "eiquadprog/eiquadprog-rt.hpp"
+#include "eiquadprog/eiquadprog-fast.hpp"
using namespace eiquadprog::solvers;
@@ -38,7 +39,8 @@ class QPWBC {
const double mu = 0.9;
// Generatrix of the linearized friction cone
- Eigen::Matrix<double, 12, 16> G = 1.0 * Eigen::Matrix<double, 12, 16>::Zero();
+ Eigen::Matrix<double, 12, 16> G = 0.0 * Eigen::Matrix<double, 12, 16>::Zero();
+ Eigen::Matrix<double, 3, 4> Gk = Eigen::Matrix<double, 3, 4>::Zero();
// Transformation matrices
Eigen::Matrix<double, 6, 6> Y = Eigen::Matrix<double, 6, 6>::Zero();
@@ -67,7 +69,7 @@ class QPWBC {
double v_warmxf[size_nz_NK] = {}; // matrix NK (lower bound)
// Matrix P
- const static int size_nz_P = 256;
+ const static int size_nz_P = 8*17; // 16*17/2;
csc *P; // Compressed Sparse Column matrix
// Matrix Q
@@ -80,14 +82,32 @@ class QPWBC {
OSQPSettings *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
//using namespace eiquadprog::solvers;
- RtEiquadprog<16, 0, 16> qp;
+ /*RtEiquadprog<16, 0, 16> qp;
RtMatrixX<16, 16>::d Q_qp;
RtVectorX<16>::d C_qp;
RtMatrixX<0, 16>::d Aeq;
RtVectorX<0>::d Beq;
RtMatrixX<16, 16>::d Aineq;
RtVectorX<16>::d Bineq;
- RtVectorX<16>::d x_qp;
+ RtVectorX<16>::d x_qp;*/
+
+ EiquadprogFast qp;
+ Eigen::MatrixXd Q_qp = Eigen::MatrixXd::Zero(16,16);
+ Eigen::VectorXd C_qp = Eigen::VectorXd::Zero(16);
+ Eigen::MatrixXd Aeq = Eigen::MatrixXd::Zero(0, 16);
+ Eigen::VectorXd Beq = Eigen::VectorXd::Zero(0);
+ Eigen::MatrixXd Aineq = Eigen::MatrixXd::Zero(16, 16);
+ Eigen::VectorXd Bineq = Eigen::VectorXd::Zero(16);
+ Eigen::VectorXd x_qp = Eigen::VectorXd::Zero(16);
+
+ /*RtEiquadprog<12, 0, 20> qp;
+ RtMatrixX<12, 12>::d Q_qp;
+ RtVectorX<12>::d C_qp;
+ RtMatrixX<0, 12>::d Aeq;
+ RtVectorX<0>::d Beq;
+ RtMatrixX<20, 12>::d Aineq;
+ RtVectorX<20>::d Bineq;
+ RtVectorX<12>::d x_qp;*/
public:
@@ -109,6 +129,7 @@ class QPWBC {
// Getters
Eigen::MatrixXd get_f_res();
Eigen::MatrixXd get_ddq_res();
+ Eigen::MatrixXd get_P();
// Utils
void my_print_csc_matrix(csc *M, const char *name);
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 493140fa53bd79c9da6e50467244a3a939d593a4..8100be2c875b0d16eeb8f82ef94c2853e41857ae 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -100,6 +100,7 @@ struct QPWBCPythonVisitor : public bp::def_visitor<QPWBCPythonVisitor<QPWBC> > {
.def("get_f_res", &QPWBC::get_f_res, "Get velocity goals matrix.\n")
.def("get_ddq_res", &QPWBC::get_ddq_res, "Get acceleration goals matrix.\n")
+ .def("get_P", &QPWBC::get_P, "Get P weight matrix.\n")
// Run QPWBC from Python
.def("run", &QPWBC::run, bp::args("M", "Jc", "f_cmd", "RNEA"), "Run QPWBC from Python.\n");
diff --git a/scripts/QP_WBC.py b/scripts/QP_WBC.py
index 769879baf3c704664f189b84e03d2488997047a4..e2072fbaddf62aa59d04bcd72ffbf3aefbdb5bba 100644
--- a/scripts/QP_WBC.py
+++ b/scripts/QP_WBC.py
@@ -100,9 +100,9 @@ class controller():
q.copy(), dq.copy(), ddq_cmd, np.array([f_cmd]).T, contacts)
# Compute the joint space inertia matrix M by using the Composite Rigid Body Algorithm
- Mfull = pin.crba(self.invKin.rmodel, self.invKin.rdata, q)
- M = np.zeros(Mfull.shape)
- M[:6, :6] = Mfull[:6, :6]
+ M = pin.crba(self.invKin.rmodel, self.invKin.rdata, q)
+ # M = np.zeros(Mfull.shape)
+ # M[:6, :6] = Mfull[:6, :6]
# Contact Jacobian
# Indexes of feet frames in this order: [FL, FR, HL, HR]
@@ -120,7 +120,7 @@ class controller():
deltaddq = self.box_qp.get_ddq_res()
f_with_delta = self.box_qp.get_f_res().reshape((-1, 1))
- ddq_with_delta = ddq_cmd_tmp.copy()
+ ddq_with_delta = ddq_cmd.copy()
ddq_with_delta[:6, 0] += deltaddq
RNEA_delta = pin.rnea(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_with_delta)[6:]
Ja = Jc[:, 6:].transpose()
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index 471465f21b538c13df43316b9318a056d1c0f285..2f07dd7f3c6e97b5a4645cf700580cdd85096782 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -3,8 +3,19 @@
QPWBC::QPWBC() {
+ // Slipping constraints
+ /*Eigen::Matrix<double, 5, 3> SC;
+ int a[9] = {0, 1, 2, 3, 0, 1, 2, 3, 4};
+ int b[9] = {0, 0, 1, 1, 2, 2, 2, 2, 2};
+ double c[9] = {1.0, -1.0, 1.0, -1.0, -mu, -mu, -mu, -mu, -1};
+ for (int i = 0; i < 8; i++) {
+ SC(a[i], b[i]) = -c[i];
+ }*/
+
+ qp.reset(16, 0, 16);
+
// Initialization of the generatrix G
- Eigen::Matrix<double, 3, 4> Gk;
+
Gk.row(0) << mu, mu, -mu, -mu;
Gk.row(1) << mu, -mu, mu, -mu;
Gk.row(2) << 1.0, 1.0, 1.0, 1.0;
@@ -27,7 +38,14 @@ QPWBC::QPWBC() {
for (int i = 0; i < 16; i++) {
Aineq(i, i) = 1.;
Bineq(i) = 0.0;
+ x_qp(i) = 3.0;
}
+ /*for (int i = 0; i < 4; i++) {
+ Aineq.block(5*i, 3*i, 5, 3) = SC;
+ }*/
+
+
+
}
/*
@@ -140,7 +158,7 @@ int QPWBC::create_weight_matrices() {
// Fill P with 1.0 so that the sparse creation process considers that all coeffs
// can have a non zero value
for (int i = 0; i < 16; i++) {
- for (int j = 0; j < 16; j++) {
+ for (int j = i; j < 16; j++) {
add_to_P(i, j, 1.0, r_P, c_P, v_P);
}
}
@@ -236,18 +254,18 @@ int QPWBC::call_solver() {
// settings->rho = 0.1f;
// settings->sigma = 1e-6f;
// settings->max_iter = 4000;
- /*settings->eps_abs = (float)1e-5;*/
- //settings->eps_rel = (float)1e-5;
+ settings->eps_abs = (float)1e-5;
+ settings->eps_rel = (float)1e-5;
/*settings->eps_prim_inf = 1e-4f;
settings->eps_dual_inf = 1e-4f;
settings->alpha = 1.6f;
settings->delta = 1e-6f;
settings->polish = 0;
settings->polish_refine_iter = 3;*/
- /*settings->adaptive_rho = (c_int)1;
+ settings->adaptive_rho = (c_int)1;
settings->adaptive_rho_interval = (c_int)200;
settings->adaptive_rho_tolerance = (float)5.0;
- settings->adaptive_rho_fraction = (float)0.7;*/
+ settings->adaptive_rho_fraction = (float)0.7;
settings->verbose = true;
int exitflag = 0;
exitflag = osqp_setup(&workspce, data, settings);
@@ -302,7 +320,7 @@ Extract relevant information from the output of the QP solver
int QPWBC::retrieve_result(const Eigen::MatrixXd &f_cmd) {
// Retrieve the "contact forces" part of the solution of the QP problem
for (int k = 0; k < 16; k++) {
- lambdas(k, 0) = x_qp(k); // (workspce->solution->x)[k];
+ lambdas(k, 0) = x_qp(k, 0); // (workspce->solution->x)[k];
}
f_res = G * lambdas;
@@ -334,6 +352,10 @@ Return the next predicted state of the base
*/
Eigen::MatrixXd QPWBC::get_f_res() { return f_res; }
Eigen::MatrixXd QPWBC::get_ddq_res() { return ddq_res; }
+Eigen::MatrixXd QPWBC::get_P() {
+ Eigen::MatrixXd Pxd = Eigen::MatrixXd::Zero(16, 16);
+ Pxd = Pw;
+ return Pxd; }
/*
Run one iteration of the whole MPC by calling all the necessary functions (data retrieval,
@@ -343,9 +365,9 @@ int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen:
// Create the constraint and weight matrices used by the QP solver
// Minimize x^T.P.x + x^T.Q with constraints M.X == N and L.X <= K
- // if (not initialized) {
- // create_matrices();
- // }
+ if (not initialized) {
+ create_matrices();
+ }
std::cout << "Creation done" << std::endl;
@@ -354,13 +376,35 @@ int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen:
std::cout << "compute_matrices done" << std::endl;
update_PQ();
+ // Bineq = Aineq * f_cmd;
std::cout << "update_PQ done" << std::endl;
// Create an initial guess and call the solver to solve the QP problem
//call_solver();
+
+ for (int i = 0; i < 4; i++) {
+ double testou = f_cmd(3*i+2, 0) * 0.25;
+ for (int j = 0; j < 4; j++) {
+ x_qp(4*i+j) = testou;
+ }
+ }
qp.solve_quadprog(Q_qp, C_qp, Aeq, Beq, Aineq, Bineq, x_qp);
+ Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(16, 1);
+ dx(0, 0) = 0.01;
+ dx(1, 0) = 0.01;
+ dx(2, 0) = 0.01;
+ dx(3, 0) = 0.01;
+ dx(12, 0) = 0.01;
+ dx(13, 0) = 0.01;
+ dx(14, 0) = 0.01;
+ dx(15, 0) = 0.01;
+ std::cout << 0.5 * x_qp.transpose() * Q_qp * x_qp + x_qp.transpose() * C_qp << std::endl;
+ std::cout << 0.5 * (x_qp-dx).transpose() * Q_qp * (x_qp-dx) + (x_qp-dx).transpose() * C_qp << std::endl;
+ std::cout << 0.5 * (x_qp+dx).transpose() * Q_qp * (x_qp+dx) + (x_qp+dx).transpose() * C_qp << std::endl;
+
+
std::cout << "A:" << std::endl << A << std::endl << "--" << std::endl;
std::cout << "Xf:" << std::endl << (X * f_cmd) << std::endl << "--" << std::endl;
std::cout << "RNEA:" << std::endl << RNEA << std::endl << "--" << std::endl;
@@ -368,6 +412,7 @@ int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen:
std::cout << "AT Q1:" << std::endl << A.transpose() * Q1 << std::endl << "--" << std::endl;
std::cout << "g:" << std::endl << g << std::endl << "--" << std::endl;
std::cout << "H:" << std::endl << H << std::endl << "--" << std::endl;
+ std::cout << "Qw:" << std::endl << Qw << std::endl << "--" << std::endl;
std::cout << Q_qp << std::endl;
std::cout << C_qp << std::endl;
std::cout << Aeq << std::endl;
@@ -378,10 +423,35 @@ int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen:
std::cout << "call_solver done" << std::endl;
std::cout << "Raw result: " << std::endl << x_qp << std::endl;
-
+ // std::cout << "F result : " << std::endl << f_cmd + x_qp << std::endl;
// Extract relevant information from the output of the QP solver
retrieve_result(f_cmd);
+ Eigen::MatrixXd df = Eigen::MatrixXd::Zero(12, 1);
+ df = f_res - f_cmd;
+ std::cout << "Cost df H df + df g" << std::endl;
+ std::cout << df.transpose() * H * df + 2 * df.transpose() * g << std::endl;
+ std::cout << "Cost lambda Q lambda + 2 * lambda * C" << std::endl;
+ std::cout << 0.5 * x_qp.transpose() * Q_qp * x_qp + x_qp.transpose() * C_qp << std::endl;
+ std::cout << "Cost dev :" << std::endl;
+ std::cout << (x_qp.transpose() * G.transpose() - f_cmd.transpose()) * H * (G * x_qp - f_cmd) + 2 * (x_qp.transpose() * G.transpose() - f_cmd.transpose()) * g << std::endl;
+ std::cout << "Cost dev 2 :" << std::endl;
+ std::cout << x_qp.transpose() * G.transpose() * H * G * x_qp + 2 * (G.transpose() * g - G.transpose() * H * f_cmd).transpose() * x_qp << std::endl;
+ std::cout << "Removed:" << f_cmd.transpose() * H * f_cmd - 2 * f_cmd.transpose() * g << std::endl;
+
+ for (int i = 0; i < 4; i++) {
+ double testou = f_cmd(3*i+2, 0) * 0.25;
+ for (int j = 0; j < 4; j++) {
+ x_qp(4*i+j) = testou;
+ }
+ }
+ std::cout << f_cmd << std::endl;
+ std::cout << G * x_qp << std::endl;
+ std::cout << "-#-" << std::endl;
+ std::cout << G << std::endl;
+ std::cout << x_qp << std::endl;
+ std::cout << "Cost: " << 0.5 * x_qp.transpose() * Q_qp * x_qp + x_qp.transpose() * C_qp << std::endl;
+
std::cout << "retrieve done" << std::endl;
//char t_char[1] = {'P'};
@@ -464,6 +534,17 @@ void QPWBC::compute_matrices(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc
gamma = Yinv * ((X * f_cmd) - RNEA);
H = A.transpose() * Q1 * A + Q2;
g = A.transpose() * Q1 * gamma;
+
+ for (int i = 0; i < 4; i++) {
+ if (f_cmd(3*i+2, 0) > 1e-4) {
+ G.block(3*i, 4*i, 3, 4) = Gk;
+ }
+ else
+ {
+ G.block(3*i, 4*i, 3, 4) = Eigen::Matrix<double, 3, 4>::Zero();
+ }
+ }
+
Pw = G.transpose() * H * G;
Qw = (G.transpose() * g) - (G.transpose() * H * f_cmd);
@@ -472,15 +553,22 @@ void QPWBC::compute_matrices(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc
void QPWBC::update_PQ() {
// Update P and Q weight matrices
- /*for (int i = 0; i < 16; i++) {
- for (int j = 0; j < 16; j++) {
- P->x[i * 16 + j] = Pw(j, i);
+ int cpt = 0;
+ for (int i = 0; i < 16; i++) {
+ for (int j = i; j < 16; j++) {
+ P->x[cpt] = Pw(i, j);
+ cpt++;
}
}
+ std::cout << "Eigenvalues" << Pw.eigenvalues() << std::endl;
+
+ //char t_char[1] = {'P'};
+ //my_print_csc_matrix(P, t_char);
+
for (int i = 0; i < 16; i++) {
Q[i] = Qw(i, 0);
- }*/
+ }
// Update P and Q weight matrices
Q_qp = Pw;