rising foot

python/quadruped_reactive_walking/Controller.py

@@ -37,17 +37,16 @@ class Interpolator:
         self.update(x0, x0)
 
     def update(self, x0, x1, x2=None):
-        self.q0 = x0[:3]
-        self.q1 = x1[:3]
-        self.v0 = x0[3:]
-        self.v1 = x1[3:]
+        self.q0 = x0[:12]
+        self.q1 = x1[:12]
+        self.v0 = x0[12:]
+        self.v1 = x1[12:]
         if self.type == 0:  # Linear
             self.alpha = 0.0
             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
@@ -70,8 +69,8 @@ class Interpolator:
             from scipy.interpolate import KroghInterpolator
 
             if x2 is not None:
-                self.q2 = x2[:3]
-                self.v2 = x2[3:]
+                self.q2 = x2[:12]
+                self.v2 = x2[12:]
                 self.y = [self.q0, self.v0, self.q1, self.v1, self.q2, self.v2]
                 self.krog = KroghInterpolator(self.ts, np.array(self.y))
             else:
@@ -103,8 +102,7 @@ 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,12 +110,10 @@ 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()
@@ -213,25 +209,14 @@ class Controller:
 
             try:
                 footstep = self.target.footstep(self.pd.T)
-                # self.mpc.solve(self.k, m["x_m"], self.xs_init, self.us_init)
-                if self.initialized:
-                    self.mpc.solve(
-                        self.k,
-                        self.mpc_result.xs[1],
-                        footstep,
-                        self.gait,
-                        self.xs_init,
-                        self.us_init,
-                    )
-                else:
-                    self.mpc.solve(
-                        self.k,
-                        m["x_m"],
-                        footstep,
-                        self.gait,
-                        self.xs_init,
-                        self.us_init,
-                    )
+                self.mpc.solve(
+                    self.k,
+                    m["x_m"],
+                    footstep,
+                    self.gait,
+                    self.xs_init,
+                    self.us_init,
+                )
             except ValueError:
                 self.error = True
                 print("MPC Problem")
@@ -241,7 +226,8 @@ class Controller:
 
         if not self.error:
             self.mpc_result = self.mpc.get_latest_result()
-            xs = self.mpc_result.xs
+            xs = np.array(self.mpc_result.xs)
+            xs_actuated = np.concatenate([xs[:, 7:19], xs[:, 19+6:]], axis=1)
             if self.mpc_result.new_result:
                 self.mpc_solved = True
                 self.k_new = self.k
@@ -257,13 +243,17 @@ class Controller:
             # 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.update(
+            #                 xs_actuated[0], xs_actuated[1], xs_actuated[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)
+            #     q = q[7: ]
+            #     v = v[6 :]
+
             q = xs[1][7: self.pd.nq]
             v = xs[1][6 + self.pd.nq:]
 
@@ -436,7 +426,7 @@ class Controller:
             ]
         )
         # x_diff = self.mpc_result.xs[0] - m["x_m"]
-        tau = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], x_diff)
+        tau = self.mpc_result.us[0] - np.dot(self.mpc_result.K[0], x_diff)
         # tau = self.mpc_result.us[0]
         return tau
 
@@ -445,14 +435,14 @@ class Controller:
         Integrate the position and velocity using the acceleration computed from the
         feedforward torque
         """
-        q0 = m["qj_m"][3:6].copy()
-        v0 = m["vj_m"][3:6].copy()
-        tau = self.result.tau_ff[3:6].copy()
+        q0 = 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)
 
         v = v0 + a * self.params.dt_wbc
-        q = q0 + v * self.params.dt_wbc
+        q = pin.integrate(self.pd.model, q0, v * self.params.dt_wbc)
 
         return q, v
 

python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py

@@ -11,7 +11,7 @@ from time import time
 class OCP:
     def __init__(self, pd: ProblemData, footsteps, gait):
         self.pd = pd
-        self.max_iter = 1000
+        self.max_iter = 100
 
         self.state = crocoddyl.StateMultibody(self.pd.model)
         self.initialized = False