working on implementation

python/quadruped_reactive_walking/Controller.py

@@ -48,7 +48,8 @@ class Interpolator:
             self.beta = self.v1
             self.gamma = self.q0
         elif self.type == 1:  # Quadratic fixed velocity
-            self.alpha = 2 * (self.q1 - self.q0 - self.v0 * self.dt) / self.dt**2
+            self.alpha = 2 * (self.q1 - self.q0 -
+                              self.v0 * self.dt) / self.dt**2
             self.beta = self.v0
             self.gamma = self.q0
         elif self.type == 2:  # Quadratic time variable
@@ -104,7 +105,8 @@ class Interpolator:
             plt.scatter(y=self.q0[i], x=0.0, color="violet", marker="+")
             plt.scatter(y=self.q1[i], x=self.dt, color="violet", marker="+")
             if self.type == 3 and self.q2 is not None:
-                plt.scatter(y=self.q2[i], x=2 * self.dt, color="violet", marker="+")
+                plt.scatter(y=self.q2[i], x=2 * self.dt,
+                            color="violet", marker="+")
 
             plt.subplot(3, 2, (i * 2) + 2)
             plt.title("Velocity interpolation")
@@ -112,10 +114,12 @@ class Interpolator:
             plt.scatter(y=self.v0[i], x=0.0, color="violet", marker="+")
             plt.scatter(y=self.v1[i], x=self.dt, color="violet", marker="+")
             if self.type == 3 and self.v2 is not None:
-                plt.scatter(y=self.v2[i], x=2 * self.dt, color="violet", marker="+")
+                plt.scatter(y=self.v2[i], x=2 * self.dt,
+                            color="violet", marker="+")
 
         plt.show()
 
+
 class DummyDevice:
     def __init__(self):
         self.imu = self.IMU()
@@ -171,9 +175,10 @@ class Controller:
         self.result.FF = self.params.Kff_main * np.ones(12)
 
         self.target = Target(params)
-        self.velocity_task =  self.target.velocity_task
+        self.velocity_task = self.target.velocity_task
 
-        self.mpc = WB_MPC_Wrapper.MPC_Wrapper(pd, params, self.velocity_task, self.gait)
+        self.mpc = WB_MPC_Wrapper.MPC_Wrapper(
+            pd, params, self.velocity_task, self.gait)
         self.mpc_solved = False
         self.k_result = 0
         self.k_solve = 0
@@ -183,8 +188,8 @@ class Controller:
             )
         try:
             file = np.load("/tmp/init_guess.npy", allow_pickle=True).item()
-            self.xs_init = list(file["xs"])
-            self.us_init = list(file["us"])
+            self.guess = {'xs': list(file['xs']), 'us': list(file['us']),
+                          'acs': file['acs'], 'fs': file['fs']}
             print("Initial guess loaded \n")
         except:
             self.xs_init = None
@@ -223,8 +228,7 @@ class Controller:
                     m["x_m"],
                     self.velocity_task.copy(),
                     self.gait,
-                    self.xs_init,
-                    self.us_init,
+                    self.guess
                 )
                 # if self.initialized:
                 #     self.mpc.solve(
@@ -279,13 +283,12 @@ class Controller:
                 q, v = self.interpolator.interpolate(t)
             else:
                 q, v = self.integrate_x(m)
-                q = q[7: ]
-                v = v[6 :]
+                q = q[7:]
+                v = v[6:]
 
             # q = xs[1][7: self.pd.nq]
             # v = xs[1][6 + self.pd.nq:]
 
-
             self.result.q_des = q[:]
             self.result.v_des = v[:]
 
@@ -299,8 +302,11 @@ class Controller:
                     )
                     for i in range(3)
                 ]
-            self.xs_init = self.mpc_result.xs[1:] + [self.mpc_result.xs[-1]]
-            self.us_init = self.mpc_result.us[1:] + [self.mpc_result.us[-1]]
+
+            self.guess["xs"] = self.mpc_result.xs[1:] + [self.mpc_result.xs[-1]]
+            self.guess["us"] = self.mpc_result.us[1:] + [self.mpc_result.us[-1]]
+            self.guess["acs"] = self.mpc_result.acs[1:] + [self.mpc_result.acs[-1]]
+            self.guess["us"] = self.mpc_result.fs[1:] + [self.mpc_result.fs[-1]]
 
         t_send = time.time()
         self.t_send = t_send - t_mpc
@@ -464,7 +470,8 @@ class Controller:
 
         self.q[:3] = self.estimator.get_q_estimate()[:3]
         self.q[6:] = self.estimator.get_q_estimate()[7:]
-        self.q[3:6] = quaternionToRPY(self.estimator.get_q_estimate()[3:7]).ravel()
+        self.q[3:6] = quaternionToRPY(
+            self.estimator.get_q_estimate()[3:7]).ravel()
         self.v = self.estimator.get_v_reference()
 
         return oRh, hRb, oTh
@@ -505,7 +512,7 @@ class Controller:
         feedforward torque
         """
         q0 = m["x_m"].copy()[: 19]
-        v0 = m["x_m"].copy()[19 :]
+        v0 = m["x_m"].copy()[19:]
         tau = np.concatenate([np.zeros(6), self.result.tau_ff.copy()])
 
         a = pin.aba(self.pd.model, self.pd.rdata, q0, v0, tau)

python/quadruped_reactive_walking/WB_MPC_Wrapper.py

@@ -55,7 +55,7 @@ class MPC_Wrapper:
         self.last_available_result = Result(pd)
         self.new_result = Value("b", False)
 
-    def solve(self, k, x0, tasks, gait, xs=None, us=None):
+    def solve(self, k, x0, tasks, gait, guess={}):
         """
         Call either the asynchronous MPC or the synchronous MPC depending on the value
         of multiprocessing during the creation of the wrapper
@@ -66,7 +66,7 @@ class MPC_Wrapper:
         if self.multiprocessing:
             self.run_MPC_asynchronous(k, x0, tasks, gait, xs, us)
         else:
-            self.run_MPC_synchronous(x0, tasks, gait, xs, us)
+            self.run_MPC_synchronous(x0, tasks, gait, guess)
 
     def get_latest_result(self):
         """
@@ -91,11 +91,11 @@ class MPC_Wrapper:
 
         return self.last_available_result
 
-    def run_MPC_synchronous(self, x0, tasks, gait, xs, us):
+    def run_MPC_synchronous(self, x0, tasks, gait, guess):
         """
         Run the MPC (synchronous version)
         """
-        self.ocp.solve(x0, tasks, gait, xs, us)
+        self.ocp.solve(x0, tasks, gait, guess)
         (
             self.last_available_result.xs,
             self.last_available_result.us,