diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py index 611e0394bb022a75f64e1a1715068f8410dd41a7..7e9a9d3b35c1219fdfb290d1a18e1e107df76320 100644 --- a/python/quadruped_reactive_walking/Controller.py +++ b/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") diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..ba0441b493ee587b0a294fe20d214248d0d13857 100644 --- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py +++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py @@ -0,0 +1,3 @@ +class OCP: + def __init__(self): + return \ No newline at end of file diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py index 74c09ac46bb63782594e72762a49f68c51492800..c605a638aad4210e7b140f8a6e80ed100cbf0d6c 100644 --- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py +++ b/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)