From 9cb07cd6320a5c42a7121e5c6a0c75362982ebb8 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Mon, 26 Apr 2021 15:25:52 +0200
Subject: [PATCH] Remove config.h and finish integration of yaml config file
---
include/qrw/FootstepPlanner.hpp | 9 ++++----
include/qrw/Gait.hpp | 3 +--
include/qrw/MPC.hpp | 6 +++---
include/qrw/config.h | 9 --------
python/gepadd.cpp | 6 +++---
scripts/Controller.py | 10 ++++-----
scripts/MPC_Wrapper.py | 16 +++++++-------
src/FootstepPlanner.cpp | 37 +++++++++++++++++++++------------
src/Gait.cpp | 26 +++++++++++++----------
src/MPC.cpp | 8 ++++---
10 files changed, 69 insertions(+), 61 deletions(-)
delete mode 100644 include/qrw/config.h
diff --git a/include/qrw/FootstepPlanner.hpp b/include/qrw/FootstepPlanner.hpp
index f26d8b00..71591ef8 100644
--- a/include/qrw/FootstepPlanner.hpp
+++ b/include/qrw/FootstepPlanner.hpp
@@ -14,7 +14,7 @@
#include "pinocchio/math/rpy.hpp"
#include "qrw/Gait.hpp"
#include "qrw/Types.h"
-
+#include <vector>
// Order of feet/legs: FL, FR, HL, HR
@@ -43,7 +43,8 @@ public:
double T_mpc_in,
double h_ref_in,
MatrixN const& shouldersIn,
- Gait& gaitIn);
+ Gait& gaitIn,
+ int N_gait);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -108,7 +109,7 @@ private:
////////////////////////////////////////////////////////////////////////////////////////////////
void updateTargetFootsteps();
- MatrixN vectorToMatrix(std::array<Matrix34, N0_gait> const& array);
+ MatrixN vectorToMatrix(std::vector<Matrix34> const& array);
Gait* gait_; // Gait object to hold the gait informations
@@ -130,7 +131,7 @@ private:
Matrix34 currentFootstep_; // Feet matrix in world frame
Matrix34 nextFootstep_; // Feet matrix in world frame
Matrix34 targetFootstep_;
- std::array<Matrix34, N0_gait> footsteps_;
+ std::vector<Matrix34> footsteps_;
Matrix3 Rz; // Predefined matrices for compute_footstep function
VectorN dt_cum;
diff --git a/include/qrw/Gait.hpp b/include/qrw/Gait.hpp
index 10fe3eef..276172fd 100644
--- a/include/qrw/Gait.hpp
+++ b/include/qrw/Gait.hpp
@@ -13,7 +13,6 @@
#define GAIT_H_INCLUDED
#include "qrw/Types.h"
-#include "qrw/config.h"
// Order of feet/legs: FL, FR, HL, HR
@@ -39,7 +38,7 @@ public:
/// \brief Initializer
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void initialize(double dt_in, double T_gait_in, double T_mpc_in);
+ void initialize(double dt_in, double T_gait_in, double T_mpc_in, int N_gait);
////////////////////////////////////////////////////////////////////////////////////////////////
///
diff --git a/include/qrw/MPC.hpp b/include/qrw/MPC.hpp
index 4622dcc6..ed66c8e8 100644
--- a/include/qrw/MPC.hpp
+++ b/include/qrw/MPC.hpp
@@ -15,7 +15,6 @@
#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;
@@ -30,7 +29,8 @@ 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, N0_gait, 4> gait = Eigen::Matrix<int, N0_gait, 4>::Zero();
+ Eigen::Matrix<int, Eigen::Dynamic, 4> gait;
+ Eigen::Matrix<int, Eigen::Dynamic, 4> inv_gait;
Eigen::Matrix<double, 12, 1> g = Eigen::Matrix<double, 12, 1>::Zero();
Eigen::Matrix<double, 12, 12> A = Eigen::Matrix<double, 12, 12>::Identity();
@@ -90,7 +90,7 @@ class MPC {
public:
MPC();
- MPC(double dt_in, int n_steps_in, double T_gait_in);
+ MPC(double dt_in, int n_steps_in, double T_gait_in, int N_gait);
int create_matrices();
int create_ML();
diff --git a/include/qrw/config.h b/include/qrw/config.h
deleted file mode 100644
index 8a218236..00000000
--- a/include/qrw/config.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef CONFIG_H_INCLUDED
-#define CONFIG_H_INCLUDED
-
-// 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 100
-
-
-#endif // CONFIG_H_INCLUDED
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 7903d8b6..97690c70 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -21,7 +21,7 @@ struct MPCPythonVisitor : public bp::def_visitor<MPCPythonVisitor<MPC>>
void visit(PyClassMPC& cl) const
{
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
- .def(bp::init<double, int, double>(bp::args("dt_in", "n_steps_in", "T_gait_in"),
+ .def(bp::init<double, int, double, int>(bp::args("dt_in", "n_steps_in", "T_gait_in", "N_gait"),
"Constructor with parameters."))
// Run MPC from Python
@@ -88,7 +88,7 @@ struct GaitPythonVisitor : public bp::def_visitor<GaitPythonVisitor<Gait>>
.def("isNewPhase", &Gait::isNewPhase, "Get newPhase_ boolean.\n")
.def("getIsStatic", &Gait::getIsStatic, "Get is_static_ boolean.\n")
- .def("initialize", &Gait::initialize, bp::args("dt_in", "T_gait_in", "T_mpc_in"),
+ .def("initialize", &Gait::initialize, bp::args("dt_in", "T_gait_in", "T_mpc_in", "N_gait"),
"Initialize Gait from Python.\n")
// Update current gait matrix from Python
@@ -122,7 +122,7 @@ struct FootstepPlannerPythonVisitor : public bp::def_visitor<FootstepPlannerPyth
.def("getFootsteps", &FootstepPlanner::getFootsteps, "Get footsteps_ matrix.\n")
- .def("initialize", &FootstepPlanner::initialize, bp::args("dt_in", "T_mpc_in", "h_ref_in", "shouldersIn", "gaitIn"),
+ .def("initialize", &FootstepPlanner::initialize, bp::args("dt_in", "T_mpc_in", "h_ref_in", "shouldersIn", "gaitIn", "N_gait"),
"Initialize FootstepPlanner from Python.\n")
// Compute target location of footsteps from Python
diff --git a/scripts/Controller.py b/scripts/Controller.py
index b29c2683..93dae208 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,
- use_flat_plane, predefined_vel, enable_pyb_GUI, kf_enabled, N0_gait, isSimulation):
+ use_flat_plane, predefined_vel, enable_pyb_GUI, kf_enabled, N_gait, isSimulation):
"""Function that runs a simulation scenario based on a reference velocity profile, an environment and
various parameters to define the gait
@@ -68,7 +68,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
+ N_gait (int): number of spare lines in the gait matrix
isSimulation (bool): if we are in simulation mode
"""
@@ -125,7 +125,7 @@ class Controller:
self.statePlanner.initialize(dt_mpc, T_mpc, self.h_ref)
self.gait = lqrw.Gait()
- self.gait.initialize(dt_mpc, T_gait, T_mpc)
+ self.gait.initialize(dt_mpc, T_gait, T_mpc, N_gait)
"""from IPython import embed
embed()"""
@@ -134,7 +134,7 @@ class Controller:
shoulders[0, :] = [0.1946, 0.1946, -0.1946, -0.1946]
shoulders[1, :] = [0.14695, -0.14695, 0.14695, -0.14695]
self.footstepPlanner = lqrw.FootstepPlanner()
- self.footstepPlanner.initialize(dt_mpc, T_mpc, self.h_ref, shoulders.copy(), self.gait)
+ self.footstepPlanner.initialize(dt_mpc, T_mpc, self.h_ref, shoulders.copy(), self.gait, N_gait)
self.footTrajectoryGenerator = lqrw.FootTrajectoryGenerator()
self.footTrajectoryGenerator.initialize(0.05, 0.07, self.fsteps_init.copy(), shoulders.copy(),
@@ -144,7 +144,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, N0_gait, self.q, self.enable_multiprocessing)
+ k_mpc, T_mpc, N_gait, self.q, self.enable_multiprocessing)
# ForceMonitor to display contact forces in PyBullet with red lines
# import ForceMonitor
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index 4f4f90e9..1c6664c9 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, N0_gait, q_init, multiprocessing=False):
+ def __init__(self, mpc_type, dt, n_steps, k_mpc, T_gait, N_gait, q_init, multiprocessing=False):
self.f_applied = np.zeros((12,))
self.not_first_iter = False
@@ -43,7 +43,7 @@ class MPC_Wrapper:
self.dt = dt
self.n_steps = n_steps
self.T_gait = T_gait
- self.N0_gait = N0_gait
+ self.N_gait = N_gait
self.gait_memory = np.zeros(4)
self.mpc_type = mpc_type
@@ -51,14 +51,14 @@ 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*self.N0_gait))
+ self.dataIn = Array('d', [0.0] * (1 + (np.int(self.n_steps)+1) * 12 + 12*self.N_gait))
self.dataOut = Array('d', [0] * 24 * (np.int(self.n_steps)))
- self.fsteps_future = np.zeros((self.N0_gait, 12))
+ self.fsteps_future = np.zeros((self.N_gait, 12))
self.running = Value('b', True)
else:
# Create the new version of the MPC solver object
if mpc_type:
- self.mpc = MPC.MPC(dt, n_steps, T_gait)
+ self.mpc = MPC.MPC(dt, n_steps, T_gait, self.N_gait)
else:
self.mpc = MPC_crocoddyl.MPC_crocoddyl(
dt=dt, T_mpc=T_gait, mu=0.9, inner=False, linearModel=True, n_period=int((dt * n_steps)/T_gait))
@@ -193,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, (self.N0_gait, 12))
+ fsteps = np.reshape(fsteps_1dim, (self.N_gait, 12))
# Create the MPC object of the parallel process during the first iteration
if k == 0:
@@ -251,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*self.N0_gait]
+ sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 12*self.N_gait]
csizes = np.cumsum(sizes)
# Return decompressed variables in a list
@@ -276,7 +276,7 @@ class MPC_Wrapper:
footstep when a new phase is created.
Args:
- fsteps (13xN0_gait array): the remaining number of steps of each phase of the gait (first column)
+ fsteps (13xN_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/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp
index bcbffb4d..31a4c6de 100644
--- a/src/FootstepPlanner.cpp
+++ b/src/FootstepPlanner.cpp
@@ -8,10 +8,10 @@ FootstepPlanner::FootstepPlanner()
, nextFootstep_(Matrix34::Zero())
, footsteps_()
, Rz(Matrix3::Zero())
- , dt_cum(VectorN::Zero(N0_gait))
- , yaws(VectorN::Zero(N0_gait))
- , dx(VectorN::Zero(N0_gait))
- , dy(VectorN::Zero(N0_gait))
+ , dt_cum()
+ , yaws()
+ , dx()
+ , dy()
, q_tmp(Vector3::Zero())
, q_dxdy(Vector3::Zero())
, RPY_(Vector3::Zero())
@@ -25,7 +25,8 @@ void FootstepPlanner::initialize(double dt_in,
double T_mpc_in,
double h_ref_in,
MatrixN const& shouldersIn,
- Gait& gaitIn)
+ Gait& gaitIn,
+ int N_gait)
{
dt = dt_in;
T_mpc = T_mpc_in;
@@ -35,13 +36,23 @@ void FootstepPlanner::initialize(double dt_in,
currentFootstep_ = shouldersIn.block(0, 0, 3, 4);
gait_ = &gaitIn;
targetFootstep_ = shouldersIn;
- footsteps_.fill(Matrix34::Zero());
+ dt_cum = VectorN::Zero(N_gait);
+ yaws = VectorN::Zero(N_gait);
+ dx = VectorN::Zero(N_gait);
+ dy = VectorN::Zero(N_gait);
+ for (int i = 0; i < N_gait; i++)
+ {
+ footsteps_.push_back(Matrix34::Zero());
+ }
Rz(2, 2) = 1.0;
}
void FootstepPlanner::computeFootsteps(VectorN const& q, Vector6 const& v, Vector6 const& vref)
{
- footsteps_.fill(Matrix34::Zero());
+ for (uint i = 0; i < footsteps_.size(); i++)
+ {
+ footsteps_[i] = Matrix34::Zero();
+ }
MatrixN gait = gait_->getCurrentGait();
// Set current position of feet for feet in stance phase
@@ -57,7 +68,7 @@ void FootstepPlanner::computeFootsteps(VectorN const& q, Vector6 const& v, Vecto
// Get future yaw yaws compared to current position
dt_cum(0) = dt;
yaws(0) = vref(5) * dt_cum(0) + RPY_(2);
- for (int j = 1; j < N0_gait; j++)
+ for (uint j = 1; j < footsteps_.size(); j++)
{
dt_cum(j) = gait.row(j).isZero() ? dt_cum(j - 1) : dt_cum(j - 1) + dt;
yaws(j) = vref(5) * dt_cum(j) + RPY_(2);
@@ -66,7 +77,7 @@ void FootstepPlanner::computeFootsteps(VectorN const& q, Vector6 const& v, Vecto
// Displacement following the reference velocity compared to current position
if (vref(5, 0) != 0)
{
- for (int j = 0; j < N0_gait; j++)
+ for (uint j = 0; j < footsteps_.size(); j++)
{
dx(j) = (v(0) * std::sin(vref(5) * dt_cum(j)) + v(1) * (std::cos(vref(5) * dt_cum(j)) - 1.0)) / vref(5);
dy(j) = (v(1) * std::sin(vref(5) * dt_cum(j)) - v(0) * (std::cos(vref(5) * dt_cum(j)) - 1.0)) / vref(5);
@@ -74,7 +85,7 @@ void FootstepPlanner::computeFootsteps(VectorN const& q, Vector6 const& v, Vecto
}
else
{
- for (int j = 0; j < N0_gait; j++)
+ for (uint j = 0; j < footsteps_.size(); j++)
{
dx(j) = v(0) * dt_cum(j);
dy(j) = v(1) * dt_cum(j);
@@ -206,10 +217,10 @@ void FootstepPlanner::updateNewContact()
MatrixN FootstepPlanner::getFootsteps() { return vectorToMatrix(footsteps_); }
MatrixN FootstepPlanner::getTargetFootsteps() { return targetFootstep_; }
-MatrixN FootstepPlanner::vectorToMatrix(std::array<Matrix34, N0_gait> const& array)
+MatrixN FootstepPlanner::vectorToMatrix(std::vector<Matrix34> const& array)
{
- MatrixN M = MatrixN::Zero(N0_gait, 12);
- for (int i = 0; i < N0_gait; i++)
+ MatrixN M = MatrixN::Zero(array.size(), 12);
+ for (uint i = 0; i < array.size(); i++)
{
for (int j = 0; j < 4; j++)
{
diff --git a/src/Gait.cpp b/src/Gait.cpp
index df9cc3ff..9d8c8542 100644
--- a/src/Gait.cpp
+++ b/src/Gait.cpp
@@ -1,9 +1,9 @@
#include "qrw/Gait.hpp"
Gait::Gait()
- : pastGait_(MatrixN::Zero(N0_gait, 4))
- , currentGait_(MatrixN::Zero(N0_gait, 4))
- , desiredGait_(MatrixN::Zero(N0_gait, 4))
+ : pastGait_()
+ , currentGait_()
+ , desiredGait_()
, dt_(0.0)
, T_gait_(0.0)
, T_mpc_(0.0)
@@ -16,15 +16,19 @@ Gait::Gait()
}
-void Gait::initialize(double dt_in, double T_gait_in, double T_mpc_in)
+void Gait::initialize(double dt_in, double T_gait_in, double T_mpc_in, int N_gait)
{
dt_ = dt_in;
T_gait_ = T_gait_in;
T_mpc_ = T_mpc_in;
n_steps_ = (int)std::lround(T_mpc_in / dt_in);
- if((n_steps_ > N0_gait) || ((int)std::lround(T_gait_in / dt_in) > N0_gait))
- throw std::invalid_argument("Sizes of matrices are too small for considered durations. Increase N0_gait in config file.");
+ pastGait_ = MatrixN::Zero(N_gait, 4);
+ currentGait_ = MatrixN::Zero(N_gait, 4);
+ desiredGait_ = MatrixN::Zero(N_gait, 4);
+
+ if((n_steps_ > N_gait) || ((int)std::lround(T_gait_in / dt_in) > N_gait))
+ throw std::invalid_argument("Sizes of matrices are too small for considered durations. Increase N_gait in config file.");
create_trot();
create_gait_f();
@@ -36,7 +40,7 @@ void Gait::create_walk()
// Number of timesteps in 1/4th period of gait
int N = (int)std::lround(0.25 * T_gait_ / dt_);
- desiredGait_ = MatrixN::Zero(N0_gait, 4);
+ desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
Eigen::Matrix<double, 1, 4> sequence;
sequence << 0.0, 1.0, 1.0, 1.0;
@@ -54,7 +58,7 @@ void Gait::create_trot()
// Number of timesteps in a half period of gait
int N = (int)std::lround(0.5 * T_gait_ / dt_);
- desiredGait_ = MatrixN::Zero(N0_gait, 4);
+ desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
Eigen::Matrix<double, 1, 4> sequence;
sequence << 1.0, 0.0, 0.0, 1.0;
@@ -68,7 +72,7 @@ void Gait::create_pacing()
// Number of timesteps in a half period of gait
int N = (int)std::lround(0.5 * T_gait_ / dt_);
- desiredGait_ = MatrixN::Zero(N0_gait, 4);
+ desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
Eigen::Matrix<double, 1, 4> sequence;
sequence << 1.0, 0.0, 1.0, 0.0;
@@ -82,7 +86,7 @@ void Gait::create_bounding()
// Number of timesteps in a half period of gait
int N = (int)std::lround(0.5 * T_gait_ / dt_);
- desiredGait_ = MatrixN::Zero(N0_gait, 4);
+ desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
Eigen::Matrix<double, 1, 4> sequence;
sequence << 1.0, 1.0, 0.0, 0.0;
@@ -96,7 +100,7 @@ void Gait::create_static()
// Number of timesteps in a period of gait
int N = (int)std::lround(T_gait_ / dt_);
- desiredGait_ = MatrixN::Zero(N0_gait, 4);
+ desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
Eigen::Matrix<double, 1, 4> sequence;
sequence << 1.0, 1.0, 1.0, 1.0;
diff --git a/src/MPC.cpp b/src/MPC.cpp
index 7de312c6..e49c3934 100644
--- a/src/MPC.cpp
+++ b/src/MPC.cpp
@@ -1,6 +1,6 @@
#include "qrw/MPC.hpp"
-MPC::MPC(double dt_in, int n_steps_in, double T_gait_in) {
+MPC::MPC(double dt_in, int n_steps_in, double T_gait_in, int N_gait) {
dt = dt_in;
n_steps = n_steps_in;
T_gait = T_gait_in;
@@ -11,6 +11,8 @@ MPC::MPC(double dt_in, int n_steps_in, double T_gait_in) {
warmxf = Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(12 * n_steps * 2, 1);
x_f_applied = Eigen::MatrixXd::Zero(24, n_steps);
+ gait = Eigen::Matrix<int, Eigen::Dynamic, 4>::Zero(N_gait, 4);
+
// Predefined variables
mass = 2.50000279f;
mu = 0.9f;
@@ -28,7 +30,7 @@ MPC::MPC(double dt_in, int n_steps_in, double T_gait_in) {
osqp_set_default_settings(settings);
}
-MPC::MPC() { MPC(0.02, 32, 0.64); }
+MPC::MPC() { }
/*
Create the constraint matrices of the MPC (M.X = N and L.X <= K)
@@ -657,7 +659,7 @@ N is the number of time step in the prediction horizon.
int MPC::construct_S() {
int i = 0;
- Eigen::Matrix<int, N0_gait, 4> inv_gait = Eigen::Matrix<int, N0_gait, 4>::Ones() - gait;
+ inv_gait = Eigen::Matrix<int, Eigen::Dynamic, 4>::Ones(gait.rows(), 4) - 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;
--
GitLab