Debugging WBC

scripts/Controller.py

@@ -323,13 +323,13 @@ class Controller:
         # Retrieve reference contact forces in horizontal frame
         self.x_f_mpc = self.mpc_wrapper.get_latest_result()
 
-        """if self.k >= 8220 and (self.k % self.k_mpc == 0):
+        if self.k >= 8220 and (self.k % self.k_mpc == 0):
             print(self.k)
             print(self.x_f_mpc[:, 0])
             from matplotlib import pyplot as plt
             plt.figure()
             plt.plot(self.x_f_mpc[6, :])
-            plt.show(block=True)"""
+            plt.show(block=True)
 
         # Store o_targetFootstep, used with MPC_planner
         self.o_targetFootstep = o_targetFootstep.copy()

src/InvKin.cpp

@@ -77,10 +77,10 @@ void InvKin::initialize(Params& params) {
 
 void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals, Matrix43 const& vgoals,
                                Matrix43 const& agoals) {
-  /*std::cout << std::fixed;
-  std::cout << std::setprecision(2);
+  std::cout << std::fixed;
+  std::cout << std::setprecision(5);
 
-  std::cout << "pgoals:" << std::endl;
+  /*std::cout << "pgoals:" << std::endl;
   std::cout << pgoals << std::endl;
   std::cout << "posf_" << std::endl;
   std::cout << posf_ << std::endl;*/

src/QPWBC.cpp

@@ -736,6 +736,16 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
   // std::cout << "agoals " << std::endl << agoals << std::endl;
   // std::cout << "ddq_cmd_bis " << std::endl << ddq_cmd_.transpose() << std::endl;
 
+  // std::cout << "M : " << std::endl << data_.M.block(0, 0, 3, 18) << std::endl;
+  // std::cout << "ddq: " << std::endl << ddq_cmd_.transpose() << std::endl;
+  
+  std::cout << "-- BEFORE QP PROBLEM --" << std::endl;
+  std::cout << "M ddq_u: " << std::endl << (data_.M.block(0, 0, 3, 6) * ddq_cmd_.head(6)).transpose() << std::endl;
+  std::cout << "M ddq_a: " << std::endl << (data_.M.block(0, 6, 3, 12) * ddq_cmd_.tail(12)).transpose() << std::endl; 
+  pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, VectorN::Zero(model_.nv));
+  std::cout << "Non linear effects: " << std::endl << data_.tau.head(6).transpose() << std::endl;
+  std::cout << "JcT f_cmd: " << std::endl << (Jc_.transpose() * (f_cmd + f_compensation)).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_);
 
@@ -744,19 +754,19 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
   ddq_with_delta_.head(6) = ddq_cmd_.head(6) + box_qp_->get_ddq_res();
   ddq_with_delta_.tail(12) = ddq_cmd_.tail(12);
 
-  /* DEBUG INERTIA AND NON LINEAR EFFECTS
+  // DEBUG INERTIA AND NON LINEAR EFFECTS
 
   Vector6 left = data_.M.block(0, 0, 6, 6) * box_qp_->get_ddq_res() - Jc_.transpose() * box_qp_->get_f_res();
   Vector6 right = - data_.tau.head(6) + Jc_.transpose() * (f_cmd + f_compensation);
   Vector6 tmp_RNEA = data_.tau.head(6);
 
   //std::cout << "RNEA: " << std::endl << data_.tau.head(6).transpose() << std::endl;
-  //std::cout << "left: " << std::endl << left.transpose() << std::endl;
-  //std::cout << "right: " << std::endl << right.transpose() << std::endl;
+  std::cout << "left: " << std::endl << left.transpose() << std::endl;
+  std::cout << "right: " << std::endl << right.transpose() << std::endl;
   //std::cout << "M: " << std::endl << data_.M.block(0, 0, 6, 6) << std::endl;
   //std::cout << "JcT: " << std::endl << Jc_.transpose() << std::endl;
   //std::cout << "M: " << std::endl << data_.M.block(0, 0, 3, 18) << std::endl;
-
+  /*
   pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, VectorN::Zero(model_.nv));
   Vector6 tmp_NLE = data_.tau.head(6);
 
@@ -824,6 +834,17 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
   pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_test);
   std::cout << "M DDQ Delta Bis: " << std::endl << (data_.tau.head(6) - tmp_NLE).transpose() << std::endl;*/
 
+
+  std::cout << "-- AFTER QP PROBLEM --" << std::endl;
+  std::cout << "M ddq_u: " << std::endl << (data_.M.block(0, 0, 3, 6) * ddq_with_delta_.head(6)).transpose() << std::endl;
+  std::cout << "M ddq_a: " << std::endl << (data_.M.block(0, 6, 3, 12) * ddq_with_delta_.tail(12)).transpose() << std::endl; 
+  pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, VectorN::Zero(model_.nv));
+  std::cout << "Non linear effects: " << std::endl << data_.tau.head(6).transpose() << std::endl;
+  std::cout << "JcT f_cmd: " << std::endl << (Jc_.transpose() * f_with_delta_).transpose() << std::endl;
+
+  std::cout << "LEFT " << (tmp_RNEA.head(3) + data_.M.block(0, 0, 3, 6) * box_qp_->get_ddq_res()).transpose() << std::endl;
+
+
   // Increment log counter
   k_log_++;
 }

src/walk_parameters.yaml

@@ -5,22 +5,22 @@ robot:
     DEMONSTRATION: false  # Enable/disable demonstration functionalities
     SIMULATION: true  # Enable/disable PyBullet simulation or running on real robot
     LOGGING: false  # Enable/disable logging during the experiment
-    PLOTTING: false  # Enable/disable automatic plotting at the end of the experiment
+    PLOTTING: true  # Enable/disable automatic plotting at the end of the experiment
     envID: 0  # Identifier of the environment to choose in which one the simulation will happen
     use_flat_plane: true  # If True the ground is flat, otherwise it has bumps
-    predefined_vel: false  # If we are using a predefined reference velocity (True) or a joystick (False)
+    predefined_vel: true  # If we are using a predefined reference velocity (True) or a joystick (False)
     velID: 10  # Identifier of the reference velocity profile to choose which one will be sent to the robot
-    N_SIMULATION: 100000  # Number of simulated wbc time steps
-    enable_pyb_GUI: true  # Enable/disable PyBullet GUI
+    N_SIMULATION: 9000  # Number of simulated wbc time steps
+    enable_pyb_GUI: false  # Enable/disable PyBullet GUI
     enable_corba_viewer: true  # Enable/disable Corba Viewer
-    enable_multiprocessing: true  # Enable/disable running the MPC in another process in parallel of the main loop
+    enable_multiprocessing: false  # Enable/disable running the MPC in another process in parallel of the main loop
     perfect_estimator: false  # Enable/disable perfect estimator by using data directly from PyBullet
     
     # General control parameters
     # [0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583] # h_com = 0.2
-    # q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407]  # h_com = 0.218
-    q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4]  # Initial articular positions
-    dt_wbc: 0.002  # Time step of the whole body control
+    q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407]  # h_com = 0.218
+    # q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4]  # Initial articular positions
+    dt_wbc: 0.001  # Time step of the whole body control
     dt_mpc: 0.02  # Time step of the model predictive control
     type_MPC: 0  # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
     Kp_main: [3.0, 3.0, 3.0]  # Proportional gains for the PD+