Fix feet logging that was disabled since we switched to c++

include/qrw/InvKin.hpp

@@ -35,6 +35,7 @@ public:
     Matrix12 get_Jf() { return Jf_; }
     int get_foot_id(int i) { return foot_ids_[i];}
     Matrix43 get_posf() { return posf_; }
+    Matrix43 get_vf() { return vf_; }
 
 private:
     // Inputs of the constructor

include/qrw/QPWBC.hpp

@@ -152,12 +152,12 @@ public:
     VectorN get_vdes() { return vdes_; }
     VectorN get_tau_ff() { return tau_ff_; }
     VectorN get_f_with_delta() { return f_with_delta_; }
-    MatrixN get_feet_pos() { return MatrixN::Zero(3, 4); }
-    MatrixN get_feet_err() { return MatrixN::Zero(3, 4); }
-    MatrixN get_feet_vel() { return MatrixN::Zero(3, 4); }
-    MatrixN get_feet_pos_target() { return MatrixN::Zero(3, 4); }
-    MatrixN get_feet_vel_target() { return MatrixN::Zero(3, 4); }
-    MatrixN get_feet_acc_target() { return MatrixN::Zero(3, 4); }
+    MatrixN get_feet_pos() { return invkin_->get_posf().transpose(); }
+    MatrixN get_feet_err() { return log_feet_pos_target - invkin_->get_posf().transpose(); }
+    MatrixN get_feet_vel() { return invkin_->get_vf().transpose(); }
+    MatrixN get_feet_pos_target() { return log_feet_pos_target; }
+    MatrixN get_feet_vel_target() { return log_feet_vel_target; }
+    MatrixN get_feet_acc_target() { return log_feet_acc_target; }
 
 private:
     
@@ -182,6 +182,10 @@ private:
 
     Matrix43 posf_tmp_;  // Temporary matrix to store posf_ from invkin_
 
+    Matrix34 log_feet_pos_target;  // Store the target feet positions
+    Matrix34 log_feet_vel_target;  // Store the target feet velocities
+    Matrix34 log_feet_acc_target;  // Store the target feet accelerations
+
     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]
 };

scripts/MPC_Wrapper.py

@@ -3,8 +3,8 @@
 import numpy as np
 import libquadruped_reactive_walking as MPC
 from multiprocessing import Process, Value, Array
-# import crocoddyl_class.MPC_crocoddyl as MPC_crocoddyl
-# import crocoddyl_class.MPC_crocoddyl_planner as MPC_crocoddyl_planner
+import crocoddyl_class.MPC_crocoddyl as MPC_crocoddyl
+import crocoddyl_class.MPC_crocoddyl_planner as MPC_crocoddyl_planner
 import pinocchio as pin
 
 class Dummy:
@@ -38,7 +38,6 @@ class MPC_Wrapper:
         self.not_first_iter = False
 
         self.params = params
-        self.testoui = True
 
         # Number of WBC steps for 1 step of the MPC
         self.k_mpc = int(params.dt_mpc/params.dt_wbc)
@@ -47,7 +46,9 @@ class MPC_Wrapper:
         self.n_steps = np.int(params.T_mpc/params.dt_mpc)
         self.T_gait = params.T_gait
         self.N_gait = params.N_gait
-        self.gait_memory = np.zeros(4)
+        self.gait_past = np.zeros(4)
+        self.gait_next = np.zeros(4)
+        self.mass = params.mass
 
         self.mpc_type = params.type_MPC
         self.multiprocessing = params.enable_multiprocessing
@@ -100,21 +101,19 @@ class MPC_Wrapper:
         else:  # Run in the same process than main loop
             self.run_MPC_synchronous(k, xref, fsteps, l_targetFootstep)
 
-        if k > 2:
-            self.last_available_result[12:(12+self.n_steps), :] = np.roll(self.last_available_result[12:(12+self.n_steps), :], -1, axis=1)
-            self.testoui = False
-
-        pt = 0
-        while (np.any(gait[pt, :])):
-            pt += 1
-        if k > 2 and not np.array_equal(gait[0, :], gait[pt-1, :]):
-            mass = 2.5  # TODO: grab from URDF?
-            nb_ctc = np.sum(gait[pt-1, :])
-            F = 9.81 * mass / nb_ctc
-            self.last_available_result[12:24, self.n_steps-1] = np.zeros(12)
-            for i in range(4):
-                if (gait[pt-1, i] == 1):
-                    self.last_available_result[12+3*i+2, self.n_steps-1] = F
+        if not np.allclose(gait[0, :], self.gait_past):  # If gait status has changed
+            if np.allclose(gait[0, :], self.gait_next):  # If we're still doing what was planned the last time MPC was solved
+                self.last_available_result[12:24, 0] = self.last_available_result[12:24, 1].copy()
+            else:  # Otherwise use a default contact force command till we get the actual result of the MPC for this new sequence
+                F = 9.81 * self.mass / np.sum(gait[0, :])
+                self.last_available_result[12:24:3, 0] = 0.0
+                self.last_available_result[13:24:3, 0] = 0.0
+                self.last_available_result[14:24:3, 0] = F
+
+            self.last_available_result[12:24, 1:] = 0.0
+            self.gait_past = gait[0, :].copy()
+
+        self.gait_next = gait[1, :].copy()
 
         return 0
 

src/QPWBC.cpp

@@ -562,6 +562,9 @@ WbcWrapper::WbcWrapper()
     , f_with_delta_(Vector12::Zero())
     , ddq_with_delta_(Vector18::Zero())
     , posf_tmp_(Matrix43::Zero())
+    , log_feet_pos_target(Matrix34::Zero())
+    , log_feet_vel_target(Matrix34::Zero())
+    , log_feet_acc_target(Matrix34::Zero())
     , k_log_(0)
 {}
 
@@ -610,6 +613,11 @@ void WbcWrapper::compute(VectorN const& q, VectorN const& dq, MatrixN const& f_c
   k_since_contact_ += contacts;  // Increment feet in stance phase
   k_since_contact_ = k_since_contact_.cwiseProduct(contacts);  // Reset feet in swing phase
 
+  // Store target positions, velocities and acceleration for logging purpose
+  log_feet_pos_target = pgoals;
+  log_feet_vel_target = vgoals;
+  log_feet_acc_target = agoals;
+
   // Compute Inverse Kinematics
   invkin_->run_InvKin(q.tail(12), dq.tail(12), contacts, pgoals.transpose(), vgoals.transpose(), agoals.transpose());
   ddq_cmd_.tail(12) = invkin_->get_ddq_cmd();