Kp and Kd are now vectors of size 3 + Ngait is automatically chosen

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

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>();