fix k initialization

python/quadruped_reactive_walking/Controller.py

@@ -274,21 +274,21 @@ class Controller:
             self.result.FF = self.params.Kff_main * np.ones(12)
             self.result.tau_ff[3:6] = self.compute_torque(m)[:]
 
-            # if self.params.interpolate_mpc:
-            #     if self.mpc_result.new_result:
-            #         if self.params.interpolation_type == 3:
-            #             self.interpolator.update(xs[0], xs[1], xs[2])
-            #         # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
-            #     t = (self.k - self.k_solve + 1) * self.pd.dt_wbc
-            #     q, v = self.interpolator.interpolate(t)
-            # else:
-            #     q, v = self.integrate_x(m)
-
-            self.result.q_des[3:6] = xs[1][:3]
-            self.result.v_des[3:6] = xs[1][3:]
-
-            # self.result.q_des[3:6] = q
-            # self.result.v_des[3:6] = v
+            if self.params.interpolate_mpc:
+                if self.mpc_result.new_result:
+                    if self.params.interpolation_type == 3:
+                        self.interpolator.update(xs[0], xs[1], xs[2])
+                    # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
+                t = (self.k - self.k_solve + 1) * self.pd.dt_wbc
+                q, v = self.interpolator.interpolate(t)
+            else:
+                q, v = self.integrate_x(m)
+
+            # self.result.q_des[3:6] = xs[1][:3]
+            # self.result.v_des[3:6] = xs[1][3:]
+
+            self.result.q_des[3:6] = q
+            self.result.v_des[3:6] = v
 
             if self.axs is not None:
                 [
@@ -315,7 +315,7 @@ class Controller:
         # self.pyb_camera(device)
 
         self.t_loop = time.time() - t_start
-        self.k += 1
+        if self.initialized: self.k += 1
         self.initialized = True
 
         return self.error