diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index e07c02c02d3ba4ba9ae6a7618c9139e84a4e2c43..47c0614edf1b1e90f76dc7d0e03e84f414fe5e4e 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -73,16 +73,20 @@ class Controller:
device = DummyDevice()
device.joints.positions = q_init
try:
- file = np.load('/tmp/init_guess.npy', allow_pickle=True).item()
- self.guess = {"xs": list(file["xs"]), "us": list(file["us"])}
- print("\nInitial guess loaded\n")
+ file = np.load("/tmp/init_guess.npy", allow_pickle=True).item()
+ self.xs_init = list(file["xs"])
+ self.us_init = list(file["us"])
+ print("Initial guess loaded \n")
except:
- self.guess = {}
- print("\nNo initial guess\n")
- # self.compute(device)
+ self.xs_init = None
+ self.us_init = None
+ print("No initial guess\n")
+
+ self.compute(device)
def compute(self, device, qc=None):
- """Run one iteration of the main control loop
+ """
+ Run one iteration of the main control loop
Args:
device (object): Interface with the masterboard or the simulation
@@ -96,19 +100,14 @@ class Controller:
self.point_target = self.target.evaluate_in_t(1)[self.pd.rfFootId]
try:
- #self.mpc.solve(self.k, m["x_m"], self.guess) # Closed loop mpc
+ # Closed-loop
+ # self.mpc.solve(self.k, m["x_m"], self.xs_init, self.us_init)
- ## Trajectory tracking
+ # Trajectory tracking
if self.initialized:
- self.mpc.solve(self.k, self.mpc_result.x[1], self.guess)
+ self.mpc.solve(self.k, self.mpc_result.xs[1], self.xs_init, self.us_init)
else:
- self.mpc.solve(self.k, m["x_m"], self.guess)
-
- ### ONLY IF YOU WANT TO STORE THE FIRST SOLUTION TO WARMSTART THE INITIAL Problem ###
- #if not self.initialized:
- # np.save(open('/tmp/init_guess.npy', "wb"), {"xs": self.mpc.ocp.get_results().x, "us": self.mpc.ocp.get_results().u} )
- # print("Initial guess saved")
-
+ self.mpc.solve(self.k, m["x_m"], self.xs_init, self.us_init)
except ValueError:
self.error = True
print("MPC Problem")
@@ -117,7 +116,12 @@ class Controller:
self.t_mpc = t_mpc - t_measures
if not self.error:
- self.mpc_result, self.mpc_cost = self.mpc.get_latest_result()
+ self.mpc_result = self.mpc.get_latest_result()
+
+ ### 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} )
+ # print("Initial guess saved")
# self.result.P = np.array(self.params.Kp_main.tolist() * 4)
self.result.P = np.array([5] * 3 + [1] * 3 + [5] * 6)
@@ -138,8 +142,8 @@ class Controller:
self.result.v_des = self.mpc_result.v[1]
self.result.tau_ff = np.zeros(12)
- self.guess["xs"] = self.mpc_result.x[1:] + [self.mpc_result.x[-1]]
- self.guess["us"] = self.mpc_result.u[1:] + [self.mpc_result.u[-1]]
+ self.xs_init = self.mpc_result.x[1:] + [self.mpc_result.x[-1]]
+ self.us_init = self.mpc_result.u[1:] + [self.mpc_result.u[-1]]
t_send = time.time()
self.t_send = t_send - t_mpc
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 431912fc4266daa792563bf1bd02c9a8e1e57c2e..97409d1aa8b2e7649c6ff90e71c32935f3f2bec1 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -2,7 +2,6 @@ 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
@@ -14,7 +13,6 @@ class OCP:
self.pd = pd
self.target = target
- self.results = OcpResult()
self.state = crocoddyl.StateMultibody(self.pd.model)
self.initialized = False
self.t_problem_update = 0
@@ -27,16 +25,18 @@ class OCP:
)
self.ddp = crocoddyl.SolverFDDP(self.problem)
- self.t_update_last_model = 0
- self.t_shift = 0
-
def initialize_models(self):
self.nodes = []
for t in range(self.pd.T):
- task = self.make_task(self.target.evaluate_in_t(t), self.target.contactSequence[t])
+ task = self.make_task(
+ self.target.evaluate_in_t(t), self.target.contactSequence[t]
+ )
self.nodes.append(
- Node(self.pd, self.state, self.target.contactSequence[t], task))
- self.terminal_node = Node(self.pd, self.state, self.target.contactSequence[self.pd.T], isTerminal=True)
+ Node(self.pd, self.state, self.target.contactSequence[t], task)
+ )
+ self.terminal_node = Node(
+ self.pd, self.state, self.target.contactSequence[self.pd.T], isTerminal=True
+ )
self.models = [node.model for node in self.nodes]
self.terminal_model = self.terminal_node.model
@@ -56,15 +56,20 @@ class OCP:
t_FK = time()
self.t_FK = t_FK - t_start
-
+
if self.initialized:
- task = self.make_task(self.target.evaluate_in_t(self.pd.T-1), self.target.contactSequence[self.pd.T-1]) # model without contact for this task
- self.nodes[0].update_model(self.target.contactSequence[self.pd.T-1], task)
+ task = self.make_task(
+ self.target.evaluate_in_t(self.pd.T - 1),
+ self.target.contactSequence[self.pd.T - 1],
+ ) # model without contact for this task
+ self.nodes[0].update_model(self.target.contactSequence[self.pd.T - 1], task)
t_update_last_model = time()
self.t_update_last_model = t_update_last_model - t_FK
- self.problem.circularAppend(self.nodes[0].model, self.nodes[0].model.createData())
+ self.problem.circularAppend(
+ self.nodes[0].model, self.nodes[0].model.createData()
+ )
t_shift = time()
self.t_shift = t_shift - t_update_last_model
@@ -76,7 +81,7 @@ class OCP:
self.initialized = True
- def solve(self, x0, guess=None):
+ def solve(self, x0, xs_init=None, us_init=None):
self.x0 = x0
@@ -87,17 +92,17 @@ class OCP:
t_update = time()
self.t_update = t_update - t_start
- if not guess:
+ if xs_init is None or us_init is None:
xs = [x0] * (self.ddp.problem.T + 1)
us = self.ddp.problem.quasiStatic([x0] * self.ddp.problem.T)
else:
- xs = guess["xs"]
- us = guess["us"]
+ xs = xs_init
+ us = us_init
t_warm_start = time()
self.t_warm_start = t_warm_start - t_update
- #self.ddp.setCallbacks([crocoddyl.CallbackVerbose()])
+ # self.ddp.setCallbacks([crocoddyl.CallbackVerbose()])
self.ddp.solve(xs, us, 1, False)
t_ddp = time()
@@ -118,10 +123,12 @@ class OCP:
return [idf for idf in self.pd.allContactIds if idf not in contactIds]
def get_results(self):
- self.results.x = self.ddp.xs.tolist()
- self.results.u = self.ddp.us.tolist()
- self.results.K = self.ddp.K
- return self.results
+ return (
+ self.ddp.xs.tolist().copy(),
+ self.ddp.us.tolist().copy(),
+ self.ddp.K.copy(),
+ self.t_ddp,
+ )
def get_croco_forces(self):
d = self.ddp.problem.runningDatas[0]
@@ -152,13 +159,14 @@ class OCP:
def get_croco_acc(self):
acc = []
- [acc.append(m.differential.xout)
- for m in self.ddp.problem.runningDatas]
+ [acc.append(m.differential.xout) for m in self.ddp.problem.runningDatas]
return acc
class Node:
- def __init__(self, pd, state, supportFootIds=[], swingFootTask = [], isTerminal=False):
+ def __init__(
+ self, pd, state, supportFootIds=[], swingFootTask=[], isTerminal=False
+ ):
self.pd = pd
self.isTerminal = isTerminal
@@ -167,15 +175,14 @@ class Node:
self.actuation = crocoddyl.ActuationModelFloatingBase(self.state)
else:
self.actuation = crocoddyl.ActuationModelFull(self.state)
- self.control = crocoddyl.ControlParametrizationModelPolyZero(
- self.actuation.nu)
+ self.control = crocoddyl.ControlParametrizationModelPolyZero(self.actuation.nu)
self.nu = self.actuation.nu
self.createStandardModel(supportFootIds)
if isTerminal:
self.make_terminal_model()
else:
- self.make_running_model(supportFootIds ,swingFootTask)
+ self.make_running_model(supportFootIds, swingFootTask)
def createStandardModel(self, supportFootIds):
"""Action model for a swing foot phase.
@@ -190,8 +197,7 @@ class Node:
self.contactModel = crocoddyl.ContactModelMultiple(self.state, self.nu)
for i in supportFootIds:
supportContactModel = crocoddyl.ContactModel3D(
- self.state, i, np.array(
- [0.0, 0.0, 0.0]), self.nu, np.array([0.0, 0.0])
+ self.state, i, np.array([0.0, 0.0, 0.0]), self.nu, np.array([0.0, 0.0])
)
self.contactModel.addContact(
self.pd.model.frames[i].name + "_contact", supportContactModel
@@ -200,8 +206,7 @@ class Node:
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, self.nu)
- stateResidual = crocoddyl.ResidualModelState(
- self.state, self.pd.xref, self.nu)
+ stateResidual = crocoddyl.ResidualModelState(self.state, self.pd.xref, self.nu)
stateActivation = crocoddyl.ActivationModelWeightedQuad(
self.pd.state_reg_w**2
)
@@ -224,8 +229,7 @@ class Node:
self.contactModel = crocoddyl.ContactModelMultiple(self.state, self.nu)
for i in supportFootIds:
supportContactModel = crocoddyl.ContactModel3D(
- self.state, i, np.array(
- [0.0, 0.0, 0.0]), self.nu, np.array([0.0, 0.0])
+ self.state, i, np.array([0.0, 0.0, 0.0]), self.nu, np.array([0.0, 0.0])
)
self.dmodel.contacts.addContact(
self.pd.model.frames[i].name + "_contact", supportContactModel
@@ -233,8 +237,7 @@ class Node:
def make_terminal_model(self):
self.isTerminal = True
- stateResidual = crocoddyl.ResidualModelState(
- self.state, self.pd.xref, self.nu)
+ stateResidual = crocoddyl.ResidualModelState(self.state, self.pd.xref, self.nu)
stateActivation = crocoddyl.ActivationModelWeightedQuad(
self.pd.terminal_velocity_w**2
)
@@ -265,18 +268,14 @@ class Node:
ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual)
self.costModel.addCost("ctrlReg", ctrlReg, self.pd.control_reg_w)
- ctrl_bound_residual = crocoddyl.ResidualModelControl(
- self.state, self.nu)
+ ctrl_bound_residual = crocoddyl.ResidualModelControl(self.state, self.nu)
ctrl_bound_activation = crocoddyl.ActivationModelQuadraticBarrier(
- crocoddyl.ActivationBounds(-self.pd.effort_limit,
- self.pd.effort_limit)
+ crocoddyl.ActivationBounds(-self.pd.effort_limit, self.pd.effort_limit)
)
ctrl_bound = crocoddyl.CostModelResidual(
self.state, ctrl_bound_activation, ctrl_bound_residual
)
- self.costModel.addCost("ctrlBound", ctrl_bound,
- self.pd.control_bound_w)
-
+ self.costModel.addCost("ctrlBound", ctrl_bound, self.pd.control_bound_w)
self.tracking_cost(swingFootTask)
diff --git a/python/quadruped_reactive_walking/WB_MPC/OcpResult.py b/python/quadruped_reactive_walking/WB_MPC/OcpResult.py
index 0c7684d2501e7e01cc9bd3f7057ecb07ab5d9240..c6ce3c5e8945ec69f24aeffd2a24e397c2f26eb7 100644
--- a/python/quadruped_reactive_walking/WB_MPC/OcpResult.py
+++ b/python/quadruped_reactive_walking/WB_MPC/OcpResult.py
@@ -1,8 +1,8 @@
class OcpResult:
def __init__(self):
- self.x = None
+ self.xs = None
self.a = None
- self.u = None
+ self.us = None
self.fs = None
self.K = None
self.f_world = None
diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index fc2c766d08c4d0703565e107841464f92545641f..a2ad642859be78423e5f60c1e8e1bbb9360ea011 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
@@ -11,18 +11,12 @@ from .WB_MPC.CrocoddylOCP import OCP
import quadruped_reactive_walking as qrw
-class DataInCtype(Structure):
- """
- Ctype data structure for the shared memory between processes.
- """
-
- params = qrw.Params()
- # TODO add the data exchanged with the OCP
- _fields_ = [
- ("k", ctypes.c_int64),
- ("x0", ctypes.c_double * 6),
- ("guess", ctypes.c_double * 12),
- ]
+class Result:
+ def __init__(self, pd):
+ self.xs = np.zeros((pd.T + 1, pd.nx))
+ self.us = np.zeros((pd.T, pd.nu))
+ self.K = None
+ self.solving_duration = 0.0
class MPC_Wrapper:
@@ -37,26 +31,26 @@ class MPC_Wrapper:
self.pd = pd
self.target = target
- n_nodes = 0
-
self.multiprocessing = params.enable_multiprocessing
if self.multiprocessing:
- self.newData = Value("b", False)
- self.newResult = Value("b", False)
+ self.new_data = Value("b", False)
+ self.new_result = Value("b", False)
self.running = Value("b", True)
- self.dataIn = Value(DataInCtype)
- # TODO: update output size
- self.dataOut = Array("d", [0] * 24 * n_nodes)
- self.cost = Value("d", 0.0)
+ self.in_k = Value("i", 0)
+ self.in_x0 = Array("d", [0] * pd.nx)
+ self.in_xs = Array("d", [0] * pd.T + 1 * pd.nx)
+ self.in_us = Array("d", [0] * pd.T * pd.nu)
+ self.out_xs = Array("d", [0] * pd.T + 1 * pd.nx)
+ self.out_us = Array("d", [0] * pd.T * pd.nu)
+ self.out_k = Array("d", [0] * 0 * 0)
+ self.out_solving_time = Value("d", 0.0)
else:
self.ocp = OCP(pd, target)
- # TODO initialize first result
- self.last_available_result = np.zeros((24, n_nodes))
- self.last_cost = 0.0
+ self.last_available_result = Result(pd)
- def solve(self, k, x0, guess=None):
+ def solve(self, k, x0, xs=None, us=None):
"""
Call either the asynchronous MPC or the synchronous MPC depending on the value
of multiprocessing during the creation of the wrapper
@@ -66,35 +60,43 @@ class MPC_Wrapper:
"""
if self.multiprocessing:
- self.run_MPC_asynchronous(k, x0, guess)
+ self.run_MPC_asynchronous(k, x0, xs, us)
else:
- self.run_MPC_synchronous(x0, guess)
+ self.run_MPC_synchronous(x0, xs, us)
def get_latest_result(self):
"""
Return the desired contact forces that have been computed by the last iteration
of the MPC.
- If a new result is available, return the new result.
+ If a new result is available, return the new result.
Otherwise return the old result again.
"""
if self.initialized:
- if self.multiprocessing and self.newResult.value:
- self.newResult.value = False
- self.last_available_result = self.convert_dataOut()
- self.last_cost = self.cost.value
+ if self.multiprocessing and self.new_result.value:
+ self.new_result.value = False
+ (
+ self.last_available_result.xs,
+ self.last_available_result.us,
+ self.last_available_result.K,
+ self.last_available_result.solving_duration,
+ ) = self.decompress_dataOut()
else:
self.initialized = True
- return self.last_available_result, self.last_cost
+ return self.last_available_result
- def run_MPC_synchronous(self, x0, guess):
+ def run_MPC_synchronous(self, x0, xs, us):
"""
Run the MPC (synchronous version)
"""
- self.ocp.solve(x0, guess)
- self.last_available_result = self.ocp.get_results()
- #self.last_cost = self.ocp.retrieve_cost()
+ self.ocp.solve(x0, xs, us)
+ (
+ self.last_available_result.xs,
+ self.last_available_result.us,
+ self.last_available_result.K,
+ self.last_available_result.solving_duration,
+ ) = self.ocp.get_results()
- def run_MPC_asynchronous(self, k, x0, guess):
+ def run_MPC_asynchronous(self, k, x0, xs, us):
"""
Run the MPC (asynchronous version)
"""
@@ -102,61 +104,96 @@ class MPC_Wrapper:
p = Process(target=self.MPC_asynchronous)
p.start()
- self.add_new_data(k, x0, guess)
+ self.add_new_data(k, x0, xs, us)
def MPC_asynchronous(self):
"""
Parallel process with an infinite loop that run the asynchronous MPC
"""
while self.running.value:
- if self.newData.value:
- self.newData.value = False
+ if not self.new_data.value:
+ continue
+
+ self.new_data.value = False
- k, x0, guess = self.decompress_dataIn(self.dataIn)
+ k, x0, xs, us = self.decompress_dataIn(self.dataIn)
- if k == 0:
- loop_ocp = OCP(self.pd, self.target)
+ if k == 0:
+ loop_ocp = OCP(self.pd, self.target)
- 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
+ loop_ocp.solve(x0, xs, us)
+ xs, us, K, solving_time = loop_ocp.get_latest_result()
+ self.compress_dataOut(xs, us, K, solving_time)
- def add_new_data(self, k, x0):
+ self.new_result.value = True
+
+ def add_new_data(self, k, x0, xs, us):
"""
- Compress data in a C-type structure that belongs to the shared memory to send
+ 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
that there is a new data
"""
- with self.dataIn.get_lock():
- self.dataIn.k = k
- # np.frombuffer(self.dataIn.xref).reshape((12, self.n_steps + 1))[
- # :, :
- # ] = xref
- self.newData.value = True
- def decompress_dataIn(self, dataIn):
+ self.compress_dataIn(k, x0, xs, us)
+ self.new_data.value = True
+
+ def compress_dataIn(self, k, x0, xs, us):
"""
Decompress data from a C-type structure that belongs to the shared memory to
retrieve data from the main control loop in the asynchronous MPC
-
- Args:
dataIn (Array): shared C-type structure that contains the input data
"""
+ with self.in_k.get_lock():
+ self.in_k.value = k
+ with self.in_x0.get_lock():
+ np.frombuffer(self.in_x0).reshape(self.pd.nx)[:] = x0
+ with self.in_xs.get_lock():
+ np.frombuffer(self.in_xs).reshape((self.pd.T + 1, self.pd.nx))[:, :] = xs
+ with self.in_us.get_lock():
+ np.frombuffer(self.in_us).reshape((self.pd.T, self.pd.nu))[:, :] = us
- with dataIn.get_lock():
+ return k, x0, xs, us
+
+ def decompress_dataIn(self):
+ """
+ Decompress data from a C-type structure that belongs to the shared memory to
+ retrieve data from the main control loop in the asynchronous MPC
+ """
+ with self.in_k.get_lock():
k = self.dataIn.k
- xref = np.frombuffer(self.dataIn.xref).reshape((12, self.n_steps + 1))
+ with self.in_x0.get_lock():
+ x0 = np.frombuffer(self.in_x0).reshape(self.pd.nx)
+ with self.in_xs.get_lock():
+ xs = np.frombuffer(self.in_xs).reshape((self.pd.T + 1, self.pd.nx))
+ with self.in_us.get_lock():
+ us = np.frombuffer(self.in_us).reshape((self.pd.T, self.pd.nu))
- return k, xref
+ return k, x0, xs, us
- def convert_dataOut(self):
+ def compress_dataOut(self, xs, us, K, solving_time):
+ """
+ Compress data to a C-type structure that belongs to the shared memory to
+ retrieve data in the main control loop from the asynchronous MPC
+ """
+ with self.out_xs.get_lock():
+ np.frombuffer(self.out_xs).reshape((self.pd.T + 1, self.pd.nx))[:, :] = xs
+ with self.out_us.get_lock():
+ np.frombuffer(self.out_us).reshape((self.pd.T, self.pd.nu))[:, :] = us
+ with self.out_k.get_lock():
+ np.frombuffer(self.out_k).reshape((0, 0))[:, :] = K
+ self.out_solving_time.value = solving_time
+
+ def decompress_dataOut(self):
"""
Return the result of the asynchronous MPC (desired contact forces) that is
stored in the shared memory
"""
+ xs = np.frombuffer(self.out_xs).reshape((self.pd.T + 1, self.pd.nx))
+ us = np.frombuffer(self.out_us).reshape((self.pd.T, self.pd.nu))
+ K = np.frombuffer(self.out_k).reshape((0, 0))
+ solving_time = self.solving_time.value
- return np.array(self.dataOut[:]).reshape((24, -1), order="F")
+ return xs, us, K, solving_time
def stop_parallel_loop(self):
"""
@@ -164,5 +201,3 @@ class MPC_Wrapper:
"""
self.running.value = False
-
- return 0
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 48750714cd5c07a509810bfd5094631b5dc956b3..654e65832b72a2adf0c4c5f46240433af6c58513 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -147,7 +147,7 @@ def control_loop():
qc = QualisysClient(ip="140.93.16.160", body_id=0)
if params.LOGGING or params.PLOTTING:
- loggerControl = LoggerControl(pd, log_size=params.N_SIMULATION-1)
+ loggerControl = LoggerControl(pd, params, log_size=params.N_SIMULATION-1)
if params.SIMULATION:
device.Init(
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 9f4833db9630f5e17e8c4172621b533143be1ec0..79794913905440d570fb4f84b435fd985780fa4d 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -5,13 +5,14 @@ from .kinematics_utils import get_translation, get_translation_array
class LoggerControl:
- def __init__(self, pd, log_size=60e3, loop_buffer=False, file=None):
+ def __init__(self, pd, params, log_size=60e3, loop_buffer=False, file=None):
if file is not None:
self.data = np.load(file, allow_pickle=True)
self.log_size = np.int(log_size)
self.i = 0
self.loop_buffer = loop_buffer
+ self.multiprocess_mpc = params.enable_multiprocessing
size = self.log_size
self.pd = pd
@@ -59,10 +60,8 @@ class LoggerControl:
self.t_ocp_update_terminal = np.zeros(size)
# MPC
- self.ocp_storage = {
- "xs": np.zeros([size, pd.T + 1, pd.nx]),
- "us": np.zeros([size, pd.T, pd.nu]),
- }
+ self.ocp_xs = np.zeros([size, pd.T + 1, pd.nx])
+ self.ocp_us = np.zeros([size, pd.T, pd.nu])
self.target = np.zeros([size, 3])
# Whole body control
@@ -103,29 +102,32 @@ class LoggerControl:
# Controller timings: MPC time, ...
self.target[self.i] = controller.point_target
- self.t_mpc[self.i] = controller.mpc.ocp.results.solver_time
+ self.t_mpc[self.i] = controller.mpc.mpc_result.solver_time
self.t_send[self.i] = controller.t_send
self.t_loop[self.i] = controller.t_loop
self.t_measures[self.i] = controller.t_measures
# Logging from model predictive control
- self.ocp_storage["xs"][self.i] = np.array(controller.mpc.ocp.results.x)
- self.ocp_storage["us"][self.i] = np.array(controller.mpc.ocp.results.u)
+ self.ocp_xs[self.i] = np.array(controller.mpc_result.xs)
+ self.ocp_us[self.i] = np.array(controller.mpc_result.us)
self.t_measures[self.i] = controller.t_measures
self.t_mpc[self.i] = controller.t_mpc
self.t_send[self.i] = controller.t_send
self.t_loop[self.i] = controller.t_loop
- self.t_ocp_update[self.i] = controller.mpc.ocp.t_update
- self.t_ocp_warm_start[self.i] = controller.mpc.ocp.t_warm_start
- self.t_ocp_ddp[self.i] = controller.mpc.ocp.t_ddp
- self.t_ocp_solve[self.i] = controller.mpc.ocp.t_solve
+ if not self.multiprocess_mpc:
+ self.t_ocp_update[self.i] = controller.mpc.ocp.t_update
+ self.t_ocp_warm_start[self.i] = controller.mpc.ocp.t_warm_start
+ self.t_ocp_ddp[self.i] = controller.mpc.ocp.t_ddp
+ self.t_ocp_solve[self.i] = controller.mpc.ocp.t_solve
- self.t_ocp_update_FK[self.i] = controller.mpc.ocp.t_FK
- self.t_ocp_shift[self.i] = controller.mpc.ocp.t_shift
- self.t_ocp_update_last[self.i] = controller.mpc.ocp.t_update_last_model
- self.t_ocp_update_terminal[self.i] = controller.mpc.ocp.t_update_terminal_model
+ self.t_ocp_update_FK[self.i] = controller.mpc.ocp.t_FK
+ self.t_ocp_shift[self.i] = controller.mpc.ocp.t_shift
+ self.t_ocp_update_last[self.i] = controller.mpc.ocp.t_update_last_model
+ self.t_ocp_update_terminal[
+ self.i
+ ] = controller.mpc.ocp.t_update_terminal_model
# Logging from whole body control
self.wbc_P[self.i] = controller.result.P
@@ -143,18 +145,15 @@ class LoggerControl:
def plot(self, save=False, fileName="tmp/"):
import matplotlib.pyplot as plt
- x_mes = np.concatenate([self.q_mes[:, 3:6], self.v_mes[:, 3:6]], axis = 1)
+ x_mes = np.concatenate([self.q_mes[:, 3:6], self.v_mes[:, 3:6]], axis=1)
- x_mpc = [self.ocp_storage["xs"][0][0, :]]
- [x_mpc.append(x[1, :]) for x in self.ocp_storage["xs"][:-1]]
+ x_mpc = [self.ocp_xs[0][0, :]]
+ [x_mpc.append(x[1, :]) for x in self.ocp_xs[:-1]]
x_mpc = np.array(x_mpc)
# Feet positions calcuilated by every ocp
all_ocp_feet_p_log = {
- idx: [
- get_translation_array(self.pd, x, idx)[0]
- for x in self.ocp_storage["xs"]
- ]
+ idx: [get_translation_array(self.pd, x, idx)[0] for x in self.ocp_xs]
for idx in self.pd.allContactIds
}
for foot in all_ocp_feet_p_log:
@@ -162,28 +161,15 @@ class LoggerControl:
# Measured feet positions
m_feet_p_log = {
- idx:
- get_translation_array(self.pd, x_mes, idx)[0]
+ idx: get_translation_array(self.pd, x_mes, idx)[0]
for idx in self.pd.allContactIds
}
- # Predicted eet positions
+ # Predicted feet positions
feet_p_log = {
- idx:
- get_translation_array(self.pd, x_mpc, idx)[0]
+ idx: get_translation_array(self.pd, x_mpc, idx)[0]
for idx in self.pd.allContactIds
}
-
-
-
- # plt.figure(figsize=(12, 6), dpi=90)
- # plt.title("Solver timings")
- # plt.hist(self.ocp_timings, 30)
- # plt.xlabel("timee [s]")
- # plt.ylabel("Number of cases [#]")
- # plt.draw()
- # if save:
- # plt.savefig(fileName + "_solver_timings")
legend = ["Hip", "Shoulder", "Knee"]
plt.figure(figsize=(12, 6), dpi=90)
@@ -235,10 +221,10 @@ class LoggerControl:
plt.savefig(fileName + "/joint_torques")
legend = ["x", "y", "z"]
- plt.figure(figsize=(12, 18), dpi = 90)
+ plt.figure(figsize=(12, 18), dpi=90)
for p in range(3):
- plt.subplot(3,1, p+1)
- plt.title('Free foot on ' + legend[p])
+ plt.subplot(3, 1, p + 1)
+ plt.title("Free foot on " + legend[p])
plt.plot(self.target[:, p])
plt.plot(m_feet_p_log[self.pd.rfFootId][:, p])
plt.plot(feet_p_log[self.pd.rfFootId][:, p])
@@ -247,8 +233,9 @@ class LoggerControl:
plt.savefig(fileName + "/target")
self.plot_controller_times()
- # self.plot_OCP_times()
- # self.plot_OCP_update_times()
+ if not self.multiprocess_mpc:
+ self.plot_OCP_times()
+ self.plot_OCP_update_times()
plt.show()
@@ -311,7 +298,8 @@ class LoggerControl:
np.savez_compressed(
name,
- ocp_storage=self.ocp_storage,
+ ocp_xs=self.ocp_xs,
+ ocp_us=self.ocp_us,
t_measures=self.t_measures,
t_mpc=self.t_mpc,
t_send=self.t_send,
@@ -381,7 +369,8 @@ class LoggerControl:
self.t_loop = self.data["t_loop"]
self.t_measures = self.data["t_meausres"]
- self.ocp_storage = self.data["ocp_storage"].item()
+ self.ocp_xs = self.data["ocp_xs"]
+ self.ocp_us = self.data["ocp_us"]
self.t_measures = self.data["t_measures"]
self.t_mpc = self.data["t_mpc"]