diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index 8cb1dfd2aec683a14542682c9082851f77cb9265..aba8f7cd68c53cdef774c8da84360c0135c9dd76 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -19,6 +19,7 @@
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
+#include <boost/circular_buffer.hpp>
class ComplementaryFilter
{
@@ -194,6 +195,7 @@ public:
VectorN getQFilt() { return q_filt_dyn_; }
VectorN getVFilt() { return v_filt_dyn_; }
VectorN getVSecu() { return v_secu_dyn_; }
+ Vector3 getVFiltBis() { return v_filt_bis_; }
Vector3 getRPY() { return IMU_RPY_; }
MatrixN getFeetStatus() { return feet_status_;}
MatrixN getFeetGoals() { return feet_goals_; }
@@ -215,6 +217,7 @@ public:
VectorN getQUpdated() { return q_up_; }
VectorN getVRef() { return v_ref_; }
VectorN getHV() { return h_v_; }
+ Vector3 getHVBis() { return h_v_bis_; }
Matrix3 getoRh() { return oRh_; }
Vector3 getoTh() { return oTh_; }
double getYawEstim() { return yaw_estim_; }
@@ -274,5 +277,11 @@ private:
Matrix3 oRh_; // Rotation between horizontal and world frame
Vector3 oTh_; // Translation between horizontal and world frame
double yaw_estim_; // Yaw angle in perfect world
+
+ int N_queue_;
+ Vector3 v_filt_bis_;
+ Vector3 h_v_bis_;
+ boost::circular_buffer<double> vx_queue_, vy_queue_, vz_queue_;
+
};
#endif // ESTIMATOR_H_INCLUDED
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index aa29366cc3af505bc9e6e058ae085a90da5b5cfb..0635b1a2bd33aec0da0545dd23864617bd903693 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -305,6 +305,7 @@ struct EstimatorPythonVisitor : public bp::def_visitor<EstimatorPythonVisitor<Es
.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("getVFiltBis", &Estimator::getVFiltBis, "Get filtered velocity.\n")
.def("getRPY", &Estimator::getRPY, "Get Roll Pitch Yaw.\n")
.def("getFeetStatus", &Estimator::getFeetStatus, "")
.def("getFeetGoals", &Estimator::getFeetGoals, "")
@@ -323,6 +324,7 @@ struct EstimatorPythonVisitor : public bp::def_visitor<EstimatorPythonVisitor<Es
.def("getQUpdated", &Estimator::getQUpdated, "")
.def("getVRef", &Estimator::getVRef, "")
.def("getHV", &Estimator::getHV, "")
+ .def("getHVBis", &Estimator::getHVBis, "")
.def("getoRh", &Estimator::getoRh, "")
.def("getoTh", &Estimator::getoTh, "")
.def("getYawEstim", &Estimator::getYawEstim, "")
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 3495a5d585ddbd252ba3b288ee34981279dcb7d8..826382ea506ab7335b6c7047d2af38a7541b3990 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -187,6 +187,7 @@ class Controller:
self.v_ref = np.zeros((18, 1))
self.h_v = np.zeros((18, 1))
+ self.h_v_bis = np.zeros((6, 1))
self.yaw_estim = 0.0
self.RPY_filt = np.zeros(3)
@@ -235,6 +236,8 @@ class Controller:
oTh = self.estimator.getoTh().reshape((3, 1))
self.v_ref[0:6, 0] = self.estimator.getVRef()
self.h_v[0:6, 0] = self.estimator.getHV()
+ self.h_v_bis[0:3, 0] = self.estimator.getHVBis()
+ self.h_v_bis[3:6, 0] = self.h_v[3:6, 0].copy()
self.q[:, 0] = self.estimator.getQUpdated()
self.yaw_estim = self.estimator.getYawEstim()
# TODO: Understand why using Python or C++ h_v leads to a slightly different result since the
@@ -249,7 +252,7 @@ class Controller:
o_targetFootstep = self.footstepPlanner.updateFootsteps(self.k % self.k_mpc == 0 and self.k != 0,
int(self.k_mpc - self.k % self.k_mpc),
self.q[:, 0:1],
- self.h_v[0:6, 0:1].copy(),
+ self.h_v_bis[0:6, 0:1].copy(),
self.v_ref[0:6, 0])
# Run state planner (outputs the reference trajectory of the base)
@@ -263,6 +266,8 @@ class Controller:
t_planner = time.time()
+ #Â TODO: Add 25Hz filter for the inputs of the MPC
+
# Solve MPC problem once every k_mpc iterations of the main loop
if (self.k % self.k_mpc) == 0:
try:
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 347aed23d72d24472863caf3a6ff289dd9e4daa8..6a39d791806c43c2ec1abb2eb25904cbf5a94848 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -47,6 +47,7 @@ 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_pos_virtual_world = np.zeros([logSize, 3]) # x, y, yaw perfect position in world
# Gait
@@ -131,6 +132,7 @@ 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_pos_virtual_world[self.i] = np.array([loop.q[0, 0], loop.q[1, 0], loop.yaw_estim])
# Logging from the planner
@@ -182,7 +184,7 @@ class LoggerControl():
self.mocap_b_v[i] = (oRb.transpose() @ loggerSensors.mocapVelocity[i].reshape((3, 1))).ravel()
self.mocap_b_w[i] = (oRb.transpose() @ loggerSensors.mocapAngularVelocity[i].reshape((3, 1))).ravel()
- self.mocap_RPY[i] = pin.rpy.matrixToRpy(pin.Quaternion(loggerSensors.mocapOrientationQuat[i]).toRotationMatrix())[:, 0]
+ self.mocap_RPY[i] = pin.rpy.matrixToRpy(pin.Quaternion(loggerSensors.mocapOrientationQuat[i]).toRotationMatrix())
def plotAll(self, loggerSensors):
@@ -297,6 +299,7 @@ class LoggerControl():
plt.plot(t_range, self.joy_v_ref[:, i], "r", linewidth=3)
if i < 3:
plt.plot(t_range, self.mocap_b_v[:, i], "k", linewidth=3)
+ plt.plot(t_range, self.loop_h_v_bis[:, i], "forestgreen", linewidth=2)
# plt.plot(t_range, self.esti_FK_lin_vel[:, i], "violet", linewidth=3, linestyle="--")
plt.plot(t_range, self.esti_filt_lin_vel[:, i], "violet", linewidth=3, linestyle="--")
else:
@@ -309,7 +312,7 @@ class LoggerControl():
# plt.plot(t_range, self.log_dq[i, :], "g", linewidth=2)
# plt.plot(t_range[:-2], self.log_dx_invkin[i, :-2], "g", linewidth=2)
# plt.plot(t_range[:-2], self.log_dx_ref_invkin[i, :-2], "violet", linewidth=2, linestyle="--")
- plt.legend(["Robot state", "Robot reference state", "Ground truth"], prop={'size': 8})
+ plt.legend(["Robot state", "Robot reference state", "Ground truth", "Alternative"], prop={'size': 8})
plt.ylabel(lgd[i])
plt.suptitle("Measured & Reference linear and angular velocities")
diff --git a/scripts/LoggerSensors.py b/scripts/LoggerSensors.py
index cc1a7a8185db5b71f0bf75a2d0669137a0c49f01..c2aaf2bac93301316c4011f47efd9681bc111bd7 100644
--- a/scripts/LoggerSensors.py
+++ b/scripts/LoggerSensors.py
@@ -45,10 +45,10 @@ class LoggerSensors():
# Logging from the device (data coming from the robot)
self.q_mes[self.i] = device.joints.positions
self.v_mes[self.i] = device.joints.velocities
- self.baseOrientation[self.i] = device.imu.attitude_euler[:, 0]
- self.baseAngularVelocity[self.i] = device.imu.gyroscope[:, 0]
- self.baseLinearAcceleration[self.i] = device.imu.linear_acceleration[:, 0]
- self.baseAccelerometer[self.i] = device.imu.accelerometer[:, 0]
+ self.baseOrientation[self.i] = device.imu.attitude_euler
+ self.baseAngularVelocity[self.i] = device.imu.gyroscope
+ self.baseLinearAcceleration[self.i] = device.imu.linear_acceleration
+ self.baseAccelerometer[self.i] = device.imu.accelerometer
self.torquesFromCurrentMeasurment[self.i] = device.joints.measured_torques
# Logging from qualisys (motion capture)
diff --git a/scripts/PyBulletSimulator.py b/scripts/PyBulletSimulator.py
index 715e3ef254cd0fd452f8a66f3cb73e77d526f2c1..a640dc0d14e759835e57054a763d9f26aa0b7676 100644
--- a/scripts/PyBulletSimulator.py
+++ b/scripts/PyBulletSimulator.py
@@ -526,10 +526,10 @@ class IMU():
"""Dummy class that simulates the IMU class used to communicate with the real masterboard"""
def __init__(self):
- self.linear_acceleration = np.zeros((3, 1))
- self.accelerometer = np.zeros((3, 1))
- self.gyroscope = np.zeros((3, 1))
- self.attitude_euler = np.zeros((3, 1))
+ self.linear_acceleration = np.zeros((3, ))
+ self.accelerometer = np.zeros((3, ))
+ self.gyroscope = np.zeros((3, ))
+ self.attitude_euler = np.zeros((3, ))
self.attitude_quaternion = np.array([[0.0, 0.0, 0.0, 1.0]]).transpose()
class Joints():
@@ -677,23 +677,23 @@ class PyBulletSimulator():
# Orientation of the base (quaternion)
self.imu.attitude_quaternion[:, 0] = np.array(self.baseState[1])
- self.imu.attitude_euler[:, 0] = pin.rpy.matrixToRpy(pin.Quaternion(self.imu.attitude_quaternion).toRotationMatrix())
+ self.imu.attitude_euler[:] = pin.rpy.matrixToRpy(pin.Quaternion(self.imu.attitude_quaternion).toRotationMatrix())
self.rot_oMb = pin.Quaternion(self.imu.attitude_quaternion).toRotationMatrix()
self.oMb = pin.SE3(self.rot_oMb, np.array([self.dummyHeight]).transpose())
# Angular velocities of the base
- self.imu.gyroscope[:] = (self.oMb.rotation.transpose() @ np.array([self.baseVel[1]]).transpose())
+ self.imu.gyroscope[:] = (self.oMb.rotation.transpose() @ np.array([self.baseVel[1]]).transpose()).ravel()
# Linear Acceleration of the base
self.o_baseVel = np.array([self.baseVel[0]]).transpose()
self.b_baseVel = (self.oMb.rotation.transpose() @ self.o_baseVel).ravel()
- self.o_imuVel = self.o_baseVel + self.oMb.rotation @ self.cross3(np.array([0.1163, 0.0, 0.02]), self.imu.gyroscope[:, 0])
+ self.o_imuVel = self.o_baseVel + self.oMb.rotation @ self.cross3(np.array([0.1163, 0.0, 0.02]), self.imu.gyroscope[:])
- self.imu.linear_acceleration[:] = (self.oMb.rotation.transpose() @ (self.o_imuVel - self.prev_o_imuVel)) / self.dt
+ self.imu.linear_acceleration[:] = (self.oMb.rotation.transpose() @ (self.o_imuVel - self.prev_o_imuVel)).ravel() / self.dt
self.prev_o_imuVel[:, 0:1] = self.o_imuVel
self.imu.accelerometer[:] = self.imu.linear_acceleration + \
- (self.oMb.rotation.transpose() @ np.array([[0.0], [0.0], [-9.81]]))
+ (self.oMb.rotation.transpose() @ np.array([[0.0], [0.0], [-9.81]])).ravel()
return
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 0791ec7171139b5468d4c9303e8bca26fd69103a..e934873b85e0d64cfe1f6e1c6dd07ac14d37f6a3 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -81,6 +81,9 @@ Estimator::Estimator()
, oRh_(Matrix3::Identity())
, oTh_(Vector3::Zero())
, yaw_estim_(0.0)
+ , N_queue_(0)
+ , v_filt_bis_(Vector3::Zero())
+ , h_v_bis_(Vector3::Zero())
{
}
@@ -96,6 +99,11 @@ void Estimator::initialize(Params& params)
double y = 1 - std::cos(2 * M_PI * fc * dt_wbc);
alpha_v_ = -y + std::sqrt(y * y + 2 * y);
+ N_queue_ = static_cast<int>(std::round(1.0/(dt_wbc * 3.0)));
+ vx_queue_.resize(N_queue_, 0.0); // Buffer full of 0.0
+ vy_queue_.resize(N_queue_, 0.0); // Buffer full of 0.0
+ vz_queue_.resize(N_queue_, 0.0); // Buffer full of 0.0
+
// Filtering velocities used for security checks
fc = 6.0; // Cut frequency
y = 1 - std::cos(2 * M_PI * fc * dt_wbc);
@@ -368,6 +376,13 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
v_filt_.block(3, 0, 3, 1) = filt_ang_vel; // Angular velocities are already directly from PyBullet
v_filt_.tail(12) = actuators_vel_; // Actuators velocities are already directly from PyBullet
+ vx_queue_.push_back(b_filt_lin_vel_(0));
+ vy_queue_.push_back(b_filt_lin_vel_(1));
+ vz_queue_.push_back(b_filt_lin_vel_(2));
+ v_filt_bis_(0) = std::accumulate(vx_queue_.begin(), vx_queue_.end(), 0.0) / N_queue_;
+ v_filt_bis_(1) = std::accumulate(vy_queue_.begin(), vy_queue_.end(), 0.0) / N_queue_;
+ v_filt_bis_(2) = std::accumulate(vz_queue_.begin(), vz_queue_.end(), 0.0) / N_queue_;
+
//////
// Update model used for the forward kinematics
@@ -440,6 +455,7 @@ void Estimator::updateState(VectorN const& joystick_v_ref, Gait& gait)
h_v_.head(3) = hRb * v_filt_dyn_.block(0, 0, 3, 1);
h_v_.tail(3) = hRb * v_filt_dyn_.block(3, 0, 3, 1);
+ h_v_bis_ = hRb * v_filt_bis_;
}
else
{