preparing whole body

config/walk_parameters.yaml

@@ -13,7 +13,7 @@ robot:
     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
     enable_corba_viewer: false  # Enable/disable Corba Viewer
-    enable_multiprocessing: true  # Enable/disable running the MPC in another process in parallel of the main loop
+    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
 
     # General control parameters
@@ -25,7 +25,7 @@ robot:
     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
-    interpolate_mpc: true # true to interpolate the impedance quantities between nodes of the MPC
+    interpolate_mpc: false # 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+
     # Kd_main: [0., 0., 0.]  # Derivative gains for the PD+

python/quadruped_reactive_walking/Controller.py

@@ -122,8 +122,8 @@ class DummyDevice:
         self.base_position = np.zeros(3)
         self.base_position[2] = 0.1944
         self.b_base_velocity = np.zeros(3)
-        self.baseState = tuple()
-        self.baseVel = tuple()
+        self.baseState =  ((0.0, 0.0, 0.2607495), (0.0, 0.0, 0.0, 1.0)) 
+        self.baseVel = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0))
 
     class IMU:
         def __init__(self):
@@ -138,6 +138,7 @@ class DummyDevice:
             self.velocities = np.zeros(12)
 
 
+
 class Controller:
     def __init__(self, pd, params, q_init, t):
         """
@@ -171,7 +172,7 @@ class Controller:
         self.k_solve = 0
         if self.params.interpolate_mpc:
             self.interpolator = Interpolator(
-                params, np.concatenate([self.result.q_des[3:6], self.result.v_des[3:6]])
+                params, np.concatenate([self.result.q_des, self.result.v_des])
             )
         try:
             file = np.load("/tmp/init_guess.npy", allow_pickle=True).item()
@@ -404,10 +405,16 @@ class Controller:
         )
         print("Initial guess saved")
 
+    def tuple_to_array(self, tup):
+        a = np.array([element for tupl in tup for element in tupl])
+        return a
+
     def read_state(self, device):
         qj_m = device.joints.positions
         vj_m = device.joints.velocities
-        x_m = np.concatenate([qj_m, vj_m])
+        bp_m = self.tuple_to_array(device.baseState)
+        bv_m = self.tuple_to_array(device.baseVel)
+        x_m = np.concatenate([bp_m, qj_m, bv_m, vj_m])
         return {"qj_m": qj_m, "vj_m": vj_m, "x_m": x_m}
 
     def compute_torque(self, m):

python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py

@@ -146,10 +146,7 @@ class Node:
         self.isTerminal = isTerminal
 
         self.state = state
-        if pd.useFixedBase == 0:
-            self.actuation = crocoddyl.ActuationModelFloatingBase(self.state)
-        else:
-            self.actuation = crocoddyl.ActuationModelFull(self.state)
+        self.actuation = crocoddyl.ActuationModelFloatingBase(self.state)
         self.control = crocoddyl.ControlParametrizationModelPolyZero(self.actuation.nu)
         self.nu = self.actuation.nu
 

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -14,6 +14,13 @@ class problemDataAbstract:
 
         self.robot = erd.load("solo12")
         self.q0 = self.robot.q0
+        self.q0[:7] = np.array([0.0,
+                               0.0,
+                               0.2607495,
+                               0,
+                               0,
+                               0,
+                               1])
         self.q0[7:] = param.q_init
 
         self.model = self.robot.model
@@ -77,85 +84,28 @@ class ProblemData(problemDataAbstract):
     def __init__(self, param):
         super().__init__(param)
 
-        self.useFixedBase = 0
+        self.useFixedBase = 1
+
+        # Cost function weights
         # Cost function weights
         self.mu = 0.7
-        self.foot_tracking_w = 1e2
-        self.friction_cone_w = 1e3
+        self.foot_tracking_w = 1e3
+        # self.friction_cone_w = 1e3 * 0
         self.control_bound_w = 1e3
         self.control_reg_w = 1e0
-        self.state_reg_w = np.array(
-            [0] * 3
-            + [1e1] * 3
-            + [1e0] * 3
-            + [1e-3] * 3
-            + [1e0] * 6
-            + [0] * 6
-            + [1e1] * 3
-            + [3 * 1e-1] * 3
-            + [1e1] * 6
-        )
-        self.terminal_velocity_w = np.array([0] * 18 + [1e3] * 18)
-        self.control_bound_w = 1e3
+        self.state_reg_w = np.array([0] * 3
+                                    + [1e1] * 3
+                                    + [1e0] * 3
+                                    + [1e-5] * 3
+                                    + [1e0] * 6
+                                    + [0] * 6
+                                    + [1e1] * 3
+                                    + [1e0] * 3
+                                    + [1e1] * 6
+                                    )
+        self.terminal_velocity_w = np.array(
+            [0] * self.nq + [1e3] * self.nv)
 
-        self.x0 = np.array(
-            [
-                0.0,
-                0.0,
-                0.2607495,
-                0,
-                0,
-                0,
-                1,
-                0,
-                0.7,
-                -1.4,
-                0.0,
-                0.7,
-                -1.4,
-                0.0,
-                -0.7,
-                1.4,
-                0.0,
-                -0.7,
-                1.4,
-                0,
-                0,
-                0,
-                0,
-                0,
-                0,
-                0,
-                0,
-                0,
-                0,
-                0,
-                0,
-                0,
-                0,
-                0,
-                0,
-                0,
-                0,
-            ]
-        )  # x0 got from PyBullet
-
-        self.u0 = np.array(
-            [
-                -0.02615051,
-                -0.25848605,
-                0.51696646,
-                0.0285894,
-                -0.25720605,
-                0.51441775,
-                -0.02614404,
-                0.25848271,
-                -0.51697107,
-                0.02859587,
-                0.25720939,
-                -0.51441314,
-            ]
-        )  # quasi static control
         self.xref = self.x0
         self.uref = self.u0
 

python/quadruped_reactive_walking/WB_MPC/Target.py

@@ -6,7 +6,7 @@ import pinocchio as pin
 class Target:
     def __init__(self, pd: ProblemData):
         self.dt = pd.dt
-        pin.forwardKinematics(pd.model, pd.rdata, pd.q0_reduced, pd.v0_reduced)
+        pin.forwardKinematics(pd.model, pd.rdata, pd.q0, pd.v0)
         pin.updateFramePlacements(pd.model, pd.rdata)
         self.foot_pose = pd.rdata.oMf[pd.rfFootId].translation.copy()
         self.A = np.array([0, 0.03, 0.03])

python/quadruped_reactive_walking/main_solo12_control.py

@@ -12,7 +12,7 @@ from .WB_MPC.ProblemData import ProblemData, ProblemDataFull
 
 
 params = qrw.Params()  # Object that holds all controller parameters
-pd = ProblemDataFull(params)
+pd = ProblemData(params)
 
 repo = git.Repo(search_parent_directories=True)
 sha = repo.head.object.hexsha