diff --git a/include/qrw/MpcWrapper.hpp b/include/qrw/MpcWrapper.hpp
index aa97e770ac7507c319e2575d7189ec6032150f99..558a1c72ad58b49a09afd5e72dbfbbc6c846cd72 100644
--- a/include/qrw/MpcWrapper.hpp
+++ b/include/qrw/MpcWrapper.hpp
@@ -112,7 +112,7 @@ class MpcWrapper {
/// \brief Destructor
///
////////////////////////////////////////////////////////////////////////////////////////////////
- ~MpcWrapper() {} // Empty destructor
+ ~MpcWrapper();
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -133,6 +133,15 @@ class MpcWrapper {
////////////////////////////////////////////////////////////////////////////////////////////////
MatrixN get_latest_result();
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Use a temporary result if contact status changes between two calls of the MPC
+ ///
+ /// \param[in] gait Current contact state of the four feet
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void get_temporary_result(RowVector4 gait);
+
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Return the solving duration of the latest MPC iteration
@@ -140,13 +149,22 @@ class MpcWrapper {
////////////////////////////////////////////////////////////////////////////////////////////////
double get_t_mpc_solving_duration() { return t_mpc_solving_duration_; }
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Stop the parallel loop
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void stop_parallel_loop();
+
private:
Params* params_; // Object that stores parameters
MPC mpc_; // MPC object used for synchronous solving (not in parallel loop)
- MatrixN last_available_result; // Latest available result of the MPC
- RowVector4 gait_past; // Gait status of the previous MPC time step
- RowVector4 gait_next; // Gait status of the next MPC time step
+ MatrixN last_available_result; // Latest available result of the MPC
+ RowVector4 gait_past; // Gait status of the previous MPC time step
+ RowVector4 gait_next; // Gait status of the next MPC time step
+ RowVector4 gait_mem_; // Gait status memory for get_temporary_result
+ bool flag_change_[4] = {false, false, false, false}; // Flags to indicate a gait change
std::chrono::time_point<std::chrono::system_clock> t_mpc_solving_start_;
double t_mpc_solving_duration_;
diff --git a/python/MpcWrapper.cpp b/python/MpcWrapper.cpp
index 8bb6a45f4bf412f26d9311a003f796b8d8e0667d..923157920093c299b6c2888bea2b2d6dcbffa592 100644
--- a/python/MpcWrapper.cpp
+++ b/python/MpcWrapper.cpp
@@ -12,8 +12,11 @@ struct MpcWrapperVisitor : public bp::def_visitor<MpcWrapperVisitor<MpcWrapper>>
"Run MpcWrapper from Python.\n")
.def("get_latest_result", &MpcWrapper::get_latest_result,
"Get latest result (predicted trajectory forces to apply).\n")
+ .def("get_temporary_result", &MpcWrapper::get_temporary_result,
+ bp::args("gait"), "Use a temporary result if contact status changes between two calls of the MPC.\n")
+ .def("stop_parallel_loop", &MpcWrapper::stop_parallel_loop, "Stop the parallel loop.\n")
.def_readonly("t_mpc_solving_duration", &MpcWrapper::get_t_mpc_solving_duration,
- "Solving time of latest MPC iteration.\n");
+ "Solving time of latest MPC iteration.\n");
}
static void expose() { bp::class_<MpcWrapper>("MpcWrapper", bp::no_init).def(MpcWrapperVisitor<MpcWrapper>()); }
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index fa6b02439b6a9f08cffb4e8f6633ec06885190e5..15b4ad0918527f0d10854452854bf405d54df12a 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -249,7 +249,7 @@ def control_loop(des_vel_analysis=None):
finished = t >= t_max
damp_control(device, 12)
- if params.enable_multiprocessing:
+ if params.enable_multiprocessing or params.type_MPC == 0:
print("Stopping parallel process MPC")
controller.mpc_wrapper.stop_parallel_loop()
diff --git a/src/MpcWrapper.cpp b/src/MpcWrapper.cpp
index 26d5256d0f0ba7fd6f8a00357dd07dd3c34090cd..1b7be9799963f92fc13e8120601c41ee7d797358 100644
--- a/src/MpcWrapper.cpp
+++ b/src/MpcWrapper.cpp
@@ -18,7 +18,7 @@ std::mutex mutexStop; // To check if the thread should still run
std::mutex mutexIn; // From main loop to MPC
std::mutex mutexOut; // From MPC to main loop
-std::thread parallel_thread(parallel_loop); // spawn new thread that runs MPC in parallel
+std::thread parallel_thread; // spawn new thread that runs MPC in parallel
void stop_thread() {
const std::lock_guard<std::mutex> lockStop(mutexStop);
@@ -105,6 +105,7 @@ void parallel_loop() {
MpcWrapper::MpcWrapper()
: gait_past(RowVector4::Zero()),
gait_next(RowVector4::Zero()),
+ gait_mem_(RowVector4::Zero()),
t_mpc_solving_duration_(0.0) {}
void MpcWrapper::initialize(Params& params) {
@@ -123,10 +124,17 @@ void MpcWrapper::initialize(Params& params) {
shared_xref = MatrixN::Zero(12, params.gait.rows() + 1);
shared_fsteps = MatrixN::Zero(params.gait.rows(), 12);
shared_params = ¶ms;
+
+ // Start the parallel loop
+ parallel_thread = std::thread(parallel_loop);
}
-void MpcWrapper::solve(int k, MatrixN xref, MatrixN fsteps, MatrixN gait) {
+MpcWrapper::~MpcWrapper() {
+ stop_thread();
+ parallel_thread.join();
+}
+void MpcWrapper::solve(int k, MatrixN xref, MatrixN fsteps, MatrixN gait) {
t_mpc_solving_start_ = std::chrono::system_clock::now();
// std::cout << "NEW DATA AVAILABLE, WRITING IN" << std::endl;
@@ -154,10 +162,64 @@ void MpcWrapper::solve(int k, MatrixN xref, MatrixN fsteps, MatrixN gait) {
MatrixN MpcWrapper::get_latest_result() {
// Retrieve data from parallel process if a new result is available
+ bool refresh = false;
if (check_new_result()) {
- t_mpc_solving_duration_ = ((std::chrono::duration<double>)(std::chrono::system_clock::now() - t_mpc_solving_start_)).count();
+ t_mpc_solving_duration_ =
+ ((std::chrono::duration<double>)(std::chrono::system_clock::now() - t_mpc_solving_start_)).count();
last_available_result = read_out();
+ refresh = true;
}
+
+ if (refresh) {
+ // Handle changes of gait between moment when the MPC solving is launched and result is retrieved
+ for (int i = 0; i < 4; i++) {
+ if (flag_change_[i]) {
+ // Foot in contact but was not in contact when we launched MPC solving (new detection)
+ // Fetch first non zero force in the prediction horizon for this foot
+ int k = 0;
+ while (k < last_available_result.cols() && last_available_result(14 + 3 * i, k) == 0) {
+ k++;
+ }
+ last_available_result.block(12 + 3 * i, 0, 3, 1) = last_available_result.block(12 + 3 * i, k, 3, 1);
+ }
+ }
+ }
+
// std::cout << "get_latest_result: " << std::endl << last_available_result.transpose() << std::endl;
return last_available_result;
}
+
+void MpcWrapper::get_temporary_result(RowVector4 gait) {
+ // Check if gait status has changed
+ bool same = false;
+ for (int i = 0; i < 4 && !same; i++) {
+ same = (gait[i] != gait_mem_[i]);
+ }
+ // If gait status has changed
+ if (!same) {
+ for (int i = 0; i < 4; i++) {
+ if (gait[i] == 0) {
+ // Foot not in contact
+ last_available_result.block(12 + 3 * i, 0, 3, 1).setZero();
+ } else if (gait[i] == 1 && gait_mem_[i] == 1) {
+ // Foot in contact and was already in contact
+ continue;
+ } else if (gait[i] == 1 && gait_mem_[i] == 0) {
+ // Foot in contact but was not in contact (new detection)
+ flag_change_[i] = true;
+ // Fetch first non zero force in the prediction horizon for this foot
+ int k = 0;
+ while (k < last_available_result.cols() && last_available_result(14 + 3 * i, k) == 0) {
+ k++;
+ }
+ last_available_result.block(12 + 3 * i, 0, 3, 1) = last_available_result.block(12 + 3 * i, k, 3, 1);
+ }
+ }
+ gait_mem_ = gait;
+ }
+}
+
+void MpcWrapper::stop_parallel_loop() {
+ stop_thread();
+ // std::terminate(parallel_thread);
+}