From e98c56e4f307bc9156855d1067c80d35bd31872f Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Fri, 6 Aug 2021 11:08:42 +0200
Subject: [PATCH] Add Filter Python bindings + Clean unused variable in Gait
---
include/qrw/Gait.hpp | 6 ++----
python/gepadd.cpp | 32 +++++++++++++++++++++++++++++++-
scripts/Controller.py | 20 +++++++++++++++++++-
src/Gait.cpp | 9 ++-------
4 files changed, 54 insertions(+), 13 deletions(-)
diff --git a/include/qrw/Gait.hpp b/include/qrw/Gait.hpp
index 020e4de1..0fe2a485 100644
--- a/include/qrw/Gait.hpp
+++ b/include/qrw/Gait.hpp
@@ -60,10 +60,9 @@ public:
/// \param[in] k numero of the current loop
/// \param[in] k_mpc number of loop per mpc time step
/// \param[in] code integer to trigger events with the joystick
- /// \param[in] q current position vector of the flying base in world frame (linear and angular stacked)
///
////////////////////////////////////////////////////////////////////////////////////////////////
- bool changeGait(int const k, int const k_mpc, int const code, VectorN const& q);
+ bool changeGait(int const k, int const k_mpc, int const code);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -75,11 +74,10 @@ public:
///
/// \param[in] k numero of the current loop
/// \param[in] k_mpc number of loop per mpc time step
- /// \param[in] q current position vector of the flying base in world frame (linear and angular stacked)
/// \param[in] joystickCode integer to trigger events with the joystick
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void updateGait(int const k, int const k_mpc, VectorN const& q, int const joystickCode);
+ void updateGait(int const k, int const k_mpc, int const joystickCode);
////////////////////////////////////////////////////////////////////////////////////////////////
///
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 0635b1a2..1b5ca0cc 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -8,6 +8,7 @@
#include "qrw/QPWBC.hpp"
#include "qrw/Estimator.hpp"
#include "qrw/Joystick.hpp"
+#include "qrw/Filter.hpp"
#include "qrw/Params.hpp"
#include <boost/python.hpp>
@@ -43,6 +44,34 @@ struct MPCPythonVisitor : public bp::def_visitor<MPCPythonVisitor<MPC>>
void exposeMPC() { MPCPythonVisitor<MPC>::expose(); }
+/////////////////////////////////
+/// Binding Filter class
+/////////////////////////////////
+template <typename Filter>
+struct FilterPythonVisitor : public bp::def_visitor<FilterPythonVisitor<Filter>>
+{
+ template <class PyClassFilter>
+ void visit(PyClassFilter& cl) const
+ {
+ cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+
+ .def("initialize", &Filter::initialize, bp::args("params"),
+ "Initialize Filter from Python.\n")
+
+ // Run Filter from Python
+ .def("filter", &Filter::filter, bp::args("x"), "Run Filter from Python.\n")
+ .def("getFilt", &Filter::getFilt, "Get filtered quantity.\n");
+ }
+
+ static void expose()
+ {
+ bp::class_<Filter>("Filter", bp::no_init).def(FilterPythonVisitor<Filter>());
+
+ ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
+ }
+};
+void exposeFilter() { FilterPythonVisitor<Filter>::expose(); }
+
/////////////////////////////////
/// Binding StatePlanner class
/////////////////////////////////
@@ -93,7 +122,7 @@ struct GaitPythonVisitor : public bp::def_visitor<GaitPythonVisitor<Gait>>
"Initialize Gait from Python.\n")
// Update current gait matrix from Python
- .def("updateGait", &Gait::updateGait, bp::args("k", "k_mpc", "q", "joystickCode"),
+ .def("updateGait", &Gait::updateGait, bp::args("k", "k_mpc", "joystickCode"),
"Update current gait matrix from Python.\n")
// Set current gait matrix from Python
@@ -436,6 +465,7 @@ BOOST_PYTHON_MODULE(libquadruped_reactive_walking)
eigenpy::enableEigenPy();
exposeMPC();
+ exposeFilter();
exposeStatePlanner();
exposeGait();
exposeFootstepPlanner();
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 08b5c36b..714b81c2 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -199,6 +199,16 @@ class Controller:
self.error_flag = 0
self.q_security = np.array([np.pi*0.4, np.pi*80/180, np.pi] * 4)
+ self.q_filt_mpc = np.zeros((6, 1))
+ self.v_filt_mpc = np.zeros((6, 1))
+ self.vref_filt_mpc = np.zeros((6, 1))
+ self.filter_mpc_q = lqrw.Filter()
+ self.filter_mpc_q.initialize(params)
+ self.filter_mpc_v = lqrw.Filter()
+ self.filter_mpc_v.initialize(params)
+ self.filter_mpc_vref = lqrw.Filter()
+ self.filter_mpc_vref.initialize(params)
+
# Interface with the PD+ on the control board
self.result = Result()
@@ -246,7 +256,15 @@ class Controller:
t_filter = time.time()
# Update gait
- self.gait.updateGait(self.k, self.k_mpc, self.q[0:7, 0:1], self.joystick.joystick_code)
+ self.gait.updateGait(self.k, self.k_mpc, self.joystick.joystick_code)
+
+ self.q[3:7, 0] = np.array([0, 0, 0.3826834, 0.9238795])
+ self.filter_mpc_q.filter(self.q[:7, 0:1])
+ self.filter_mpc_v.filter(self.h_v[:6, 0:1])
+ self.filter_mpc_vref.filter(self.v_ref[:6, 0:1])
+
+ from IPython import embed
+ embed()
# Compute target footstep based on current and reference velocities
o_targetFootstep = self.footstepPlanner.updateFootsteps(self.k % self.k_mpc == 0 and self.k != 0,
diff --git a/src/Gait.cpp b/src/Gait.cpp
index 697d18aa..146478e1 100644
--- a/src/Gait.cpp
+++ b/src/Gait.cpp
@@ -187,15 +187,14 @@ double Gait::getPhaseDuration(int i, int j, double value)
void Gait::updateGait(int const k,
int const k_mpc,
- VectorN const& q,
int const joystickCode)
{
- changeGait(k, k_mpc, joystickCode, q);
+ changeGait(k, k_mpc, joystickCode);
if (k % k_mpc == 0 && k > 0)
rollGait();
}
-bool Gait::changeGait(int const k, int const k_mpc, int const code, VectorN const& q)
+bool Gait::changeGait(int const k, int const k_mpc, int const code)
{
if (code != 0 && switch_to_gait_ == 0)
{
@@ -213,10 +212,6 @@ bool Gait::changeGait(int const k, int const k_mpc, int const code, VectorN cons
switch_to_gait_ = 0;
}
- /* create_static();
- q_static_.head(7) = q.head(7);
- is_static_ = true; */
-
return is_static_;
}
--
GitLab