Log and display input state of the WBC

python/quadruped_reactive_walking/tools/LoggerControl.py

@@ -109,9 +109,7 @@ class LoggerControl:
         self.loop_o_v = np.zeros(
             [logSize, 18]
         )  # estimated velocity of the robot in the ideal world (h frame)
-        self.loop_h_v = np.zeros(
-            [logSize, 6]
-        )  # estimated velocity in horizontal frame
+        self.loop_h_v = np.zeros([logSize, 6])  # estimated velocity in horizontal frame
         self.loop_h_v_windowed = np.zeros(
             [logSize, 6]
         )  # estimated velocity in horizontal frame (windowed)
@@ -207,6 +205,11 @@ class LoggerControl:
             [logSize, 18]
         )  # position error of tasks in InvKin
 
+        # State vector given to the WBC
+        self.wbc_q = np.zeros([logSize, 18])
+        # Velocity state vector given to the WBC
+        self.wbc_dq = np.zeros([logSize, 18])
+
         # Timestamps
         self.tstamps = np.zeros(logSize)
 
@@ -322,6 +325,8 @@ class LoggerControl:
         self.wbc_tasks_acc[self.i] = wbc.get_tasks_acc()
         self.wbc_tasks_vel[self.i] = wbc.get_tasks_vel()
         self.wbc_tasks_err[self.i] = wbc.get_tasks_err()
+        self.wbc_q[self.i] = controller.q_wbc
+        self.wbc_dq[self.i] = controller.dq_wbc
 
         # Logging timestamp
         self.tstamps[self.i] = time()
@@ -348,7 +353,11 @@ class LoggerControl:
 
         # Robot world to Mocap initial translationa and rotation
         mTo = np.array(
-            [loggerSensors.mocapPosition[0, 0], loggerSensors.mocapPosition[0, 1], 0.0155]
+            [
+                loggerSensors.mocapPosition[0, 0],
+                loggerSensors.mocapPosition[0, 1],
+                0.0155,
+            ]
         )
         mRo = pin.rpy.rpyToMatrix(0.0, 0.0, self.mocap_RPY[0, 2])
 
@@ -647,8 +656,11 @@ class LoggerControl:
                 plt.plot(t_range, self.loop_o_q[:, i], "r", linewidth=3)
             else:
                 plt.plot(t_range, self.planner_xref[:, i, 1], "r", linewidth=3)
+
+            plt.plot(t_range, self.wbc_q[:, i], "forestgreen", linewidth=3)
+
             plt.legend(
-                ["Robot state", "Ground truth", "Robot reference state"],
+                ["Robot state", "Ground truth", "Robot reference state", "WBC state"],
                 prop={"size": 8},
             )
             plt.ylabel(lgd[i])
@@ -697,7 +709,10 @@ class LoggerControl:
             if i in [0, 1, 5]:
                 plt.plot(t_range, self.joy_v_ref[:, i], "r", linewidth=3)
             else:
-                plt.plot(t_range, self.planner_xref[:, 6+i, 1], "r", linewidth=3)
+                plt.plot(t_range, self.planner_xref[:, 6 + i, 1], "r", linewidth=3)
+
+            plt.plot(t_range, self.wbc_dq[:, i], "forestgreen", linewidth=3)
+
             if i < 3:
                 plt.legend(
                     [
@@ -706,11 +721,12 @@ class LoggerControl:
                         "State (LP 15Hz)",
                         "State (windowed)",
                         "Ref state",
+                        "WBC state"
                     ],
                     prop={"size": 8},
                 )
             else:
-                plt.legend(["State", "Ground truth", "Ref state"], prop={"size": 8})
+                plt.legend(["State", "Ground truth", "Ref state", "WBC state"], prop={"size": 8})
             plt.ylabel(lgd[i])
             plt.ylim([y_low[i], y_high[i]])
         self.custom_suptitle("Linear and angular velocities")
@@ -808,6 +824,7 @@ class LoggerControl:
                 plt.subplot(3, 4, index12[i], sharex=ax0)
             (h1,) = plt.plot(t_range, self.wbc_q_des[:, i], color="r", linewidth=3)
             (h2,) = plt.plot(t_range, self.loop_o_q[:, 6 + i], color="b", linewidth=3)
+            (h3,) = plt.plot(t_range, self.wbc_q[:, 6 + i], "forestgreen", linewidth=3)
             plt.xlabel("Time [s]")
             plt.ylabel(lgd1[i % 3] + " " + lgd2[int(i / 3)] + " [rad]")
             plt.legend(
@@ -833,6 +850,7 @@ class LoggerControl:
                 plt.subplot(3, 4, index12[i], sharex=ax0)
             (h1,) = plt.plot(t_range, self.wbc_v_des[:, i], color="r", linewidth=3)
             (h2,) = plt.plot(t_range, self.loop_o_v[:, 6 + i], color="b", linewidth=3)
+            (h3,) = plt.plot(t_range, self.wbc_dq[:, 6 + i], "forestgreen", linewidth=3)
             plt.xlabel("Time [s]")
             plt.ylabel(lgd1[i % 3] + " " + lgd2[int(i / 3)] + " [rad]")
             plt.legend(
@@ -1536,9 +1554,8 @@ if __name__ == "__main__":
 
     sys.path.insert(0, os.getcwd())
 
-    parser = argparse.ArgumentParser(description='Process logs.')
-    parser.add_argument('--file', type=str,
-                        help='A valid log file path')
+    parser = argparse.ArgumentParser(description="Process logs.")
+    parser.add_argument("--file", type=str, help="A valid log file path")
     args = parser.parse_args()
 
     params = qrw.Params()