From ef52643147b7283abb94044fccfe5957e493512d Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Tue, 13 Jul 2021 17:13:09 +0200
Subject: [PATCH] Debug and test C++ version of Estimator
---
CMakeLists.txt | 2 ++
include/qrw/Estimator.hpp | 28 +++++++++++++++++-------
include/qrw/Types.h | 4 +++-
python/gepadd.cpp | 35 ++++++++++++++++++++++++++++++
scripts/Controller.py | 45 +++++++++++++++++++++++++++++++++++++--
src/Estimator.cpp | 36 +++++++++++++++++++++----------
6 files changed, 128 insertions(+), 22 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1a17b43c..af4f5208 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -57,6 +57,7 @@ set(${PROJECT_NAME}_HEADERS
include/qrw/InvKin.hpp
include/qrw/QPWBC.hpp
include/qrw/Params.hpp
+ include/qrw/Estimator.hpp
include/other/st_to_cc.hpp
)
@@ -71,6 +72,7 @@ set(${PROJECT_NAME}_SOURCES
src/InvKin.cpp
src/QPWBC.cpp
src/Params.cpp
+ src/Estimator.cpp
)
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index 0cf1e874..ad50a3e2 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -37,21 +37,24 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////
~ComplementaryFilter() {} // Empty destructor
-
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Initialize
///
+ /// \param[in] dt time step of the complementary filter
+ /// \param[in] HP_x initial value for the high pass filter
+ /// \param[in] LP_x initial value for the low pass filter
+ ///
////////////////////////////////////////////////////////////////////////////////////////////////
- void initialize(double dt);
+ void initialize(double dt, Vector3 HP_x, Vector3 LP_x);
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Compute the filtered output of the complementary filter
///
- /// \param[in] x (Vector3): quantity handled by the filter
- /// \param[in] dx (Vector3): derivative of the quantity
- /// \param[in] alpha (Vector3): filtering coefficient between x and dx quantities
+ /// \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);
@@ -161,9 +164,14 @@ public:
/// \param[in] b_baseVel velocity of the robot in PyBullet simulator (only for simulation)
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void run_filter(MatrixN4 gait, Matrix34 goals, Vector3 baseLinearAcceleration,
- Vector3 baseAngularVelocity, Vector4 baseOrientation, Vector12 q_mes, Vector12 v_mes,
- Vector3 dummyPos, Vector3 b_baseVel);
+ void run_filter(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& dummyPos, VectorN const& b_baseVel);
+
+ MatrixN getQFilt() { return q_filt_dyn_; }
+ MatrixN getVFilt() { return v_filt_dyn_; }
+ MatrixN getVSecu() { return v_secu_dyn_; }
+ Vector3 getRPY() { return IMU_RPY_; }
private:
@@ -202,6 +210,10 @@ private:
Vector18 v_filt_; // Filtered output velocity
Vector12 v_secu_; // Filtered output velocity for security check
+ MatrixN q_filt_dyn_; // Dynamic size version of q_filt_
+ MatrixN v_filt_dyn_; // Dynamic size version of v_filt_
+ MatrixN v_secu_dyn_; // Dynamic size version of v_secu_
+
pinocchio::SE3 _1Mi_; // Transform between the base frame and the IMU frame
};
diff --git a/include/qrw/Types.h b/include/qrw/Types.h
index c7ad9376..233c80df 100644
--- a/include/qrw/Types.h
+++ b/include/qrw/Types.h
@@ -18,13 +18,15 @@ using Vector6 = Eigen::Matrix<double, 6, 1>;
using Vector7 = Eigen::Matrix<double, 7, 1>;
using Vector11 = Eigen::Matrix<double, 11, 1>;
using Vector12 = Eigen::Matrix<double, 12, 1>;
-using Vector19 = Eigen::Matrix<double, 1, 1>;
+using Vector18 = Eigen::Matrix<double, 18, 1>;
+using Vector19 = Eigen::Matrix<double, 19, 1>;
using VectorN = Eigen::Matrix<double, Eigen::Dynamic, 1>;
using Matrix3 = Eigen::Matrix<double, 3, 3>;
using Matrix4 = Eigen::Matrix<double, 4, 4>;
using Matrix34 = Eigen::Matrix<double, 3, 4>;
using Matrix64 = Eigen::Matrix<double, 6, 4>;
+using MatrixN4 = Eigen::Matrix<double, Eigen::Dynamic, 4>;
using Matrix3N = Eigen::Matrix<double, 3, Eigen::Dynamic>;
using Matrix6N = Eigen::Matrix<double, 6, Eigen::Dynamic>;
using MatrixN = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 018c5378..ebf779f1 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -6,6 +6,7 @@
#include "qrw/FootstepPlanner.hpp"
#include "qrw/FootTrajectoryGenerator.hpp"
#include "qrw/QPWBC.hpp"
+#include "qrw/Estimator.hpp"
#include "qrw/Params.hpp"
#include <boost/python.hpp>
@@ -242,6 +243,39 @@ struct QPWBCPythonVisitor : public bp::def_visitor<QPWBCPythonVisitor<QPWBC>>
};
void exposeQPWBC() { QPWBCPythonVisitor<QPWBC>::expose(); }
+/////////////////////////////////
+/// Binding Estimator class
+/////////////////////////////////
+template <typename Estimator>
+struct EstimatorPythonVisitor : public bp::def_visitor<EstimatorPythonVisitor<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("getQFilt", &Estimator::getQFilt, "Get filtered configuration.\n")
+ .def("getVFilt", &Estimator::getVFilt, "Get filtered velocity.\n")
+ .def("getVSecu", &Estimator::getVSecu, "Get filtered velocity for security check.\n")
+ .def("getRPY", &Estimator::getRPY, "Get Roll Pitch Yaw.\n")
+
+ // Run Estimator from Python
+ .def("run_filter", &Estimator::run_filter, bp::args("gait", "goals", "baseLinearAcceleration",
+ "baseAngularVelocity", "baseOrientation", "q_mes", "v_mes",
+ "dummyPos", "b_baseVel"), "Run Estimator from Python.\n");
+ }
+
+ static void expose()
+ {
+ bp::class_<Estimator>("Estimator", bp::no_init).def(EstimatorPythonVisitor<Estimator>());
+
+ ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
+ }
+};
+void exposeEstimator() { EstimatorPythonVisitor<Estimator>::expose(); }
+
/////////////////////////////////
/// Binding Params class
/////////////////////////////////
@@ -313,5 +347,6 @@ BOOST_PYTHON_MODULE(libquadruped_reactive_walking)
exposeFootTrajectoryGenerator();
exposeInvKin();
exposeQPWBC();
+ exposeEstimator();
exposeParams();
}
\ No newline at end of file
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 0bc45948..ef8d7a46 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -131,6 +131,9 @@ class Controller:
self.footTrajectoryGenerator = lqrw.FootTrajectoryGenerator()
self.footTrajectoryGenerator.initialize(params, self.gait)
+ self.estimator_bis = lqrw.Estimator()
+ self.estimator_bis.initialize(params)
+
# Wrapper that makes the link with the solver that you want to use for the MPC
self.mpc_wrapper = MPC_Wrapper.MPC_Wrapper(params, self.q)
@@ -199,9 +202,47 @@ class Controller:
# Update the reference velocity coming from the gamepad
self.joystick.update_v_ref(self.k, self.velID)
+ self.estimator_bis.run_filter(self.gait.getCurrentGait(),
+ self.footTrajectoryGenerator.getFootPosition(),
+ device.baseLinearAcceleration.reshape((-1, 1)),
+ device.baseAngularVelocity.reshape((-1, 1)),
+ device.baseOrientation.reshape((-1, 1)),
+ device.q_mes.reshape((-1, 1)),
+ device.v_mes.reshape((-1, 1)),
+ device.dummyPos.reshape((-1, 1)),
+ device.b_baseVel.reshape((-1, 1)))
+
# Process state estimator
- self.estimator.run_filter(self.k, self.gait.getCurrentGait(),
- device, self.footTrajectoryGenerator.getFootPosition())
+ """self.estimator.run_filter(self.k, self.gait.getCurrentGait(),
+ device, self.footTrajectoryGenerator.getFootPosition())"""
+ """, self.estimator_bis.getVFilt()[0:3]),
+ self.estimator_bis.debug2(), self.estimator_bis.debug3(), self.estimator_bis.debug4(), self.estimator_bis.debug5(),
+ self.estimator_bis.debug6(), self.estimator_bis.debug7(),
+ self.estimator_bis.debug8(), self.estimator_bis.debug9())"""
+
+ self.estimator.q_filt[:, 0] = self.estimator_bis.getQFilt()
+ self.estimator.v_filt[:, 0] = self.estimator_bis.getVFilt()
+ self.estimator.v_secu[:] = self.estimator_bis.getVSecu()
+ self.estimator.RPY = self.estimator_bis.getRPY()
+
+ """from IPython import embed
+ embed()"""
+
+ """if self.k % 10 == 0:
+ print("q_filt: ", np.allclose(self.estimator.q_filt.ravel(), self.estimator_bis.getQFilt()))
+ print("v_filt: ", np.allclose(self.estimator.v_filt.ravel(), self.estimator_bis.getVFilt()))
+ print("v_secu: ", np.allclose(self.estimator.v_secu.ravel(), self.estimator_bis.getVSecu()))
+ print("RPY: ", np.allclose(self.estimator.RPY.ravel(), self.estimator_bis.getRPY()))
+
+ from IPython import embed
+ embed()"""
+ """if self.k == 50:
+ print("q_filt: ", self.estimator.q_filt.ravel())
+ print("v_filt: ", self.estimator.v_filt.ravel())
+ print("v_secu: ", self.estimator.v_secu.ravel())
+ print("RPY: ", self.estimator.RPY.ravel())
+ from IPython import embed
+ embed()"""
t_filter = time.time()
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 167ddf7c..14c8b84f 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -14,9 +14,11 @@ ComplementaryFilter::ComplementaryFilter()
}
-void ComplementaryFilter::initialize(double dt)
+void ComplementaryFilter::initialize(double dt, Vector3 HP_x, Vector3 LP_x)
{
dt_ = dt;
+ HP_x_ = HP_x;
+ LP_x_ = LP_x;
}
@@ -65,6 +67,9 @@ Estimator::Estimator()
, q_filt_(Vector19::Zero())
, v_filt_(Vector18::Zero())
, v_secu_(Vector12::Zero())
+ , q_filt_dyn_(MatrixN::Zero(19, 1))
+ , v_filt_dyn_(MatrixN::Zero(18, 1))
+ , v_secu_dyn_(MatrixN::Zero(12, 1))
{
}
@@ -85,13 +90,16 @@ void Estimator::initialize(Params& params)
y = 1 - std::cos(2 * M_PI * fc * dt_wbc);
alpha_secu_ = -y + std::sqrt(y * y + 2 * y);
- filter_xyz_vel_.initialize(dt_wbc);
- filter_xyz_pos_.initialize(dt_wbc);
+ FK_xyz_(2, 0) = params.h_ref;
+
+ filter_xyz_vel_.initialize(dt_wbc, Vector3::Zero(), Vector3::Zero());
+ filter_xyz_pos_.initialize(dt_wbc, Vector3::Zero(), FK_xyz_);
_1Mi_ = pinocchio::SE3(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0), Vector3(0.1163, 0.0, 0.02));
q_FK_(6, 0) = 1.0; // Last term of the quaternion
q_filt_(6, 0) = 1.0; // Last term of the quaternion
+ q_filt_dyn_(6, 0) = 1.0; // Last term of the quaternion
// Path to the robot URDF (TODO: Automatic path)
const std::string filename = std::string("/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf");
@@ -244,12 +252,12 @@ Vector3 Estimator::BaseVelocityFromKinAndIMU(int contactFrameId)
}
-void Estimator::run_filter(MatrixN4 gait, Matrix34 goals, Vector3 baseLinearAcceleration,
- Vector3 baseAngularVelocity, Vector4 baseOrientation, Vector12 q_mes, Vector12 v_mes,
- Vector3 dummyPos, Vector3 b_baseVel)
+void Estimator::run_filter(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& dummyPos, VectorN const& b_baseVel)
{
int remaining_steps = 1; // Remaining MPC steps for the current gait phase
- while((gait.row(0)).isApprox(gait.row(remaining_steps))) {
+ while((gait.block(0, 0, 1, 4)).isApprox(gait.row(remaining_steps))) {
remaining_steps++;
}
@@ -266,14 +274,14 @@ void Estimator::run_filter(MatrixN4 gait, Matrix34 goals, Vector3 baseLinearAcce
get_data_joints(q_mes, v_mes);
// Update nb of iterations since contact
- k_since_contact_ += gait.row(0); // Increment feet in stance phase
- k_since_contact_ = k_since_contact_.cwiseProduct(gait.row(0)); // Reset feet in swing phase
+ k_since_contact_ += gait.block(0, 0, 1, 4); // Increment feet in stance phase
+ k_since_contact_ = k_since_contact_.cwiseProduct(gait.block(0, 0, 1, 4)); // Reset feet in swing phase
// Update forward kinematics data
- get_data_FK(gait.row(0));
+ get_data_FK(gait.block(0, 0, 1, 4));
// Update forward geometry data
- get_xyz_feet(gait.row(0), goals);
+ get_xyz_feet(gait.block(0, 0, 1, 4), goals);
// Tune alpha depending on the state of the gait (close to contact switch or not)
double a = std::ceil(k_since_contact_.maxCoeff() * 0.1) - 1;
@@ -372,6 +380,12 @@ void Estimator::run_filter(MatrixN4 gait, Matrix34 goals, Vector3 baseLinearAcce
// Output filtered actuators velocity for security checks
v_secu_ = (1 - alpha_secu_) * actuators_vel_ + alpha_secu_ * v_secu_;
+ // Copy data to dynamic sized matrices since Python converters for big sized fixed matrices do not exist
+ // TODO: Find a way to cast a fixed size eigen matrix as dynamic size to remove the need for those variables
+ q_filt_dyn_ = q_filt_;
+ v_filt_dyn_ = v_filt_;
+ v_secu_dyn_ = v_secu_;
+
// Increment iteration counter
k_log_++;
}
--
GitLab