diff --git a/CMakeLists.txt b/CMakeLists.txt
index dd63e3147e209b29163687d5e686788527985ecf..203666d729c8f99538de5de85db17b32548412fe 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -48,7 +48,7 @@ set(${PROJECT_NAME}_HEADERS
include/${PROJECT_NAME}/MPC.hpp
include/${PROJECT_NAME}/Planner.hpp
include/${PROJECT_NAME}/InvKin.hpp
- include/${PROJECT_NAME}/QP_WBC.hpp
+ include/${PROJECT_NAME}/QPWBC.hpp
include/other/st_to_cc.hpp
#include/osqp_folder/include/osqp_configure.h
#include/osqp_folder/include/osqp.h
@@ -62,7 +62,7 @@ set(${PROJECT_NAME}_SOURCES
src/MPC.cpp
src/Planner.cpp
src/InvKin.cpp
- src/QP_WBC.cpp
+ src/QPWBC.cpp
)
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
@@ -84,6 +84,12 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC pinocchio::pinocchio)
# Link parametric curves library
# target_link_libraries(${PROJECT_NAME} PUBLIC curves::curves)
+# Find eiquadprog library and headers
+add_project_dependency(eiquadprog REQUIRED)
+
+# Link eiquadprog library
+target_link_libraries(${PROJECT_NAME} PUBLIC eiquadprog::eiquadprog)
+
# Find OSQP library and headers
find_package(osqp REQUIRED)
diff --git a/include/quadruped-reactive-walking/InvKin.hpp b/include/quadruped-reactive-walking/InvKin.hpp
index 1d7b29772d27f50f2fc90475ef15dbb9a41bc30c..2c50b7c4ae9e2f7bfff06b99dac6827c335a5d15 100644
--- a/include/quadruped-reactive-walking/InvKin.hpp
+++ b/include/quadruped-reactive-walking/InvKin.hpp
@@ -192,4 +192,13 @@ class InvKin {
Eigen::MatrixXd get_agoals();*/
};
+template<typename _Matrix_Type_>
+_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon())
+{
+ Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);
+ double tolerance = epsilon * static_cast<double>(std::max(a.cols(), a.rows())) *svd.singularValues().array().abs()(0);
+ return svd.matrixV() * (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
+}
+
+
#endif // INVKIN_H_INCLUDED
diff --git a/include/quadruped-reactive-walking/QPWBC.hpp b/include/quadruped-reactive-walking/QPWBC.hpp
index dd798f0d8a12a9c11ec222899fae1cfde7860f1d..a0c5206945c565def4acb6493545a32ae5e93305 100644
--- a/include/quadruped-reactive-walking/QPWBC.hpp
+++ b/include/quadruped-reactive-walking/QPWBC.hpp
@@ -1,6 +1,7 @@
#ifndef QPWBC_H_INCLUDED
#define QPWBC_H_INCLUDED
+#include "quadruped-reactive-walking/InvKin.hpp" // For pseudoinverse
#include <iostream>
#include <fstream>
#include <string>
@@ -16,6 +17,8 @@
#include "osqp_folder/include/osqp_configure.h"
#include "other/st_to_cc.hpp"
+
+
class QPWBC {
private:
@@ -35,6 +38,17 @@ class QPWBC {
// Generatrix of the linearized friction cone
Eigen::Matrix<double, 12, 16> G = 1.0 * Eigen::Matrix<double, 12, 16>::Zero();
+ // Transformation matrices
+ Eigen::Matrix<double, 6, 6> Y = Eigen::Matrix<double, 6, 6>::Zero();
+ Eigen::Matrix<double, 6, 12> X = Eigen::Matrix<double, 6, 12>::Zero();
+ Eigen::Matrix<double, 6, 6> Yinv = Eigen::Matrix<double, 6, 6>::Zero();
+ Eigen::Matrix<double, 6, 12> A = Eigen::Matrix<double, 6, 12>::Zero();
+ Eigen::Matrix<double, 6, 1> gamma = Eigen::Matrix<double, 6, 1>::Zero();
+ Eigen::Matrix<double, 12, 12> H = Eigen::Matrix<double, 12, 12>::Zero();
+ Eigen::Matrix<double, 12, 1> g = Eigen::Matrix<double, 12, 1>::Zero();
+ Eigen::Matrix<double, 16, 16> Pw = Eigen::Matrix<double, 16, 16>::Zero();
+ Eigen::Matrix<double, 16, 1> Qw = Eigen::Matrix<double, 16, 1>::Zero();
+
// Results
Eigen::Matrix<double, 16, 1> lambdas = Eigen::Matrix<double, 16, 1>::Zero();
Eigen::MatrixXd f_res = Eigen::MatrixXd::Zero(12, 1);
@@ -73,15 +87,15 @@ class QPWBC {
int create_matrices();
int create_ML();
int create_weight_matrices();
- int compute_matrices(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd, const Eigen::MatrixXd &RNEA);
+ void compute_matrices(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd, const Eigen::MatrixXd &RNEA);
void update_PQ();
- int call_solver(int);
- int retrieve_result();
+ int call_solver();
+ int retrieve_result(const Eigen::MatrixXd &f_cmd);
int run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd, const Eigen::MatrixXd &RNEA);
// Getters
- Eigen::MatrixXd QPWBC::get_f_res();
- Eigen::MatrixXd QPWBC::get_dsqq_res();
+ Eigen::MatrixXd get_f_res();
+ Eigen::MatrixXd get_ddq_res();
// Utils
void my_print_csc_matrix(csc *M, const char *name);
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 53708fcadd752f8ad8989b15986c1fd2755b400a..493140fa53bd79c9da6e50467244a3a939d593a4 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -2,6 +2,7 @@
#include "quadruped-reactive-walking/MPC.hpp"
#include "quadruped-reactive-walking/Planner.hpp"
#include "quadruped-reactive-walking/InvKin.hpp"
+#include "quadruped-reactive-walking/QPWBC.hpp"
#include <eigenpy/eigenpy.hpp>
#include <boost/python.hpp>
@@ -91,6 +92,28 @@ struct InvKinPythonVisitor : public bp::def_visitor<InvKinPythonVisitor<InvKin>
void exposeInvKin() { InvKinPythonVisitor<InvKin>::expose(); }
+template <typename QPWBC>
+struct QPWBCPythonVisitor : public bp::def_visitor<QPWBCPythonVisitor<QPWBC> > {
+ template <class PyClassQPWBC>
+ void visit(PyClassQPWBC& cl) const {
+ cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+
+ .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")
+
+ // Run QPWBC from Python
+ .def("run", &QPWBC::run, bp::args("M", "Jc", "f_cmd", "RNEA"), "Run QPWBC from Python.\n");
+ }
+
+ static void expose() {
+ bp::class_<QPWBC>("QPWBC", bp::no_init).def(QPWBCPythonVisitor<QPWBC>());
+
+ ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
+ }
+};
+
+void exposeQPWBC() { QPWBCPythonVisitor<QPWBC>::expose(); }
+
BOOST_PYTHON_MODULE(libquadruped_reactive_walking) {
boost::python::def("add", gepetto::example::add);
boost::python::def("sub", gepetto::example::sub);
@@ -100,4 +123,5 @@ BOOST_PYTHON_MODULE(libquadruped_reactive_walking) {
exposeMPC();
exposePlanner();
exposeInvKin();
+ exposeQPWBC();
}
\ No newline at end of file
diff --git a/scripts/QP_WBC.py b/scripts/QP_WBC.py
index 4b209ccfa5dcf14cc654df258e26c5d347e505a2..1402bf4963b2d1ad94b7692c51db23e5180f493e 100644
--- a/scripts/QP_WBC.py
+++ b/scripts/QP_WBC.py
@@ -7,7 +7,7 @@ from example_robot_data import load
import osqp as osqp
from solo12InvKin import Solo12InvKin
from time import clock, time
-
+import libquadruped_reactive_walking as lrw
class controller():
@@ -17,7 +17,8 @@ class controller():
self.invKin = Solo12InvKin(dt) # Inverse Kinematics object
self.qp_wbc = QP_WBC() # QP of the WBC
- self.box_qp_wbc = BOX_QP_WBC() # QP of the WBC
+ # self.box_qp_wbc = BOX_QP_WBC() # QP of the WBC
+ self.box_qp = lrw.QPWBC()
self.x = np.zeros(18) # solution of WBC QP
self.error = False # Set to True when an error happens in the controller
@@ -91,20 +92,38 @@ class controller():
"""self.box_qp_wbc.compute_matrices(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_cmd, np.array([f_cmd]).T, contacts)
self.box_qp_wbc.create_weight_matrices()"""
# Solve QP problem
- self.box_qp_wbc.compute(self.invKin.rmodel, self.invKin.rdata,
- q.copy(), dq.copy(), ddq_cmd, np.array([f_cmd]).T, contacts)
+ # self.box_qp_wbc.compute(self.invKin.rmodel, self.invKin.rdata,
+ # q.copy(), dq.copy(), ddq_cmd, np.array([f_cmd]).T, contacts)
# Solve QP problem
self.qp_wbc.compute(self.invKin.rmodel, self.invKin.rdata,
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)
+
+ # Contact Jacobian
+ # Indexes of feet frames in this order: [FL, FR, HL, HR]
+ indexes = [10, 18, 26, 34]
+ Jc = np.zeros((12, 18))
+ for i in range(4):
+ 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()
+
+ self.box_qp.run(M.copy(), Jc.copy(), f_cmd.reshape((-1, 1)).copy(), RNEA.reshape((-1, 1)).copy())
+
"""if dq[0, 0] > 0.4:
from IPython import embed
embed()"""
# Retrieve joint torques
- tmp_res = self.box_qp_wbc.get_joint_torques(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_cmd)
+ # 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()
self.toc = time()
@@ -687,8 +706,8 @@ class BOX_QP_WBC():
embed()"""
print(self.f_res.ravel())
print(self.x.ravel())
- from IPython import embed
- embed()
+ """from IPython import embed
+ embed()"""
return 0
diff --git a/src/InvKin.cpp b/src/InvKin.cpp
index d9083a010e18cb93652f1487648e506e29d21ff3..917c01fae071762c3699edf7439022d7736be595 100644
--- a/src/InvKin.cpp
+++ b/src/InvKin.cpp
@@ -19,13 +19,6 @@ Eigen::Matrix<double, 1, 3> InvKin::cross3(Eigen::Matrix<double, 1, 3> left, Eig
return res;
}
-template<typename _Matrix_Type_>
-_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon())
-{
- Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);
- double tolerance = epsilon * static_cast<double>(std::max(a.cols(), a.rows())) *svd.singularValues().array().abs()(0);
- return svd.matrixV() * (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
-}
Eigen::MatrixXd InvKin::refreshAndCompute(const Eigen::MatrixXd &x_cmd, const Eigen::MatrixXd &contacts,
const Eigen::MatrixXd &goals, const Eigen::MatrixXd &vgoals, const Eigen::MatrixXd &agoals,
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index 5d5dd12d0e421bee84d4819a5b9bedf23400a8da..d62bbef85f62e5e68f1a31a9884c5caf68e57198 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -67,7 +67,7 @@ int QPWBC::create_ML() {
std::fill_n(v_ML, size_nz_ML, 0.0);
// ML is the identity matrix of size 12
- for (int k = 0; k < 12; k++) {
+ for (int k = 0; k < 16; k++) {
add_to_ML(k, k, 1.0, r_ML, c_ML, v_ML);
}
@@ -77,8 +77,10 @@ int QPWBC::create_ML() {
double *acc; // coeff values
int nst = cpt_ML; // number of non zero elements
int ncc = st_to_cc_size(nst, r_ML, c_ML); // number of CC values
- int m = 12; // number of rows
- int n = 12; // number of columns
+ int m = 16; // number of rows
+ int n = 16; // number of columns
+
+ std::cout << "Number of CC values: " << ncc << std::endl;
int i_min = i4vec_min(nst, r_ML);
int i_max = i4vec_max(nst, r_ML);
@@ -97,8 +99,8 @@ int QPWBC::create_ML() {
// Assign values to the csc object
ML = (csc *)c_malloc(sizeof(csc));
- ML->m = 12;
- ML->n = 12;
+ ML->m = 16;
+ ML->n = 16;
ML->nz = -1;
ML->nzmax = ncc;
ML->x = acc;
@@ -129,7 +131,7 @@ int QPWBC::create_weight_matrices() {
// can have a non zero value
for (int i = 0; i < 16; i++) {
for (int j = 0; j < 16; j++) {
- add_to_P(j, i, 1.0, r_P, c_P, v_P);
+ add_to_P(i, j, 1.0, r_P, c_P, v_P);
}
}
@@ -142,6 +144,8 @@ int QPWBC::create_weight_matrices() {
int m = 16; // number of rows
int n = 16; // number of columns
+ std::cout << "Number of CC values: " << ncc << std::endl;
+
// Get the CC indices.
icc = (int *)malloc(ncc * sizeof(int));
ccc = (int *)malloc((n + 1) * sizeof(int));
@@ -177,7 +181,7 @@ int QPWBC::create_weight_matrices() {
/*
Create an initial guess and call the solver to solve the QP problem
*/
-int QPWBC::call_solver(int k) {
+int QPWBC::call_solver() {
// Initial guess for forces (mass evenly supported by all legs in contact)
//warmxf.block(0, 0, 12 * (n_steps - 1), 1) = x.block(12, 0, 12 * (n_steps - 1), 1);
//warmxf.block(12 * n_steps, 0, 12 * (n_steps - 1), 1) = x.block(12 * (n_steps + 1), 0, 12 * (n_steps - 1), 1);
@@ -185,16 +189,33 @@ int QPWBC::call_solver(int k) {
//Eigen::Matrix<double, Eigen::Dynamic, 1>::Map(&v_warmxf[0], warmxf.size()) = warmxf;
// Setup the solver (first iteration) then just update it
- if (k == 0) // Setup the solver with the matrices
+ if (not initialized) // Setup the solver with the matrices
{
data = (OSQPData *)c_malloc(sizeof(OSQPData));
data->n = 16; // number of variables
data->m = 16; // number of constraints
data->P = P; // the upper triangular part of the quadratic cost matrix P in csc format (size n x n)
data->A = ML; // linear constraints matrix A in csc format (size m x n)
- data->q = &Q[0]; // dense array for linear part of cost function (size n)
- data->l = &v_NK_low[0]; // dense array for lower bound (size m)
- data->u = &v_NK_up[0]; // dense array for upper bound (size m)
+ data->q = Q; // dense array for linear part of cost function (size n)
+ data->l = v_NK_low; // dense array for lower bound (size m)
+ data->u = v_NK_up; // dense array for upper bound (size m)
+
+ std::cout << "PASS" << std::endl;
+
+ for (int j = 0; j < 16; j++) {
+ std::cout << Q[j] << " ";
+ }
+ std::cout << std::endl;
+ std::cout << "--" << std::endl;
+ for (int j = 0; j < 16; j++) {
+ std::cout << v_NK_low[j] << " ";
+ }
+ std::cout << std::endl;
+ std::cout << "--" << std::endl;
+ for (int j = 0; j < 16; j++) {
+ std::cout << v_NK_up[j] << " ";
+ }
+ std::cout << std::endl;
/*save_csc_matrix(ML, "ML");
save_csc_matrix(P, "P");
@@ -205,34 +226,59 @@ int QPWBC::call_solver(int k) {
// 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;
- osqp_setup(&workspce, data, settings);
+ settings->adaptive_rho_fraction = (float)0.7;*/
+ settings->verbose = true;
+ int exitflag = 0;
+ exitflag = osqp_setup(&workspce, data, settings);
+ std::cout << "Setup exitflag: " << exitflag << std::endl;
+ std::cout << "PASS 2" << std::endl;
/*self.prob.setup(P=self.P, q=self.Q, A=self.ML, l=self.NK_inf, u=self.NK.ravel(), verbose=False)
self.prob.update_settings(eps_abs=1e-5)
self.prob.update_settings(eps_rel=1e-5)*/
+
+ initialized = true;
} else // Code to update the QP problem without creating it again
{
+ std::cout << "PASS 3" << std::endl;
osqp_update_P(workspce, &P->x[0], OSQP_NULL, 0);
+ std::cout << "PASS 4" << std::endl;
osqp_update_lin_cost(workspce, &Q[0]);
// osqp_update_A(workspce, &ML->x[0], OSQP_NULL, 0);
// osqp_update_bounds(workspce, &v_NK_low[0], &v_NK_up[0]);
// osqp_warm_start_x(workspce, &v_warmxf[0]);
}
+ std::cout << "PASS 5" << std::endl;
+
+ /*char t_char[1] = {'P'};
+ my_print_csc_matrix(P, t_char);
+
+ char tm_char[1] = {'M'};
+ my_print_csc_matrix(ML, tm_char);*/
+ double v_warmxf[16] = {};
+ std::fill_n(v_warmxf, 16, 2.0);
+
+ std::cout << "PASS 5.5" << std::endl;
+
+ osqp_warm_start_x(workspce, v_warmxf);
+
+ std::cout << "Warm" << std::endl;
// Run the solver to solve the QP problem
osqp_solve(workspce);
+
+ std::cout << "PASS 6" << std::endl;
/*self.sol = self.prob.solve()
self.x = self.sol.x*/
// solution in workspce->solution->x
@@ -243,7 +289,7 @@ int QPWBC::call_solver(int k) {
/*
Extract relevant information from the output of the QP solver
*/
-int QPWBC::retrieve_result() {
+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];
@@ -272,10 +318,6 @@ int QPWBC::retrieve_result() {
return 0;
}
-/*
-Return the latest desired contact forces that have been computed
-*/
-Eigen::MatrixXd QPWBC::get_latest_result() { return x_f_applied; }
/*
Return the next predicted state of the base
@@ -295,23 +337,32 @@ int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen:
create_matrices();
}
+ std::cout << "Creation done" << std::endl;
+
compute_matrices(M, Jc, f_cmd, RNEA);
+
+ std::cout << "compute_matrices done" << std::endl;
+
update_PQ();
+ std::cout << "update_PQ done" << std::endl;
+
// Create an initial guess and call the solver to solve the QP problem
call_solver();
+ std::cout << "call_solver done" << std::endl;
+
// Extract relevant information from the output of the QP solver
- retrieve_result();
+ // retrieve_result(f_cmd);
+
+ std::cout << "retrieve done" << std::endl;
+
+ //char t_char[1] = {'P'};
+ //my_print_csc_matrix(P, t_char);
return 0;
}
-/*
-Set all the parameters of the OSQP solver
-*/
-int set_settings() { return 0; }
-
void QPWBC::my_print_csc_matrix(csc *M, const char *name) {
c_int j, i, row_start, row_stop;
c_int k = 0;
@@ -330,10 +381,8 @@ void QPWBC::my_print_csc_matrix(csc *M, const char *name) {
int a = (int)M->i[i];
int b = (int)j;
double c = M->x[k++];
- if ((a >= 12 * (n_steps - 1)) && (a < 12 * (n_steps - 1) + 24) && (b >= 12 * (n_steps - 1)) &&
- (b < 12 * (n_steps - 1) * 2)) {
- c_print("\t%3u [%3u,%3u] = %.3g\n", k - 1, a, b - 12 * n_steps, c);
- }
+ c_print("\t%3u [%3u,%3u] = %.3g\n", k - 1, a, b, c);
+
}
}
}
@@ -382,7 +431,7 @@ void QPWBC::compute_matrices(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc
// Compute all matrices of the Box QP problem
Y = M.block(0, 0, 6, 6);
- X = Jc.block(0, 0, 12, 6).transpose()
+ X = Jc.block(0, 0, 12, 6).transpose();
Yinv = pseudoInverse(Y);
A = Yinv * X;
gamma = Yinv * ((X * f_cmd) - RNEA);
diff --git a/src/main.cpp b/src/main.cpp
index cc7b8449dada54975454b4edf32bc897713f89e2..25fce8efc3dde6bcf88e36acf94d0c3b5df86c74 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -9,6 +9,9 @@
#include "quadruped-reactive-walking/Planner.hpp"
#include "pinocchio/math/rpy.hpp"
+#include "eiquadprog/eiquadprog-rt.hpp"
+using namespace eiquadprog::solvers;
+
int main(int argc, char** argv) {
if (argc == 3) {
int arg_a = std::atoi(argv[1]), arg_b = std::atoi(argv[2]);
@@ -23,7 +26,48 @@ int main(int argc, char** argv) {
Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitX());
std::cout << pinocchio::rpy::matrixToRpy(quat.toRotationMatrix()) << std::endl;
- std::cout << "-- Test Planner --" << std::endl;
+ std::cout << "-- Test Eiquadprog --" << std::endl;
+
+ RtEiquadprog<2, 0, 2> qp;
+
+ RtMatrixX<2, 2>::d Q;
+ Q.setZero();
+ Q(0, 0) = 1.0;
+ Q(1, 1) = 1.0;
+
+ RtVectorX<2>::d C;
+ C.setZero();
+
+ RtMatrixX<0, 2>::d Aeq;
+
+ RtVectorX<0>::d Beq(0);
+
+ RtMatrixX<2, 2>::d Aineq;
+ Aineq.setZero();
+ Aineq(0, 0) = 1.;
+ Aineq(1, 1) = 1.;
+
+ RtVectorX<2>::d Bineq;
+ Bineq(0) = -1.;
+ Bineq(1) = -1.;
+
+ RtVectorX<2>::d x;
+
+ RtVectorX<2>::d solution;
+ solution(0) = 1.;
+ solution(1) = 1.;
+
+ double val = 1.;
+
+ RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
+
+ RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+
+ std::cout << "Solution: " << std::endl;
+ std::cout << x << std::endl;
+
+
+ /*std::cout << "-- Test Planner --" << std::endl;
double dt_in = 0.02; // Time step of the MPC
double dt_wbc_in = 0.002; // Time step of the WBC
@@ -54,7 +98,7 @@ int main(int argc, char** argv) {
std::cout << "#### " << k << std::endl;
planner.Print();
}
- }
+ }*/
return EXIT_SUCCESS;
} else {