1KHz mpc (reduced model)

Closed loop
Still target
Impedance + FeedForward

python/quadruped_reactive_walking/Controller.py

@@ -108,9 +108,9 @@ class Controller:
             self.mpc_result, self.mpc_cost = self.mpc.get_latest_result()
 
             #self.result.P = np.array(self.params.Kp_main.tolist() * 4)
-            self.result.P = np.array([5] * 3 + [0] * 3 + [5]*6)
+            self.result.P = np.array([5] * 3 + [3] * 3 + [5]*6)
             #self.result.D = np.array(self.params.Kd_main.tolist() * 4)
-            self.result.D = np.array([0.3] * 3 + [0] * 3 + [0.3]*6)
+            self.result.D = np.array([0.3] * 3 + [0.5] * 3 + [0.3]*6)
             tauFF = self.mpc_result.u[0]
             self.result.FF = self.params.Kff_main * np.array([0] * 3 + list(tauFF) + [0]*6)