Proper handling of parallel loop + Implement get_temporary_result

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_;

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>()); }

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()
 

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 = &params;
+
+  // 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);
+}