From 21c67132fcc3d02785abcf6f586eb1ac4e6fe168 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Wed, 20 Oct 2021 14:49:18 +0200
Subject: [PATCH] Implementation of polynomial interpolation of a velocity
profile in C++ version of Joystick
---
include/qrw/Joystick.hpp | 25 ++++++++++++++++++-
python/gepadd.cpp | 2 +-
src/Joystick.cpp | 54 ++++++++++++++++++++++++++++++++--------
src/MpcWrapper.cpp | 3 +--
4 files changed, 69 insertions(+), 15 deletions(-)
diff --git a/include/qrw/Joystick.hpp b/include/qrw/Joystick.hpp
index df41b779..a7de046c 100644
--- a/include/qrw/Joystick.hpp
+++ b/include/qrw/Joystick.hpp
@@ -76,7 +76,17 @@ class Joystick {
/// \param[in] v_switch Information about the desired velocity for each key frame
///
////////////////////////////////////////////////////////////////////////////////////////////////
- VectorN handle_v_switch(double k, VectorN const& k_switch, MatrixN const& v_switch);
+ VectorN handle_v_switch_py(double k, VectorN const& k_switch_py, MatrixN const& v_switch_py);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Compute the remaining and total duration of a swing phase or a stance phase based
+ /// on the content of the gait matrix
+ ///
+ /// \param[in] k Numero of the current loop
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void handle_v_switch(int k);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -110,6 +120,16 @@ class Joystick {
////////////////////////////////////////////////////////////////////////////////////////////////
void update_v_ref_gamepad(int k, bool gait_is_static);
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update the status of the joystick using polynomial interpolation
+ ///
+ /// \param[in] k Numero of the current loop
+ /// \param[in] velID Identifier of the current velocity profile to be able to handle different scenarios
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void update_v_ref_predefined(int k, int velID);
+
Vector6 getPRef() { return p_ref_; }
Vector6 getVRef() { return v_ref_; }
int getJoystickCode() { return joystick_code_; }
@@ -142,6 +162,9 @@ class Joystick {
double dt_wbc = 0.0; // Time step of the WBC
int k_mpc = 0; // Number of WBC time step for one MPC time step
+ Eigen::Matrix<int, 1, Eigen::Dynamic> k_switch; // Key frames for the polynomial velocity interpolation
+ Eigen::Matrix<double, 6, Eigen::Dynamic> v_switch; // Target velocity for the key frames
+
// How much the gamepad velocity and position is filtered to avoid sharp changes
double gp_alpha_vel = 0.0; // Low pass filter coefficient for v_ref_ (if gamepad-controlled)
double gp_alpha_pos = 0.0; // Low pass filter coefficient for p_ref_
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 3629ca01..43319cbe 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -423,7 +423,7 @@ struct JoystickPythonVisitor : public bp::def_visitor<JoystickPythonVisitor<Joys
.def("getSquare", &Joystick::getSquare, "Get Joystick Square status")
.def("getL1", &Joystick::getL1, "Get Joystick L1 status")
.def("getR1", &Joystick::getR1, "Get Joystick R1 status")
- .def("handle_v_switch", &Joystick::handle_v_switch, bp::args("k", "k_switch", "v_switch"), "Run security check.\n");
+ .def("handle_v_switch", &Joystick::handle_v_switch_py, bp::args("k", "k_switch", "v_switch"), "Run security check.\n");
}
static void expose()
diff --git a/src/Joystick.cpp b/src/Joystick.cpp
index 2bd10052..3e060d0d 100644
--- a/src/Joystick.cpp
+++ b/src/Joystick.cpp
@@ -3,10 +3,10 @@
Joystick::Joystick()
: A3_(Vector6::Zero()),
A2_(Vector6::Zero()),
- v_ref_(Vector6::Zero()),
- v_gp_(Vector6::Zero()),
p_ref_(Vector6::Zero()),
p_gp_(Vector6::Zero()),
+ v_ref_(Vector6::Zero()),
+ v_gp_(Vector6::Zero()),
v_ref_heavy_filter_(Vector6::Zero()) {}
void Joystick::initialize(Params& params) {
@@ -30,7 +30,22 @@ void Joystick::initialize(Params& params) {
}
}
-VectorN Joystick::handle_v_switch(double k, VectorN const& k_switch, MatrixN const& v_switch) {
+VectorN Joystick::handle_v_switch_py(double k, VectorN const& k_switch_py, MatrixN const& v_switch_py) {
+ int i = 1;
+ while ((i < k_switch_py.rows()) && k_switch_py[i] <= k) {
+ i++;
+ }
+ if (i != k_switch_py.rows()) {
+ double ev = k - k_switch_py[i - 1];
+ double t1 = k_switch_py[i] - k_switch_py[i - 1];
+ A3_ = 2 * (v_switch_py.col(i - 1) - v_switch_py.col(i)) / pow(t1, 3);
+ A2_ = (-3.0 / 2.0) * t1 * A3_;
+ v_ref_ = v_switch_py.col(i - 1) + A2_ * pow(ev, 2) + A3_ * pow(ev, 3);
+ }
+ return v_ref_;
+}
+
+void Joystick::handle_v_switch(int k) {
int i = 1;
while ((i < k_switch.rows()) && k_switch[i] <= k) {
i++;
@@ -42,17 +57,14 @@ VectorN Joystick::handle_v_switch(double k, VectorN const& k_switch, MatrixN con
A2_ = (-3.0 / 2.0) * t1 * A3_;
v_ref_ = v_switch.col(i - 1) + A2_ * pow(ev, 2) + A3_ * pow(ev, 3);
}
- return v_ref_;
}
void Joystick::update_v_ref(int k, int velID, bool gait_is_static) {
- /* ONLY GAMEPAD CONTROL FOR NOW
- if (predefined):
+ if (predefined) {
update_v_ref_predefined(k, velID);
- else:
- */
-
- update_v_ref_gamepad(k, gait_is_static);
+ } else {
+ update_v_ref_gamepad(k, gait_is_static);
+ }
}
int Joystick::read_event(int fd, struct js_event* event) {
@@ -144,6 +156,7 @@ void Joystick::update_v_ref_gamepad(int k, bool gait_is_static) {
gamepad.w_yaw = 0.0;
}*/
+ // Remember when L1 was pressed for the last time
if (gamepad.L1 == 1) {
lock_time_L1_ = std::chrono::system_clock::now();
}
@@ -237,4 +250,23 @@ void Joystick::update_v_ref_gamepad(int k, bool gait_is_static) {
// Low pass filter to slow down the changes of position when moving the joysticks
p_ref_ = gp_alpha_pos * p_gp_ + (1 - gp_alpha_pos) * p_ref_;
-}
\ No newline at end of file
+}
+
+void Joystick::update_v_ref_predefined(int k, int velID) {
+ // Initialization of velocity profile during first call
+ if (k == 0) {
+ MatrixN t_switch;
+ switch (velID) {
+ case 10:
+ t_switch = MatrixN::Zero(1, 7);
+ t_switch << 0, 2, 4, 6, 8, 10, 15;
+ v_switch = MatrixN::Zero(6, 7);
+ v_switch.row(0) << 0, 2, 4, 6, 8, 10, 15;
+ break;
+ default:
+ throw std::runtime_error("Unknown velocity ID for the polynomial interpolation.");
+ }
+ k_switch = (t_switch / dt_wbc).cast<int>();
+ }
+ handle_v_switch(k); // Polynomial interpolation to generate the velocity profile
+}
diff --git a/src/MpcWrapper.cpp b/src/MpcWrapper.cpp
index 090a3800..138fd34d 100644
--- a/src/MpcWrapper.cpp
+++ b/src/MpcWrapper.cpp
@@ -96,8 +96,7 @@ void parallel_loop() {
}
MpcWrapper::MpcWrapper()
- : test(0),
- last_available_result(Eigen::Matrix<double, 24, 2>::Zero()),
+ : last_available_result(Eigen::Matrix<double, 24, 2>::Zero()),
gait_past(Matrix14::Zero()),
gait_next(Matrix14::Zero()) {}
--
GitLab