From 57fb6d448c378ce29ae0bf89904275f472d7c18b Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Tue, 16 Feb 2021 14:41:53 +0100
Subject: [PATCH] Cleaning MPC + fix MPC default output (the one before the MPC
finishes its computation)
---
include/quadruped-reactive-walking/MPC.hpp | 2 +-
scripts/LoggerControl.py | 25 +++++++++++-----------
scripts/MPC_Wrapper.py | 19 ++++++++++------
src/MPC.cpp | 21 +++++++++---------
4 files changed, 35 insertions(+), 32 deletions(-)
diff --git a/include/quadruped-reactive-walking/MPC.hpp b/include/quadruped-reactive-walking/MPC.hpp
index d52fb4ab..ce83187d 100644
--- a/include/quadruped-reactive-walking/MPC.hpp
+++ b/include/quadruped-reactive-walking/MPC.hpp
@@ -120,7 +120,7 @@ class MPC {
void save_dns_matrix(double *M, int size, std::string filename);
// Bindings
- void run_python(int num_iter, const matXd &xref_py, const matXd &fsteps_py);
+ void run_python(const matXd &xref_py, const matXd &fsteps_py);
// Eigen::Matrix<double, 12, 12> getA() { return A; }
// Eigen::MatrixXf getML() { return ML; }
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 95699703..4bce112b 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -354,7 +354,7 @@ class LoggerControl():
embed()"""
titles = ["X", "Y", "Z", "Roll", "Pitch", "Yaw"]
- step = 200
+ step = 2000
plt.figure()
for j in range(6):
plt.subplot(3, 2, index6[j])
@@ -391,6 +391,7 @@ class LoggerControl():
plt.title("Predicted trajectory for velocity in " + titles[j])
plt.suptitle("Analysis of trajectories of linear and angular velocities computed by the MPC")
+ step = 1000
lgd1 = ["Ctct force X", "Ctct force Y", "Ctct force Z"]
lgd2 = ["FL", "FR", "HL", "HR"]
plt.figure()
@@ -414,24 +415,22 @@ class LoggerControl():
lgd1 = ["Ctct force X", "Ctct force Y", "Ctct force Z"]
lgd2 = ["FL", "FR", "HL", "HR"]
plt.figure()
- for i in range(12):
+ for i in range(4):
if i == 0:
- ax0 = plt.subplot(3, 4, index12[i])
+ ax0 = plt.subplot(1, 4, i+1)
else:
- plt.subplot(3, 4, index12[i], sharex=ax0)
+ plt.subplot(1, 4, i+1, sharex=ax0)
for k in range(0, self.mpc_x_f.shape[0], step):
- h2, = plt.plot(log_t_pred+k*self.dt, self.mpc_x_f[k, 12+i, :], linestyle="--", marker='x', color="g", linewidth=2)
- h1, = plt.plot(t_range, self.mpc_x_f[:, 12+i, 0], "r", linewidth=3)
+ h2, = plt.plot(log_t_pred+k*self.dt, self.mpc_x_f[k, 12+(3*i+2), :], linestyle="--", marker='x', linewidth=2)
+ h1, = plt.plot(t_range, self.mpc_x_f[:, 12+(3*i+2), 0], "r", linewidth=3)
# h3, = plt.plot(t_range, self.wbc_f_ctc[:, i], "b", linewidth=3, linestyle="--")
+ plt.plot(t_range, self.esti_feet_status[:, i], "k", linestyle="--")
plt.xlabel("Time [s]")
- plt.ylabel(lgd1[i % 3]+" "+lgd2[int(i/3)]+" [N]")
- plt.legend([h1, h2], ["MPC " + lgd1[i % 3]+" "+lgd2[int(i/3)],
- "MPC " + lgd1[i % 3]+" "+lgd2[int(i/3)]+" trajectory"])
- if (i % 3) == 2:
- plt.ylim([-0.0, 26.0])
- else:
- plt.ylim([-26.0, 26.0])
+ plt.ylabel(lgd2[i]+" [N]")
+ plt.legend([h1, h2], ["MPC "+lgd2[i],
+ "MPC "+lgd2[i]+" trajectory"])
+ plt.ylim([-1.0, 26.0])
plt.suptitle("Contact forces trajectories & Actual forces trajectories")
plt.show(block=True)
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index 9a6724a0..d084bdac 100644
--- a/scripts/MPC_Wrapper.py
+++ b/scripts/MPC_Wrapper.py
@@ -83,15 +83,20 @@ class MPC_Wrapper:
else: # Run in the same process than main loop
self.run_MPC_synchronous(k, fstep_planner)
- if k > 2 and not np.array_equal(self.gait_memory, fstep_planner.gait[0, 1:]):
- self.gait_memory = (fstep_planner.gait[0, 1:]).copy()
- mass = 2.5
- nb_ctc = np.sum(fstep_planner.gait[0, 1:])
+ if k > 2:
+ self.last_available_result[12:(12+self.n_steps), :] = np.roll(self.last_available_result[12:(12+self.n_steps), :], -1, axis=1)
+
+ pt = 0
+ while (fstep_planner.gait[pt, 0] != 0):
+ pt += 1
+ if k > 2 and not np.array_equal(fstep_planner.gait[0, 1:], fstep_planner.gait[pt-1, 1:]):
+ mass = 2.5 # Todo: grab from URDF?
+ nb_ctc = np.sum(fstep_planner.gait[pt-1, 1:])
F = 9.81 * mass / nb_ctc
- self.last_available_result[12:, 0] = np.zeros(12)
+ self.last_available_result[12:, self.n_steps-1] = np.zeros(12)
for i in range(4):
- if (fstep_planner.gait[0, 1+i] == 1):
- self.last_available_result[12+3*i+2, 0] = F
+ if (fstep_planner.gait[pt-1, 1+i] == 1):
+ self.last_available_result[12+3*i+2, self.n_steps-1] = F
return 0
diff --git a/src/MPC.cpp b/src/MPC.cpp
index ca459a1b..f8fa1086 100644
--- a/src/MPC.cpp
+++ b/src/MPC.cpp
@@ -148,15 +148,14 @@ int MPC::create_ML() {
double *acc; // coeff values
int nst = cpt_ML; // number of non zero elements
int ncc = st_to_cc_size(nst, r_ML, c_ML); // number of CC values
- int m = 12 * n_steps * 2 + 20 * n_steps; // number of rows
+ // int m = 12 * n_steps * 2 + 20 * n_steps; // number of rows
int n = 12 * n_steps * 2; // number of columns
- int i_min = i4vec_min(nst, r_ML);
+ /*int i_min = i4vec_min(nst, r_ML);
int i_max = i4vec_max(nst, r_ML);
int j_min = i4vec_min(nst, c_ML);
int j_max = i4vec_max(nst, c_ML);
-
- // st_header_print(i_min, i_max, j_min, j_max, m, n, nst);
+ st_header_print(i_min, i_max, j_min, j_max, m, n, nst);*/
// Get the CC indices.
icc = (int *)malloc(ncc * sizeof(int));
@@ -182,8 +181,8 @@ int MPC::create_ML() {
delete[] v_ML;
// Print CC matrix
- char rt_char[2] = {'R', 'T'};
- char cc_char[2] = {'C', 'C'};
+ // char rt_char[2] = {'R', 'T'};
+ // char cc_char[2] = {'C', 'C'};
// st_print(m, n, 25, r_ML, c_ML, v_ML, rt_char);
// cc_print ( m, n, 25, icc, ccc, acc, cc_char);
@@ -340,9 +339,9 @@ int MPC::create_weight_matrices() {
// Define weights for the force components of the optimization vector
for (int k = n_steps; k < (2 * n_steps); k++) {
for (int i = 0; i < 4; i++) {
- add_to_P(12 * k + 3 * i + 0, 12 * k + 3 * i + 0, 1e-4f, r_P, c_P, v_P);
- add_to_P(12 * k + 3 * i + 1, 12 * k + 3 * i + 1, 1e-4f, r_P, c_P, v_P);
- add_to_P(12 * k + 3 * i + 2, 12 * k + 3 * i + 2, 1e-4f, r_P, c_P, v_P);
+ add_to_P(12 * k + 3 * i + 0, 12 * k + 3 * i + 0, 1e-5f, r_P, c_P, v_P);
+ add_to_P(12 * k + 3 * i + 1, 12 * k + 3 * i + 1, 1e-5f, r_P, c_P, v_P);
+ add_to_P(12 * k + 3 * i + 2, 12 * k + 3 * i + 2, 1e-5f, r_P, c_P, v_P);
}
}
@@ -352,7 +351,7 @@ int MPC::create_weight_matrices() {
double *acc; // coeff values
int nst = cpt_P; // number of non zero elements
int ncc = st_to_cc_size(nst, r_P, c_P); // number of CC values
- int m = 12 * n_steps * 2; // number of rows
+ // int m = 12 * n_steps * 2; // number of rows
int n = 12 * n_steps * 2; // number of columns
// Get the CC indices.
@@ -604,7 +603,7 @@ double *MPC::get_x_next() { return x_next; }
/*
Run function with arrays as input for compatibility between Python and C++
*/
-void MPC::run_python(int num_iter, const matXd &xref_py, const matXd &fsteps_py) {
+void MPC::run_python(const matXd &xref_py, const matXd &fsteps_py) {
printf("Trigger bindings \n");
printf("xref: %f %f %f \n", xref_py(0, 0), xref_py(1, 0), xref_py(2, 0));
printf("fsteps: %f %f %f \n", fsteps_py(0, 0), fsteps_py(1, 0), fsteps_py(2, 0));
--
GitLab