From 44fc04c46d7c0860c149940edc819fa50292040d Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Fri, 6 Aug 2021 16:26:26 +0200
Subject: [PATCH] Time spent by each block added to Logger + Integration of
15Hz low pass filters in loop + Logging of filtered quantities
---
include/qrw/Filter.hpp | 8 ++--
include/qrw/Types.h | 1 +
scripts/Controller.py | 37 +++++++----------
scripts/LoggerControl.py | 74 +++++++++++++++++++++++++++++++++-
scripts/main_solo12_control.py | 13 ------
src/Filter.cpp | 28 +++++--------
6 files changed, 101 insertions(+), 60 deletions(-)
diff --git a/include/qrw/Filter.hpp b/include/qrw/Filter.hpp
index 9b3b3417..da37c386 100644
--- a/include/qrw/Filter.hpp
+++ b/include/qrw/Filter.hpp
@@ -38,12 +38,12 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////
///
- /// \brief Run one iteration of the filter
+ /// \brief Run one iteration of the filter and return the filtered measurement
///
/// \param[in] x Quantity to filter
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void filter(VectorN const& x);
+ VectorN filter(VectorN const& x);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -59,8 +59,8 @@ public:
private:
- Vector5 b_; // Denominator coefficients of the filter transfer function
- Vector5 a_; // Numerator coefficients of the filter transfer function
+ Vector1 b_; // Denominator coefficients of the filter transfer function
+ Vector2 a_; // Numerator coefficients of the filter transfer function
Vector6 x_; // Latest measurement
VectorN y_; // Latest result
Vector6 accum_; // Used to compute the result (accumulation)
diff --git a/include/qrw/Types.h b/include/qrw/Types.h
index 3eb700e8..553d6fd8 100644
--- a/include/qrw/Types.h
+++ b/include/qrw/Types.h
@@ -10,6 +10,7 @@
#include <string>
#include <memory>
+using Vector1 = Eigen::Matrix<double, 1, 1>;
using Vector2 = Eigen::Matrix<double, 2, 1>;
using Vector3 = Eigen::Matrix<double, 3, 1>;
using Vector4 = Eigen::Matrix<double, 4, 1>;
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 6162a4fe..56988095 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -96,16 +96,6 @@ class Controller:
# Parameters definition #
########################################################################
- # Lists to log the duration of 1 iteration of the MPC/TSID
- self.t_list_filter = [0] * int(params.N_SIMULATION)
- self.t_list_planner = [0] * int(params.N_SIMULATION)
- self.t_list_mpc = [0] * int(params.N_SIMULATION)
- self.t_list_wbc = [0] * int(params.N_SIMULATION)
- self.t_list_loop = [0] * int(params.N_SIMULATION)
-
- self.t_list_InvKin = [0] * int(params.N_SIMULATION)
- self.t_list_QPWBC = [0] * int(params.N_SIMULATION)
-
# Init joint torques to correct shape
self.jointTorques = np.zeros((12, 1))
@@ -196,12 +186,15 @@ class Controller:
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.h_v_filt_mpc = np.zeros((6, 1))
+ self.h_v_bis_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_v_bis = lqrw.Filter()
+ self.filter_mpc_v_bis.initialize(params)
self.filter_mpc_vref = lqrw.Filter()
self.filter_mpc_vref.initialize(params)
@@ -254,13 +247,11 @@ class Controller:
# Update gait
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()
+ #Â Quantities go through a 1st order low pass filter with fc = 15 Hz
+ self.q_filt_mpc[:, 0] = self.filter_mpc_q.filter(self.q[:7, 0:1])
+ self.h_v_filt_mpc[:, 0] = self.filter_mpc_v.filter(self.h_v[:6, 0:1])
+ self.h_v_bis_filt_mpc[:, 0] = self.filter_mpc_v_bis.filter(self.h_v_bis[:6, 0:1])
+ self.vref_filt_mpc[:, 0] = self.filter_mpc_vref.filter(self.v_ref[:6, 0:1])
# Compute target footstep based on current and reference velocities
o_targetFootstep = self.footstepPlanner.updateFootsteps(self.k % self.k_mpc == 0 and self.k != 0,
@@ -398,11 +389,11 @@ class Controller:
def log_misc(self, tic, t_filter, t_planner, t_mpc, t_wbc):
- self.t_list_filter[self.k] = t_filter - tic
- self.t_list_planner[self.k] = t_planner - t_filter
- self.t_list_mpc[self.k] = t_mpc - t_planner
- self.t_list_wbc[self.k] = t_wbc - t_mpc
- self.t_list_loop[self.k] = time.time() - tic
+ self.t_filter = t_filter - tic
+ self.t_planner = t_planner - t_filter
+ self.t_mpc = t_mpc - t_planner
+ self.t_wbc = t_wbc - t_mpc
+ self.t_loop = time.time() - tic
def updateState(self):
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 5ff6bb1a..065a58aa 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -47,8 +47,17 @@ class LoggerControl():
self.loop_o_q_int = np.zeros([logSize, 19]) # position in world frame (esti_q_filt + dt * loop_o_v)
self.loop_o_v = np.zeros([logSize, 18]) # estimated velocity in world frame
self.loop_h_v = np.zeros([logSize, 18]) # estimated velocity in horizontal frame
- self.loop_h_v_bis = np.zeros([logSize, 3]) # estimated velocity in horizontal frame
+ self.loop_h_v_bis = np.zeros([logSize, 6]) # estimated velocity in horizontal frame
self.loop_pos_virtual_world = np.zeros([logSize, 3]) # x, y, yaw perfect position in world
+ self.loop_t_filter = np.zeros([logSize]) # time taken by the estimator
+ self.loop_t_planner = np.zeros([logSize]) #Â time taken by the planning
+ self.loop_t_mpc = np.zeros([logSize]) # time taken by the mcp
+ self.loop_t_wbc = np.zeros([logSize]) # time taken by the whole body control
+ self.loop_t_loop = np.zeros([logSize]) # time taken by the whole loop (without interface)
+ self.loop_q_filt_mpc = np.zeros([logSize, 6]) #Â position in world frame filtered by 1st order low pass
+ self.loop_h_v_filt_mpc = np.zeros([logSize, 6]) #Â vel in base frame filtered by 1st order low pass
+ self.loop_h_v_bis_filt_mpc = np.zeros([logSize, 6]) #Â alt vel in base frame filtered by 1st order low pass
+ self.loop_vref_filt_mpc = np.zeros([logSize, 6]) #Â ref vel in base frame filtered by 1st order low pass
# Gait
self.planner_gait = np.zeros([logSize, N0_gait, 4]) # Gait sequence
@@ -131,8 +140,17 @@ class LoggerControl():
self.loop_o_q_int[self.i] = loop.q[:, 0]
self.loop_o_v[self.i] = loop.v[:, 0]
self.loop_h_v[self.i] = loop.h_v[:, 0]
- self.loop_h_v_bis[self.i] = loop.h_v_bis[0:3, 0]
+ self.loop_h_v_bis[self.i] = loop.h_v_bis[:, 0]
self.loop_pos_virtual_world[self.i] = np.array([loop.q[0, 0], loop.q[1, 0], loop.yaw_estim])
+ self.loop_t_filter[self.i] = loop.t_filter
+ self.loop_t_planner[self.i] = loop.t_planner
+ self.loop_t_mpc[self.i] = loop.t_mpc
+ self.loop_t_wbc[self.i] = loop.t_wbc
+ self.loop_t_loop[self.i] = loop.t_loop
+ self.loop_q_filt_mpc[self.i] = loop.q_filt_mpc[:, 0]
+ self.loop_h_v_filt_mpc[self.i] = loop.h_v_filt_mpc[:, 0]
+ self.loop_h_v_bis_filt_mpc[self.i] = loop.h_v_bis_filt_mpc[:, 0]
+ self.loop_vref_filt_mpc[self.i] = loop.vref_filt_mpc[:, 0]
# Logging from the planner
# self.planner_q_static[self.i] = planner.q_static[:]
@@ -554,6 +572,40 @@ class LoggerControl():
plt.ylabel("Bus energy [J]")
plt.xlabel("Time [s]")
+ # Plot estimated computation time for each step for the control architecture
+ plt.figure()
+ plt.plot(t_range, self.loop_t_filter, 'r+')
+ plt.plot(t_range, self.loop_t_planner, 'g+')
+ plt.plot(t_range, self.loop_t_mpc, 'b+')
+ plt.plot(t_range, self.loop_t_wbc, '+', color="violet")
+ plt.plot(t_range, self.loop_t_loop, 'k+')
+ plt.legend(["Estimator", "Planner", "MPC", "WBC", "Whole loop"])
+ plt.xlabel("Time [s]")
+ plt.ylabel("Time [s]")
+
+ #Â Comparison between quantities before and after 15Hz low-pass filtering
+ plt.figure()
+ lgd = ["Linear vel X", "Linear vel Y", "Linear vel Z",
+ "Angular vel Roll", "Angular vel Pitch", "Angular vel Yaw"]
+ plt.figure()
+ for i in range(6):
+ if i == 0:
+ ax0 = plt.subplot(3, 2, index6[i])
+ else:
+ plt.subplot(3, 2, index6[i], sharex=ax0)
+
+ plt.plot(t_range, self.loop_h_v[:, i], linewidth=3)
+ plt.plot(t_range, self.loop_h_v_bis[:, i], linewidth=3)
+ plt.plot(t_range, self.loop_h_v_filt_mpc[:, i], linewidth=3)
+ plt.plot(t_range, self.loop_h_v_bis_filt_mpc[:, i], linewidth=3)
+ plt.plot(t_range, self.loop_vref_filt_mpc[:, i], linewidth=3)
+
+ plt.legend(["Estimated", "Estimated 3Hz windowed", "Estimated 15Hz filt",
+ "Estimated 15Hz filt 3Hz windowed", "Reference 15Hz filt"], prop={'size': 8})
+ plt.ylabel(lgd[i])
+ plt.suptitle("Comparison between quantities before and after 15Hz low-pass filtering")
+
+
# Display all graphs and wait
plt.show(block=True)
@@ -595,6 +647,15 @@ class LoggerControl():
loop_o_v=self.loop_o_v,
loop_h_v=self.loop_h_v,
loop_pos_virtual_world=self.loop_pos_virtual_world,
+ loop_t_filter=self.loop_t_filter,
+ loop_t_planner=self.loop_t_planner,
+ loop_t_mpc=self.loop_t_mpc,
+ loop_t_wbc=self.loop_t_wbc,
+ loop_t_loop=self.loop_t_loop,
+ loop_q_filt_mpc=self.loop_q_filt_mpc,
+ loop_h_v_filt_mpc=self.loop_h_v_filt_mpc,
+ loop_h_v_bis_filt_mpc=self.loop_h_v_bis_filt_mpc,
+ loop_vref_mpc=self.loop_vref_filt_mpc,
planner_q_static=self.planner_q_static,
planner_RPY_static=self.planner_RPY_static,
@@ -679,6 +740,15 @@ class LoggerControl():
self.loop_o_v = data["loop_o_v"]
self.loop_h_v = data["loop_h_v"]
self.loop_pos_virtual_world = data["loop_pos_virtual_world"]
+ self.loop_t_filter = data["loop_t_filter"]
+ self.loop_t_planner = data["loop_t_planner"]
+ self.loop_t_mpc = data["loop_t_mpc"]
+ self.loop_t_wbc = data["loop_t_wbc"]
+ self.loop_t_loop = data["loop_t_loop"]
+ self.loop_q_filt_mpc = data["loop_q_filt_mpc"]
+ self.loop_h_v_filt_mpc = data["loop_h_v_filt_mpc"]
+ self.loop_h_v_bis_filt_mpc = data["loop_h_v_bis_filt_mpc"]
+ self.loop_vref_filt_mpc = data["loop_vref_filt_mpc"]
self.planner_q_static = data["planner_q_static"]
self.planner_RPY_static = data["planner_RPY_static"]
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index 36674dbb..99c3c2f9 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -307,19 +307,6 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
print("Masterboard timeout detected.")
print("Either the masterboard has been shut down or there has been a connection issue with the cable/wifi.")
- # Plot estimated computation time for each step for the control architecture
- from matplotlib import pyplot as plt
- """plt.figure()
- plt.plot(controller.t_list_filter[1:], 'r+')
- plt.plot(controller.t_list_planner[1:], 'g+')
- plt.plot(controller.t_list_mpc[1:], 'b+')
- plt.plot(controller.t_list_wbc[1:], '+', color="violet")
- plt.plot(controller.t_list_loop[1:], 'k+')
- plt.plot(t_log_whole, 'x')
- plt.legend(["Estimator", "Planner", "MPC", "WBC", "Whole loop", "Whole loop + Interface"])
- plt.title("Loop time [s]")
- plt.show(block=True)"""
-
# Plot recorded data
if params.PLOTTING:
loggerControl.plotAll(loggerSensors)
diff --git a/src/Filter.cpp b/src/Filter.cpp
index 291cfc5e..a77ef5aa 100644
--- a/src/Filter.cpp
+++ b/src/Filter.cpp
@@ -1,8 +1,8 @@
#include "qrw/Filter.hpp"
Filter::Filter()
- : b_(Vector5::Zero())
- , a_(Vector5::Zero())
+ : b_(Vector1::Zero())
+ , a_(Vector2::Zero())
, x_(Vector6::Zero())
, y_(VectorN::Zero(6, 1))
, accum_(Vector6::Zero())
@@ -13,26 +13,17 @@ Filter::Filter()
void Filter::initialize(Params& params)
{
- if (params.dt_wbc == 0.001)
- {
- b_ << 3.12389769e-5, 1.24955908e-4, 1.87433862e-4, 1.24955908e-4, 3.12389769e-5;
- a_ << 1., -3.58973389, 4.85127588, -2.92405266, 0.66301048;
- }
- else if (params.dt_wbc == 0.002)
- {
- b_ << 0.0004166, 0.0016664, 0.0024996, 0.0016664, 0.0004166;
- a_ << 1., -3.18063855, 3.86119435, -2.11215536, 0.43826514;
- }
- else
- {
- throw std::runtime_error("No coefficients defined for this time step.");
- }
+ const double fc = 15.0;
+ double alpha = (2 * M_PI * params.dt_wbc * fc) / (2 * M_PI * params.dt_wbc * fc + 1.0);
+ b_ << alpha;
+ a_ << 1.0, -(1.0 - alpha);
+
x_queue_.resize(b_.rows(), Vector6::Zero());
y_queue_.resize(a_.rows()-1, Vector6::Zero());
}
-void Filter::filter(VectorN const& x)
+VectorN Filter::filter(VectorN const& x)
{
// If x is position + quaternion then we convert quaternion to RPY
if (x.rows() == 7)
@@ -48,7 +39,6 @@ void Filter::filter(VectorN const& x)
{
if (std::abs(x_(i, 0) - y_(i, 0)) > 1.5 * M_PI)
{
- std::cout << "Modulo for " << i << std::endl;
handle_modulo(i, x_(i, 0) - y_(i, 0) > 0);
}
}
@@ -88,6 +78,8 @@ void Filter::filter(VectorN const& x)
// Filtered result is stored in y_queue_.front()
// Assigned to dynamic-sized vector for binding purpose
y_ = y_queue_.front();
+
+ return y_;
}
void Filter::handle_modulo(int a, bool dir)
--
GitLab