small fixes

python/quadruped_reactive_walking/Controller.py

@@ -5,8 +5,6 @@ import pinocchio as pin
 import pybullet as pyb
 
 from . import WB_MPC_Wrapper
-from .solo3D.utils import quaternionToRPY
-from .tools.utils_mpc import init_robot
 
 
 class Result:
@@ -60,13 +58,18 @@ class Controller:
 
         self.mpc = WB_MPC_Wrapper.MPC_Wrapper(params)
 
+        self.k = 0
         self.error = False
         self.result = Result(params)
-
+        self.params = params
+        self.q_init = np.zeros(18)
+        
         device = DummyDevice()
         device.joints.positions = q_init
         self.compute(device)
 
+
+
     def compute(self, device, qc=None):
         """Run one iteration of the main control loop
 
@@ -76,7 +79,7 @@ class Controller:
         t_start = time.time()
 
         try:
-            self.mpc.solve()
+            self.mpc.solve(self.k, None)
         except ValueError:
             self.error = True
             print("MPC Problem")

python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py

+class OCP:
+    def __init__(self):
+        return
\ No newline at end of file

python/quadruped_reactive_walking/WB_MPC_Wrapper.py

@@ -6,7 +6,7 @@ from time import time
 
 import numpy as np
 
-from .Crocoddyl.CrocoddylOCP import OCP
+from .WB_MPC.CrocoddylOCP import OCP
 
 from . import quadruped_reactive_walking as qrw
 
@@ -18,9 +18,9 @@ class DataInCtype(Structure):
 
     params = qrw.Params()
     # TODO add the data exchanged with the OCP
-    fields_ = [
+    _fields_ = [
         ("k", ctypes.c_int64),
-        ("blop", ctypes.c_double * 12),
+        ("blop", ctypes.c_double * 12)
     ]
 
 
@@ -62,7 +62,7 @@ class MPC_Wrapper:
             k (int): Number of inv dynamics iterations since the start of the simulation
         """
         if self.multiprocessing:
-            self.run_MPC_asynchronous(inputs)
+            self.run_MPC_asynchronous(k, inputs)
         else:
             self.run_MPC_synchronous(inputs)