1KHz mpc (reduced model)

Closed loop
EndEff tracking Circular motion cost
Impedance + Feedforward control

python/quadruped_reactive_walking/Controller.py

@@ -96,13 +96,13 @@ class Controller:
 
         self.point_target = self.target.evaluate_in_t(1)[self.pd.rfFootId]
         try:
-            #self.mpc.solve(self.k, m["x_m"], self.guess)  # Closed loop mpc
+            self.mpc.solve(self.k, m["x_m"], self.guess)  # Closed loop mpc
 
             ## Trajectory tracking
-            if self.initialized:
-               self.mpc.solve(self.k, self.mpc_result.x[1], self.guess)
-            else:
-               self.mpc.solve(self.k, m["x_m"], self.guess)
+            #if self.initialized:
+            #   self.mpc.solve(self.k, self.mpc_result.x[1], self.guess)
+            #else:
+            #   self.mpc.solve(self.k, m["x_m"], self.guess)
 
              ### ONLY IF YOU WANT TO STORE THE FIRST SOLUTION TO WARMSTART THE INITIAL Problem ###
             #if not self.initialized:

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*0, 2.5*0])
+        self.freq = np.array([0, 2.5, 2.5])
         self.phase = np.array([0, 0, np.pi / 2])
 
     def patternToId(self, gait):

python/quadruped_reactive_walking/main_solo12_control.py

@@ -112,7 +112,7 @@ def damp_control(device, nb_motors):
         device.joints.set_velocity_gains(0.1 * np.ones(nb_motors))
         device.joints.set_desired_positions(np.zeros(nb_motors))
         device.joints.set_desired_velocities(np.zeros(nb_motors))
-        #device.joints.set_torques(np.zeros(nb_motors))
+        device.joints.set_torques(np.zeros(nb_motors))
 
         # Send command to the robot
         device.send_command_and_wait_end_of_cycle(params.dt_wbc)