From 03f525fd732836b422ae3ffb158950be2a1d0381 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Wed, 20 Oct 2021 12:05:38 +0200
Subject: [PATCH] Formatting Params and QPWBC
---
include/qrw/Params.hpp | 58 ++++++++++++++++++++++++------------------
include/qrw/QPWBC.hpp | 35 +++++++++++++++----------
src/Params.cpp | 17 +++++--------
src/QPWBC.cpp | 42 +++++++++++++++---------------
4 files changed, 81 insertions(+), 71 deletions(-)
diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
index 35d7bc54..799929ff 100644
--- a/include/qrw/Params.hpp
+++ b/include/qrw/Params.hpp
@@ -34,6 +34,8 @@ class Params {
///
/// \brief Initializer
///
+ /// \param[in] file_path File path to the yaml file
+ ///
////////////////////////////////////////////////////////////////////////////////////////////////
void initialize(const std::string& file_path);
@@ -48,31 +50,31 @@ class Params {
// See .yaml file for meaning of parameters
// General parameters
- std::string config_file; // Name of the yaml file containing hardware information
- std::string interface; // Name of the communication interface (check with ifconfig)
- bool SIMULATION; // Enable/disable PyBullet simulation or running on real robot
- bool LOGGING; // Enable/disable logging during the experiment
- bool PLOTTING; // Enable/disable automatic plotting at the end of the experiment
- int envID; // Identifier of the environment to choose in which one the simulation will happen
- bool use_flat_plane; // If True the ground is flat, otherwise it has bumps
- bool predefined_vel; // If we are using a predefined reference velocity (True) or a joystick (False)
- int velID; // Identifier of the reference velocity profile to choose which one will be sent to the robot
- int N_SIMULATION; // Number of simulated wbc time steps
- bool enable_pyb_GUI; // Enable/disable PyBullet GUI
+ std::string config_file; // Name of the yaml file containing hardware information
+ std::string interface; // Name of the communication interface (check with ifconfig)
+ bool SIMULATION; // Enable/disable PyBullet simulation or running on real robot
+ bool LOGGING; // Enable/disable logging during the experiment
+ bool PLOTTING; // Enable/disable automatic plotting at the end of the experiment
+ int envID; // Identifier of the environment to choose in which one the simulation will happen
+ bool use_flat_plane; // If True the ground is flat, otherwise it has bumps
+ bool predefined_vel; // If we are using a predefined reference velocity (True) or a joystick (False)
+ int velID; // Identifier of the reference velocity profile for interpolation
+ int N_SIMULATION; // Number of simulated wbc time steps
+ bool enable_pyb_GUI; // Enable/disable PyBullet GUI
bool enable_corba_viewer; // Enable/disable Corba Viewer
bool enable_multiprocessing; // Enable/disable running the MPC in another process in parallel of the main loop
bool perfect_estimator; // Enable/disable perfect estimator by using data directly from PyBullet
// General control parameters
- std::vector<double> q_init; // Initial articular positions
- double dt_wbc; // Time step of the whole body control
- double dt_mpc; // Time step of the model predictive control
- int N_periods; // Number of gait periods in the MPC prediction horizon
- int type_MPC; // Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
- bool kf_enabled; // Use complementary filter (False) or kalman filter (True) for the estimator
- std::vector<double> Kp_main; // Proportional gains for the PD+
- std::vector<double> Kd_main; // Derivative gains for the PD+
- double Kff_main; // Feedforward torques multiplier for the PD+
+ std::vector<double> q_init; // Initial articular positions
+ double dt_wbc; // Time step of the whole body control
+ double dt_mpc; // Time step of the model predictive control
+ int N_periods; // Number of gait periods in the MPC prediction horizon
+ int type_MPC; // Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
+ bool kf_enabled; // Use complementary filter (False) or kalman filter (True) for the estimator
+ std::vector<double> Kp_main; // Proportional gains for the PD+
+ std::vector<double> Kd_main; // Derivative gains for the PD+
+ double Kff_main; // Feedforward torques multiplier for the PD+
// Parameters of Gait
std::vector<int> gait_vec; // Initial gait matrix (vector)
@@ -98,10 +100,10 @@ class Params {
double osqp_Nz_lim; // Maximum vertical force that can be applied at contact points
// Parameters of InvKin
- double Kp_flyingfeet; // Proportional gain for feet position tasks
- double Kd_flyingfeet; // Derivative gain for feet position tasks
- std::vector<double> Kp_base_position; // Proportional gains for the base position task
- std::vector<double> Kd_base_position; // Derivative gains for the base position task
+ double Kp_flyingfeet; // Proportional gain for feet position tasks
+ double Kd_flyingfeet; // Derivative gain for feet position tasks
+ std::vector<double> Kp_base_position; // Proportional gains for the base position task
+ std::vector<double> Kd_base_position; // Derivative gains for the base position task
std::vector<double> Kp_base_orientation; // Proportional gains for the base orientation task
std::vector<double> Kd_base_orientation; // Derivative gains for the base orientation task
std::vector<double> w_tasks; //Â Tasks weights: [feet/base, vx, vy, vz, roll+wroll, pitch+wpitch, wyaw, contacts]
@@ -113,7 +115,7 @@ class Params {
double Fz_min; // Minimal vertical contact force [N]
// Not defined in yaml
- Eigen::MatrixXd gait; // Initial gait matrix (Eigen)
+ Eigen::MatrixXd gait; // Initial gait matrix (Eigen)
double T_gait; // Period of the gait
double mass; // Mass of the robot
std::vector<double> I_mat; // Inertia matrix
@@ -128,6 +130,10 @@ class Params {
///
/// \brief Check if a parameter exists in a given yaml file (bofore we try retrieving its value)
///
+/// \param[in] yaml_node Name of the yaml file
+/// \param[in] parent_node_name Name of the parent node
+/// \param[in] child_node_name Name of the child node
+///
////////////////////////////////////////////////////////////////////////////////////////////////
namespace yaml_control_interface {
#define assert_yaml_parsing(yaml_node, parent_node_name, child_node_name) \
@@ -144,6 +150,8 @@ namespace yaml_control_interface {
///
/// \brief Check if a file exists (before we try loading it)
///
+/// \param[in] filename File path to check
+///
////////////////////////////////////////////////////////////////////////////////////////////////
#define assert_file_exists(filename) \
std::ifstream f(filename.c_str()); \
diff --git a/include/qrw/QPWBC.hpp b/include/qrw/QPWBC.hpp
index c10d89c1..d4235ba1 100644
--- a/include/qrw/QPWBC.hpp
+++ b/include/qrw/QPWBC.hpp
@@ -64,6 +64,13 @@ class QPWBC {
/// \brief Run one iteration of the whole WBC QP problem by calling all the necessary functions (data retrieval,
/// update of constraint matrices, update of the solver, running the solver, retrieving result)
///
+ /// \param[in] M Generalized mass matrix
+ /// \param[in] Jc Contact Jacobian
+ /// \param[in] ddq_cmd Command base accelerations
+ /// \param[in] f_cmd Desired contact forces of the MPC
+ /// \param[in] RNEA M(q) ddq + NLE(q, dq) computed by rnea(q_wbc_, dq_wbc_, ddq_cmd_)
+ /// \param[in] k_contact Number of time step during which feet have been in the current stance phase
+ ///
////////////////////////////////////////////////////////////////////////////////////////////////
int run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &ddq_cmd,
const Eigen::MatrixXd &f_cmd, const Eigen::MatrixXd &RNEA, const Eigen::MatrixXd &k_contact);
@@ -230,7 +237,7 @@ class QPWBC {
// Cumulative non zero coefficients per column in friction cone constraint block
// In each column: 2, 2, 5, 2, 2, 5, 2, 2, 5, 2, 2, 5
- int fric_nz [12] = { 2, 4, 9, 11, 13, 18, 20, 22, 27, 29, 31, 36};
+ int fric_nz[12] = {2, 4, 9, 11, 13, 18, 20, 22, 27, 29, 31, 36};
// Generalized mass matrix and contact Jacobian (transposed)
// Eigen::Matrix<double, 6, 6> M = Eigen::Matrix<double, 6, 6>::Zero();
@@ -255,7 +262,7 @@ class QPWBC {
// Matrix ML
const static int size_nz_ML = (20 + 6) * 18;
- csc *ML; // Compressed Sparse Column matrix
+ csc *ML; // Compressed Sparse Column matrix
// Matrix NK
const static int size_nz_NK = (20 + 6);
@@ -265,7 +272,7 @@ class QPWBC {
// Matrix P
const static int size_nz_P = 18;
- csc *P; // Compressed Sparse Column matrix
+ csc *P; // Compressed Sparse Column matrix
// Matrix Q
const static int size_nz_Q = 18;
@@ -335,11 +342,11 @@ class WbcWrapper {
MatrixN get_feet_vel_target() { return log_feet_vel_target; }
MatrixN get_feet_acc_target() { return log_feet_acc_target; }
- VectorN get_Mddq() { return Mddq;};
- VectorN get_NLE() { return NLE;};
- VectorN get_JcTf() { return JcTf;};
- VectorN get_Mddq_out() { return Mddq_out;};
- VectorN get_JcTf_out() { return JcTf_out;};
+ VectorN get_Mddq() { return Mddq; };
+ VectorN get_NLE() { return NLE; };
+ VectorN get_JcTf() { return JcTf; };
+ VectorN get_Mddq_out() { return Mddq_out; };
+ VectorN get_JcTf_out() { return JcTf_out; };
private:
Params *params_; // Object that stores parameters
@@ -352,7 +359,7 @@ class WbcWrapper {
Eigen::Matrix<double, 18, 18> M_; // Mass matrix
Eigen::Matrix<double, 12, 6> Jc_; // Jacobian matrix
Matrix14 k_since_contact_; // Number of time step during which feet have been in the current stance phase
- Vector7 bdes_; // Desired base positions
+ Vector7 bdes_; // Desired base positions
Vector12 qdes_; // Desired actuator positions
Vector12 vdes_; // Desired actuator velocities
Vector12 tau_ff_; // Desired actuator torques (feedforward)
@@ -370,11 +377,11 @@ class WbcWrapper {
Matrix34 log_feet_acc_target; // Store the target feet accelerations
// Log
- Vector6 Mddq;
- Vector6 NLE;
- Vector6 JcTf;
- Vector6 Mddq_out;
- Vector6 JcTf_out;
+ Vector6 Mddq; // Generalized mass matrix * acceleration vector
+ Vector6 NLE; // Non linear effects
+ Vector6 JcTf; // Contact Jacobian * contact forces vector
+ Vector6 Mddq_out; // Generalized mass matrix * acceleration vector (after QP problem)
+ Vector6 JcTf_out; // Contact Jacobian * contact forces vector (after QP problem)
int k_log_; // Counter for logging purpose
int indexes_[4] = {10, 18, 26, 34}; // Indexes of feet frames in this order: [FL, FR, HL, HR]
diff --git a/src/Params.cpp b/src/Params.cpp
index dcc4df56..1ecb0f7e 100644
--- a/src/Params.cpp
+++ b/src/Params.cpp
@@ -45,11 +45,11 @@ Params::Params()
Kp_flyingfeet(0.0),
Kd_flyingfeet(0.0),
- Kp_base_position(3, 0.0), // Fill with zeros, will be filled with values later
- Kd_base_position(3, 0.0), // Fill with zeros, will be filled with values later
- Kp_base_orientation(3, 0.0), // Fill with zeros, will be filled with values later
- Kd_base_orientation(3, 0.0), // Fill with zeros, will be filled with values later
- w_tasks(8, 0.0), // Fill with zeros, will be filled with values later
+ Kp_base_position(3, 0.0), // Fill with zeros, will be filled with values later
+ Kd_base_position(3, 0.0), // Fill with zeros, will be filled with values later
+ Kp_base_orientation(3, 0.0), // Fill with zeros, will be filled with values later
+ Kd_base_orientation(3, 0.0), // Fill with zeros, will be filled with values later
+ w_tasks(8, 0.0), // Fill with zeros, will be filled with values later
Q1(0.0),
Q2(0.0),
@@ -195,7 +195,7 @@ void Params::initialize(const std::string& file_path) {
assert_yaml_parsing(robot_node, "robot", "Kd_base_orientation");
Kd_base_orientation = robot_node["Kd_base_orientation"].as<std::vector<double> >();
-
+
assert_yaml_parsing(robot_node, "robot", "w_tasks");
w_tasks = robot_node["w_tasks"].as<std::vector<double> >();
@@ -213,7 +213,6 @@ void Params::initialize(const std::string& file_path) {
}
void Params::convert_gait_vec() {
-
if (gait_vec.size() % 5 != 0) {
throw std::runtime_error(
"gait matrix in yaml is not in the correct format. It should have five "
@@ -238,8 +237,7 @@ void Params::convert_gait_vec() {
int k = 0;
for (uint i = 0; i < gait_vec.size() / 5; i++) {
for (int j = 0; j < gait_vec[5 * i]; j++) {
- gait.row(k) << gait_vec[5 * i + 1], gait_vec[5 * i + 2],
- gait_vec[5 * i + 3], gait_vec[5 * i + 4];
+ gait.row(k) << gait_vec[5 * i + 1], gait_vec[5 * i + 2], gait_vec[5 * i + 3], gait_vec[5 * i + 4];
k++;
}
}
@@ -248,5 +246,4 @@ void Params::convert_gait_vec() {
for (int i = 1; i < N_periods; i++) {
gait.block(i * N_gait, 0, N_gait, 4) = gait.block(0, 0, N_gait, 4);
}
-
}
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index 365d29fc..3a715478 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -77,7 +77,9 @@ int QPWBC::create_ML() {
// Friction cone constraints
for (int i = 0; i < 20; i++) {
for (int j = 0; j < 12; j++) {
- if (G(i, j) != 0.0) {add_to_ML(i, 6 + j, G(i, j), r_ML, c_ML, v_ML);}
+ if (G(i, j) != 0.0) {
+ add_to_ML(i, 6 + j, G(i, j), r_ML, c_ML, v_ML);
+ }
}
}
// Dynamics equation constraints
@@ -140,7 +142,6 @@ int QPWBC::create_ML() {
int QPWBC::create_NK(const Eigen::Matrix<double, 6, 12> &JcT, const Eigen::Matrix<double, 12, 1> &f_cmd,
const Eigen::Matrix<double, 6, 1> &RNEA) {
-
// Fill upper bound of the friction cone contraints
for (int i = 0; i < 4; i++) {
v_NK_up[5 * i + 0] = std::numeric_limits<double>::infinity();
@@ -153,9 +154,9 @@ int QPWBC::create_NK(const Eigen::Matrix<double, 6, 12> &JcT, const Eigen::Matri
// Fill lower bound of the friction cone contraints
for (int i = 0; i < 4; i++) {
v_NK_low[5 * i + 0] = f_cmd(3 * i + 0, 0) - mu * f_cmd(3 * i + 2, 0);
- v_NK_low[5 * i + 1] = - f_cmd(3 * i + 0, 0) - mu * f_cmd(3 * i + 2, 0);
+ v_NK_low[5 * i + 1] = -f_cmd(3 * i + 0, 0) - mu * f_cmd(3 * i + 2, 0);
v_NK_low[5 * i + 2] = f_cmd(3 * i + 1, 0) - mu * f_cmd(3 * i + 2, 0);
- v_NK_low[5 * i + 3] = - f_cmd(3 * i + 1, 0) - mu * f_cmd(3 * i + 2, 0);
+ v_NK_low[5 * i + 3] = -f_cmd(3 * i + 1, 0) - mu * f_cmd(3 * i + 2, 0);
v_NK_low[5 * i + 4] = Fz_min - f_cmd(3 * i + 2, 0);
}
@@ -181,10 +182,10 @@ 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 < 6; i++) {
- add_to_P(i, i, Q1(i, i), r_P, c_P, v_P);
+ add_to_P(i, i, Q1(i, i), r_P, c_P, v_P);
}
for (int i = 0; i < 12; i++) {
- add_to_P(6 + i, 6+i, Q2(i, i), r_P, c_P, v_P);
+ add_to_P(6 + i, 6 + i, Q2(i, i), r_P, c_P, v_P);
}
// Creation of CSC matrix
@@ -265,12 +266,11 @@ int QPWBC::update_matrices(const Eigen::Matrix<double, 6, 6> &M, const Eigen::Ma
}
int QPWBC::update_ML(const Eigen::Matrix<double, 6, 6> &M, const Eigen::Matrix<double, 6, 12> &JcT) {
-
// Update the part of ML that contains the dynamics constraint
// Coefficients are stored in column order and we want to update the block (20, 0, 6, 18)
// [0Â fric
// M -JcT] with fric having [2 2 5 2 2 5 2 2 5 2 2 5] non zeros coefficient for each one of its 12 columns
-
+
// Update M, no need to be careful because there is only zeros coefficients above M
for (int j = 0; j < 6; j++) {
for (int i = 0; i < 6; i++) {
@@ -392,8 +392,6 @@ Eigen::MatrixXd QPWBC::get_H() {
int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &ddq_cmd,
const Eigen::MatrixXd &f_cmd, const Eigen::MatrixXd &RNEA, const Eigen::MatrixXd &k_contact) {
-
-
// Create the constraint and weight matrices used by the QP solver
// Minimize x^T.P.x + 2 x^T.Q with constraints M.X == N and L.X <= K
/*
@@ -410,7 +408,7 @@ int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen:
update_matrices(M, Jc, f_cmd, RNEA);
// Update P and Q matrices of the cost function xT P x + 2 xT g
- //update_PQ();
+ // update_PQ();
Eigen::Matrix<double, 20, 1> Gf = G * f_cmd;
@@ -645,7 +643,6 @@ void WbcWrapper::initialize(Params ¶ms) {
void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_cmd, MatrixN const &contacts,
MatrixN const &pgoals, MatrixN const &vgoals, MatrixN const &agoals, VectorN const &xgoals) {
-
if (f_cmd.rows() != 12) {
throw std::runtime_error("f_cmd should be a vector of size 12");
}
@@ -661,8 +658,9 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
// Retrieve configuration data
q_wbc_.head(3) = q.head(3);
- q_wbc_.block(3, 0, 4, 1) = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(q(3, 0), q(4, 0), q(5, 0))).coeffs(); // Roll, Pitch
- q_wbc_.tail(12) = q.tail(12); // Encoders
+ q_wbc_.block(3, 0, 4, 1) =
+ pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(q(3, 0), q(4, 0), q(5, 0))).coeffs(); // Roll, Pitch
+ q_wbc_.tail(12) = q.tail(12); // Encoders
// Retrieve velocity data
dq_wbc_ = dq;
@@ -728,19 +726,18 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
std::cout << "k_since" << std::endl;
std::cout << k_since_contact_ << std::endl;*/
- //std::cout << "Force compensation " << std::endl;
-
+ // std::cout << "Force compensation " << std::endl;
+
/*for (int i = 0; i < 4; i++) {
f_compensation(3*i+2, 0) = 0.0;
}*/
- //std::cout << f_compensation << std::endl;
+ // std::cout << f_compensation << std::endl;
- //std::cout << "agoals " << std::endl << agoals << std::endl;
- //std::cout << "ddq_cmd_bis " << std::endl << ddq_cmd_.transpose() << std::endl;
+ // std::cout << "agoals " << std::endl << agoals << std::endl;
+ // std::cout << "ddq_cmd_bis " << std::endl << ddq_cmd_.transpose() << std::endl;
// Solve the QP problem
- box_qp_->run(data_.M, Jc_, ddq_cmd_, f_cmd + f_compensation, data_.tau.head(6),
- k_since_contact_);
+ box_qp_->run(data_.M, Jc_, ddq_cmd_, f_cmd + f_compensation, data_.tau.head(6), k_since_contact_);
// Add to reference quantities the deltas found by the QP solver
f_with_delta_ = f_cmd + f_compensation + box_qp_->get_f_res();
@@ -790,7 +787,8 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
std::cout << "Jf" << std::endl;
std::cout << invkin_->get_Jf().block(0, 6, 12, 12).transpose() << std::endl;*/
- // std::cout << " -- " << std::endl << invkin_->get_Jf().block(0, 6, 12, 12) << std::endl << " -- " << std::endl << Jc_u_ << std::endl;
+ // std::cout << " -- " << std::endl << invkin_->get_Jf().block(0, 6, 12, 12) << std::endl << " -- " << std::endl <<
+ // Jc_u_ << std::endl;
tau_ff_ = data_.tau.tail(12) - Jc_u_.transpose() * f_with_delta_;
// tau_ff_ = - Jc_u_.transpose() * (f_cmd + f_compensation);
--
GitLab