From e2b0ba3e26fdc65df669c9fb63fbdafcedcfdca8 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Thu, 25 Aug 2022 16:58:03 +0200
Subject: [PATCH] Bindings for C++ MpcWrapper + Use this wrapper when using
OSQP MPC
---
include/qrw/MpcWrapper.hpp | 18 ++++++++++---
python/CMakeLists.txt | 1 +
python/MpcWrapper.cpp | 13 +++++----
.../quadruped_reactive_walking/Controller.py | 13 +++------
src/MpcWrapper.cpp | 27 +++++++++++++------
5 files changed, 46 insertions(+), 26 deletions(-)
diff --git a/include/qrw/MpcWrapper.hpp b/include/qrw/MpcWrapper.hpp
index bdf630e2..aa97e770 100644
--- a/include/qrw/MpcWrapper.hpp
+++ b/include/qrw/MpcWrapper.hpp
@@ -131,15 +131,25 @@ class MpcWrapper {
/// \brief Return the latest available result of the MPC
///
////////////////////////////////////////////////////////////////////////////////////////////////
- Eigen::Matrix<double, 24, 2> get_latest_result();
+ MatrixN get_latest_result();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Return the solving duration of the latest MPC iteration
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ double get_t_mpc_solving_duration() { return t_mpc_solving_duration_; }
private:
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
- 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
+
+ std::chrono::time_point<std::chrono::system_clock> t_mpc_solving_start_;
+ double t_mpc_solving_duration_;
};
#endif // MPCWRAPPER_H_INCLUDED
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 35bb50e9..ee9fb478 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -3,6 +3,7 @@ set(${PY_NAME}_SOURCES
Estimator.cpp
Filter.cpp
MPC.cpp
+ MpcWrapper.cpp
FootstepPlanner.cpp
FootTrajectoryGenerator.cpp
Gait.cpp
diff --git a/python/MpcWrapper.cpp b/python/MpcWrapper.cpp
index 0a567ae9..8bb6a45f 100644
--- a/python/MpcWrapper.cpp
+++ b/python/MpcWrapper.cpp
@@ -5,12 +5,15 @@
template <typename MpcWrapper>
struct MpcWrapperVisitor : public bp::def_visitor<MpcWrapperVisitor<MpcWrapper>> {
template <class PyClassMpcWrapper>
- void visit(PyClassMpcWrapper& mpc) const {
- mpc.def(bp::init<>(bp::arg(""), "Default constructor."))
- .def(bp::init<Params&>(bp::args("params"), "Constructor with parameters."))
- .def("solve", &MpcWrapper::solve, bp::args("k", "xref_in", "fsteps_in", "gait_in"), "Run MpcWrapper from Python.\n")
+ void visit(PyClassMpcWrapper& cl) const {
+ cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+ .def("initialize", &MpcWrapper::initialize, bp::args("params"), "Initialize Gait from Python.\n")
+ .def("solve", &MpcWrapper::solve, bp::args("k", "xref_in", "fsteps_in", "gait_in"),
+ "Run MpcWrapper from Python.\n")
.def("get_latest_result", &MpcWrapper::get_latest_result,
- "Get latest result (predicted trajectory forces to apply).\n");
+ "Get latest result (predicted trajectory forces to apply).\n")
+ .def_readonly("t_mpc_solving_duration", &MpcWrapper::get_t_mpc_solving_duration,
+ "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/Controller.py b/python/quadruped_reactive_walking/Controller.py
index bdcbfcdd..6bcb8044 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -86,8 +86,7 @@ class Controller:
self.q_display = np.zeros(19)
if self.type_MPC == 0:
- print("Create wrapper")
- self.mpc_wrapper = qrw.mpcWrapper()
+ self.mpc_wrapper = qrw.MpcWrapper()
self.mpc_wrapper.initialize(params)
else:
self.mpc_wrapper = MPC_Wrapper.MPC_Wrapper(params, self.q_init)
@@ -560,7 +559,6 @@ class Controller:
- self.footTrajectoryGenerator.get_elapsed_durations(),
)
elif self.type_MPC == 0:
- print("Call Wrapper")
self.mpc_wrapper.solve(
self.k,
reference_state,
@@ -577,15 +575,12 @@ class Controller:
except ValueError:
print("MPC Problem")
- # Use a temporary result if contact status changes between two calls of the MPC
- self.mpc_wrapper.get_temporary_result(self.gait.matrix[0, :])
-
if self.type_MPC == 0:
- print("get latest result")
self.mpc_result = self.mpc_wrapper.get_latest_result()
- from IPython import embed
- embed()
+ self.mpc_cost = 0.0
else:
+ # Use a temporary result if contact status changes between two calls of the MPC
+ self.mpc_wrapper.get_temporary_result(self.gait.matrix[0, :])
self.mpc_result, self.mpc_cost = self.mpc_wrapper.get_latest_result()
if self.k > 100 and self.type_MPC == 3:
diff --git a/src/MpcWrapper.cpp b/src/MpcWrapper.cpp
index 1aa24e55..26d5256d 100644
--- a/src/MpcWrapper.cpp
+++ b/src/MpcWrapper.cpp
@@ -18,6 +18,8 @@ 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
+
void stop_thread() {
const std::lock_guard<std::mutex> lockStop(mutexStop);
shared_running = false;
@@ -69,6 +71,9 @@ MatrixN read_out() {
}
void parallel_loop() {
+ while (shared_params == nullptr) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1)); // Wait a bit
+ }
int k;
MatrixN xref;
MatrixN fsteps;
@@ -87,8 +92,7 @@ void parallel_loop() {
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)
+ // MPC::get_latest_result() returns a matrix of size 24 x N
// std::cout << "NEW RESULT AVAILABLE, WRITING OUT" << std::endl;
result = loop_mpc.get_latest_result();
write_out(result);
@@ -99,26 +103,32 @@ void parallel_loop() {
}
MpcWrapper::MpcWrapper()
- : last_available_result(Eigen::Matrix<double, 24, 2>::Zero()),
- gait_past(RowVector4::Zero()),
- gait_next(RowVector4::Zero()) {}
+ : gait_past(RowVector4::Zero()),
+ gait_next(RowVector4::Zero()),
+ t_mpc_solving_duration_(0.0) {}
void MpcWrapper::initialize(Params& params) {
params_ = ¶ms;
mpc_ = MPC(params);
+ t_mpc_solving_start_ = std::chrono::system_clock::now();
+
// Default result for first step
+ last_available_result = MatrixN::Zero(24, params.gait.rows());
last_available_result(2, 0) = params.h_ref;
last_available_result.col(0).tail(12) = (Vector3(0.0, 0.0, 8.0)).replicate<4, 1>();
// Initialize the shared memory
- shared_params = ¶ms;
shared_k = 42;
shared_xref = MatrixN::Zero(12, params.gait.rows() + 1);
shared_fsteps = MatrixN::Zero(params.gait.rows(), 12);
+ shared_params = ¶ms;
}
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;
write_in(k, xref, fsteps);
@@ -142,10 +152,11 @@ 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() {
+MatrixN MpcWrapper::get_latest_result() {
// Retrieve data from parallel process if a new result is available
if (check_new_result()) {
- last_available_result = read_out().block(0, 0, 24, 2);
+ t_mpc_solving_duration_ = ((std::chrono::duration<double>)(std::chrono::system_clock::now() - t_mpc_solving_start_)).count();
+ last_available_result = read_out();
}
// std::cout << "get_latest_result: " << std::endl << last_available_result.transpose() << std::endl;
return last_available_result;
--
GitLab