reach point working well

config/walk_parameters.yaml

@@ -11,7 +11,7 @@ robot:
     envID: 0  # Identifier of the environment to choose in which one the simulation will happen
     use_flat_plane: true  # If True the ground is flat, otherwise it has bumps
     predefined_vel: true  # If we are using a predefined reference velocity (True) or a joystick (False)
-    N_SIMULATION: 10000  # Number of simulated wbc time steps
+    N_SIMULATION: 5000  # Number of simulated wbc time steps
     enable_corba_viewer: false  # Enable/disable Corba Viewer
     enable_multiprocessing: false  # Enable/disable running the MPC in another process in parallel of the main loop
     perfect_estimator: true  # Enable/disable perfect estimator by using data directly from PyBullet
@@ -21,13 +21,13 @@ robot:
     # q_init: [0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583] # h_com = 0.2
     # q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407]  # h_com = 0.218
     q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4]  # Initial articular positions
-    dt_wbc: 0.0014  # Time step of the whole body control
+    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
 #     Kp_main: [0.0, 0.0, 0.0]  # Proportional gains for the PD+
     Kp_main: [3, 3, 3]  # Proportional gains for the PD+
 #     Kd_main: [0., 0., 0.]  # Derivative gains for the PD+
-    Kd_main: [0.3, 0.3, 0.3]  # Derivative gains for the PD+
+    Kd_main: [0.1, 0.1, 0.1]  # Derivative gains for the PD+
 #     Kff_main: 0.0  # Feedforward torques multiplier for the PD+
     Kff_main: 1.0  # Feedforward torques multiplier for the PD+
 

python/quadruped_reactive_walking/Controller.py

@@ -154,7 +154,9 @@ class Controller:
             self.result.q_des = self.q
             self.result.v_des = self.v
             self.result.tau_ff = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], 
-                                                                self.mpc.ocp.state.diff(m["x_m"],  self.mpc_result.xs[0]))
+                                                         np.concatenate([pin.difference(self.pd.model, m["x_m"][: self.pd.nq],
+                                                                                        self.mpc_result.xs[0][: self.pd.nq]), 
+                                                                        m["x_m"][self.pd.nq] -  self.mpc_result.xs[0][self.pd.nq:] ]) )
 
             self.xs_init = self.mpc_result.xs[1:] + [self.mpc_result.xs[-1]]
             self.us_init = self.mpc_result.us[1:] + [self.mpc_result.us[-1]]
@@ -162,8 +164,8 @@ class Controller:
         t_send = time.time()
         self.t_send = t_send - t_mpc
 
-        #self.clamp_result(device)
-        #self.security_check(m)
+        self.clamp_result(device)
+        self.security_check(m)
 
         if self.error:
             self.set_null_control()

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -118,7 +118,7 @@ class ProblemDataFull(problemDataAbstract):
                             + [1e-3] * 3\
                             + [1e2] * 6
                             + [1e1] * 3 \
-                            + [3* 1e-1] * 3\
+                            + [1e0] * 3\
                             + [1e1] * 6 
                             ) 
         self.terminal_velocity_w = np.array([0] * 12 + [1e3] * 12 )

python/quadruped_reactive_walking/main_solo12_control.py

@@ -85,7 +85,7 @@ def check_position_error(device, controller):
         device (robot wrapper): a wrapper to communicate with the robot
         controller (array): the controller storing the desired position
     """
-    if np.max(np.abs(controller.result.q_des - device.joints.positions)) > 15:
+    if np.max(np.abs(controller.result.q_des - device.joints.positions)) > 0.15:
         print("DIFFERENCE: ", controller.result.q_des - device.joints.positions)
         print("q_des: ", controller.result.q_des)
         print("q_mes: ", device.joints.positions)