From 9c976517a470928e1697bb119d91a9a1dbd504b0 Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Tue, 19 Apr 2022 14:15:18 +0200
Subject: [PATCH] Add parameter to yaml config file to enable/disable
compensation forces in QP of WBC
---
config/walk_parameters.yaml | 1 +
include/qrw/Params.hpp | 9 +++++----
include/qrw/WbcWrapper.hpp | 1 +
python/Params.cpp | 1 +
src/Params.cpp | 4 ++++
src/WbcWrapper.cpp | 34 ++++++++++++++++++++--------------
6 files changed, 32 insertions(+), 18 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 72bcfb2a..afa9e1b8 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -79,6 +79,7 @@ robot:
Q2: 10.0 # Weights for the "delta contact forces" optimization variables
Fz_max: 35.0 # Maximum vertical contact force [N]
Fz_min: 0.0 # Minimal vertical contact force [N]
+ enable_comp_forces: false # Enable the use of compensation forces in the QP problem
# Parameters fro solo3D simulation
solo3D: false # Activation of the 3D environment, and corresponding planner blocks
diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
index 80554286..6ec37eb4 100644
--- a/include/qrw/Params.hpp
+++ b/include/qrw/Params.hpp
@@ -134,10 +134,11 @@ class Params {
std::vector<double> w_tasks; //Â Tasks weights: [feet/base, vx, vy, vz, roll+wroll, pitch+wpitch, wyaw, contacts]
// Parameters of WBC QP problem
- double Q1; // Weights for the "delta articular accelerations" optimization variables
- double Q2; // Weights for the "delta contact forces" optimization variables
- double Fz_max; // Maximum vertical contact force [N]
- double Fz_min; // Minimal vertical contact force [N]
+ double Q1; // Weights for the "delta articular accelerations" optimization variables
+ double Q2; // Weights for the "delta contact forces" optimization variables
+ double Fz_max; // Maximum vertical contact force [N]
+ double Fz_min; // Minimal vertical contact force [N]
+ bool enable_comp_forces; // Enable the use of compensation forces in the QP problem
// Parameters of MIP
bool solo3D; // Enable the 3D environment with corresponding planner blocks
diff --git a/include/qrw/WbcWrapper.hpp b/include/qrw/WbcWrapper.hpp
index 99c4eb51..eccf0e36 100644
--- a/include/qrw/WbcWrapper.hpp
+++ b/include/qrw/WbcWrapper.hpp
@@ -116,6 +116,7 @@ class WbcWrapper {
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]
+ bool enable_comp_forces_; // Enable compensation forces for the QP problem
};
#endif // WBC_WRAPPER_H_INCLUDED
diff --git a/python/Params.cpp b/python/Params.cpp
index d903d3f7..d8db929f 100644
--- a/python/Params.cpp
+++ b/python/Params.cpp
@@ -51,6 +51,7 @@ struct ParamsVisitor : public bp::def_visitor<ParamsVisitor<Params>> {
.def_readwrite("vert_time", &Params::vert_time)
.def_readwrite("footsteps_init", &Params::footsteps_init)
.def_readwrite("footsteps_under_shoulders", &Params::footsteps_under_shoulders)
+ .def_readwrite("enable_comp_forces", &Params::enable_comp_forces)
.def_readwrite("solo3D", &Params::solo3D)
.def_readwrite("enable_multiprocessing_mip", &Params::enable_multiprocessing_mip)
.def_readwrite("environment_URDF", &Params::environment_URDF)
diff --git a/src/Params.cpp b/src/Params.cpp
index 414e4f81..2c19abe0 100644
--- a/src/Params.cpp
+++ b/src/Params.cpp
@@ -57,6 +57,7 @@ Params::Params()
Q2(0.0),
Fz_max(0.0),
Fz_min(0.0),
+ enable_comp_forces(false),
T_gait(0.0), // Period of the gait
mass(0.0), // Mass of the robot
@@ -221,6 +222,9 @@ void Params::initialize(const std::string& file_path) {
assert_yaml_parsing(robot_node, "robot", "Fz_min");
Fz_min = robot_node["Fz_min"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "enable_comp_forces");
+ enable_comp_forces = robot_node["enable_comp_forces"].as<bool>();
+
assert_yaml_parsing(robot_node, "robot", "solo3D");
solo3D = robot_node["solo3D"].as<bool>();
diff --git a/src/WbcWrapper.cpp b/src/WbcWrapper.cpp
index 1ad28388..c618b1ad 100644
--- a/src/WbcWrapper.cpp
+++ b/src/WbcWrapper.cpp
@@ -24,12 +24,16 @@ WbcWrapper::WbcWrapper()
log_feet_pos_target(Matrix34::Zero()),
log_feet_vel_target(Matrix34::Zero()),
log_feet_acc_target(Matrix34::Zero()),
- k_log_(0) {}
+ k_log_(0),
+ enable_comp_forces_(false) {}
void WbcWrapper::initialize(Params ¶ms) {
// Params store parameters
params_ = ¶ms;
+ // Set if compensation forces should be used or not
+ enable_comp_forces_ = params_->enable_comp_forces;
+
// Path to the robot URDF
const std::string filename = std::string(EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf");
@@ -124,19 +128,21 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
// Compute the inverse dynamics, aka the joint torques according to the current state of the system,
// the desired joint accelerations and the external forces, using the Recursive Newton Euler Algorithm.
// Result is stored in data_.tau
- /*pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_cmd_);
- Vector12 f_compensation = Vector12::Zero();*/
-
- // FORCE COMPENSATION TERM
- Vector18 ddq_test = Vector18::Zero();
- ddq_test.head(6) = ddq_cmd_.head(6);
- pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_test);
- Vector6 RNEA_without_joints = data_.tau.head(6);
- pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, VectorN::Zero(model_.nv));
- Vector6 RNEA_NLE = data_.tau.head(6);
- RNEA_NLE(2, 0) -= 9.81 * data_.mass[0];
- pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_cmd_);
- Vector12 f_compensation = pseudoInverse(Jc_.transpose()) * (data_.tau.head(6) - RNEA_without_joints + RNEA_NLE);
+ Vector12 f_compensation;
+ if (!enable_comp_forces_) {
+ pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_cmd_);
+ f_compensation = Vector12::Zero();
+ } else {
+ Vector18 ddq_test = Vector18::Zero();
+ ddq_test.head(6) = ddq_cmd_.head(6);
+ pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_test);
+ Vector6 RNEA_without_joints = data_.tau.head(6);
+ pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, VectorN::Zero(model_.nv));
+ Vector6 RNEA_NLE = data_.tau.head(6);
+ RNEA_NLE(2, 0) -= 9.81 * data_.mass[0];
+ pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_cmd_);
+ f_compensation = pseudoInverse(Jc_.transpose()) * (data_.tau.head(6) - RNEA_without_joints + RNEA_NLE);
+ }
/*std::cout << "M inertia" << std::endl;
std::cout << data_.M << std::endl;*/
--
GitLab