diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 51368c8546d2aa4b6b3c01cfb925507289f11b15..53bd57e3f98ca5522a91a6375a7968a52c152322 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -22,7 +22,7 @@ robot:
# 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.001 # Time step of the whole body control
- dt_mpc: 0.015 # Time step of the model predictive control
+ dt_mpc: 0.01 # 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
interpolate_mpc: true # true to interpolate the impedance quantities between nodes of the MPC
interpolation_type: 0 # 0,1,2,3 decide which kind of interpolation is used
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 8b1e93b96f206d8cddc8749ab6f6cd7380f5a176..fb2a2508b71f23c7f3c13a53e182e131dc8c3aad 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -61,14 +61,14 @@ class Interpolation:
gamma = self.q0
v_t = beta + alpha * t
- q_t = gamma + beta * t + alpha * t**2
+ q_t = gamma + beta * t + alpha * t**2
# Quadratic
if self.params.interpolation_type == 3:
if (self.q1-self.q0 == 0).any():
alpha = np.zeros(len(self.q0))
else:
- alpha = self.v1 *(self.v1- self.v0)/(self.q1 - self.q0)
+ alpha = self.v1 * (self.v1 - self.v0)/(self.q1 - self.q0)
beta = self.v0
gamma = self.q0
@@ -147,6 +147,7 @@ class Controller:
self.cnt_wbc = 0
self.error = False
self.initialized = False
+ self.first_step = False
self.interpolator = Interpolation(params)
self.result = Result(params)
self.result.q_des = self.pd.q0[7:].copy()
@@ -183,9 +184,10 @@ class Controller:
try:
self.target.update(self.cnt_mpc)
self.target.shift_gait()
- self.cnt_wbc = 0
+ #self.cnt_wbc = 0
self.mpc.solve(self.k, m["x_m"], self.xs_init, self.us_init)
+ if self.k == 0: time.sleep(1)
self.cnt_mpc += 1
except ValueError:
@@ -200,10 +202,12 @@ class Controller:
if self.params.enable_multiprocessing:
if self.mpc_result.new_result:
print("new result! at iter: ", str(self.cnt_wbc))
- print(self.mpc_result.xs[1], "\n")
- self.cnt_wbc = 0
+ #self.cnt_wbc = 0
-
+
+ print("MPC iter: ", self.cnt_mpc,
+ " / Counter value: ", self.cnt_wbc,
+ " / k value: ", self.k )
# ## ONLY IF YOU WANT TO STORE THE FIRST SOLUTION TO WARM-START THE INITIAL Problem ###
# if not self.initialized:
# np.save(open('/tmp/init_guess.npy', "wb"), {"xs": self.mpc_result.xs, "us": self.mpc_result.us} )
@@ -211,6 +215,7 @@ class Controller:
# Keep only the actuated joints and set the other to default values
self.result.FF = self.params.Kff_main * np.ones(12)
+
actuated_tau_ff = self.compute_torque(m)
self.result.tau_ff = np.array(
[0] * 3 + list(actuated_tau_ff) + [0] * 6)
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 155afa63223fc0279232267b5361d07a1acbea20..7b2dfba24e676e0d5fb170c17a826c3c20680294 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -12,7 +12,7 @@ class OCP:
def __init__(self, pd: ProblemData, target: Target):
self.pd = pd
self.target = target
- self.max_iter=1000
+ self.max_iter=5
self.state = crocoddyl.StateMultibody(self.pd.model)
self.initialized = False
@@ -107,8 +107,9 @@ class OCP:
t_warm_start = time()
self.t_warm_start = t_warm_start - t_update
-
- # self.ddp.setCallbacks([crocoddyl.CallbackVerbose()])
+ print("CROCODDYL START")
+ print("CROCODDYL INITIAL POINT: ", x0)
+ self.ddp.setCallbacks([crocoddyl.CallbackVerbose()])
self.ddp.solve(xs, us, self.max_iter, False)
t_ddp = time()
diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index 820e39b78c2ccf670861555a6f3fb2faaaf7dc06..117401973e18adedd54417c39c72657d2dc64976 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
@@ -2,7 +2,7 @@ import ctypes
from ctypes import Structure
from enum import Enum
from multiprocessing import Process, Value, Array
-from time import time
+from time import time, sleep
import numpy as np
@@ -83,6 +83,10 @@ class MPC_Wrapper:
self.last_available_result.solving_duration,
) = self.decompress_dataOut()
self.last_available_result.new_result = True
+
+ elif self.multiprocessing and not self.new_result.value:
+ self.last_available_result.new_result = False
+
else:
self.initialized = True
return self.last_available_result
@@ -103,11 +107,11 @@ class MPC_Wrapper:
"""
Run the MPC (asynchronous version)
"""
+ print("Call to solve")
if k == 0:
self.last_available_result.xs = [x0 for _ in range (self.pd.T + 1)]
p = Process(target=self.MPC_asynchronous)
p.start()
- self.last_available_result.new_result = False
self.add_new_data(k, x0, xs, us)
def MPC_asynchronous(self):