addeded files to repo

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: 180000  # Number of simulated wbc time steps
+    N_SIMULATION: 2000  # 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
@@ -25,7 +25,7 @@ robot:
     dt_mpc: 0.02  # 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.0, 3.0, 3.0]  # Proportional gains for the PD+
+    Kp_main: [100.0, 100.0, 100.0]  # 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+
 #     Kff_main: 0.0  # Feedforward torques multiplier for the PD+

python/CMakeLists.txt

@@ -31,6 +31,7 @@ set(${PY_NAME}_WB_MPC_PYTHON
   ShootingNode.py
   ProblemData.py
   Target.py
+  OcpResult.py
   )
 
 set(${PY_NAME}_TOOLS_PYTHON

python/quadruped_reactive_walking/Controller.py

@@ -30,6 +30,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()
 
     class IMU:
         def __init__(self):
@@ -57,6 +59,8 @@ class Controller:
         self.q_security = np.array([1.2, 2.1, 3.14] * 4)
 
         self.mpc = WB_MPC_Wrapper.MPC_Wrapper(pd, target, params)
+        self.pd = pd
+        self.target = target
 
         self.k = 0
         self.error = False
@@ -66,8 +70,8 @@ class Controller:
         
         device = DummyDevice()
         device.joints.positions = q_init
-        self.compute(device)
-
+        self.guess = {}
+        #self.compute(device)
 
 
     def compute(self, device, qc=None):
@@ -78,8 +82,10 @@ class Controller:
         """
         t_start = time.time()
 
+        m = self.read_state(device)
+
         try:
-            self.mpc.solve(self.k, None)
+            self.mpc.solve(self.k, m['x_m'], self.guess)
         except ValueError:
             self.error = True
             print("MPC Problem")
@@ -89,11 +95,15 @@ class Controller:
 
             self.result.P = np.array(self.params.Kp_main.tolist() * 4)
             self.result.D = np.array(self.params.Kd_main.tolist() * 4)
-            self.result.FF = self.params.Kff_main * np.ones(12)
-            self.result.q_des = np.zeros(12)
-            self.result.v_des = np.zeros(12)
+            self.result.FF = np.zeros(12)
+            # self.result.FF = self.params.Kff_main * np.ones(12)
+            self.result.q_des = self.mpc_result.q[1]
+            self.result.v_des = self.mpc_result.v[1]
             self.result.tau_ff = np.zeros(12)
 
+            self.guess["xs"] = self.mpc_result.x
+            self.guess["us"] = self.mpc_result.u
+
         self.t_wbc = time.time() - t_start
 
         self.clamp_result(device)
@@ -114,7 +124,7 @@ class Controller:
         Update position of PyBullet camera on the robot position to do as if it was
         attached to the robot
         """
-        if self.k > 10 and self.enable_pyb_GUI:
+        if self.k > 10 and self.params.enable_pyb_GUI:
             pyb.resetDebugVisualizerCamera(
                 cameraDistance=0.6,
                 cameraYaw=45,
@@ -148,10 +158,10 @@ class Controller:
 
     def clamp(self, num, min_value=None, max_value=None):
         clamped = False
-        if min_value is not None and num <= min_value:
+        if min_value is not None and num.any() <= min_value:
             num = min_value
             clamped = True
-        if max_value is not None and num >= max_value:
+        if max_value is not None and num.any() >= max_value:
             num = max_value
             clamped = True
         return clamped
@@ -208,3 +218,19 @@ class Controller:
         self.result.q_des[:] = np.zeros(12)
         self.result.v_des[:] = np.zeros(12)
         self.result.tau_ff[:] = np.zeros(12)
+
+    def read_state(self, device):
+        qj_m = device.joints.positions
+        vj_m = device.joints.velocities
+        bp_m = self.tuple_to_array(device.baseState)
+        bv_m = self.tuple_to_array(device.baseVel)
+        if self.pd.useFixedBase == 0:
+            x_m = np.concatenate([bp_m, qj_m, bv_m, vj_m])
+        else:
+            x_m = np.concatenate([qj_m[3:6], vj_m[3:6]])
+
+        return {'qj_m': qj_m, 'vj_m': vj_m, 'x_m': x_m}
+
+    def tuple_to_array(self, tup):
+        a = np.array([element for tupl in tup for element in tupl])
+        return a

python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py

 from tracemalloc import start
 from .ProblemData import ProblemData
 from .Target import Target
+from .OcpResult import OcpResult
 import crocoddyl
 import pinocchio as pin
 import numpy as np
@@ -11,6 +12,7 @@ class OCP:
         self.pd = pd
         self.target = target
         self.state = crocoddyl.StateMultibody(self.pd.model)
+        self.results = OcpResult()
         if pd.useFixedBase == 0:
             self.actuation = crocoddyl.ActuationModelFloatingBase(self.state)
         else:
@@ -143,9 +145,28 @@ class OCP:
             us = guess['us']
             print("Using warmstart")
         start_time = time()
-        self.ddp.solve(xs, us, 1, False)
+        self.ddp.solve(xs, us, 30, False)
         print("Solver time: ", time()- start_time, "\n")
 
+
+    def get_results(self):
+        self.results.x = self.ddp.xs.tolist()
+        self.results.a = self.get_croco_acc()
+        self.results.u = self.ddp.us.tolist()
+        self.results.K = self.ddp.K
+
+        if self.pd.useFixedBase == 0:
+            self.results.q = np.array(self.results.x)[:, 7: self.pd.nq]
+            self.results.v = np.array(self.results.x)[:, self.pd.nq + 6: ]
+        else:
+            self.results.q = self.pd.q0
+            self.results.v = self.pd.q0
+            self.results.q[3:6] = np.array(self.results.x)[:, : self.pd.nq]
+            self.results.v[3:6] = np.array(self.results.x)[:, self.pd.nq :]
+
+
+        return self.results
+
     def get_croco_forces(self):
         d = self.ddp.problem.runningDatas[0]
         cnames = d.differential.multibody.contacts.contacts.todict().keys()
@@ -179,13 +200,4 @@ class OCP:
          for m in self.ddp.problem.runningDatas]
         return acc
 
-    def get_results(self):
-        x = self.ddp.xs.tolist()
-        a = self.get_croco_acc()
-        u = self.ddp.us.tolist()
-        if self.pd.useFixedBase == 0:
-            f_ws = self.get_croco_forces_ws()
-        else:
-            f_ws = []
-
-        return None, x, a, u, f_ws, None
+    
\ No newline at end of file

python/quadruped_reactive_walking/WB_MPC/OcpResult.py

+class OcpResult:
+    def __init__(self):
+        self.x = None
+        self.a = None
+        self.u = None
+        self.fs = None
+        self.K = None
+        self.f_world = None
+        self.q = None
+        self.v = None
\ No newline at end of file

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -87,7 +87,7 @@ class ProblemData(problemDataAbstract):
         self.terminal_velocity_w = np.array([0] * 18 + [1e3] * 18 )
         self.control_bound_w = 1e3
 
-        self.x0 = np.array([ 0, 0, 0.23289725, 0, 0, 0, 1, 0.1, 0.8, -1.6,
+        self.x0 = np.array([ 0.0, 0.0, 0.2607495, 0, 0, 0, 1, 0.1, 0.8, -1.6,
                             -0.1,  0.8, -1.6,  0.1, -0.8, 1.6, -0.1, -0.8, 1.6,
                             0, 0, 0,  0,  0,  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) # x0 got from PyBullet
                             

python/quadruped_reactive_walking/WB_MPC_Wrapper.py

@@ -20,7 +20,8 @@ class DataInCtype(Structure):
     # TODO add the data exchanged with the OCP
     _fields_ = [
         ("k", ctypes.c_int64),
-        ("blop", ctypes.c_double * 12)
+        ("x0", ctypes.c_double * 6),
+        ("guess", ctypes.c_double * 12),
     ]
 
 
@@ -33,6 +34,8 @@ class MPC_Wrapper:
     def __init__(self, pd, target, params):
         self.initialized = False
         self.params = params
+        self.pd = pd
+        self.target = target
 
         n_nodes = 0
 
@@ -53,7 +56,7 @@ class MPC_Wrapper:
         self.last_available_result = np.zeros((24, n_nodes))
         self.last_cost = 0.0
 
-    def solve(self, k, inputs):
+    def solve(self, k, x0, guess=None):
         """
         Call either the asynchronous MPC or the synchronous MPC depending on the value
         of multiprocessing during the creation of the wrapper
@@ -61,10 +64,11 @@ class MPC_Wrapper:
         Args:
             k (int): Number of inv dynamics iterations since the start of the simulation
         """
+
         if self.multiprocessing:
-            self.run_MPC_asynchronous(k, inputs)
+            self.run_MPC_asynchronous(k, x0, guess)
         else:
-            self.run_MPC_synchronous(inputs)
+            self.run_MPC_synchronous(x0, guess)
 
     def get_latest_result(self):
         """
@@ -82,15 +86,15 @@ class MPC_Wrapper:
             self.initialized = True
         return self.last_available_result, self.last_cost
 
-    def run_MPC_synchronous(self, inputs):
+    def run_MPC_synchronous(self, x0, guess):
         """
         Run the MPC (synchronous version)
         """
-        self.ocp.solve(inputs)
-        self.last_available_result = self.ocp.get_latest_result()
-        self.last_cost = self.ocp.retrieve_cost()
+        self.ocp.solve(x0, guess)
+        self.last_available_result = self.ocp.get_results()
+        #self.last_cost = self.ocp.retrieve_cost()
 
-    def run_MPC_asynchronous(self, k, inputs):
+    def run_MPC_asynchronous(self, k, x0, guess):
         """
         Run the MPC (asynchronous version)
         """
@@ -98,7 +102,7 @@ class MPC_Wrapper:
             p = Process(target=self.MPC_asynchronous)
             p.start()
 
-        self.add_new_data(k, inputs)
+        self.add_new_data(k, x0, guess)
 
     def MPC_asynchronous(self):
         """
@@ -108,17 +112,17 @@ class MPC_Wrapper:
             if self.newData.value:
                 self.newData.value = False
 
-                k, inputs = self.decompress_dataIn(self.dataIn)
+                k, x0, guess = self.decompress_dataIn(self.dataIn)
 
                 if k == 0:
-                    loop_ocp = OCP(self.params)
+                    loop_ocp = OCP(self.pd, self.target)
 
-                loop_ocp.solve(inputs)
+                loop_ocp.solve(x0, guess)
                 self.dataOut[:] = loop_ocp.get_latest_result().ravel(order="F")
                 self.cost.value = loop_ocp.retrieve_cost()
                 self.newResult.value = True
 
-    def add_new_data(self, k, inputs):
+    def add_new_data(self, k, x0):
         """
         Compress data in a C-type structure that belongs to the shared memory to send 
         data from the main control loop to the asynchronous MPC and notify the process

python/quadruped_reactive_walking/main_solo12_control.py

@@ -6,12 +6,13 @@ import numpy as np
 import quadruped_reactive_walking as qrw
 from .Controller import Controller
 from .tools.LoggerControl import LoggerControl
-from .WB_MPC.ProblemData import ProblemData
+from .WB_MPC.ProblemData import ProblemData, ProblemDataFull
 from .WB_MPC.Target import Target
 
 params = qrw.Params()  # Object that holds all controller parameters
-pd = ProblemData(params)
+pd = ProblemDataFull(params)
 target = Target(pd)
+target.update(0)
 
 if params.SIMULATION:
     from .tools.PyBulletSimulator import PyBulletSimulator
@@ -165,10 +166,12 @@ def control_loop():
     k_log_whole = 0
     T_whole = time.time()
     dT_whole = 0.0
+    cnt = 0
     while (not device.is_timeout) and (t < t_max) and (not controller.error):
         t_start_whole = time.time()
 
         device.parse_sensor_data()
+        target.update(cnt)
         if controller.compute(device, qc):
             break
 
@@ -198,6 +201,7 @@ def control_loop():
 
         t_log_whole[k_log_whole] = t_end_whole - t_start_whole
         k_log_whole += 1
+        cnt += 1
 
     # ****************************************************************
     finished = t >= t_max