From d86b22047cef6620936aacb6fa0a7f199c46b9c4 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Wed, 20 Oct 2021 11:45:26 +0200
Subject: [PATCH] Formatting MPC Wrapper
---
include/qrw/FootTrajectoryGenerator.hpp | 2 +-
include/qrw/MpcWrapper.hpp | 97 +++++++++++++---
src/MpcWrapper.cpp | 146 ++++++++----------------
3 files changed, 129 insertions(+), 116 deletions(-)
diff --git a/include/qrw/FootTrajectoryGenerator.hpp b/include/qrw/FootTrajectoryGenerator.hpp
index 4121085b..543731ad 100644
--- a/include/qrw/FootTrajectoryGenerator.hpp
+++ b/include/qrw/FootTrajectoryGenerator.hpp
@@ -55,7 +55,7 @@ class FootTrajectoryGenerator {
/// \brief Update the 3D desired position for feet in swing phase by using a 5-th order polynomial that lead them
/// to the desired position on the ground (computed by the footstep planner)
///
- /// \param[in] k Number of time steps since the start of the simulation
+ /// \param[in] k Numero of the current loop
/// \param[in] targetFootstep Desired target locations at the end of the swing phases
///
////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/include/qrw/MpcWrapper.hpp b/include/qrw/MpcWrapper.hpp
index eec79bc8..debe8ba4 100644
--- a/include/qrw/MpcWrapper.hpp
+++ b/include/qrw/MpcWrapper.hpp
@@ -9,10 +9,6 @@
#ifndef MPCWRAPPER_H_INCLUDED
#define MPCWRAPPER_H_INCLUDED
-// #include <string>
-// #include <boost/interprocess/managed_shared_memory.hpp>
-// #include <boost/interprocess/shared_memory_object.hpp>
-// #include <boost/interprocess/mapped_region.hpp>
#include "pinocchio/math/rpy.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
@@ -22,16 +18,81 @@
#include "qrw/Types.h"
#include "qrw/MPC.hpp"
-// Functions acting on shared memory
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// Functions acting on shared memory
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief Stop the thread running the MPC in parallel
+///
+////////////////////////////////////////////////////////////////////////////////////////////////
void stop_thread();
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief Check if the thread running the MPC in parallel should stop
+///
+////////////////////////////////////////////////////////////////////////////////////////////////
bool check_stop_thread();
-void write_in(int & k, MatrixN & xref, MatrixN & fsteps);
-bool read_in(int & k, MatrixN & xref, MatrixN & fsteps);
-void write_out(MatrixN & result);
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief Write data to shared memory (from main loop to MPC)
+///
+/// \param[in] k Numero of the current loop
+/// \param[in] xref Desired state vector for the whole prediction horizon
+/// \param[in] fsteps The [x, y, z]^T desired position of each foot for each time step of the horizon
+///
+////////////////////////////////////////////////////////////////////////////////////////////////
+void write_in(int& k, MatrixN& xref, MatrixN& fsteps);
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief Read data in shared memory (from main loop to MPC)
+///
+/// \param[in] k Numero of the current loop
+/// \param[in] xref Desired state vector for the whole prediction horizon
+/// \param[in] fsteps The [x, y, z]^T desired position of each foot for each time step of the horizon
+///
+////////////////////////////////////////////////////////////////////////////////////////////////
+bool read_in(int& k, MatrixN& xref, MatrixN& fsteps);
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief Write data to shared memory (from MPC to main loop)
+///
+/// \param[in] result Predicted state and desired contact forces resulting of the MPC. The first 12 rows contains the
+/// predicted position/orientation/velocity and the last 12 rows contains the 3D contact forces for each foot. The
+/// first column corresponds to the predicted state in dt_mpc seconds if the contact forces are applied. The second
+/// column is the predicted state in 2*dt_mpc seconds if the contact forces of the first column are applied during
+/// dt_mpc then those of the second column during dt_mpc. And so on...
+///
+////////////////////////////////////////////////////////////////////////////////////////////////
+void write_out(MatrixN& result);
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief Check if a new MPC result is available
+///
+////////////////////////////////////////////////////////////////////////////////////////////////
bool check_new_result();
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief Read data in shared memory (from MPC to main loop)
+///
+////////////////////////////////////////////////////////////////////////////////////////////////
MatrixN read_out();
-void parallel_loop();
+////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief Run the MPC in parallel (launched by a thread)
+///
+////////////////////////////////////////////////////////////////////////////////////////////////
+void parallel_loop();
class MpcWrapper {
public:
@@ -62,7 +123,10 @@ class MpcWrapper {
///
/// \brief Send data to the MPC to solve one iteration of MPC
///
- /// \param[in] robot Pointer to the robot interface
+ /// \param[in] k Numero of the current loop
+ /// \param[in] xref Desired state vector for the whole prediction horizon
+ /// \param[in] fsteps The [x, y, z]^T desired position of each foot for each time step of the horizon
+ /// \param[in] gait Current gait matrix
///
////////////////////////////////////////////////////////////////////////////////////////////////
void solve(int k, MatrixN xref, MatrixN fsteps, MatrixN gait);
@@ -75,15 +139,12 @@ class MpcWrapper {
Eigen::Matrix<double, 24, 2> get_latest_result();
private:
-
- Params* params_;
- MPC mpc_;
-
- int test = 0;
- Eigen::Matrix<double, 24, 2> last_available_result;
- Matrix14 gait_past;
- Matrix14 gait_next;
+ Params* params_; // Object that stores parameters
+ MPC mpc_; // MPC object used for synchronous solving (not in parallel loop)
+ Eigen::Matrix<double, 24, 2> last_available_result; // Latest available result of the MPC
+ Matrix14 gait_past; // Gait status of the previous MPC time step
+ Matrix14 gait_next; // Gait status of the next MPC time step
};
#endif // MPCWRAPPER_H_INCLUDED
diff --git a/src/MpcWrapper.cpp b/src/MpcWrapper.cpp
index 63ccb62d..090a3800 100644
--- a/src/MpcWrapper.cpp
+++ b/src/MpcWrapper.cpp
@@ -1,54 +1,41 @@
#include "qrw/MpcWrapper.hpp"
// Shared global variables
-Params* shared_params = nullptr;
-bool shared_running = true;
-bool shared_newIn = false;
-bool shared_newOut = false;
-int shared_k;
-MatrixN shared_xref;
-MatrixN shared_fsteps;
-MatrixN shared_result;
+Params* shared_params = nullptr; // Shared pointer to object that stores parameters
+bool shared_running = true; // Flag to stop the parallel thread
+bool shared_newIn = false; // Flag to indicate new data has been written by main loop for MPC
+bool shared_newOut = false; // Flag to indicate new data has been written by MPC for main loop
+int shared_k; // Numero of the current loop
+MatrixN shared_xref; // Desired state vector for the whole prediction horizon
+MatrixN shared_fsteps; // The [x, y, z]^T desired position of each foot for each time step of the horizon
+MatrixN shared_result; // Predicted state and desired contact forces resulting of the MPC
// Mutexes to protect the global variables
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::mutex mutexIn; // From main loop to MPC
+std::mutex mutexOut; // From MPC to main loop
-void stop_thread()
-{
+void stop_thread() {
const std::lock_guard<std::mutex> lockStop(mutexStop);
shared_running = false;
}
-bool check_stop_thread()
-{
+bool check_stop_thread() {
const std::lock_guard<std::mutex> lockStop(mutexStop);
return shared_running;
}
-void write_in(int & k, MatrixN & xref, MatrixN & fsteps)
-{
+void write_in(int& k, MatrixN& xref, MatrixN& fsteps) {
const std::lock_guard<std::mutex> lockIn(mutexIn);
shared_k = k;
shared_xref = xref;
shared_fsteps = fsteps;
- // std::cout << "Writing memory" << std::endl;
- /*std::cout << "Shared k" << std::endl << shared_k << std::endl;
- std::cout << "Shared xref" << std::endl << shared_xref << std::endl;
- std::cout << "Shared fsteps" << std::endl << shared_fsteps << std::endl;*/
- shared_newIn = true; // New data is available
+ shared_newIn = true; // New data is available
}
-bool read_in(int & k, MatrixN & xref, MatrixN & fsteps)
-{
+bool read_in(int& k, MatrixN& xref, MatrixN& fsteps) {
const std::lock_guard<std::mutex> lockIn(mutexIn);
- if (shared_newIn)
- {
- // std::cout << "Reading memory" << std::endl;
- /*std::cout << "Shared k" << std::endl << shared_k << std::endl;
- std::cout << "Shared xref" << std::endl << shared_xref << std::endl;
- std::cout << "Shared fsteps" << std::endl << shared_fsteps << std::endl;*/
+ if (shared_newIn) {
k = shared_k;
xref = shared_xref;
fsteps = shared_fsteps;
@@ -58,77 +45,63 @@ bool read_in(int & k, MatrixN & xref, MatrixN & fsteps)
return false;
}
-void write_out(MatrixN & result)
-{
+void write_out(MatrixN& result) {
const std::lock_guard<std::mutex> lockOut(mutexOut);
- // std::cout << "Writing result" << std::endl;
shared_result = result;
- // std::cout << "Shared result" << std::endl << shared_result << std::endl;
- shared_newOut = true; // New data is available
+ shared_newOut = true; // New data is available
}
-bool check_new_result()
-{
+bool check_new_result() {
const std::lock_guard<std::mutex> lockOut(mutexOut);
- if (shared_newOut)
- {
- // std::cout << "NEW RESULT DETECTED" << std::endl;
+ if (shared_newOut) {
shared_newOut = false;
return true;
}
return false;
}
-MatrixN read_out()
-{
+MatrixN read_out() {
const std::lock_guard<std::mutex> lockOut(mutexOut);
- // std::cout << "NEW RESULT OUTPUTTING" << std::endl;
return shared_result;
}
-void parallel_loop()
-{
+void parallel_loop() {
int k;
MatrixN xref;
MatrixN fsteps;
MatrixN result;
MPC loop_mpc = MPC(*shared_params); // Create the MPC object running in parallel
- while (check_stop_thread())
- {
+ while (check_stop_thread()) {
// Checking if new data is available to trigger the asynchronous MPC
- if (read_in(k, xref, fsteps))
- {
- // std::cout << "NEW DATA AVAILABLE, LAUNCHING MPC" << std::endl;
-
- /*std::cout << "Parallel k" << std::endl << k << std::endl;
- std::cout << "Parallel xref" << std::endl << xref << std::endl;
- std::cout << "Parallel fsteps" << std::endl << fsteps << std::endl;*/
-
- // Run the asynchronous MPC with the data that as been retrieved
- loop_mpc.run(k, xref, fsteps);
-
- // Store the result (predicted state + desired forces) in the shared memory
- // MPC::get_latest_result() returns a matrix of size 24 x N and we want to
- // retrieve only the 2 first columns i.e. dataOut.block(0, 0, 24, 2)
- // std::cout << "NEW RESULT AVAILABLE, WRITING OUT" << std::endl;
- result = loop_mpc.get_latest_result();
- write_out(result);
- }
- else
- {
+ if (read_in(k, xref, fsteps)) {
+ // std::cout << "NEW DATA AVAILABLE, LAUNCHING MPC" << std::endl;
+
+ /*std::cout << "Parallel k" << std::endl << k << std::endl;
+ std::cout << "Parallel xref" << std::endl << xref << std::endl;
+ std::cout << "Parallel fsteps" << std::endl << fsteps << std::endl;*/
+
+ // Run the asynchronous MPC with the data that as been retrieved
+ loop_mpc.run(k, xref, fsteps);
+
+ // Store the result (predicted state + desired forces) in the shared memory
+ // MPC::get_latest_result() returns a matrix of size 24 x N and we want to
+ // retrieve only the 2 first columns i.e. dataOut.block(0, 0, 24, 2)
+ // std::cout << "NEW RESULT AVAILABLE, WRITING OUT" << std::endl;
+ result = loop_mpc.get_latest_result();
+ write_out(result);
+ } else {
std::this_thread::sleep_for(std::chrono::milliseconds(1)); // Wait a bit
}
}
}
MpcWrapper::MpcWrapper()
- : test(0),
- last_available_result(Eigen::Matrix<double, 24, 2>::Zero()),
- gait_past(Matrix14::Zero()),
- gait_next(Matrix14::Zero()) {}
+ : test(0),
+ last_available_result(Eigen::Matrix<double, 24, 2>::Zero()),
+ gait_past(Matrix14::Zero()),
+ gait_next(Matrix14::Zero()) {}
void MpcWrapper::initialize(Params& params) {
-
params_ = ¶ms;
mpc_ = MPC(params);
@@ -141,27 +114,9 @@ void MpcWrapper::initialize(Params& params) {
shared_k = 42;
shared_xref = MatrixN::Zero(12, params.gait.rows() + 1);
shared_fsteps = MatrixN::Zero(params.gait.rows(), 12);
-
}
void MpcWrapper::solve(int k, MatrixN xref, MatrixN fsteps, MatrixN gait) {
-
- /*std::cout << "SIZES" << std::endl;
- std::cout << sizeof((int)5) << std::endl;
- std::cout << sizeof(MatrixN::Zero(12, params_->gait.rows() + 1)) << std::endl;
- std::cout << sizeof(MatrixN::Zero(params_->gait.rows(), 12)) << std::endl;
- std::cout << params_->gait << std::endl;*/
-
- /*double t = 0;
- while (t < 15.0)
- {
- std::cout << "Waiting" << std::endl;
- write_in(k, xref, fsteps);
- k++;
- t += 0.5;
- std::this_thread::sleep_for(std::chrono::milliseconds(500));
- }*/
-
// std::cout << "NEW DATA AVAILABLE, WRITING IN" << std::endl;
write_in(k, xref, fsteps);
@@ -171,12 +126,11 @@ void MpcWrapper::solve(int k, MatrixN xref, MatrixN fsteps, MatrixN gait) {
if (gait_next.isApprox(gait.row(0))) // If we're still doing what was planned the last time MPC was solved
{
last_available_result.col(0).tail(12) = last_available_result.col(1).tail(12);
- }
- else // Otherwise use a default contact force command till we get the actual result of the MPC for this new sequence
+ } else // Otherwise use a default contact force command till we get the actual result of the MPC for this new
+ // sequence
{
double F = 9.81 * params_->mass / gait.row(0).sum();
- for (int i = 0; i < 4; i++)
- {
+ for (int i = 0; i < 4; i++) {
last_available_result.block(12 + 3 * i, 0, 3, 1) << 0.0, 0.0, F;
}
}
@@ -186,11 +140,9 @@ void MpcWrapper::solve(int k, MatrixN xref, MatrixN fsteps, MatrixN gait) {
gait_next = gait.row(1);
}
-Eigen::Matrix<double, 24, 2> MpcWrapper::get_latest_result()
-{
+Eigen::Matrix<double, 24, 2> MpcWrapper::get_latest_result() {
// Retrieve data from parallel process if a new result is available
- if (check_new_result())
- {
+ if (check_new_result()) {
last_available_result = read_out().block(0, 0, 24, 2);
}
// std::cout << "get_latest_result: " << std::endl << last_available_result.transpose() << std::endl;
--
GitLab