circular motion ok

config/walk_parameters.yaml

@@ -24,8 +24,8 @@ robot:
     dt_wbc: 0.001  # Time step of the whole body control
     dt_mpc: 0.015  # Time step of the model predictive control
     type_MPC: 3  # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
-    save_guess: false # true to interpolate the impedance quantities between nodes of the MPC
-    movement: "step" # name of the movement to perform
+    save_guess: true # true to interpolate the impedance quantities between nodes of the MPC
+    movement: "circle" # name of the movement to perform
     interpolate_mpc: true # true to interpolate the impedance quantities between nodes of the MPC
     interpolation_type: 3 # 0,1,2,3 decide which kind of interpolation is used
     # Kp_main: [0.0, 0.0, 0.0]  # Proportional gains for the PD+

python/quadruped_reactive_walking/Controller.py

@@ -169,6 +169,7 @@ class Controller:
         self.result = Result(params)
         self.result.q_des = self.pd.q0[7:].copy()
         self.result.v_des = self.pd.v0[6:].copy()
+        self.result.FF = self.params.Kff_main * np.ones(12)
 
         self.target = Target(params)
         self.footsteps = []
@@ -211,7 +212,7 @@ class Controller:
 
         m = self.read_state(device)
 
-        oRh, hRb, oTh = self.run_estimator(device)
+        # oRh, hRb, oTh = self.run_estimator(device)
 
         t_measures = time.time()
         self.t_measures = t_measures - t_start
@@ -271,9 +272,6 @@ class Controller:
             if not self.initialized and self.params.save_guess:
                 self.save_guess()
 
-            self.result.FF = self.params.Kff_main * np.ones(12)
-            self.result.tau_ff = self.compute_torque(m)[:]
-
             if self.params.interpolate_mpc:
                 if self.mpc_result.new_result:
                     if self.params.interpolation_type == 3:
@@ -284,6 +282,7 @@ class Controller:
             else:
                 q, v = self.integrate_x(m)
 
+            self.result.tau_ff = self.compute_torque(m)[:]
             self.result.q_des = q[:]
             self.result.v_des = v[:]
 

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -23,7 +23,8 @@ class problemDataAbstract:
 
         self.frozen_names = frozen_names
         if frozen_names:
-            self.frozen_idxs = [self.model.getJointId(id) for id in frozen_names]
+            self.frozen_idxs = [self.model.getJointId(
+                id) for id in frozen_names]
             self.freeze()
 
         self.nq = self.model.nq
@@ -35,7 +36,7 @@ class problemDataAbstract:
         )  # -1 to take into account the freeflyer
         self.ntau = self.nv
 
-        self.effort_limit = np.ones(self.nu) * 3
+        self.effort_limit = np.ones(self.nu) * 2.5
 
         self.v0 = np.zeros(18)
         self.x0 = np.concatenate([self.q0, self.v0])
@@ -167,15 +168,15 @@ class ProblemDataFull(problemDataAbstract):
 
         self.useFixedBase = 1
 
-        # Cost function weights
         # Cost function weights
         self.mu = 0.7
         self.foot_tracking_w = 1e4
-        # self.friction_cone_w = 1e3 * 0
+        self.friction_cone_w = 1e3
         self.control_bound_w = 1e3
-        self.control_reg_w = 1e-1
+        self.control_reg_w = 1e2
         self.state_reg_w = np.array(
-            [1e1] * 3 + [1e-2] * 3 + [1e1] * 6 + [1e1] * 3 + [3 * 1e-1] * 3 + [1e1] * 6
+            [1e1] * 3 + [1e-2] * 3 + [1e1] * 6 +
+            [1e1] * 3 + [1e0] * 3 + [1e1] * 6
         )
         self.terminal_velocity_w = np.array([0] * 12 + [1e3] * 12)