From 5e351f9f77325d9d7e8b028d587e6563a087050d Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Wed, 25 Aug 2021 16:21:58 +0200
Subject: [PATCH] Kp and Kd are now vectors of size 3 + Ngait is automatically
chosen
---
include/qrw/Params.hpp | 8 ++++----
src/Params.cpp | 16 +++++-----------
2 files changed, 9 insertions(+), 15 deletions(-)
diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
index 47a79e73..ec0b3bb3 100644
--- a/include/qrw/Params.hpp
+++ b/include/qrw/Params.hpp
@@ -56,15 +56,13 @@ class Params {
// General control parameters
std::vector<double> q_init; // Initial articular positions
double dt_wbc; // Time step of the whole body control
- int N_gait; // Number of rows in the gait matrix. Arbitrary value that should be set high enough so that there is
- // always at least one empty line at the end of the gait matrix
double dt_mpc; // Time step of the model predictive control
double T_gait; // Duration of one gait period
double T_mpc; // Duration of the 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
- double Kp_main; // Proportional gains for the PD+
- double Kd_main; // Derivative gains for the PD+
+ 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 Estimator
@@ -94,6 +92,8 @@ class Params {
double Fz_min; // Minimal vertical contact force [N]
// Not defined in yaml
+ int N_gait; // Number of rows in the gait matrix. Arbitrary value that should be set high enough so that there is
+ // always at least one empty line at the end of the gait matrix
double mass; // Mass of the robot
std::vector<double> I_mat; // Inertia matrix
double h_ref; // Reference height for the base
diff --git a/src/Params.cpp b/src/Params.cpp
index 9201be39..6609ed9d 100644
--- a/src/Params.cpp
+++ b/src/Params.cpp
@@ -19,14 +19,13 @@ Params::Params()
q_init(12, 0.0), // Fill with zeros, will be filled with values later
dt_wbc(0.0),
- N_gait(0),
dt_mpc(0.0),
T_gait(0.0),
T_mpc(0.0),
type_MPC(0),
kf_enabled(false),
- Kp_main(0.0),
- Kd_main(0.0),
+ Kp_main(3, 0.0),
+ Kd_main(3, 0.0),
Kff_main(0.0),
fc_v_esti(0.0),
@@ -49,6 +48,7 @@ Params::Params()
Fz_max(0.0),
Fz_min(0.0),
+ N_gait(0),
mass(0.0),
I_mat(9, 0.0), // Fill with zeros, will be filled with values later
h_ref(0.0),
@@ -84,9 +84,6 @@ void Params::initialize(const std::string& file_path) {
assert_yaml_parsing(robot_node, "robot", "dt_wbc");
dt_wbc = robot_node["dt_wbc"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "N_gait");
- N_gait = robot_node["N_gait"].as<int>();
-
assert_yaml_parsing(robot_node, "robot", "envID");
envID = robot_node["envID"].as<int>();
@@ -117,9 +114,6 @@ void Params::initialize(const std::string& file_path) {
assert_yaml_parsing(robot_node, "robot", "predefined_vel");
predefined_vel = robot_node["predefined_vel"].as<bool>();
- assert_yaml_parsing(robot_node, "robot", "kf_enabled");
- kf_enabled = robot_node["kf_enabled"].as<bool>();
-
assert_yaml_parsing(robot_node, "robot", "enable_pyb_GUI");
enable_pyb_GUI = robot_node["enable_pyb_GUI"].as<bool>();
@@ -133,10 +127,10 @@ void Params::initialize(const std::string& file_path) {
perfect_estimator = robot_node["perfect_estimator"].as<bool>();
assert_yaml_parsing(robot_node, "robot", "Kp_main");
- Kp_main = robot_node["Kp_main"].as<double>();
+ Kp_main = robot_node["Kp_main"].as<std::vector<double> >();
assert_yaml_parsing(robot_node, "robot", "Kd_main");
- Kd_main = robot_node["Kd_main"].as<double>();
+ Kd_main = robot_node["Kd_main"].as<std::vector<double> >();
assert_yaml_parsing(robot_node, "robot", "Kff_main");
Kff_main = robot_node["Kff_main"].as<double>();
--
GitLab