diff --git a/include/qrw/MPC.hpp b/include/qrw/MPC.hpp
index 78df15fe50279452a30c2739d19e4da8ca622146..4622dcc60c8181b08d154c99177adcd302f46d07 100644
--- a/include/qrw/MPC.hpp
+++ b/include/qrw/MPC.hpp
@@ -15,6 +15,7 @@
#include "osqp_folder/include/util.h"
#include "osqp_folder/include/osqp_configure.h"
#include "other/st_to_cc.hpp"
+#include "qrw/config.h"
typedef Eigen::MatrixXd matXd;
@@ -29,7 +30,7 @@ class MPC {
Eigen::Matrix<double, 3, 4> footholds = Eigen::Matrix<double, 3, 4>::Zero();
Eigen::Matrix<double, 1, 12> footholds_tmp = Eigen::Matrix<double, 12, 1>::Zero();
Eigen::Matrix<double, 3, 4> lever_arms = Eigen::Matrix<double, 3, 4>::Zero();
- Eigen::Matrix<int, 20, 4> gait = Eigen::Matrix<int, 20, 4>::Zero();
+ Eigen::Matrix<int, N0_gait, 4> gait = Eigen::Matrix<int, N0_gait, 4>::Zero();
Eigen::Matrix<double, 12, 1> g = Eigen::Matrix<double, 12, 1>::Zero();
Eigen::Matrix<double, 12, 12> A = Eigen::Matrix<double, 12, 12>::Identity();
diff --git a/include/qrw/config.h b/include/qrw/config.h
index 97577104c602c2ac7c0b98c265d73b2046769d55..8a218236a30e24e270dd5a4bec193a0a599e08d1 100644
--- a/include/qrw/config.h
+++ b/include/qrw/config.h
@@ -3,7 +3,7 @@
// Number of rows in the gait matrix. Arbitrary value that should be set high enough so that there is always at
// least one empty line at the end of the gait matrix
-#define N0_gait 20
+#define N0_gait 100
#endif // CONFIG_H_INCLUDED
diff --git a/scripts/Controller.py b/scripts/Controller.py
index ca84f3ef50bab9c417de8eb711b4f00c363d6c5a..08c0de32106f1a3355174c0aa283ebff2c774ba8 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -49,7 +49,7 @@ class dummyDevice:
class Controller:
def __init__(self, q_init, envID, velID, dt_wbc, dt_mpc, k_mpc, t, T_gait, T_mpc, N_SIMULATION, type_MPC,
- pyb_feedback, on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI, kf_enabled):
+ pyb_feedback, on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI, kf_enabled, N0_gait):
"""Function that runs a simulation scenario based on a reference velocity profile, an environment and
various parameters to define the gait
@@ -70,6 +70,7 @@ class Controller:
predefined_vel (bool): to use either a predefined velocity profile or a gamepad
enable_pyb_GUI (bool): to display PyBullet GUI or not
kf_enabled (bool): complementary filter (False) or kalman filter (True)
+ N0_gait (int): number of spare lines in the gait matrix
"""
########################################################################
@@ -135,7 +136,7 @@ class Controller:
# First argument to True to have PA's MPC, to False to have Thomas's MPC
self.enable_multiprocessing = False
self.mpc_wrapper = MPC_Wrapper.MPC_Wrapper(type_MPC, dt_mpc, np.int(T_mpc/dt_mpc),
- k_mpc, T_mpc, self.q, self.enable_multiprocessing)
+ k_mpc, T_mpc, N0_gait, self.q, self.enable_multiprocessing)
# ForceMonitor to display contact forces in PyBullet with red lines
# import ForceMonitor
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 048cdefefb900b67e9046ea03ea573158f9f3adb..f65e22f577d4c52cae0500766dd119bcdfea2536 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -6,7 +6,7 @@ from utils_mpc import quaternionToRPY
class LoggerControl():
- def __init__(self, dt, joystick=None, estimator=None, loop=None, gait=None, statePlanner=None,
+ def __init__(self, dt, N0_gait, joystick=None, estimator=None, loop=None, gait=None, statePlanner=None,
footstepPlanner=None, footTrajectoryGenerator=None, logSize=60e3, ringBuffer=False):
self.ringBuffer = ringBuffer
logSize = np.int(logSize)
@@ -48,7 +48,7 @@ class LoggerControl():
self.loop_o_v = np.zeros([logSize, 18]) # estimated velocity in world frame
# Gait
- self.planner_gait = np.zeros([logSize, 20, 4]) # Gait sequence
+ self.planner_gait = np.zeros([logSize, N0_gait, 4]) # Gait sequence
self.planner_is_static = np.zeros([logSize]) # if the planner is in static mode or not
self.planner_q_static = np.zeros([logSize, 19]) # position in static mode (4 stance phase)
self.planner_RPY_static = np.zeros([logSize, 3]) # RPY orientation in static mode (4 stance phase)
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index 29b76e11f61b39522b4084f1ba36f2cb96d7deec..4f4f90e9885efa509aa4f948e4aa707397a1566f 100644
--- a/scripts/MPC_Wrapper.py
+++ b/scripts/MPC_Wrapper.py
@@ -32,7 +32,7 @@ class MPC_Wrapper:
multiprocessing (bool): Enable/Disable running the MPC with another process
"""
- def __init__(self, mpc_type, dt, n_steps, k_mpc, T_gait, q_init, multiprocessing=False):
+ def __init__(self, mpc_type, dt, n_steps, k_mpc, T_gait, N0_gait, q_init, multiprocessing=False):
self.f_applied = np.zeros((12,))
self.not_first_iter = False
@@ -43,6 +43,7 @@ class MPC_Wrapper:
self.dt = dt
self.n_steps = n_steps
self.T_gait = T_gait
+ self.N0_gait = N0_gait
self.gait_memory = np.zeros(4)
self.mpc_type = mpc_type
@@ -50,9 +51,9 @@ class MPC_Wrapper:
if multiprocessing: # Setup variables in the shared memory
self.newData = Value('b', False)
self.newResult = Value('b', False)
- self.dataIn = Array('d', [0.0] * (1 + (np.int(self.n_steps)+1) * 12 + 12*20))
+ self.dataIn = Array('d', [0.0] * (1 + (np.int(self.n_steps)+1) * 12 + 12*self.N0_gait))
self.dataOut = Array('d', [0] * 24 * (np.int(self.n_steps)))
- self.fsteps_future = np.zeros((20, 12))
+ self.fsteps_future = np.zeros((self.N0_gait, 12))
self.running = Value('b', True)
else:
# Create the new version of the MPC solver object
@@ -192,7 +193,7 @@ class MPC_Wrapper:
# Reshaping 1-dimensional data
k = int(kf[0])
xref = np.reshape(xref_1dim, (12, self.n_steps+1))
- fsteps = np.reshape(fsteps_1dim, (20, 12))
+ fsteps = np.reshape(fsteps_1dim, (self.N0_gait, 12))
# Create the MPC object of the parallel process during the first iteration
if k == 0:
@@ -250,7 +251,7 @@ class MPC_Wrapper:
"""
# Sizes of the different variables that are stored in the C-type array
- sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 12*20]
+ sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 12*self.N0_gait]
csizes = np.cumsum(sizes)
# Return decompressed variables in a list
@@ -275,7 +276,7 @@ class MPC_Wrapper:
footstep when a new phase is created.
Args:
- fsteps (13x20 array): the remaining number of steps of each phase of the gait (first column)
+ fsteps (13xN0_gait array): the remaining number of steps of each phase of the gait (first column)
and the [x, y, z]^T desired position of each foot for each phase of the gait (12 other columns)
"""
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index 45b0a8a45fc6ffa4950448ef1500bdfb1780fe7f..5e3d947085f3024cabef5c3635cec1a1e29bd2ee 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -26,6 +26,7 @@ else:
from solopython.utils.qualisysClient import QualisysClient
DT = 0.0020
+N0_gait = 100 # Same value than N0_gait in include/config.h
key_pressed = False
@@ -145,7 +146,7 @@ def control_loop(name_interface, name_interface_clone=None):
# Run a scenario and retrieve data thanks to the logger
controller = Controller(q_init, envID, velID, dt_wbc, dt_mpc, k_mpc, t, T_gait, T_mpc, N_SIMULATION, type_MPC,
- pyb_feedback, on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI, kf_enabled)
+ pyb_feedback, on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI, kf_enabled, N0_gait)
####
@@ -172,7 +173,7 @@ def control_loop(name_interface, name_interface_clone=None):
if LOGGING or PLOTTING:
loggerSensors = LoggerSensors(device, qualisys=qc, logSize=N_SIMULATION-3)
- loggerControl = LoggerControl(dt_wbc, joystick=controller.joystick, estimator=controller.estimator,
+ loggerControl = LoggerControl(dt_wbc, N0_gait, joystick=controller.joystick, estimator=controller.estimator,
loop=controller, gait=controller.gait, statePlanner=controller.statePlanner,
footstepPlanner=controller.footstepPlanner,
footTrajectoryGenerator=controller.footTrajectoryGenerator,
diff --git a/src/MPC.cpp b/src/MPC.cpp
index 391c92a6123ee98209953482cb44d1072f10558c..7de312c6c5cdbc706f7f214cbf941dd9aa178acc 100644
--- a/src/MPC.cpp
+++ b/src/MPC.cpp
@@ -657,7 +657,7 @@ N is the number of time step in the prediction horizon.
int MPC::construct_S() {
int i = 0;
- Eigen::Matrix<int, 20, 4> inv_gait = Eigen::Matrix<int, 20, 4>::Ones() - gait;
+ Eigen::Matrix<int, N0_gait, 4> inv_gait = Eigen::Matrix<int, N0_gait, 4>::Ones() - gait;
while (!gait.row(i).isZero()) {
// S_gait.block(k*12, 0, gait[i, 0]*12, 1) = (1 - (gait.block(i, 1, 1, 4)).transpose()).replicate<gait[i, 0], 1>()
// not finished;
@@ -763,16 +763,12 @@ void MPC::save_dns_matrix(double *M, int size, std::string filename) {
}
Eigen::MatrixXd MPC::get_gait() {
- // Eigen::MatrixXd tmp = Eigen::MatrixXd::Zero(20, 5);
- // tmp.block(0, 0, 20, 5) = gait.block(0, 0, 20, 5);
Eigen::MatrixXd tmp;
tmp = gait.cast<double>();
return tmp;
}
Eigen::MatrixXd MPC::get_Sgait() {
- // Eigen::MatrixXd tmp = Eigen::MatrixXd::Zero(12 * n_steps, 1);
- // tmp.col(0) = S_gait.col(0);
Eigen::MatrixXd tmp;
tmp = S_gait.cast<double>();
return tmp;