From 08e32ccfaff44fd82380f68a84f47c0c2760d20a Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Mon, 20 Sep 2021 12:13:45 +0200
Subject: [PATCH] Adapt MPC Crocoddyl
---
scripts/crocoddyl_class/MPC_crocoddyl.py | 19 ++++++++-----------
1 file changed, 8 insertions(+), 11 deletions(-)
diff --git a/scripts/crocoddyl_class/MPC_crocoddyl.py b/scripts/crocoddyl_class/MPC_crocoddyl.py
index 3852b330..f31b9c0d 100644
--- a/scripts/crocoddyl_class/MPC_crocoddyl.py
+++ b/scripts/crocoddyl_class/MPC_crocoddyl.py
@@ -19,9 +19,8 @@ class MPC_crocoddyl:
def __init__(self, params, mu=1, inner=True, linearModel=True):
- self.dt = params.dt_mpc # Time step of the solver
- self.T_mpc = params.T_mpc # Period of the MPC
- self.n_nodes = int(self.T_mpc/self.dt) # Number of nodes
+ self.dt = params.dt_mpc # Time step of the solver
+ self.n_nodes = int(params.gait.shape[0]) # Number of nodes
self.mass = params.mass # Mass of the robot
self.gI = np.array(params.I_mat.tolist()).reshape((3, 3)) # Inertia matrix in ody frame
@@ -67,9 +66,9 @@ class MPC_crocoddyl:
self.warm_start = True # Warm Start for the solver
# Position of the feet
- self.fsteps = np.full((params.N_gait, 12), np.nan)
- self.gait = np.zeros((params.N_gait, 4))
- self.xref = np.full((12, int(params.T_gait / params.dt_mpc + 1 )), np.nan)
+ self.fsteps = np.full((self.n_nodes, 12), np.nan)
+ self.gait = np.zeros((self.n_nodes, 4))
+ self.xref = np.full((12, self.n_nodes + 1), np.nan)
self.index = 0
# Offset CoM
@@ -111,9 +110,7 @@ class MPC_crocoddyl:
self.problem.x0 = xref[:, 0]
# Construction of the gait matrix representing the feet in contact with the ground
- self.index = 0
- while (np.any(self.fsteps[self.index, :])):
- self.index += 1
+ self.index = self.n_nodes
self.gait[:self.index, :] = 1.0 - (self.fsteps[:self.index, 0::3] == 0.0)
self.gait[self.index:, :] = 0.0
@@ -166,8 +163,8 @@ class MPC_crocoddyl:
Args:
"""
- output = np.zeros((24, int(self.T_mpc/self.dt)))
- for i in range(int(self.T_mpc/self.dt)):
+ output = np.zeros((24, self.n_nodes))
+ for i in range(self.n_nodes):
output[:12, i] = np.asarray(self.ddp.xs[i+1])
output[12:, i] = np.asarray(self.ddp.us[i])
return output
--
GitLab