1KHz mpc (reduced model)

Closed loop
EndEff tracking Circular motion cost
Impedance + Feedforward control

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -117,7 +117,7 @@ class ProblemDataFull(problemDataAbstract):
         #self.friction_cone_w = 1e3 * 0
         self.control_bound_w = 1e3
         self.control_reg_w = 1e0
-        self.state_reg_w = np.array([1e-3] * 3 + [5*1e-1]*3)
+        self.state_reg_w = np.array([1e-3] * 3 + [2*1e-1]*3)
         self.terminal_velocity_w = np.array([0] * 3 + [1e3] * 3 )
 
         self.q0_reduced = self.q0[10:13]

python/quadruped_reactive_walking/WB_MPC/Target.py

@@ -30,7 +30,7 @@ class Target:
         self.FR_foot0 = pd.rdata.oMf[pd.rfFootId].translation.copy()
         self.A = np.array([0, 0.03, 0.03])
         self.offset = np.array([0.08, 0, 0.06])
-        self.freq = np.array([0, 2.5, 2.5])
+        self.freq = np.array([0, 1, 1])
         self.phase = np.array([0, 0, np.pi / 2])
 
     def patternToId(self, gait):