Add parameter to yaml config file to enable/disable compensation forces in QP of WBC

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

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

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

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)

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

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 &params) {
   // Params store parameters
   params_ = &params;
 
+  // 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;*/