From cf855882c50ed84df149cdb7b18a256221632194 Mon Sep 17 00:00:00 2001
From: Fanny Risbourg <frisbourg@laas.fr>
Date: Fri, 12 Aug 2022 14:10:56 +0200
Subject: [PATCH] add Estimator
---
CMakeLists.txt | 6 +
config/walk_parameters.yaml | 3 +-
include/bindings/python.hpp | 2 +
include/qrw/ComplementaryFilter.hpp | 80 +++++
include/qrw/Estimator.hpp | 266 ++++++++++++++++
include/qrw/Filter.hpp | 77 +++++
python/CMakeLists.txt | 2 +
python/Estimator.cpp | 51 ++++
python/Filter.cpp | 16 +
python/main.cpp | 2 +
.../quadruped_reactive_walking/Controller.py | 52 +++-
.../WB_MPC/ProblemData.py | 20 +-
.../WB_MPC/Target.py | 5 +-
.../quadruped_reactive_walking/tools/Utils.py | 33 ++
src/ComplementaryFilter.cpp | 37 +++
src/Estimator.cpp | 283 ++++++++++++++++++
src/Filter.cpp | 73 +++++
17 files changed, 991 insertions(+), 17 deletions(-)
create mode 100644 include/qrw/ComplementaryFilter.hpp
create mode 100644 include/qrw/Estimator.hpp
create mode 100644 include/qrw/Filter.hpp
create mode 100644 python/Estimator.cpp
create mode 100644 python/Filter.cpp
create mode 100644 src/ComplementaryFilter.cpp
create mode 100644 src/Estimator.cpp
create mode 100644 src/Filter.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 68c04dde..4800cc0b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -51,11 +51,17 @@ set(${PROJECT_NAME}_HEADERS
include/qrw/Types.h
include/qrw/Params.hpp
include/qrw/Joystick.hpp
+ include/qrw/Estimator.hpp
+ include/qrw/Filter.hpp
+ include/qrw/ComplementaryFilter.hpp
)
set(${PROJECT_NAME}_SOURCES
src/Params.cpp
src/Joystick.cpp
+ src/Estimator.cpp
+ src/Filter.cpp
+ src/ComplementaryFilter.cpp
)
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index b11e42aa..afd1cb81 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -37,8 +37,7 @@ robot:
# Parameters of Gait
N_periods: 1
- gait: [8, 1, 0, 0, 1,
- 8, 0, 1, 1, 0] # Initial gait matrix
+ gait: [100, 0, 0, 0, 0] # Initial gait matrix
# Parameters of Joystick
gp_alpha_vel: 0.003 #Â Coefficient of the low pass filter applied to gamepad velocity
diff --git a/include/bindings/python.hpp b/include/bindings/python.hpp
index 9e6d9c95..cfd713cd 100644
--- a/include/bindings/python.hpp
+++ b/include/bindings/python.hpp
@@ -13,5 +13,7 @@ namespace bp = boost::python;
void exposeJoystick();
void exposeParams();
+void exposeEstimator();
+void exposeFilter();
#endif
\ No newline at end of file
diff --git a/include/qrw/ComplementaryFilter.hpp b/include/qrw/ComplementaryFilter.hpp
new file mode 100644
index 00000000..b12fb47c
--- /dev/null
+++ b/include/qrw/ComplementaryFilter.hpp
@@ -0,0 +1,80 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for Estimator and ComplementaryFilter classes
+///
+/// \details These classes estimate the state of the robot based on sensor measurements
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef COMPLEMENTARY_FILTER_H_INCLUDED
+#define COMPLEMENTARY_FILTER_H_INCLUDED
+
+#include "qrw/Types.h"
+
+class ComplementaryFilter {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ComplementaryFilter();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor witht initialization
+ ///
+ /// \param[in] dt Time step of the complementary filter
+ /// \param[in] HighPass Initial value for the high pass filter
+ /// \param[in] LowPass Initial value for the low pass filter
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ComplementaryFilter(double dt, Vector3 HighPass, Vector3 LowPass);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~ComplementaryFilter() {} // Empty destructor
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize
+ ///
+ /// \param[in] dt Time step of the complementary filter
+ /// \param[in] HighPass Initial value for the high pass filter
+ /// \param[in] LowPass Initial value for the low pass filter
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(double dt, Vector3 HighPass, Vector3 LowPass);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Compute the filtered output of the complementary filter
+ ///
+ /// \param[in] x Quantity handled by the filter
+ /// \param[in] dx Derivative of the quantity
+ /// \param[in] alpha Filtering coefficient between x and dx quantities
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Vector3 compute(Vector3 const& x, Vector3 const& dx, Vector3 const& alpha);
+
+ Vector3 getX() { return x_; } // Get the input quantity
+ Vector3 getDx() { return dx_; } // Get the derivative of the input quantity
+ Vector3 getHighPass() { return HighPass_; } // Get the high-passed internal quantity
+ Vector3 getLowPass() { return LowPass_; } // Get the low-passed internal quantity
+ Vector3 getAlpha() { return alpha_; } // Get the alpha coefficient of the filter
+ Vector3 getFilteredX() { return filteredX_; } // Get the filtered output
+
+ private:
+ double dt_; // Time step of the complementary filter
+ Vector3 HighPass_; // Initial value for the high pass filter
+ Vector3 LowPass_; // Initial value for the low pass filter
+ Vector3 alpha_; // Filtering coefficient between x and dx quantities
+ Vector3 x_; // Quantity to filter
+ Vector3 dx_; // Quantity to filter derivative's
+ Vector3 filteredX_; // Filtered output
+};
+
+#endif // COMPLEMENTARY_FILTER_H_INCLUDED
diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
new file mode 100644
index 00000000..2e35fbaa
--- /dev/null
+++ b/include/qrw/Estimator.hpp
@@ -0,0 +1,266 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for Estimator and ComplementaryFilter classes
+///
+/// \details These classes estimate the state of the robot based on sensor measurements
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef ESTIMATOR_H_INCLUDED
+#define ESTIMATOR_H_INCLUDED
+
+#include <deque>
+
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "qrw/ComplementaryFilter.hpp"
+#include "qrw/Params.hpp"
+
+class Estimator {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Estimator();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~Estimator() {} // Empty destructor
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize with given data
+ ///
+ /// \param[in] params Object that stores parameters
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params& params);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Run one iteration of the estimator to get the position and velocity states of the robot
+ ///
+ /// \param[in] gait Gait matrix that stores current and future contact status of the feet
+ /// \param[in] goals Target positions of the four feet
+ /// \param[in] baseLinearAcceleration Linear acceleration of the IMU (gravity compensated)
+ /// \param[in] baseAngularVelocity Angular velocity of the IMU
+ /// \param[in] baseOrientation Quaternion orientation of the IMU
+ /// \param[in] q_mes Position of the 12 actuators
+ /// \param[in] v_mes Velocity of the 12 actuators
+ /// \param[in] perfectPosition Position of the robot in world frame
+ /// \param[in] b_perfectVelocity Velocity of the robot in base frame
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void run(MatrixN const& gait, MatrixN const& goals, VectorN const& baseLinearAcceleration,
+ VectorN const& baseAngularVelocity, VectorN const& baseOrientation, VectorN const& q_mes,
+ VectorN const& v_mes, VectorN const& perfectPosition, Vector3 const& b_perfectVelocity);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update state vectors of the robot (q and v)
+ /// Update transformation matrices between world and horizontal frames
+ ///
+ /// \param[in] joystick_v_ref Reference velocity from the joystick
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void updateReferenceState(VectorN const& vRef);
+
+ VectorN getQEstimate() { return qEstimate_; }
+ VectorN getVEstimate() { return vEstimate_; }
+ VectorN getVSecurity() { return vSecurity_; }
+ VectorN getFeetStatus() { return feetStatus_; }
+ MatrixN getFeetTargets() { return feetTargets_; }
+ Vector3 getBaseVelocityFK() { return baseVelocityFK_; }
+ Vector3 getBasePositionFK() { return basePositionFK_; }
+ Vector3 getFeetPositionBarycenter() { return feetPositionBarycenter_; }
+ Vector3 getBBaseVelocity() { return b_baseVelocity_; }
+
+ Vector3 getFilterVelX() { return velocityFilter_.getX(); }
+ Vector3 getFilterVelDX() { return velocityFilter_.getDx(); }
+ Vector3 getFilterVelAlpha() { return velocityFilter_.getAlpha(); }
+ Vector3 getFilterVelFiltX() { return velocityFilter_.getFilteredX(); }
+
+ Vector3 getFilterPosX() { return positionFilter_.getX(); }
+ Vector3 getFilterPosDX() { return positionFilter_.getDx(); }
+ Vector3 getFilterPosAlpha() { return positionFilter_.getAlpha(); }
+ Vector3 getFilterPosFiltX() { return positionFilter_.getFilteredX(); }
+
+ VectorN getQReference() { return qRef_; }
+ VectorN getVReference() { return vRef_; }
+ VectorN getBaseVelRef() { return baseVelRef_; }
+ VectorN getBaseAccRef() { return baseAccRef_; }
+ VectorN getHV() { return h_v_; }
+ VectorN getVFiltered() { return vFiltered_; }
+ VectorN getHVFiltered() { return h_vFiltered_; }
+ Matrix3 getoRh() { return oRh_; }
+ Matrix3 gethRb() { return hRb_; }
+ Vector3 getoTh() { return oTh_; }
+
+ private:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Retrieve and update IMU data
+ ///
+ /// \param[in] baseLinearAcceleration Linear acceleration of the IMU (gravity compensated)
+ /// \param[in] baseAngularVelocity Angular velocity of the IMU
+ /// \param[in] baseOrientation Euler orientation of the IMU
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void updateIMUData(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity,
+ Vector3 const& baseOrientation, VectorN const& perfectPosition);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Retrieve and update position and velocity of the 12 actuators
+ ///
+ /// \param[in] q Position of the 12 actuators
+ /// \param[in] v Velocity of the 12 actuators
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void updateJointData(Vector12 const& q, Vector12 const& v);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update the feet relative data
+ /// \details update feetStatus_, feetTargets_, feetStancePhaseDuration_ and phaseRemainingDuration_
+ ///
+ /// \param[in] gait Gait matrix that stores current and future contact status of the feet
+ /// \param[in] feetTargets Target positions of the four feet
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void updatFeetStatus(MatrixN const& gait, MatrixN const& feetTargets);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Estimate base position and velocity using Forward Kinematics
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void updateForwardKinematics();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Compute barycenter of feet in contact
+ ///
+ /// \param[in] feet_status Contact status of the four feet
+ /// \param[in] goals Target positions of the four feet
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void computeFeetPositionBarycenter();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Estimate the velocity of the base with forward kinematics using a contact point that
+ /// is supposed immobile in world frame
+ ///
+ /// \param[in] contactFrameId Frame ID of the contact foot
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Vector3 computeBaseVelocityFromFoot(int footId);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Estimate the position of the base with forward kinematics using a contact point that
+ /// is supposed immobile in world frame
+ ///
+ /// \param[in] footId Frame ID of the contact foot
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Vector3 computeBasePositionFromFoot(int footId);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Compute the alpha coefficient for the complementary filter
+ /// \return alpha
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ double computeAlphaVelocity();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Estimates the velocity vector
+ /// \details The complementary filter combines data from the FK and the IMU acceleration data
+ ///
+ /// \param[in] b_perfectVelocity Perfect velocity of the base in the base frame
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void estimateVelocity(Vector3 const& b_perfectVelocity);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Estimates the configuration vector
+ /// \details The complementary filter combines data from the FK and the estimated velocity
+ ///
+ /// \param[in] perfectPosition Perfect position of the base in the world frame
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void estimatePosition(Vector3 const& perfectPosition);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Filter the estimated velocity over a moving window
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void filterVelocity();
+
+ bool perfectEstimator_; // Enable perfect estimator (directly from the PyBullet simulation)
+ bool solo3D_; // Perfect estimator including yaw angle
+ double dt_; // Time step of the estimator
+ bool initialized_; // Is intiialized after the first update of the IMU
+ Vector4 feetFrames_; // Frame indexes of the four feet
+ double footRadius_; // radius of a foot
+ Vector3 alphaPos_; // Alpha coeefficient for the position complementary filter
+ double alphaVelMax_; // Maximum alpha value for the velocity complementary filter
+ double alphaVelMin_; // Minimum alpha value for the velocity complementary filter
+ double alphaSecurity_; // Low pass coefficient for the outputted filtered velocity for security check
+
+ pinocchio::SE3 b_M_IMU_; // Transform between the base frame and the IMU frame
+ double IMUYawOffset_; // Yaw orientation of the IMU at startup
+ Vector3 IMULinearAcceleration_; // Linear acceleration of the IMU (gravity compensated)
+ Vector3 IMUAngularVelocity_; // Angular velocity of the IMU
+ Vector3 IMURpy_; // Roll Pitch Yaw orientation of the IMU
+ pinocchio::SE3::Quaternion IMUQuat_; // Quaternion orientation of the IMU
+
+ Vector12 qActuators_; // Measured positions of actuators
+ Vector12 vActuators_; // Measured velocities of actuators
+
+ int phaseRemainingDuration_; // Number of iterations left for the current gait phase
+ Vector4 feetStancePhaseDuration_; // Number of loops during which each foot has been in contact
+ Vector4 feetStatus_; // Contact status of the four feet
+ Matrix34 feetTargets_; // Target positions of the four feet
+
+ pinocchio::Model velocityModel_, positionModel_; // Pinocchio models for frame computations and forward kinematics
+ pinocchio::Data velocityData_, positionData_; // Pinocchio datas for frame computations and forward kinematics
+ Vector19 q_FK_; // Configuration vector for Forward Kinematics
+ Vector18 v_FK_; // Velocity vector for Forward Kinematics
+ Vector3 baseVelocityFK_; // Base linear velocity estimated by Forward Kinematics
+ Vector3 basePositionFK_; // Base position estimated by Forward Kinematics
+ Vector3 b_baseVelocity_; // Filtered estimated velocity at center base (base frame)
+ Vector3 feetPositionBarycenter_; // Barycenter of feet in contact
+
+ ComplementaryFilter positionFilter_; // Complementary filter for base position
+ ComplementaryFilter velocityFilter_; // Complementary filter for base velocity
+ Vector19 qEstimate_; // Filtered output configuration
+ Vector18 vEstimate_; // Filtered output velocity
+ Vector12 vSecurity_; // Filtered output velocity for security check
+
+ int windowSize_; // Number of samples in the averaging window
+ Vector6 vFiltered_; // Base velocity (in base frame) filtered by averaging window
+ std::deque<double> vx_queue_, vy_queue_, vz_queue_; // Queues that hold samples
+
+ Vector18 qRef_; // Configuration vector in ideal world frame
+ Vector18 vRef_; // Velocity vector in ideal world frame
+ Vector6 baseVelRef_; // Reference velocity vector
+ Vector6 baseAccRef_; // Reference acceleration vector
+ Matrix3 oRh_; // Rotation between horizontal and world frame
+ Matrix3 hRb_; // Rotation between base and horizontal frame
+ Vector3 oTh_; // Translation between horizontal and world frame
+ Vector6 h_v_; // Velocity vector in horizontal frame
+ Vector6 h_vFiltered_; // Base velocity (in horizontal frame) filtered by averaging window
+};
+#endif // ESTIMATOR_H_INCLUDED
diff --git a/include/qrw/Filter.hpp b/include/qrw/Filter.hpp
new file mode 100644
index 00000000..65af7687
--- /dev/null
+++ b/include/qrw/Filter.hpp
@@ -0,0 +1,77 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for Filter class
+///
+/// \details This class applies a low pass filter to estimated data to avoid keeping high frequency components into
+/// what is given to the "low frequency" model predictive control
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef FILTER_H_INCLUDED
+#define FILTER_H_INCLUDED
+
+#include <deque>
+
+#include "qrw/Params.hpp"
+
+class Filter {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Filter();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize with given data
+ ///
+ /// \param[in] params Object that stores parameters
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params& params);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~Filter() {} // Empty destructor
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Run one iteration of the filter and return the filtered measurement
+ ///
+ /// \param[in] x Quantity to filter
+ /// \param[in] check_modulo Check for the +-pi modulo of orientation if true
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ VectorN filter(Vector6 const& x, bool check_modulo);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Add or remove 2 PI to all elements in the queues to handle +- pi modulo
+ ///
+ /// \param[in] a Angle that needs change (3, 4, 5 for Roll, Pitch, Yaw respectively)
+ /// \param[in] dir Direction of the change (+pi to -pi or -pi to +pi)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void handle_modulo(int a, bool dir);
+
+ VectorN getFilt() { return y_; }
+
+ private:
+ double 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)
+
+ std::deque<Vector6> x_queue_; // Store the last measurements for product with denominator coefficients
+ std::deque<Vector6> y_queue_; // Store the last results for product with numerator coefficients
+
+ bool init_; // Initialisation flag
+};
+
+#endif // FILTER_H_INCLUDED
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 8739abe9..1128124a 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -2,6 +2,8 @@ set(${PY_NAME}_SOURCES
main.cpp
Joystick.cpp
Params.cpp
+ Estimator.cpp
+ Filter.cpp
)
set(PY_LIB ${${PY_NAME}_LIB})
diff --git a/python/Estimator.cpp b/python/Estimator.cpp
new file mode 100644
index 00000000..116688a6
--- /dev/null
+++ b/python/Estimator.cpp
@@ -0,0 +1,51 @@
+#include "qrw/Estimator.hpp"
+
+#include "bindings/python.hpp"
+
+template <typename Estimator>
+struct EstimatorVisitor : public bp::def_visitor<EstimatorVisitor<Estimator>> {
+ template <class PyClassEstimator>
+ void visit(PyClassEstimator& cl) const {
+ cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+
+ .def("initialize", &Estimator::initialize, bp::args("params"), "Initialize Estimator from Python.\n")
+ .def("update_reference_state", &Estimator::updateReferenceState, bp::args("v_ref"), "Update robot state.\n")
+
+ .def("get_q_estimate", &Estimator::getQEstimate, "Get filtered configuration.\n")
+ .def("get_v_estimate", &Estimator::getVEstimate, "Get filtered velocity.\n")
+ .def("get_v_security", &Estimator::getVSecurity, "Get filtered velocity for security check.\n")
+ .def("get_feet_status", &Estimator::getFeetStatus, "")
+ .def("get_feet_targets", &Estimator::getFeetTargets, "")
+ .def("get_base_velocity_FK", &Estimator::getBaseVelocityFK, "")
+ .def("get_base_position_FK", &Estimator::getBasePositionFK, "")
+ .def("get_feet_position_barycenter", &Estimator::getFeetPositionBarycenter, "")
+ .def("get_b_base_velocity", &Estimator::getBBaseVelocity, "")
+ .def("get_filter_vel_X", &Estimator::getFilterVelX, "")
+ .def("get_filter_vel_DX", &Estimator::getFilterVelDX, "")
+ .def("get_filter_vel_Alpha", &Estimator::getFilterVelAlpha, "")
+ .def("get_filter_vel_FiltX", &Estimator::getFilterVelFiltX, "")
+ .def("get_filter_pos_X", &Estimator::getFilterPosX, "")
+ .def("get_filter_pos_DX", &Estimator::getFilterPosDX, "")
+ .def("get_filter_pos_Alpha", &Estimator::getFilterPosAlpha, "")
+ .def("get_filter_pos_FiltX", &Estimator::getFilterPosFiltX, "")
+ .def("get_q_reference", &Estimator::getQReference, "")
+ .def("get_v_reference", &Estimator::getVReference, "")
+ .def("get_base_vel_ref", &Estimator::getBaseVelRef, "")
+ .def("get_base_acc_ref", &Estimator::getBaseAccRef, "")
+ .def("get_h_v", &Estimator::getHV, "")
+ .def("get_v_filtered", &Estimator::getVFiltered, "Get filtered velocity.\n")
+ .def("get_h_v_filtered", &Estimator::getHVFiltered, "")
+ .def("get_oRh", &Estimator::getoRh, "")
+ .def("get_hRb", &Estimator::gethRb, "")
+ .def("get_oTh", &Estimator::getoTh, "")
+
+ .def("run", &Estimator::run,
+ bp::args("gait", "goals", "baseLinearAcceleration", "baseAngularVelocity", "baseOrientation", "q_mes",
+ "v_mes", "base_position", "b_base_velocity"),
+ "Run Estimator from Python.\n");
+ }
+
+ static void expose() { bp::class_<Estimator>("Estimator", bp::no_init).def(EstimatorVisitor<Estimator>()); }
+};
+
+void exposeEstimator() { EstimatorVisitor<Estimator>::expose(); }
diff --git a/python/Filter.cpp b/python/Filter.cpp
new file mode 100644
index 00000000..00fc7e81
--- /dev/null
+++ b/python/Filter.cpp
@@ -0,0 +1,16 @@
+#include "qrw/Filter.hpp"
+
+#include "bindings/python.hpp"
+template <typename Filter>
+struct FilterVisitor : public bp::def_visitor<FilterVisitor<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")
+ .def("filter", &Filter::filter, bp::args("x", "check_modulo"), "Run Filter from Python.\n");
+ }
+
+ static void expose() { bp::class_<Filter>("Filter", bp::no_init).def(FilterVisitor<Filter>()); }
+};
+
+void exposeFilter() { FilterVisitor<Filter>::expose(); }
\ No newline at end of file
diff --git a/python/main.cpp b/python/main.cpp
index be09c5e7..a74a5454 100644
--- a/python/main.cpp
+++ b/python/main.cpp
@@ -3,4 +3,6 @@
BOOST_PYTHON_MODULE(quadruped_reactive_walking_pywrap) {
exposeJoystick();
exposeParams();
+ exposeEstimator();
+ exposeFilter();
}
\ No newline at end of file
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 32c81be0..8db3642d 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -4,9 +4,10 @@ import numpy as np
import pinocchio as pin
import pybullet as pyb
+import quadruped_reactive_walking as qrw
from . import WB_MPC_Wrapper
from .WB_MPC.Target import Target
-from .tools.Utils import init_robot
+from .tools.Utils import init_robot, quaternionToRPY
COLORS = ["#1f77b4", "#ff7f0e", "#2ca02c"]
@@ -161,6 +162,10 @@ class Controller:
self.error = False
self.initialized = False
+ self.estimator = qrw.Estimator()
+ self.estimator.initialize(params)
+ self.q = np.zeros(18)
+
self.result = Result(params)
self.result.q_des = self.pd.q0[7:].copy()
self.result.v_des = self.pd.v0[6:].copy()
@@ -205,6 +210,9 @@ class Controller:
t_start = time.time()
m = self.read_state(device)
+
+ oRh, hRb, oTh = self.run_estimator(device)
+
t_measures = time.time()
self.t_measures = t_measures - t_start
@@ -417,6 +425,48 @@ class Controller:
)
print("Initial guess saved")
+ def run_estimator(
+ self, device, q_perfect=np.zeros(6), b_baseVel_perfect=np.zeros(3)
+ ):
+ """
+ Call the estimator and retrieve the reference and estimated quantities.
+ Run a filter on q, h_v and v_ref.
+
+ @param device device structure holding simulation data
+ @param q_perfect 6D perfect position of the base in world frame
+ @param v_baseVel_perfect 3D perfect linear velocity of the base in base frame
+ """
+
+ self.estimator.run(
+ self.gait,
+ self.target.compute(self.k),
+ device.imu.linear_acceleration,
+ device.imu.gyroscope,
+ device.imu.attitude_euler,
+ device.joints.positions,
+ device.joints.velocities,
+ q_perfect,
+ b_baseVel_perfect,
+ )
+
+ self.estimator.update_reference_state(np.zeros(6))
+ # Add joystck reference velocity when needed
+
+ oRh = self.estimator.get_oRh()
+ hRb = self.estimator.get_hRb()
+ oTh = self.estimator.get_oTh().reshape((3, 1))
+
+ self.v_ref = self.estimator.get_base_vel_ref()
+ self.h_v = self.estimator.get_h_v()
+ self.h_v_windowed = self.estimator.get_h_v_filtered()
+
+ self.q[:3] = self.estimator.get_q_estimate()[:3]
+ self.q[6:] = self.estimator.get_q_estimate()[7:]
+ self.q[3:6] = quaternionToRPY(self.estimator.get_q_estimate()[3:7]).ravel()
+ self.v = self.estimator.get_v_reference()
+
+ return oRh, hRb, oTh
+
def read_state(self, device):
qj_m = device.joints.positions
vj_m = device.joints.velocities
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 56541ea0..568a58ae 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -9,8 +9,8 @@ class problemDataAbstract:
self.dt_wbc = param.dt_wbc
self.mpc_wbc_ratio = int(self.dt / self.dt_wbc)
self.init_steps = 0
- self.target_steps = 100
- self.T = self.init_steps + self.target_steps - 1
+ self.target_steps = param.gait.shape[0]
+ self.T = param.gait.shape[0] - 1
self.robot = erd.load("solo12")
self.q0 = self.robot.q0
@@ -23,8 +23,7 @@ class problemDataAbstract:
self.frozen_names = frozen_names
if frozen_names:
- self.frozen_idxs = [self.model.getJointId(
- id) for id in frozen_names]
+ self.frozen_idxs = [self.model.getJointId(id) for id in frozen_names]
self.freeze()
self.nq = self.model.nq
@@ -162,8 +161,7 @@ class ProblemData(problemDataAbstract):
class ProblemDataFull(problemDataAbstract):
def __init__(self, param):
- frozen_names = [
- "root_joint"]
+ frozen_names = ["root_joint"]
super().__init__(param, frozen_names)
@@ -176,13 +174,9 @@ class ProblemDataFull(problemDataAbstract):
# self.friction_cone_w = 1e3 * 0
self.control_bound_w = 1e3
self.control_reg_w = 1e-1
- self.state_reg_w = np.array([1e1] * 3
- + [1e-2] * 3
- + [1e1] * 6
- + [1e1] * 3
- + [3*1e-1] * 3
- + [1e1] * 6
- )
+ self.state_reg_w = np.array(
+ [1e1] * 3 + [1e-2] * 3 + [1e1] * 6 + [1e1] * 3 + [3 * 1e-1] * 3 + [1e1] * 6
+ )
self.terminal_velocity_w = np.array([0] * 12 + [1e3] * 12)
self.q0_reduced = self.q0[7:]
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index a5b84bd6..fbbf86a4 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -27,6 +27,8 @@ class Target:
self.max_height = params.max_height
self.T = self.k_per_step * self.dt_wbc
self.A = np.zeros((6, 3))
+
+ self.update_time = -1
else:
self.target_footstep = self.position + np.array([0.0, 0.0, 0.10])
@@ -61,8 +63,9 @@ class Target:
target = self.initial
k_step = k % self.k_per_step
- if k_step == 0:
+ if n_step != self.update_time:
self.update_polynomial(initial, target)
+ self.update_time = n_step
t = k_step * self.dt_wbc
return self.compute_position(j, t)
diff --git a/python/quadruped_reactive_walking/tools/Utils.py b/python/quadruped_reactive_walking/tools/Utils.py
index 1af8310e..c58f23a0 100644
--- a/python/quadruped_reactive_walking/tools/Utils.py
+++ b/python/quadruped_reactive_walking/tools/Utils.py
@@ -68,3 +68,36 @@ def init_robot(q_init, params):
params.shoulders[3 * i + j] = shoulders_init[j, i]
params.footsteps_init[3 * i + j] = fsteps_init[j, i]
params.footsteps_under_shoulders[3 * i + j] = fsteps_init[j, i]
+
+
+def quaternionToRPY(quat):
+ """Quaternion (4 x 0) to Roll Pitch Yaw (3 x 1)"""
+
+ qx = quat[0]
+ qy = quat[1]
+ qz = quat[2]
+ qw = quat[3]
+
+ rotateXa0 = 2.0 * (qy * qz + qw * qx)
+ rotateXa1 = qw * qw - qx * qx - qy * qy + qz * qz
+ rotateX = 0.0
+
+ if (rotateXa0 != 0.0) and (rotateXa1 != 0.0):
+ rotateX = np.arctan2(rotateXa0, rotateXa1)
+
+ rotateYa0 = -2.0 * (qx * qz - qw * qy)
+ rotateY = 0.0
+ if rotateYa0 >= 1.0:
+ rotateY = np.pi / 2.0
+ elif rotateYa0 <= -1.0:
+ rotateY = -np.pi / 2.0
+ else:
+ rotateY = np.arcsin(rotateYa0)
+
+ rotateZa0 = 2.0 * (qx * qy + qw * qz)
+ rotateZa1 = qw * qw + qx * qx - qy * qy - qz * qz
+ rotateZ = 0.0
+ if (rotateZa0 != 0.0) and (rotateZa1 != 0.0):
+ rotateZ = np.arctan2(rotateZa0, rotateZa1)
+
+ return np.array([[rotateX], [rotateY], [rotateZ]])
diff --git a/src/ComplementaryFilter.cpp b/src/ComplementaryFilter.cpp
new file mode 100644
index 00000000..64190ddd
--- /dev/null
+++ b/src/ComplementaryFilter.cpp
@@ -0,0 +1,37 @@
+#include "qrw/ComplementaryFilter.hpp"
+
+ComplementaryFilter::ComplementaryFilter()
+ : dt_(0.),
+ HighPass_(Vector3::Zero()),
+ LowPass_(Vector3::Zero()),
+ alpha_(Vector3::Zero()),
+ x_(Vector3::Zero()),
+ dx_(Vector3::Zero()),
+ filteredX_(Vector3::Zero()) {}
+
+ComplementaryFilter::ComplementaryFilter(double dt, Vector3 HighPass, Vector3 LowPass)
+ : dt_(dt),
+ HighPass_(HighPass),
+ LowPass_(LowPass),
+ alpha_(Vector3::Zero()),
+ x_(Vector3::Zero()),
+ dx_(Vector3::Zero()),
+ filteredX_(Vector3::Zero()) {}
+
+void ComplementaryFilter::initialize(double dt, Vector3 HighPass, Vector3 LowPass) {
+ dt_ = dt;
+ HighPass_ = HighPass;
+ LowPass_ = LowPass;
+}
+
+Vector3 ComplementaryFilter::compute(Vector3 const& x, Vector3 const& dx, Vector3 const& alpha) {
+ alpha_ = alpha;
+ x_ = x;
+ dx_ = dx;
+
+ HighPass_ = alpha.cwiseProduct(HighPass_ + dx_ * dt_);
+ LowPass_ = alpha.cwiseProduct(LowPass_) + (Vector3::Ones() - alpha).cwiseProduct(x_);
+ filteredX_ = HighPass_ + LowPass_;
+
+ return filteredX_;
+}
\ No newline at end of file
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
new file mode 100644
index 00000000..7485d184
--- /dev/null
+++ b/src/Estimator.cpp
@@ -0,0 +1,283 @@
+#include "qrw/Estimator.hpp"
+
+#include <example-robot-data/path.hpp>
+
+#include "pinocchio/algorithm/compute-all-terms.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+#include "pinocchio/math/rpy.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+
+Estimator::Estimator()
+ : perfectEstimator_(false),
+ solo3D_(false),
+ dt_(0.0),
+ initialized_(false),
+ feetFrames_(Vector4::Zero()),
+ footRadius_(0.155),
+ alphaPos_({0.995, 0.995, 0.9}),
+ alphaVelMax_(1.),
+ alphaVelMin_(0.97),
+ alphaSecurity_(0.),
+ IMUYawOffset_(0.),
+ IMULinearAcceleration_(Vector3::Zero()),
+ IMUAngularVelocity_(Vector3::Zero()),
+ IMURpy_(Vector3::Zero()),
+ IMUQuat_(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0)),
+ qActuators_(Vector12::Zero()),
+ vActuators_(Vector12::Zero()),
+ phaseRemainingDuration_(0),
+ feetStancePhaseDuration_(Vector4::Zero()),
+ feetStatus_(Vector4::Zero()),
+ feetTargets_(Matrix34::Zero()),
+ q_FK_(Vector19::Zero()),
+ v_FK_(Vector18::Zero()),
+ baseVelocityFK_(Vector3::Zero()),
+ basePositionFK_(Vector3::Zero()),
+ b_baseVelocity_(Vector3::Zero()),
+ feetPositionBarycenter_(Vector3::Zero()),
+ qEstimate_(Vector19::Zero()),
+ vEstimate_(Vector18::Zero()),
+ vSecurity_(Vector12::Zero()),
+ windowSize_(0),
+ vFiltered_(Vector6::Zero()),
+ qRef_(Vector18::Zero()),
+ vRef_(Vector18::Zero()),
+ baseVelRef_(Vector6::Zero()),
+ baseAccRef_(Vector6::Zero()),
+ oRh_(Matrix3::Identity()),
+ hRb_(Matrix3::Identity()),
+ oTh_(Vector3::Zero()),
+ h_v_(Vector6::Zero()),
+ h_vFiltered_(Vector6::Zero()) {
+ b_M_IMU_ = pinocchio::SE3(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0), Vector3(0.1163, 0.0, 0.02));
+ q_FK_(6) = 1.0;
+ qEstimate_(6) = 1.0;
+}
+
+void Estimator::initialize(Params& params) {
+ dt_ = params.dt_wbc;
+ perfectEstimator_ = params.perfect_estimator;
+ solo3D_ = params.solo3D;
+
+ // Filtering estimated linear velocity
+ int k_mpc = (int)(std::round(params.dt_mpc / params.dt_wbc));
+ windowSize_ = (int)(k_mpc * params.gait.rows() / params.N_periods);
+ vx_queue_.resize(windowSize_, 0.0); // List full of 0.0
+ vy_queue_.resize(windowSize_, 0.0); // List full of 0.0
+ vz_queue_.resize(windowSize_, 0.0); // List full of 0.0
+
+ // Filtering velocities used for security checks
+ double fc = 6.0;
+ double y = 1 - std::cos(2 * M_PI * 6. * dt_);
+ alphaSecurity_ = -y + std::sqrt(y * y + 2 * y);
+
+ // Initialize Quantities
+ basePositionFK_(2) = params.h_ref;
+ velocityFilter_.initialize(dt_, Vector3::Zero(), Vector3::Zero());
+ positionFilter_.initialize(dt_, Vector3::Zero(), basePositionFK_);
+ qRef_(2, 0) = params.h_ref;
+ qRef_.tail(12) = Vector12(params.q_init.data());
+
+ // Initialize Pinocchio
+ const std::string filename = std::string(EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf");
+ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), velocityModel_, false);
+ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), positionModel_, false);
+ velocityData_ = pinocchio::Data(velocityModel_);
+ positionData_ = pinocchio::Data(positionModel_);
+ pinocchio::computeAllTerms(velocityModel_, velocityData_, qEstimate_, vEstimate_);
+ pinocchio::computeAllTerms(positionModel_, positionData_, qEstimate_, vEstimate_);
+ feetFrames_ << (int)positionModel_.getFrameId("FL_FOOT"), (int)positionModel_.getFrameId("FR_FOOT"),
+ (int)positionModel_.getFrameId("HL_FOOT"), (int)positionModel_.getFrameId("HR_FOOT");
+}
+
+void Estimator::run(MatrixN const& gait, MatrixN const& feetTargets, VectorN const& baseLinearAcceleration,
+ VectorN const& baseAngularVelocity, VectorN const& baseOrientation, VectorN const& q,
+ VectorN const& v, VectorN const& perfectPosition, Vector3 const& b_perfectVelocity) {
+ updatFeetStatus(gait, feetTargets);
+ updateIMUData(baseLinearAcceleration, baseAngularVelocity, baseOrientation, perfectPosition);
+ updateJointData(q, v);
+
+ updateForwardKinematics();
+ computeFeetPositionBarycenter();
+
+ estimateVelocity(b_perfectVelocity);
+ estimatePosition(perfectPosition.head(3));
+
+ filterVelocity();
+
+ vSecurity_ = (1 - alphaSecurity_) * vActuators_ + alphaSecurity_ * vSecurity_;
+}
+
+void Estimator::updateReferenceState(VectorN const& vRef) {
+ // Update reference acceleration and velocities
+ Matrix3 Rz = pinocchio::rpy::rpyToMatrix(0., 0., -baseVelRef_[5] * dt_);
+ baseAccRef_.head(3) = (vRef.head(3) - Rz * baseVelRef_.head(3)) / dt_;
+ baseAccRef_.tail(3) = (vRef.tail(3) - Rz * baseVelRef_.tail(3)) / dt_;
+ baseVelRef_ = vRef;
+
+ // Update position and velocity state vectors
+ Rz = pinocchio::rpy::rpyToMatrix(0., 0., qRef_[5]);
+ vRef_.head(2) = Rz.topLeftCorner(2, 2) * baseVelRef_.head(2);
+ vRef_[5] = baseVelRef_[5];
+ vRef_.tail(12) = vActuators_;
+
+ qRef_.head(2) += vRef_.head(2) * dt_;
+ qRef_[2] = qEstimate_[2];
+ qRef_.segment(3, 2) = IMURpy_.head(2);
+ qRef_[5] += baseVelRef_[5] * dt_;
+ qRef_.tail(12) = qActuators_;
+
+ // Transformation matrices
+ hRb_ = pinocchio::rpy::rpyToMatrix(IMURpy_[0], IMURpy_[1], 0.);
+ oRh_ = pinocchio::rpy::rpyToMatrix(0., 0., qRef_[5]);
+ oTh_.head(2) = qRef_.head(2);
+
+ // Express estimated velocity and filtered estimated velocity in horizontal frame
+ h_v_.head(3) = hRb_ * vEstimate_.head(3);
+ h_v_.tail(3) = hRb_ * vEstimate_.segment(3, 3);
+ h_vFiltered_.head(3) = hRb_ * vFiltered_.head(3);
+ h_vFiltered_.tail(3) = hRb_ * vFiltered_.tail(3);
+}
+
+void Estimator::updatFeetStatus(MatrixN const& gait, MatrixN const& feetTargets) {
+ feetStatus_ = gait.row(0);
+ feetTargets_ = feetTargets;
+
+ feetStancePhaseDuration_ += feetStatus_;
+ feetStancePhaseDuration_ = feetStancePhaseDuration_.cwiseProduct(feetStatus_);
+
+ phaseRemainingDuration_ = 1;
+ while (feetStatus_.isApprox((Vector4)gait.row(phaseRemainingDuration_))) {
+ phaseRemainingDuration_++;
+ }
+}
+
+void Estimator::updateIMUData(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity,
+ Vector3 const& baseOrientation, VectorN const& perfectPosition) {
+ IMULinearAcceleration_ = baseLinearAcceleration;
+ IMUAngularVelocity_ = baseAngularVelocity;
+ IMURpy_ = baseOrientation;
+
+ if (!initialized_) {
+ IMUYawOffset_ = IMURpy_(2);
+ initialized_ = true;
+ }
+ IMURpy_(2) -= IMUYawOffset_;
+
+ if (solo3D_) {
+ IMURpy_.tail(1) = perfectPosition.tail(1);
+ }
+
+ IMUQuat_ = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(IMURpy_(0), IMURpy_(1), IMURpy_(2)));
+}
+
+void Estimator::updateJointData(Vector12 const& q, Vector12 const& v) {
+ qActuators_ = q;
+ vActuators_ = v;
+}
+
+void Estimator::updateForwardKinematics() {
+ q_FK_.tail(12) = qActuators_;
+ v_FK_.tail(12) = vActuators_;
+ q_FK_.segment(3, 4) << 0., 0., 0., 1.;
+ pinocchio::forwardKinematics(velocityModel_, velocityData_, q_FK_, v_FK_);
+
+ q_FK_.segment(3, 4) = IMUQuat_.coeffs();
+ pinocchio::forwardKinematics(positionModel_, positionData_, q_FK_);
+
+ int nContactFeet = 0;
+ Vector3 baseVelocityEstimate = Vector3::Zero();
+ Vector3 basePositionEstimate = Vector3::Zero();
+ for (int foot = 0; foot < 4; foot++) {
+ if (feetStatus_(foot) == 1. && feetStancePhaseDuration_[foot] >= 16) {
+ baseVelocityEstimate += computeBaseVelocityFromFoot(foot);
+ basePositionEstimate += computeBasePositionFromFoot(foot);
+ nContactFeet++;
+ }
+ }
+
+ if (nContactFeet > 0) {
+ baseVelocityFK_ = baseVelocityEstimate / nContactFeet;
+ basePositionFK_ = basePositionEstimate / nContactFeet;
+ }
+}
+
+Vector3 Estimator::computeBaseVelocityFromFoot(int footId) {
+ pinocchio::updateFramePlacement(velocityModel_, velocityData_, feetFrames_[footId]);
+ pinocchio::SE3 contactFrame = velocityData_.oMf[feetFrames_[footId]];
+ Vector3 frameVelocity =
+ pinocchio::getFrameVelocity(velocityModel_, velocityData_, feetFrames_[footId], pinocchio::LOCAL).linear();
+
+ return contactFrame.translation().cross(IMUAngularVelocity_) - contactFrame.rotation() * frameVelocity;
+}
+
+Vector3 Estimator::computeBasePositionFromFoot(int footId) {
+ pinocchio::updateFramePlacement(positionModel_, positionData_, feetFrames_[footId]);
+ Vector3 basePosition = -positionData_.oMf[feetFrames_[footId]].translation();
+ basePosition(0) += footRadius_ * (vActuators_(1 + 3 * footId) + vActuators_(2 + 3 * footId));
+
+ return basePosition;
+}
+
+void Estimator::computeFeetPositionBarycenter() {
+ int nContactFeet = 0;
+ Vector3 feetPositions = Vector3::Zero();
+ for (int j = 0; j < 4; j++) {
+ if (feetStatus_(j) == 1.) {
+ feetPositions += feetTargets_.col(j);
+ nContactFeet++;
+ }
+ }
+ if (nContactFeet > 0) feetPositionBarycenter_ = feetPositions / nContactFeet;
+}
+
+double Estimator::computeAlphaVelocity() {
+ double a = std::ceil(feetStancePhaseDuration_.maxCoeff() * 0.1) - 1;
+ double b = static_cast<double>(phaseRemainingDuration_);
+ double c = ((a + b) - 2) * 0.5;
+ if (a <= 0 || b <= 1)
+ return alphaVelMax_;
+ else
+ return alphaVelMin_ + (alphaVelMax_ - alphaVelMin_) * std::abs(c - (a - 1)) / c;
+}
+
+void Estimator::estimateVelocity(Vector3 const& b_perfectVelocity) {
+ Vector3 alpha = Vector3::Ones() * computeAlphaVelocity();
+ Matrix3 oRb = IMUQuat_.toRotationMatrix();
+ Vector3 bTi = (b_M_IMU_.translation()).cross(IMUAngularVelocity_);
+
+ // At IMU location in world frame
+ Vector3 oi_baseVelocityFK = solo3D_ ? oRb * (b_perfectVelocity + bTi) : oRb * (baseVelocityFK_ + bTi);
+ Vector3 oi_baseVelocity = velocityFilter_.compute(oi_baseVelocityFK, oRb * IMULinearAcceleration_, alpha);
+
+ // At base location in base frame
+ b_baseVelocity_ = oRb.transpose() * oi_baseVelocity - bTi;
+
+ vEstimate_.head(3) = perfectEstimator_ ? b_perfectVelocity : b_baseVelocity_;
+ vEstimate_.segment(3, 3) = IMUAngularVelocity_;
+ vEstimate_.tail(12) = vActuators_;
+}
+
+void Estimator::estimatePosition(Vector3 const& perfectPosition) {
+ Matrix3 oRb = IMUQuat_.toRotationMatrix();
+
+ Vector3 basePosition = solo3D_ ? perfectPosition : (basePositionFK_ + feetPositionBarycenter_);
+ qEstimate_.head(3) = positionFilter_.compute(basePosition, oRb * b_baseVelocity_, alphaPos_);
+
+ if (perfectEstimator_ || solo3D_) qEstimate_(2) = perfectPosition(2);
+ qEstimate_.segment(3, 4) = IMUQuat_.coeffs();
+ qEstimate_.tail(12) = qActuators_;
+}
+
+void Estimator::filterVelocity() {
+ vFiltered_ = vEstimate_.head(6);
+ vx_queue_.pop_back();
+ vy_queue_.pop_back();
+ vz_queue_.pop_back();
+ vx_queue_.push_front(vEstimate_(0));
+ vy_queue_.push_front(vEstimate_(1));
+ vz_queue_.push_front(vEstimate_(2));
+ vFiltered_(0) = std::accumulate(vx_queue_.begin(), vx_queue_.end(), 0.) / windowSize_;
+ vFiltered_(1) = std::accumulate(vy_queue_.begin(), vy_queue_.end(), 0.) / windowSize_;
+ vFiltered_(2) = std::accumulate(vz_queue_.begin(), vz_queue_.end(), 0.) / windowSize_;
+}
diff --git a/src/Filter.cpp b/src/Filter.cpp
new file mode 100644
index 00000000..fac84ef2
--- /dev/null
+++ b/src/Filter.cpp
@@ -0,0 +1,73 @@
+#include "qrw/Filter.hpp"
+
+Filter::Filter()
+ : b_(0.),
+ a_(Vector2::Zero()),
+ x_(Vector6::Zero()),
+ y_(VectorN::Zero(6, 1)),
+ accum_(Vector6::Zero()),
+ init_(false) {
+ // Empty
+}
+
+void Filter::initialize(Params& params) {
+ const double fc = 15.0;
+ b_ = (2 * M_PI * params.dt_wbc * fc) / (2 * M_PI * params.dt_wbc * fc + 1.0);
+
+ a_ << 1.0, -(1.0 - b_);
+
+ x_queue_.resize(1, Vector6::Zero());
+ y_queue_.resize(a_.rows() - 1, Vector6::Zero());
+}
+
+VectorN Filter::filter(Vector6 const& x, bool check_modulo) {
+ // Retrieve measurement
+ x_ = x;
+
+ // Handle modulo for orientation
+ if (check_modulo) {
+ // Handle 2 pi modulo for roll, pitch and yaw
+ // Should happen sometimes for yaw but now for roll and pitch except
+ // if the robot rolls over
+ for (int i = 3; i < 6; i++) {
+ if (std::abs(x_(i, 0) - y_(i, 0)) > 1.5 * M_PI) {
+ handle_modulo(i, x_(i, 0) - y_(i, 0) > 0);
+ }
+ }
+ }
+
+ // Initialisation of the value in the queues to the first measurement
+ if (!init_) {
+ init_ = true;
+ std::fill(x_queue_.begin(), x_queue_.end(), x_.head(6));
+ std::fill(y_queue_.begin(), y_queue_.end(), x_.head(6));
+ }
+
+ // Store measurement in x queue
+ x_queue_.pop_back();
+ x_queue_.push_front(x_.head(6));
+
+ // Compute result (y/x = b/a for the transfert function)
+ accum_ = b_ * x_queue_[0];
+ for (int i = 1; i < a_.rows(); i++) {
+ accum_ -= a_[i] * y_queue_[i - 1];
+ }
+
+ // Store result in y queue for recursion
+ y_queue_.pop_back();
+ y_queue_.push_front(accum_ / a_[0]);
+
+ // 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) {
+ // Add or remove 2 PI to all elements in the queues
+ x_queue_[0](a, 0) += dir ? 2.0 * M_PI : -2.0 * M_PI;
+ for (int i = 1; i < a_.rows(); i++) {
+ (y_queue_[i - 1])(a, 0) += dir ? 2.0 * M_PI : -2.0 * M_PI;
+ }
+}
--
GitLab