diff --git a/include/quadruped-reactive-walking/QPWBC.hpp b/include/quadruped-reactive-walking/QPWBC.hpp
index a0c5206945c565def4acb6493545a32ae5e93305..1cd25493f0f0535b7e4b4884b19a5bb9e03ea95c 100644
--- a/include/quadruped-reactive-walking/QPWBC.hpp
+++ b/include/quadruped-reactive-walking/QPWBC.hpp
@@ -17,7 +17,9 @@
#include "osqp_folder/include/osqp_configure.h"
#include "other/st_to_cc.hpp"
+#include "eiquadprog/eiquadprog-rt.hpp"
+using namespace eiquadprog::solvers;
class QPWBC {
private:
@@ -77,6 +79,17 @@ class QPWBC {
OSQPData *data;
OSQPSettings *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
+ //using namespace eiquadprog::solvers;
+ 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;
+
+
public:
QPWBC(); // Constructor
diff --git a/scripts/QP_WBC.py b/scripts/QP_WBC.py
index 1402bf4963b2d1ad94b7692c51db23e5180f493e..769879baf3c704664f189b84e03d2488997047a4 100644
--- a/scripts/QP_WBC.py
+++ b/scripts/QP_WBC.py
@@ -100,7 +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
- M = pin.crba(self.invKin.rmodel, self.invKin.rdata, q)
+ Mfull = 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]
@@ -110,13 +112,30 @@ class controller():
if contacts[i]:
Jc[(3*i):(3*(i+1)), :] = pin.getFrameJacobian(self.invKin.rmodel, self.invKin.rdata, indexes[i],
pin.LOCAL_WORLD_ALIGNED)[:3, :]
- RNEA = pin.rnea(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_cmd)[:6]
-
- from IPython import embed
- embed()
+ ddq_cmd_tmp = np.zeros((18, 1))
+ ddq_cmd_tmp[:6, 0] = ddq_cmd[:6, 0]
+ RNEA = pin.rnea(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_cmd_tmp)[:6]
self.box_qp.run(M.copy(), Jc.copy(), f_cmd.reshape((-1, 1)).copy(), RNEA.reshape((-1, 1)).copy())
+ 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[:6, 0] += deltaddq
+ RNEA_delta = pin.rnea(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_with_delta)[6:]
+ Ja = Jc[:, 6:].transpose()
+ tauff = RNEA_delta - (Ja @ f_with_delta).ravel()
+
+ ta = pin.rnea(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_with_delta)[:6]
+ tb = Jc[:, :6].transpose()
+ tc = ta - (tb @ f_with_delta).ravel()
+ print(tc)
+
+
+ #return (self.A @ (self.ddq_cmd + self.delta_ddq) + np.array([self.NLE]).transpose()
+ # - self.JcT @ (self.f_cmd + np.array([self.x[6:]]).transpose()))[6:, ]
+
+
"""if dq[0, 0] > 0.4:
from IPython import embed
embed()"""
@@ -125,6 +144,17 @@ class controller():
# Retrieve joint torques
# tmp_res = self.box_qp_wbc.get_joint_torques(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_cmd)
self.tau_ff[:] = self.qp_wbc.get_joint_torques().ravel()
+
+ print("f python:", self.qp_wbc.f_cmd.ravel() + np.array(self.qp_wbc.x[6:]))
+ print("f cpp :", f_with_delta.ravel())
+
+ print("ddq python:", (self.qp_wbc.ddq_cmd + self.qp_wbc.delta_ddq).ravel())
+ print("ddq cpp :", ddq_with_delta.ravel())
+
+
+ from IPython import embed
+ embed()
+
self.toc = time()
"""from IPython import embed
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index d62bbef85f62e5e68f1a31a9884c5caf68e57198..471465f21b538c13df43316b9318a056d1c0f285 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -18,6 +18,16 @@ QPWBC::QPWBC() {
// Set OSQP settings to default
osqp_set_default_settings(settings);
+
+ Q_qp.setZero();
+ C_qp.setZero();
+ Beq.setZero();
+
+ Aineq.setZero();
+ for (int i = 0; i < 16; i++) {
+ Aineq(i, i) = 1.;
+ Bineq(i) = 0.0;
+ }
}
/*
@@ -292,7 +302,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) = (workspce->solution->x)[k];
+ lambdas(k, 0) = x_qp(k); // (workspce->solution->x)[k];
}
f_res = G * lambdas;
@@ -333,9 +343,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;
@@ -348,12 +358,29 @@ int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen:
std::cout << "update_PQ done" << std::endl;
// Create an initial guess and call the solver to solve the QP problem
- call_solver();
+ //call_solver();
+ qp.solve_quadprog(Q_qp, C_qp, Aeq, Beq, Aineq, Bineq, x_qp);
+
+ 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;
+ std::cout << "B:" << std::endl << gamma << std::endl << "--" << std::endl;
+ 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 << Q_qp << std::endl;
+ std::cout << C_qp << std::endl;
+ std::cout << Aeq << std::endl;
+ std::cout << Beq << std::endl;
+ std::cout << Aineq << std::endl;
+ std::cout << Bineq << std::endl;
std::cout << "call_solver done" << std::endl;
+ std::cout << "Raw result: " << std::endl << x_qp << std::endl;
+
// Extract relevant information from the output of the QP solver
- // retrieve_result(f_cmd);
+ retrieve_result(f_cmd);
std::cout << "retrieve done" << std::endl;
@@ -445,7 +472,7 @@ 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 i = 0; i < 16; i++) {
for (int j = 0; j < 16; j++) {
P->x[i * 16 + j] = Pw(j, i);
}
@@ -453,6 +480,12 @@ void QPWBC::update_PQ() {
for (int i = 0; i < 16; i++) {
Q[i] = Qw(i, 0);
+ }*/
+
+ // Update P and Q weight matrices
+ Q_qp = Pw;
+ for (int i = 0; i < 16; i++) {
+ C_qp(i) = Qw(i, 0);
}
}
\ No newline at end of file