From c22e0f2c7eba67a9c7686550efed2b8a91df8f4e Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Tue, 13 Jul 2021 18:15:02 +0200
Subject: [PATCH] Replace python estimator by C++ version + Adapt logger for
new estimator
---
include/qrw/Estimator.hpp | 30 ++++++++++++---
python/gepadd.cpp | 14 +++++++
scripts/Controller.py | 79 +++++++++++----------------------------
scripts/LoggerControl.py | 43 ++++++++++-----------
scripts/utils_mpc.py | 6 +--
src/Estimator.cpp | 44 ++++++++++------------
6 files changed, 101 insertions(+), 115 deletions(-)
diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index ad50a3e2..254400dc 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -63,12 +63,13 @@ public:
Vector3 getDX() { return dx_; } ///< Get the derivative of the input quantity
Vector3 getHpX() { return HP_x_; } ///< Get the high-passed internal quantity
Vector3 getLpX() { return LP_x_; } ///< Get the low-passed internal quantity
+ Vector3 getAlpha() {return alpha_; } ///< Get the alpha coefficient of the filter
Vector3 getFiltX() { return filt_x_; } ///< Get the filtered output
private:
double dt_;
- Vector3 x_, dx_, HP_x_, LP_x_, filt_x_;
+ Vector3 x_, dx_, HP_x_, LP_x_, alpha_, filt_x_;
};
@@ -108,7 +109,7 @@ public:
/// \param[in] baseOrientation Quaternion orientation of the IMU
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void get_data_IMU(Vector3 baseLinearAcceleration, Vector3 baseAngularVelocity, Vector4 baseOrientation);
+ void get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity, Vector4 const& baseOrientation);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -118,7 +119,7 @@ public:
/// \param[in] v_mes velocity of the 12 actuators
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void get_data_joints(Vector12 q_mes, Vector12 v_mes);
+ void get_data_joints(Vector12 const& q_mes, Vector12 const& v_mes);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -127,7 +128,7 @@ public:
/// \param[in] feet_status contact status of the four feet
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void get_data_FK(Eigen::Matrix<double, 1, 4> feet_status);
+ void get_data_FK(Eigen::Matrix<double, 1, 4> const& feet_status);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -137,7 +138,7 @@ public:
/// \param[in] goals target positions of the four feet
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void get_xyz_feet(Eigen::Matrix<double, 1, 4> feet_status, Matrix34 goals);
+ void get_xyz_feet(Eigen::Matrix<double, 1, 4> const& feet_status, Matrix34 const& goals);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -172,6 +173,22 @@ public:
MatrixN getVFilt() { return v_filt_dyn_; }
MatrixN getVSecu() { return v_secu_dyn_; }
Vector3 getRPY() { return IMU_RPY_; }
+ MatrixN getFeetStatus() { return feet_status_;}
+ MatrixN getFeetGoals() { return feet_goals_; }
+ Vector3 getFKLinVel() { return FK_lin_vel_; }
+ Vector3 getFKXYZ() { return FK_xyz_; }
+ Vector3 getXYZMeanFeet() { return xyz_mean_feet_; }
+ Vector3 getFiltLinVel() { return b_filt_lin_vel_; }
+
+ Vector3 getFilterVelX() { return filter_xyz_vel_.getX(); }
+ Vector3 getFilterVelDX() { return filter_xyz_vel_.getDX(); }
+ Vector3 getFilterVelAlpha() { return filter_xyz_vel_.getAlpha(); }
+ Vector3 getFilterVelFiltX() { return filter_xyz_vel_.getFiltX(); }
+
+ Vector3 getFilterPosX() { return filter_xyz_pos_.getX(); }
+ Vector3 getFilterPosDX() { return filter_xyz_pos_.getDX(); }
+ Vector3 getFilterPosAlpha() { return filter_xyz_pos_.getAlpha(); }
+ Vector3 getFilterPosFiltX() { return filter_xyz_pos_.getFiltX(); }
private:
@@ -195,8 +212,11 @@ private:
Vector19 q_FK_; // Configuration vector for Forward Kinematics
Vector18 v_FK_; // Velocity vector for Forward Kinematics
+ MatrixN feet_status_; // Contact status of the four feet
+ MatrixN feet_goals_; // Target positions of the four feet
Vector3 FK_lin_vel_; // Base linear velocity estimated by Forward Kinematics
Vector3 FK_xyz_; // Base position estimated by Forward Kinematics
+ Vector3 b_filt_lin_vel_; // Filtered estimated velocity at center base (base frame)
Vector3 xyz_mean_feet_; // Barycenter of feet in contact
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index ebf779f1..5d5bd30d 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -260,6 +260,20 @@ struct EstimatorPythonVisitor : public bp::def_visitor<EstimatorPythonVisitor<Es
.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")
+ .def("getFeetStatus", &Estimator::getFeetStatus, "")
+ .def("getFeetGoals", &Estimator::getFeetGoals, "")
+ .def("getFKLinVel", &Estimator::getFKLinVel, "")
+ .def("getFKXYZ", &Estimator::getFKXYZ, "")
+ .def("getXYZMeanFeet", &Estimator::getXYZMeanFeet, "")
+ .def("getFiltLinVel", &Estimator::getFiltLinVel, "")
+ .def("getFilterVelX", &Estimator::getFilterVelX, "")
+ .def("getFilterVelDX", &Estimator::getFilterVelDX, "")
+ .def("getFilterVelAlpha", &Estimator::getFilterVelAlpha, "")
+ .def("getFilterVelFiltX", &Estimator::getFilterVelFiltX, "")
+ .def("getFilterPosX", &Estimator::getFilterPosX, "")
+ .def("getFilterPosDX", &Estimator::getFilterPosDX, "")
+ .def("getFilterPosAlpha", &Estimator::getFilterPosAlpha, "")
+ .def("getFilterPosFiltX", &Estimator::getFilterPosFiltX, "")
// Run Estimator from Python
.def("run_filter", &Estimator::run_filter, bp::args("gait", "goals", "baseLinearAcceleration",
diff --git a/scripts/Controller.py b/scripts/Controller.py
index ef8d7a46..3f906b34 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -105,8 +105,8 @@ class Controller:
# Initialisation of the solo model/data and of the Gepetto viewer
self.solo = utils_mpc.init_robot(q_init, params, self.enable_gepetto_viewer)
- # Create Joystick, FootstepPlanner, Logger and Interface objects
- self.joystick, self.estimator = utils_mpc.init_objects(params)
+ # Create Joystick object
+ self.joystick = utils_mpc.init_objects(params)
# Enable/Disable hybrid control
self.enable_hybrid_control = True
@@ -131,8 +131,8 @@ class Controller:
self.footTrajectoryGenerator = lqrw.FootTrajectoryGenerator()
self.footTrajectoryGenerator.initialize(params, self.gait)
- self.estimator_bis = lqrw.Estimator()
- self.estimator_bis.initialize(params)
+ self.estimator = lqrw.Estimator()
+ self.estimator.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)
@@ -202,47 +202,16 @@ 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_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()"""
+ self.estimator.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)))
t_filter = time.time()
@@ -385,14 +354,14 @@ class Controller:
def security_check(self):
if (self.error_flag == 0) and (not self.myController.error) and (not self.joystick.stop):
- if np.any(np.abs(self.estimator.q_filt[7:, 0]) > self.q_security):
+ if np.any(np.abs(self.estimator.getQFilt()[7:]) > self.q_security):
self.myController.error = True
self.error_flag = 1
- self.error_value = self.estimator.q_filt[7:, 0] * 180 / 3.1415
- if np.any(np.abs(self.estimator.v_secu) > 50):
+ self.error_value = self.estimator.getQFilt()[7:] * 180 / 3.1415
+ if np.any(np.abs(self.estimator.getVSecu()) > 50):
self.myController.error = True
self.error_flag = 2
- self.error_value = self.estimator.v_secu
+ self.error_value = self.estimator.getVSecu()
if np.any(np.abs(self.myController.tau_ff) > 8):
print(self.result.P)
print(self.result.D)
@@ -415,10 +384,6 @@ class Controller:
def log_misc(self, tic, t_filter, t_planner, t_mpc, t_wbc):
- # Log joystick command
- if self.joystick is not None:
- self.estimator.v_ref = self.joystick.v_ref
-
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
@@ -443,18 +408,18 @@ class Controller:
self.q[0:2, 0:1] = self.q[0:2, 0:1] + Ryaw @ self.v_ref[0:2, 0:1] * self.myController.dt
# Mix perfect x and y with height measurement
- self.q[2, 0] = self.estimator.q_filt[2, 0]
+ self.q[2, 0] = self.estimator.getQFilt()[2]
# Mix perfect yaw with pitch and roll measurements
self.yaw_estim += self.v_ref[5, 0] * self.myController.dt
- self.q[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(self.estimator.RPY[0], self.estimator.RPY[1], self.yaw_estim)).coeffs()
+ self.q[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(self.estimator.getRPY()[0], self.estimator.getRPY()[1], self.yaw_estim)).coeffs()
# Actuators measurements
- self.q[7:, 0] = self.estimator.q_filt[7:, 0]
+ self.q[7:, 0] = self.estimator.getQFilt()[7:]
# Velocities are the one estimated by the estimator
- self.v = self.estimator.v_filt.copy()
- hRb = pin.rpy.rpyToMatrix(self.estimator.RPY[0], self.estimator.RPY[1], 0.0)
+ self.v = self.estimator.getVFilt().reshape((-1, 1))
+ hRb = pin.rpy.rpyToMatrix(self.estimator.getRPY()[0], self.estimator.getRPY()[1], 0.0)
self.h_v[0:3, 0:1] = hRb @ self.v[0:3, 0:1]
self.h_v[3:6, 0:1] = hRb @ self.v[3:6, 0:1]
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index af1a27f8..a22fcffa 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -109,29 +109,26 @@ class LoggerControl():
self.joy_v_ref[self.i] = joystick.v_ref[:, 0]
# Logging from estimator
- self.esti_feet_status[self.i] = estimator.feet_status[:]
- self.esti_feet_goals[self.i] = estimator.feet_goals
- self.esti_q_filt[self.i] = estimator.q_filt[:, 0]
- self.esti_v_filt[self.i] = estimator.v_filt[:, 0]
- self.esti_v_secu[self.i] = estimator.v_secu[:]
-
- self.esti_FK_lin_vel[self.i] = estimator.FK_lin_vel[:]
- self.esti_FK_xyz[self.i] = estimator.FK_xyz[:]
- self.esti_xyz_mean_feet[self.i] = estimator.xyz_mean_feet[:]
- self.esti_filt_lin_vel[self.i] = estimator.filt_lin_vel[:]
- if not estimator.kf_enabled:
- self.esti_HP_x[self.i] = estimator.filter_xyz_vel.x
- self.esti_HP_dx[self.i] = estimator.filter_xyz_vel.dx
- self.esti_HP_alpha[self.i] = estimator.filter_xyz_vel.alpha
- self.esti_HP_filt_x[self.i] = estimator.filter_xyz_vel.filt_x
-
- self.esti_LP_x[self.i] = estimator.filter_xyz_pos.x
- self.esti_LP_dx[self.i] = estimator.filter_xyz_pos.dx
- self.esti_LP_alpha[self.i] = estimator.filter_xyz_pos.alpha
- self.esti_LP_filt_x[self.i] = estimator.filter_xyz_pos.filt_x
- else:
- self.esti_kf_X[self.i] = estimator.kf.X[:, 0]
- self.esti_kf_Z[self.i] = estimator.Z[:, 0]
+ self.esti_feet_status[self.i] = estimator.getFeetStatus()
+ self.esti_feet_goals[self.i] = estimator.getFeetGoals()
+ self.esti_q_filt[self.i] = estimator.getQFilt()
+ self.esti_v_filt[self.i] = estimator.getVFilt()
+ self.esti_v_secu[self.i] = estimator.getVSecu()
+
+ self.esti_FK_lin_vel[self.i] = estimator.getFKLinVel()
+ self.esti_FK_xyz[self.i] = estimator.getFKXYZ()
+ self.esti_xyz_mean_feet[self.i] = estimator.getXYZMeanFeet()
+ self.esti_filt_lin_vel[self.i] = estimator.getFiltLinVel()
+
+ self.esti_HP_x[self.i] = estimator.getFilterVelX()
+ self.esti_HP_dx[self.i] = estimator.getFilterVelDX()
+ self.esti_HP_alpha[self.i] = estimator.getFilterVelAlpha()
+ self.esti_HP_filt_x[self.i] = estimator.getFilterVelFiltX()
+
+ self.esti_LP_x[self.i] = estimator.getFilterPosX()
+ self.esti_LP_dx[self.i] = estimator.getFilterPosDX()
+ self.esti_LP_alpha[self.i] = estimator.getFilterPosAlpha()
+ self.esti_LP_filt_x[self.i] = estimator.getFilterPosFiltX()
# Logging from the main loop
self.loop_o_q_int[self.i] = loop.q[:, 0]
diff --git a/scripts/utils_mpc.py b/scripts/utils_mpc.py
index 43a87308..563cae75 100644
--- a/scripts/utils_mpc.py
+++ b/scripts/utils_mpc.py
@@ -201,11 +201,7 @@ def init_objects(params):
# Create Joystick object
joystick = Joystick.Joystick(params.predefined_vel)
- # Create Estimator object
- estimator = Estimator.Estimator(params.dt_wbc, params.N_SIMULATION, params.h_ref, params.kf_enabled,
- params.perfect_estimator)
-
- return joystick, estimator
+ return joystick
def display_all(solo, k, sequencer, fstep_planner, ftraj_gen, mpc):
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 14c8b84f..3bb1b2fa 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -9,6 +9,7 @@ ComplementaryFilter::ComplementaryFilter()
, dx_(Vector3::Zero())
, HP_x_(Vector3::Zero())
, LP_x_(Vector3::Zero())
+ , alpha_(Vector3::Zero())
, filt_x_(Vector3::Zero())
{
}
@@ -27,6 +28,7 @@ Vector3 ComplementaryFilter::compute(Vector3 const& x, Vector3 const& dx, Vector
// For logging
x_ = x;
dx_ = dx;
+ alpha_ = alpha;
// Process high pass filter
HP_x_ = alpha.cwiseProduct(HP_x_ + dx_ * dt_);
@@ -60,8 +62,11 @@ Estimator::Estimator()
, actuators_vel_(Vector12::Zero())
, q_FK_(Vector19::Zero())
, v_FK_(Vector18::Zero())
+ , feet_status_(MatrixN::Zero(1, 4))
+ , feet_goals_(MatrixN::Zero(3, 4))
, FK_lin_vel_(Vector3::Zero())
, FK_xyz_(Vector3::Zero())
+ , b_filt_lin_vel_(Vector3::Zero())
, xyz_mean_feet_(Vector3::Zero())
, k_since_contact_(Eigen::Matrix<double, 1, 4>::Zero())
, q_filt_(Vector19::Zero())
@@ -118,7 +123,7 @@ void Estimator::initialize(Params& params)
}
-void Estimator::get_data_IMU(Vector3 baseLinearAcceleration, Vector3 baseAngularVelocity, Vector4 baseOrientation)
+void Estimator::get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity, Vector4 const& baseOrientation)
{
// Linear acceleration of the trunk (base frame)
IMU_lin_acc_ = baseLinearAcceleration;
@@ -142,14 +147,14 @@ void Estimator::get_data_IMU(Vector3 baseLinearAcceleration, Vector3 baseAngular
}
-void Estimator::get_data_joints(Vector12 q_mes, Vector12 v_mes)
+void Estimator::get_data_joints(Vector12 const& q_mes, Vector12 const& v_mes)
{
actuators_pos_ = q_mes;
actuators_vel_ = v_mes;
}
-void Estimator::get_data_FK(Eigen::Matrix<double, 1, 4> feet_status)
+void Estimator::get_data_FK(Eigen::Matrix<double, 1, 4> const& feet_status)
{
// Update estimator FK model
q_FK_.tail(12) = actuators_pos_; // Position of actuators
@@ -209,7 +214,7 @@ void Estimator::get_data_FK(Eigen::Matrix<double, 1, 4> feet_status)
}
-void Estimator::get_xyz_feet(Eigen::Matrix<double, 1, 4> feet_status, Matrix34 goals)
+void Estimator::get_xyz_feet(Eigen::Matrix<double, 1, 4> const& feet_status, Matrix34 const& goals)
{
int cpt = 0;
Vector3 xyz_feet = Vector3::Zero();
@@ -256,6 +261,9 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
VectorN const& baseAngularVelocity, VectorN const& baseOrientation, VectorN const& q_mes,
VectorN const& v_mes, VectorN const& dummyPos, VectorN const& b_baseVel)
{
+ feet_status_ = gait.block(0, 0, 1, 4);
+ feet_goals_ = goals;
+
int remaining_steps = 1; // Remaining MPC steps for the current gait phase
while((gait.block(0, 0, 1, 4)).isApprox(gait.row(remaining_steps))) {
remaining_steps++;
@@ -274,14 +282,14 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
get_data_joints(q_mes, v_mes);
// Update nb of iterations since contact
- 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
+ k_since_contact_ += feet_status_; // Increment feet in stance phase
+ k_since_contact_ = k_since_contact_.cwiseProduct(feet_status_); // Reset feet in swing phase
// Update forward kinematics data
- get_data_FK(gait.block(0, 0, 1, 4));
+ get_data_FK(feet_status_);
// Update forward geometry data
- get_xyz_feet(gait.block(0, 0, 1, 4), goals);
+ get_xyz_feet(feet_status_, 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;
@@ -321,29 +329,15 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
Vector3 i_filt_lin_vel = oRb.transpose() * oi_filt_lin_vel;
// Filtered estimated velocity at center base (base frame)
- Vector3 b_filt_lin_vel = i_filt_lin_vel - cross_product;
+ b_filt_lin_vel_ = i_filt_lin_vel - cross_product;
// Filtered estimated velocity at center base (world frame)
- Vector3 ob_filt_lin_vel = oRb * b_filt_lin_vel;
+ Vector3 ob_filt_lin_vel = oRb * b_filt_lin_vel_;
// Position of the center of the base from FGeometry and filtered velocity (world frame)
Vector3 filt_lin_pos = filter_xyz_pos_.compute(FK_xyz_ + xyz_mean_feet_, ob_filt_lin_vel,
Vector3(0.995, 0.995, 0.9));
- // Velocity of the center of the base (base frame)
- Vector3 filt_lin_vel = b_filt_lin_vel;
-
- // Logging
- /*self.log_alpha[self.k_log] = self.alpha
- self.feet_status[:] = feet_status // Save contact status sent to the estimator for logging
- self.feet_goals[:, :] = goals.copy() // Save feet goals sent to the estimator for logging
- self.log_IMU_lin_acc[:, self.k_log] = self.IMU_lin_acc[:]
- self.log_HP_lin_vel[:, self.k_log] = self.HP_lin_vel[:]
- self.log_LP_lin_vel[:, self.k_log] = self.LP_lin_vel[:]
- self.log_FK_lin_vel[:, self.k_log] = self.FK_lin_vel[:]
- self.log_filt_lin_vel[:, self.k_log] = self.filt_lin_vel[:]
- self.log_o_filt_lin_vel[:, self.k_log] = self.o_filt_lin_vel[:, 0]*/
-
// Output filtered position vector (19 x 1)
q_filt_.head(3) = filt_lin_pos;
if(perfect_estimator) { // Base height directly from PyBullet
@@ -357,7 +351,7 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
v_filt_.head(3) = (1 - alpha_v_) * v_filt_.head(3) + alpha_v_ * b_baseVel;
}
else {
- v_filt_.head(3) = (1 - alpha_v_) * v_filt_.head(3) + alpha_v_ * filt_lin_vel;
+ v_filt_.head(3) = (1 - alpha_v_) * v_filt_.head(3) + alpha_v_ * b_filt_lin_vel_;
}
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
--
GitLab