diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1b09cb376338018e783562875a289915301171dc..c546d4b470c3e80a23b8e05d725a47c7ab2c5bfb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -62,6 +62,7 @@ set(${PROJECT_NAME}_HEADERS
include/qrw/WbcWrapper.hpp
include/qrw/Params.hpp
include/qrw/Estimator.hpp
+ include/qrw/ComplementaryFilter.hpp
include/qrw/Joystick.hpp
include/qrw/Filter.hpp
include/qrw/Controller.hpp
@@ -89,6 +90,7 @@ set(${PROJECT_NAME}_SOURCES
src/WbcWrapper.cpp
src/Params.cpp
src/Estimator.cpp
+ src/ComplementaryFilter.cpp
src/Joystick.cpp
src/Filter.cpp
src/Controller.cpp
diff --git a/include/qrw/ComplementaryFilter.hpp b/include/qrw/ComplementaryFilter.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b12fb47c84f617d7d8bf4555da3280841ad91a95
--- /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/Controller.hpp b/include/qrw/Controller.hpp
index 959e1970ecb917d63c230584ed1a996d4bdd6be8..6a61828ee8b38406cc6cc806fbf032fb7b4ab860 100644
--- a/include/qrw/Controller.hpp
+++ b/include/qrw/Controller.hpp
@@ -12,11 +12,6 @@
#include <odri_control_interface/robot.hpp>
#include <boost/interprocess/managed_shared_memory.hpp>
#include "qrw/FakeRobot.hpp"
-#include <cmath>
-#include <fstream>
-#include <iostream>
-#include <string>
-#include "qrw/Types.h"
#include "qrw/Params.hpp"
#include "qrw/Joystick.hpp"
#include "qrw/Estimator.hpp"
diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index d3f328f32a9676abfb3936daa9d648de543a8f01..d8c014f233fc205aaebff5c387c3dec768e9c4a8 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -9,93 +9,100 @@
#ifndef ESTIMATOR_H_INCLUDED
#define ESTIMATOR_H_INCLUDED
-#include "qrw/Gait.hpp"
-#include "qrw/Params.hpp"
-#include "qrw/Types.h"
-#include "pinocchio/math/rpy.hpp"
-#include "pinocchio/spatial/se3.hpp"
-#include "pinocchio/algorithm/frames.hpp"
-#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/multibody/data.hpp"
-#include "pinocchio/parsers/urdf.hpp"
-#include "pinocchio/algorithm/compute-all-terms.hpp"
#include <deque>
-class ComplementaryFilter {
+#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
///
////////////////////////////////////////////////////////////////////////////////////////////////
- ComplementaryFilter();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Destructor
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ~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, Vector3 HP_x, Vector3 LP_x);
+ Estimator();
////////////////////////////////////////////////////////////////////////////////////////////////
///
- /// \brief Compute the filtered output of the complementary filter
+ /// \brief Initialize with given data
///
- /// \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
+ /// \param[in] params Object that stores parameters
///
////////////////////////////////////////////////////////////////////////////////////////////////
- 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 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_, alpha_, filt_x_;
-};
+ void initialize(Params& params);
-class Estimator {
- public:
////////////////////////////////////////////////////////////////////////////////////////////////
///
- /// \brief Constructor
+ /// \brief Destructor.
///
////////////////////////////////////////////////////////////////////////////////////////////////
- Estimator();
+ ~Estimator() {} // Empty destructor
////////////////////////////////////////////////////////////////////////////////////////////////
///
- /// \brief Initialize with given data
+ /// \brief Run one iteration of the estimator to get the position and velocity states of the robot
///
- /// \param[in] params Object that stores parameters
+ /// \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 initialize(Params& params);
+ 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 Destructor.
+ /// \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
///
////////////////////////////////////////////////////////////////////////////////////////////////
- ~Estimator() {} // Empty destructor
+ 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
@@ -105,27 +112,36 @@ class Estimator {
/// \param[in] baseOrientation Euler orientation of the IMU
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity,
- Vector3 const& baseOrientation, VectorN const& q_perfect);
+ 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_mes Position of the 12 actuators
- /// \param[in] v_mes Velocity of the 12 actuators
+ /// \param[in] q Position of the 12 actuators
+ /// \param[in] v Velocity of the 12 actuators
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void get_data_joints(Vector12 const& q_mes, Vector12 const& v_mes);
+ void updateJointData(Vector12 const& q, Vector12 const& v);
////////////////////////////////////////////////////////////////////////////////////////////////
///
- /// \brief Estimate base position and velocity using Forward Kinematics
+ /// \brief Update the feet relative data
+ /// \details update feetStatus_, feetTargets_, feetStancePhaseDuration_ and phaseRemainingDuration_
///
- /// \param[in] feet_status Contact status of the four feet
+ /// \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 get_data_FK(Eigen::Matrix<double, 1, 4> const& feet_status);
+ void updatFeetStatus(MatrixN const& gait, MatrixN const& feetTargets);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Estimate base position and velocity using Forward Kinematics
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void updateForwardKinematics();
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -135,155 +151,116 @@ class Estimator {
/// \param[in] goals Target positions of the four feet
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void get_xyz_feet(Eigen::Matrix<double, 1, 4> const& feet_status, Matrix34 const& goals);
+ 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 point
+ /// \param[in] contactFrameId Frame ID of the contact foot
///
////////////////////////////////////////////////////////////////////////////////////////////////
- Vector3 BaseVelocityFromKinAndIMU(int contactFrameId);
+ Vector3 computeBaseVelocityFromFoot(int footId);
////////////////////////////////////////////////////////////////////////////////////////////////
///
- /// \brief Run one iteration of the estimator to get the position and velocity states of the robot
+ /// \brief Estimate the position of the base with forward kinematics using a contact point that
+ /// is supposed immobile in world frame
///
- /// \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] q_perfect Position of the robot in PyBullet simulator (only for simulation)
- /// \param[in] b_baseVel_perfect Velocity of the robot in PyBullet simulator (only for simulation)
+ /// \param[in] footId Frame ID of the contact foot
///
////////////////////////////////////////////////////////////////////////////////////////////////
- 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& q_perfect, Vector3 const& b_baseVel_perfect);
+ Vector3 computeBasePositionFromFoot(int footId);
////////////////////////////////////////////////////////////////////////////////////////////////
///
- /// \brief Security check to verify that measured positions, velocities and required torques
- /// are not above defined tresholds
- ///
- /// \param[in] tau_ff Feedforward torques outputted by the whole body control for the PD+
+ /// \brief Compute the alpha coefficient for the complementary filter
+ /// \return alpha
///
////////////////////////////////////////////////////////////////////////////////////////////////
- int security_check(VectorN const& tau_ff);
+ double computeAlphaVelocity();
////////////////////////////////////////////////////////////////////////////////////////////////
///
- /// \brief Update state vectors of the robot (q and v)
- /// Update transformation matrices between world and horizontal frames
+ /// \brief Estimates the velocity vector
+ /// \details The complementary filter combines data from the FK and the IMU acceleration data
///
- /// \param[in] joystick_v_ref Reference velocity from the joystick
- /// \param[in] gait Gait object
+ /// \param[in] b_perfectVelocity Perfect velocity of the base in the base frame
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void updateState(VectorN const& joystick_v_ref, Gait& gait);
-
- VectorN getQFilt() { return q_filt_dyn_; }
- VectorN getVFilt() { return v_filt_dyn_; }
- VectorN getVSecu() { return v_secu_dyn_; }
- VectorN getVFiltBis() { return v_filt_bis_; }
- 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(); }
+ void estimateVelocity(Vector3 const& b_perfectVelocity);
- 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(); }
-
- VectorN getQUpdated() { return q_up_; }
- VectorN getVUpdated() { return v_up_; }
- VectorN getVRef() { return v_ref_; }
- VectorN getARef() { return a_ref_; }
- VectorN getHV() { return h_v_; }
- VectorN getHVWindowed() { return h_v_windowed_; }
- Matrix3 getoRb() { return oRb_; }
- Matrix3 getoRh() { return oRh_; }
- Matrix3 gethRb() { return hRb_; }
- Vector3 getoTh() { return oTh_; }
- double getYawEstim() { return yaw_estim_; }
-
- private:
- ComplementaryFilter filter_xyz_pos_; // Complementary filter for base position
- ComplementaryFilter filter_xyz_vel_; // Complementary filter for base velocity
-
- double dt_wbc; // Time step of the estimator
- double alpha_secu_; // Low pass coefficient for the outputted filtered velocity for security check
- double offset_yaw_IMU_; // Yaw orientation of the IMU at startup
- bool perfect_estimator; // Enable perfect estimator (directly from the PyBullet simulation)
- bool solo3D; // Perfect estimator including yaw angle
- int N_SIMULATION; // Number of loops before the control ends
- int k_log_; // Number of time the estimator has been called
-
- Vector3 IMU_lin_acc_; // Linear acceleration of the IMU (gravity compensated)
- Vector3 IMU_ang_vel_; // Angular velocity of the IMU
- Vector3 IMU_RPY_; // Roll Pitch Yaw orientation of the IMU
- Matrix3 oRb_; // Rotation between base and world frame
- pinocchio::SE3::Quaternion IMU_ang_pos_; // Quaternion orientation of the IMU
- Vector12 actuators_pos_; // Measured positions of actuators
- Vector12 actuators_vel_; // Measured velocities of actuators
-
- 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
-
- Eigen::Matrix<double, 1, 4> k_since_contact_; // Number of loops during which each foot has been in contact
- int feet_indexes_[4] = {10, 18, 26, 34}; // Frame indexes of the four feet
-
- pinocchio::Model model_, model_for_xyz_; // Pinocchio models for frame computations and forward kinematics
- pinocchio::Data data_, data_for_xyz_; // Pinocchio datas for frame computations and forward kinematics
-
- Vector19 q_filt_; // Filtered output configuration
- Vector18 v_filt_; // Filtered output velocity
- Vector12 v_secu_; // Filtered output velocity for security check
-
- VectorN q_filt_dyn_; // Dynamic size version of q_filt_
- VectorN v_filt_dyn_; // Dynamic size version of v_filt_
- VectorN v_secu_dyn_; // Dynamic size version of v_secu_
-
- pinocchio::SE3 _1Mi_; // Transform between the base frame and the IMU frame
-
- Vector12 q_security_; // Position limits for the actuators above which safety controller is triggered
-
- // For updateState function
- VectorN q_up_; // Configuration vector in ideal world frame
- VectorN v_up_; // Velocity vector in ideal world frame
- VectorN v_ref_; // Reference velocity vector
- VectorN a_ref_; // Reference acceleration vector
- VectorN h_v_; // Velocity vector in horizontal frame
- Matrix3 oRh_; // Rotation between horizontal and world frame
- Matrix3 hRb_; // Rotation between base and horizontal frame
- Vector3 oTh_; // Translation between horizontal and world frame
- double yaw_estim_; // Yaw angle in perfect world
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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);
- // Filter the base velocity with an averaging window to remove oscillations at multiples of gait frequency
- int N_queue_; // Number of samples in the averaging window
- VectorN v_filt_bis_; // Base velocity (in base frame) filtered by averaging window
- VectorN h_v_windowed_; // Base velocity (in horizontal frame) filtered by averaging window
- std::deque<double> vx_queue_, vy_queue_, vz_queue_, wR_queue_, wP_queue_, wY_queue_; // Queues that hold samples
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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/python/Estimator.cpp b/python/Estimator.cpp
index 2a71eed6d49be2e4c3211254bd7f4c0919161b09..4095a258b0282080489146fe81f6d0919e524187 100644
--- a/python/Estimator.cpp
+++ b/python/Estimator.cpp
@@ -9,41 +9,37 @@ struct EstimatorVisitor : public bp::def_visitor<EstimatorVisitor<Estimator>> {
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
.def("initialize", &Estimator::initialize, bp::args("params"), "Initialize Estimator from Python.\n")
- .def("security_check", &Estimator::security_check, bp::args("tau_ff"), "Run security check.\n")
- .def("updateState", &Estimator::updateState, bp::args("joystick_v_ref", "gait"), "Update robot state.\n")
+ .def("update_reference_state", &Estimator::updateReferenceState, bp::args("v_ref"), "Update robot state.\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("getVFiltBis", &Estimator::getVFiltBis, "Get filtered velocity.\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, "")
- .def("getQUpdated", &Estimator::getQUpdated, "")
- .def("getVUpdated", &Estimator::getVUpdated, "")
- .def("getVRef", &Estimator::getVRef, "")
- .def("getARef", &Estimator::getARef, "")
- .def("getHV", &Estimator::getHV, "")
- .def("getHVWindowed", &Estimator::getHVWindowed, "")
- .def("getoRb", &Estimator::getoRb, "")
- .def("getoRh", &Estimator::getoRh, "")
- .def("gethRb", &Estimator::gethRb, "")
- .def("getoTh", &Estimator::getoTh, "")
- .def("getYawEstim", &Estimator::getYawEstim, "")
+ .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_filter", &Estimator::run_filter,
+ .def("run", &Estimator::run,
bp::args("gait", "goals", "baseLinearAcceleration", "baseAngularVelocity", "baseOrientation", "q_mes",
"v_mes", "dummyPos", "b_baseVel"),
"Run Estimator from Python.\n");
diff --git a/scripts/Controller.py b/scripts/Controller.py
index ebf10c930db941ac1e0cb2e49d25e978aaf5e226..8ea72b260f8be4c764752b9b42ad42a0f3c8def5 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -85,6 +85,8 @@ class Controller:
# Parameters definition #
########################################################################
+ self.q_security = np.array([1.2, 2.1, 3.14, 1.2, 2.1, 3.14, 1.2, 2.1, 3.14, 1.2, 2.1, 3.14])
+
# Init joint torques to correct shape
self.jointTorques = np.zeros((12, 1))
@@ -288,32 +290,32 @@ class Controller:
self.last_b_vel = b_baseVel_perfect
self.n_nan = 0
- self.estimator.run_filter(self.gait.getCurrentGait(),
- self.footTrajectoryGenerator.getFootPosition(),
- device.imu.linear_acceleration,
- device.imu.gyroscope,
- device.imu.attitude_euler,
- device.joints.positions,
- device.joints.velocities,
- q_perfect, b_baseVel_perfect)
+ self.estimator.run(self.gait.getCurrentGait(),
+ self.footTrajectoryGenerator.getFootPosition(),
+ device.imu.linear_acceleration,
+ device.imu.gyroscope,
+ device.imu.attitude_euler,
+ device.joints.positions,
+ device.joints.velocities,
+ q_perfect, b_baseVel_perfect)
# Update state vectors of the robot (q and v) + transformation matrices between world and horizontal frames
- self.estimator.updateState(self.joystick.getVRef(), self.gait)
- oRh = self.estimator.getoRh()
- hRb = self.estimator.gethRb()
- oTh = self.estimator.getoTh().reshape((3, 1))
- self.a_ref[:6, 0] = self.estimator.getARef()
- self.v_ref[:6, 0] = self.estimator.getVRef()
- self.h_v[:6, 0] = self.estimator.getHV()
- self.h_v_windowed[:6, 0] = self.estimator.getHVWindowed()
+ self.estimator.update_reference_state(self.joystick.getVRef())
+ oRh = self.estimator.get_oRh()
+ hRb = self.estimator.get_hRb()
+ oTh = self.estimator.get_oTh().reshape((3, 1))
+ self.a_ref[:6, 0] = self.estimator.get_base_acc_ref()
+ self.v_ref[:6, 0] = self.estimator.get_base_vel_ref()
+ self.h_v[:6, 0] = self.estimator.get_h_v()
+ self.h_v_windowed[:6, 0] = self.estimator.get_h_v_filtered()
if self.solo3D:
- self.q[:3, 0] = self.estimator.getQFilt()[:3]
- self.q[6:, 0] = self.estimator.getQFilt()[7:]
- self.q[3:6] = quaternionToRPY(self.estimator.getQFilt()[3:7])
+ self.q[:3, 0] = self.estimator.get_q_estimate()[:3]
+ self.q[6:, 0] = self.estimator.get_q_estimate()[7:]
+ self.q[3:6] = quaternionToRPY(self.estimator.get_q_estimate()[3:7])
else:
- self.q[:, 0] = self.estimator.getQUpdated()
- self.v[:, 0] = self.estimator.getVUpdated()
- self.yaw_estim = self.estimator.getYawEstim()
+ self.q[:, 0] = self.estimator.get_q_reference()
+ self.v[:, 0] = self.estimator.get_v_reference()
+ self.yaw_estim = self.q[5, 0]
# Quantities go through a 1st order low pass filter with fc = 15 Hz (avoid >25Hz foldback)
self.q_filter[:6, 0] = self.filter_mpc_q.filter(self.q[:6, 0], True)
@@ -422,7 +424,7 @@ class Controller:
self.q_wbc[6:, 0] = self.wbcWrapper.qdes[:]
# Update velocity vector for wbc
- self.dq_wbc[:6, 0] = self.estimator.getVFilt()[:6] # Â Velocities in base frame (not horizontal frame!)
+ self.dq_wbc[:6, 0] = self.estimator.get_v_estimate()[:6] # Â Velocities in base frame (not horizontal frame!)
self.dq_wbc[6:, 0] = self.wbcWrapper.vdes[:] # with reference angular velocities of previous loop
# Feet command position, velocity and acceleration in base frame
@@ -567,19 +569,23 @@ class Controller:
"""
Check if the command is fine and set the command to zero in case of error
"""
+
if not (self.error or self.joystick.getStop()):
- error_flag = self.estimator.security_check(self.wbcWrapper.tau_ff)
- if (error_flag != 0):
+ if (np.abs(self.estimator.get_q_estimate()[7:]) > self.q_security).any():
+ print("-- POSITION LIMIT ERROR --")
+ print(self.estimator.get_q_estimate()[7:])
+ print(np.abs(self.estimator.get_q_estimate()[7:]) > self.q_security)
+ self.error = True
+ elif (np.abs(self.estimator.get_v_security()) > 100.).any():
+ print("-- VELOCITY TOO HIGH ERROR --")
+ print(self.estimator.get_v_security())
+ print(np.abs(self.estimator.get_v_security()) > 100.)
+ self.error = True
+ elif (np.abs(self.wbcWrapper.tau_ff) > 8.0).any():
+ print("-- FEEDFORWARD TORQUES TOO HIGH ERROR --")
+ print(self.wbcWrapper.tau_ff)
+ print(np.abs(self.wbcWrapper.tau_ff) > 8.0)
self.error = True
- if (error_flag == 1):
- print("-- POSITION LIMIT ERROR --")
- print(self.estimator.getQFilt()[7:])
- elif (error_flag == 2):
- print("-- VELOCITY TOO HIGH ERROR --")
- print(self.estimator.getVSecu())
- else:
- print("-- FEEDFORWARD TORQUES TOO HIGH ERROR --")
- print(self.wbcWrapper.tau_ff)
def clamp(self, num, min_value=None, max_value=None):
clamped = False
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index a29d3ca122099fafc22c92d2ee6291e356d97a2a..667173c9822137bbe4918ed8e7925489725cad1b 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -139,31 +139,31 @@ class LoggerControl():
self.joy_v_ref[self.i] = joystick.getVRef()
# Logging from estimator
- 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_q_up[self.i] = estimator.getQUpdated()
- self.esti_v_filt[self.i] = estimator.getVFilt()
- self.esti_v_filt_bis[self.i, :6] = estimator.getVFiltBis()
- self.esti_v_up[self.i] = estimator.getVUpdated()
- self.esti_v_ref[self.i] = estimator.getVRef()
- self.esti_v_secu[self.i] = estimator.getVSecu()
- self.esti_a_ref[self.i] = estimator.getARef()
-
- 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()
+ self.esti_feet_status[self.i] = estimator.get_feet_status()
+ self.esti_feet_goals[self.i] = estimator.get_feet_targets()
+ self.esti_q_filt[self.i] = estimator.get_q_estimate()
+ self.esti_q_up[self.i] = estimator.get_q_reference()
+ self.esti_v_filt[self.i] = estimator.get_v_estimate()
+ self.esti_v_filt_bis[self.i, :6] = estimator.get_v_filtered()
+ self.esti_v_up[self.i] = estimator.get_v_reference()
+ self.esti_v_ref[self.i] = estimator.get_base_vel_ref()
+ self.esti_v_secu[self.i] = estimator.get_v_security()
+ self.esti_a_ref[self.i] = estimator.get_base_acc_ref()
+
+ self.esti_FK_lin_vel[self.i] = estimator.get_base_velocity_FK()
+ self.esti_FK_xyz[self.i] = estimator.get_base_position_FK()
+ self.esti_xyz_mean_feet[self.i] = estimator.get_feet_position_barycenter()
+ self.esti_filt_lin_vel[self.i] = estimator.get_b_base_velocity()
+
+ self.esti_HP_x[self.i] = estimator.get_filter_vel_X()
+ self.esti_HP_dx[self.i] = estimator.get_filter_vel_DX()
+ self.esti_HP_alpha[self.i] = estimator.get_filter_vel_Alpha()
+ self.esti_HP_filt_x[self.i] = estimator.get_filter_vel_FiltX()
+
+ self.esti_LP_x[self.i] = estimator.get_filter_pos_X()
+ self.esti_LP_dx[self.i] = estimator.get_filter_pos_DX()
+ self.esti_LP_alpha[self.i] = estimator.get_filter_pos_Alpha()
+ self.esti_LP_filt_x[self.i] = estimator.get_filter_pos_FiltX()
# Logging from the main loop
self.loop_o_q[self.i] = controller.q[:, 0]
diff --git a/src/ComplementaryFilter.cpp b/src/ComplementaryFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..64190ddd98d9fd988a4c46362761d5416f7d0e11
--- /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/Controller.cpp b/src/Controller.cpp
index bb5810ce887c3bc95e10e3dc7ee7588c9a99c0d0..e9b787c478fad0094d7ca3190309862bcc74a5f2 100644
--- a/src/Controller.cpp
+++ b/src/Controller.cpp
@@ -1,7 +1,7 @@
#include "qrw/Controller.hpp"
-#include "pinocchio/math/rpy.hpp"
#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/math/rpy.hpp"
Controller::Controller()
: P(Vector12::Zero()),
@@ -61,27 +61,26 @@ void Controller::compute(FakeRobot* robot) {
joystick.update_v_ref(k, params_->velID, gait.getIsStatic());
// Process state estimator
- estimator.run_filter(gait.getCurrentGait(), footTrajectoryGenerator.getFootPosition(),
- robot->imu->GetLinearAcceleration(), robot->imu->GetGyroscope(), robot->imu->GetAttitudeEuler(),
- robot->joints->GetPositions(), robot->joints->GetVelocities(), Vector3::Zero(),
- Vector3::Zero());
+ estimator.run(gait.getCurrentGait(), footTrajectoryGenerator.getFootPosition(), robot->imu->GetLinearAcceleration(),
+ robot->imu->GetGyroscope(), robot->imu->GetAttitudeEuler(), robot->joints->GetPositions(),
+ robot->joints->GetVelocities(), Vector3::Zero(), Vector3::Zero());
// Update state vectors of the robot (q and v) + transformation matrices between world and horizontal frames
- estimator.updateState(joystick.getVRef(), gait);
+ estimator.updateReferenceState(joystick.getVRef());
// Update gait
gait.updateGait(k, k_mpc, joystick.getJoystickCode());
// Quantities go through a 1st order low pass filter with fc = 15 Hz (avoid >25Hz foldback)
- q_filt_mpc.head(6) = filter_mpc_q.filter(estimator.getQUpdated().head(6), true);
- q_filt_mpc.tail(12) = estimator.getQUpdated().tail(12);
+ q_filt_mpc.head(6) = filter_mpc_q.filter(estimator.getQReference().head(6), true);
+ q_filt_mpc.tail(12) = estimator.getQReference().tail(12);
h_v_filt_mpc = filter_mpc_v.filter(estimator.getHV().head(6), false);
- vref_filt_mpc = filter_mpc_vref.filter(estimator.getVRef().head(6), false);
+ vref_filt_mpc = filter_mpc_vref.filter(estimator.getBaseVelRef().head(6), false);
// Compute target footstep based on current and reference velocities
o_targetFootstep = footstepPlanner.updateFootsteps(
- (k % k_mpc == 0) && (k != 0), static_cast<int>(k_mpc - (k % k_mpc)), estimator.getQUpdated().head(18),
- estimator.getHVWindowed().head(6), estimator.getVRef().head(6));
+ (k % k_mpc == 0) && (k != 0), static_cast<int>(k_mpc - (k % k_mpc)), estimator.getQReference().head(18),
+ estimator.getHVFiltered().head(6), estimator.getBaseVelRef().head(6));
// Run state planner (outputs the reference trajectory of the base)
statePlanner.computeReferenceStates(q_filt_mpc.head(6), h_v_filt_mpc, vref_filt_mpc);
@@ -132,7 +131,7 @@ void Controller::compute(FakeRobot* robot) {
q_wbc.tail(12) = wbcWrapper.get_qdes(); // with reference angular positions of previous loop
// Update velocity vector for wbc
- dq_wbc.head(6) = estimator.getVFilt().head(6); // Velocities in base frame (not horizontal frame!)
+ dq_wbc.head(6) = estimator.getVEstimate().head(6); // Velocities in base frame (not horizontal frame!)
dq_wbc.tail(12) = wbcWrapper.get_vdes(); // with reference angular velocities of previous loop
// Desired position, orientation and velocities of the base
@@ -245,16 +244,32 @@ void Controller::init_robot(Params& params) {
}
void Controller::security_check() {
+ Vector12 q_security_ = (Vector3(1.2, 2.1, 3.14)).replicate<4, 1>();
+
+ if (((estimator.getQEstimate().tail(12).cwiseAbs()).array() > q_security_.array()).any()) {
+ std::cout << "Position limit error "
+ << ((estimator.getQEstimate().tail(12).cwiseAbs()).array() > q_security_.array()).transpose() << std::endl;
+ error_flag = 1;
+ } else if (((estimator.getVSecurity().cwiseAbs()).array() > 100.0).any()) {
+ std::cout << "Velocity limit error " << ((estimator.getVSecurity().cwiseAbs()).array() > 100.0).transpose()
+ << std::endl;
+ error_flag = 2;
+ } else if (((tau_ff.cwiseAbs()).array() > 8.0).any()) {
+ std::cout << "Feedforward limit error " << ((tau_ff.cwiseAbs()).array() > 8.0).transpose() << std::endl;
+ error_flag = 3;
+ } else {
+ error_flag = 0;
+ }
+
if (error_flag == 0 && !error) {
- error_flag = estimator.security_check(tau_ff); // Check position, velocity and feedforward torque limits
if (error_flag != 0) {
error = true;
switch (error_flag) {
case 1: // Out of position limits
- error_value = estimator.getQFilt().tail(12) * 180 / 3.1415;
+ error_value = estimator.getQEstimate().tail(12) * 180 / 3.1415;
break;
case 2: // Out of velocity limits
- error_value = estimator.getVSecu();
+ error_value = estimator.getVSecurity();
break;
default: // Out of torques limits
error_value = tau_ff;
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index cffeeabcaeee4bb33931fa408d0b6d7f2aac5e50..7485d1841f472e6b3961c6cc34d7557fe5dbe930 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -1,482 +1,283 @@
-#include <example-robot-data/path.hpp>
-
#include "qrw/Estimator.hpp"
-////////////////////////////////////
-// Complementary filter functions
-////////////////////////////////////
-
-ComplementaryFilter::ComplementaryFilter()
- : x_(Vector3::Zero()),
- dx_(Vector3::Zero()),
- HP_x_(Vector3::Zero()),
- LP_x_(Vector3::Zero()),
- alpha_(Vector3::Zero()),
- filt_x_(Vector3::Zero()) {}
-
-void ComplementaryFilter::initialize(double dt, Vector3 HP_x, Vector3 LP_x) {
- dt_ = dt;
- HP_x_ = HP_x;
- LP_x_ = LP_x;
-}
-
-Vector3 ComplementaryFilter::compute(Vector3 const& x, Vector3 const& dx, Vector3 const& alpha) {
- // For logging
- x_ = x;
- dx_ = dx;
- alpha_ = alpha;
-
- // Process high pass filter
- HP_x_ = alpha.cwiseProduct(HP_x_ + dx_ * dt_);
-
- // Process low pass filter
- LP_x_ = alpha.cwiseProduct(LP_x_) + (Vector3::Ones() - alpha).cwiseProduct(x_);
-
- // Add both to get the filtered output
- filt_x_ = HP_x_ + LP_x_;
-
- return filt_x_;
-}
+#include <example-robot-data/path.hpp>
-/////////////////////////
-// Estimator functions
-/////////////////////////
+#include "pinocchio/algorithm/compute-all-terms.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+#include "pinocchio/math/rpy.hpp"
+#include "pinocchio/parsers/urdf.hpp"
Estimator::Estimator()
- : dt_wbc(0.0),
- alpha_secu_(0.0),
- offset_yaw_IMU_(0.0),
- perfect_estimator(false),
- solo3D(false),
- N_SIMULATION(0),
- k_log_(0),
- IMU_lin_acc_(Vector3::Zero()),
- IMU_ang_vel_(Vector3::Zero()),
- IMU_RPY_(Vector3::Zero()),
- oRb_(Matrix3::Identity()),
- IMU_ang_pos_(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0)),
- actuators_pos_(Vector12::Zero()),
- actuators_vel_(Vector12::Zero()),
+ : 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()),
- 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()),
- v_filt_(Vector18::Zero()),
- v_secu_(Vector12::Zero()),
- q_filt_dyn_(VectorN::Zero(19, 1)),
- v_filt_dyn_(VectorN::Zero(18, 1)),
- v_secu_dyn_(VectorN::Zero(12, 1)),
- q_up_(VectorN::Zero(18)),
- v_up_(VectorN::Zero(18)),
- v_ref_(VectorN::Zero(6)),
- a_ref_(VectorN::Zero(6)),
- h_v_(VectorN::Zero(6)),
+ 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()),
- yaw_estim_(0.0),
- N_queue_(0),
- v_filt_bis_(VectorN::Zero(6, 1)),
- h_v_windowed_(VectorN::Zero(6, 1)) {}
+ 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_wbc = params.dt_wbc;
- N_SIMULATION = params.N_SIMULATION;
- perfect_estimator = params.perfect_estimator;
- solo3D = params.solo3D;
+ dt_ = params.dt_wbc;
+ perfectEstimator_ = params.perfect_estimator;
+ solo3D_ = params.solo3D;
// Filtering estimated linear velocity
- int k_mpc = static_cast<int>(std::round(params.dt_mpc / params.dt_wbc));
- N_queue_ = static_cast<int>(k_mpc * params.gait.rows() / params.N_periods);
- vx_queue_.resize(N_queue_, 0.0); // List full of 0.0
- vy_queue_.resize(N_queue_, 0.0); // List full of 0.0
- vz_queue_.resize(N_queue_, 0.0); // List full of 0.0
- wR_queue_.resize(N_queue_, 0.0); // List full of 0.0
- wP_queue_.resize(N_queue_, 0.0); // List full of 0.0
- wY_queue_.resize(N_queue_, 0.0); // List full of 0.0
+ 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; // Cut frequency
- double y = 1 - std::cos(2 * M_PI * fc * dt_wbc);
- alpha_secu_ = -y + std::sqrt(y * y + 2 * y);
-
- 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_);
+ 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");
+}
- _1Mi_ = pinocchio::SE3(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0), Vector3(0.1163, 0.0, 0.02));
+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);
- q_security_ = (Vector3(1.2, 2.1, 3.14)).replicate<4, 1>();
-// q_security_ = (Vector3(M_PI * 0.4, M_PI * 80 / 180, M_PI)).replicate<4, 1>();
+ updateForwardKinematics();
+ computeFeetPositionBarycenter();
- 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
+ estimateVelocity(b_perfectVelocity);
+ estimatePosition(perfectPosition.head(3));
- q_up_(2, 0) = params.h_ref; // Reference height
- q_up_.tail(12) = Vector12(params.q_init.data()); // Actuator initial positions
+ filterVelocity();
- // Path to the robot URDF
- const std::string filename =
- std::string(EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf");
+ vSecurity_ = (1 - alphaSecurity_) * vActuators_ + alphaSecurity_ * vSecurity_;
+}
- // Build model from urdf
- pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_, false);
- pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_for_xyz_, false);
+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;
- // Construct data from model
- data_ = pinocchio::Data(model_);
- data_for_xyz_ = pinocchio::Data(model_for_xyz_);
+ // 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);
- // Update all the quantities of the model
- pinocchio::computeAllTerms(model_, data_, q_filt_, v_filt_);
- pinocchio::computeAllTerms(model_for_xyz_, data_for_xyz_, q_filt_, v_filt_);
+ // 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::get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity,
- Vector3 const& baseOrientation, VectorN const& q_perfect) {
- // Linear acceleration of the trunk (base frame)
- IMU_lin_acc_ = baseLinearAcceleration;
-
- // Angular velocity of the trunk (base frame)
- IMU_ang_vel_ = baseAngularVelocity;
+void Estimator::updatFeetStatus(MatrixN const& gait, MatrixN const& feetTargets) {
+ feetStatus_ = gait.row(0);
+ feetTargets_ = feetTargets;
- // Angular position of the trunk (local frame)
- IMU_RPY_ = baseOrientation;
+ feetStancePhaseDuration_ += feetStatus_;
+ feetStancePhaseDuration_ = feetStancePhaseDuration_.cwiseProduct(feetStatus_);
- if (k_log_ <= 1) {
- offset_yaw_IMU_ = IMU_RPY_(2, 0);
+ phaseRemainingDuration_ = 1;
+ while (feetStatus_.isApprox((Vector4)gait.row(phaseRemainingDuration_))) {
+ phaseRemainingDuration_++;
}
- IMU_RPY_(2, 0) -= offset_yaw_IMU_; // Remove initial offset of IMU
-
- if (solo3D) {
- IMU_RPY_.tail(1) = q_perfect.tail(1); // Yaw angle from motion capture
- }
-
- IMU_ang_pos_ =
- pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(IMU_RPY_(0, 0), IMU_RPY_(1, 0), IMU_RPY_(2, 0)));
- // Above could be commented since IMU_ang_pos yaw is not used anywhere and instead
- // replace by: IMU_ang_pos_ = baseOrientation_
}
-void Estimator::get_data_joints(Vector12 const& q_mes, Vector12 const& v_mes) {
- actuators_pos_ = q_mes;
- actuators_vel_ = v_mes;
-}
+void Estimator::updateIMUData(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity,
+ Vector3 const& baseOrientation, VectorN const& perfectPosition) {
+ IMULinearAcceleration_ = baseLinearAcceleration;
+ IMUAngularVelocity_ = baseAngularVelocity;
+ IMURpy_ = baseOrientation;
-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
- v_FK_.tail(12) = actuators_vel_; // Velocity of actuators
- // Position and orientation of the base remain at 0
- // Linear and angular velocities of the base remain at 0
-
- // Update model used for the forward kinematics
- q_FK_.block(3, 0, 4, 1) << 0.0, 0.0, 0.0, 1.0;
- pinocchio::forwardKinematics(model_, data_, q_FK_, v_FK_);
- // pin.updateFramePlacements(self.model, self.data)
-
- // Update model used for the forward geometry
- q_FK_.block(3, 0, 4, 1) = IMU_ang_pos_.coeffs();
- pinocchio::forwardKinematics(model_for_xyz_, data_for_xyz_, q_FK_);
-
- // Get estimated velocity from updated model
- int cpt = 0;
- Vector3 vel_est = Vector3::Zero();
- Vector3 xyz_est = Vector3::Zero();
- for (int j = 0; j < 4; j++) {
- // Consider only feet in contact + Security margin after the contact switch
- if (feet_status(0, j) == 1.0 && k_since_contact_[j] >= 16) {
- // Estimated velocity of the base using the considered foot
- Vector3 vel_estimated_baseframe = BaseVelocityFromKinAndIMU(feet_indexes_[j]);
-
- // Estimated position of the base using the considered foot
- pinocchio::updateFramePlacement(model_for_xyz_, data_for_xyz_, feet_indexes_[j]);
- Vector3 xyz_estimated = -data_for_xyz_.oMf[feet_indexes_[j]].translation();
-
- // Logging
- // self.log_v_est[:, i, self.k_log] = vel_estimated_baseframe[0:3, 0]
- // self.log_h_est[i, self.k_log] = xyz_estimated[2]
-
- // Increment counter and add estimated quantities to the storage variables
- cpt++;
- vel_est += vel_estimated_baseframe; // Linear velocity
- xyz_est += xyz_estimated; // Position
-
- double r_foot = 0.0155; // 31mm of diameter on meshlab
- if (j <= 1) {
- vel_est(0, 0) += r_foot * (actuators_vel_(1 + 3 * j, 0) - actuators_vel_(2 + 3 * j, 0));
- } else {
- vel_est(0, 0) += r_foot * (actuators_vel_(1 + 3 * j, 0) + actuators_vel_(2 + 3 * j, 0));
- }
- }
+ if (!initialized_) {
+ IMUYawOffset_ = IMURpy_(2);
+ initialized_ = true;
}
+ IMURpy_(2) -= IMUYawOffset_;
- // If at least one foot is in contact, we do the average of feet results
- if (cpt > 0) {
- FK_lin_vel_ = vel_est / cpt;
- FK_xyz_ = xyz_est / cpt;
+ if (solo3D_) {
+ IMURpy_.tail(1) = perfectPosition.tail(1);
}
+
+ IMUQuat_ = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(IMURpy_(0), IMURpy_(1), IMURpy_(2)));
}
-void Estimator::get_xyz_feet(Eigen::Matrix<double, 1, 4> const& feet_status, Matrix34 const& goals) {
- int cpt = 0;
- Vector3 xyz_feet = Vector3::Zero();
+void Estimator::updateJointData(Vector12 const& q, Vector12 const& v) {
+ qActuators_ = q;
+ vActuators_ = v;
+}
- // Consider only feet in contact
- for (int j = 0; j < 4; j++) {
- if (feet_status(0, j) == 1.0) {
- cpt++;
- xyz_feet += goals.col(j);
+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 at least one foot is in contact, we do the average of feet results
- if (cpt > 0) {
- xyz_mean_feet_ = xyz_feet / cpt;
+ if (nContactFeet > 0) {
+ baseVelocityFK_ = baseVelocityEstimate / nContactFeet;
+ basePositionFK_ = basePositionEstimate / nContactFeet;
}
}
-Vector3 Estimator::BaseVelocityFromKinAndIMU(int contactFrameId) {
- Vector3 frameVelocity = pinocchio::getFrameVelocity(model_, data_, contactFrameId, pinocchio::LOCAL).linear();
- pinocchio::updateFramePlacement(model_, data_, contactFrameId);
-
- // Angular velocity of the base wrt the world in the base frame (Gyroscope)
- Vector3 _1w01 = IMU_ang_vel_;
- // Linear velocity of the foot wrt the base in the foot frame
- Vector3 _Fv1F = frameVelocity;
- // Level arm between the base and the foot
- Vector3 _1F = data_.oMf[contactFrameId].translation();
- // Orientation of the foot wrt the base
- Matrix3 _1RF = data_.oMf[contactFrameId].rotation();
- // Linear velocity of the base wrt world in the base frame
- Vector3 _1v01 = _1F.cross(_1w01) - _1RF * _Fv1F;
-
- // IMU and base frames have the same orientation
- // _iv0i = _1v01 + self.cross3(self._1Mi.translation.ravel(), _1w01.ravel())
-
- return _1v01;
-}
-
-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& q_perfect, Vector3 const& b_baseVel_perfect) {
- 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++;
- }
-
- // Update IMU data
- get_data_IMU(baseLinearAcceleration, baseAngularVelocity, baseOrientation, q_perfect);
-
- // Angular position of the trunk
- Vector4 filt_ang_pos = IMU_ang_pos_.coeffs();
-
- // Angular velocity of the trunk
- Vector3 filt_ang_vel = IMU_ang_vel_;
-
- // Update joints data
- get_data_joints(q_mes, v_mes);
+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();
- // Update nb of iterations since contact
- 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(feet_status_);
-
- // Update forward geometry data
- 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;
- double b = static_cast<double>(remaining_steps);
- const double n = 1; // Nb of steps of margin around contact switch
+ return contactFrame.translation().cross(IMUAngularVelocity_) - contactFrame.rotation() * frameVelocity;
+}
- const double v_max = 1.00; // Maximum alpha value
- const double v_min = 0.97; // Minimum alpha value
- double c = ((a + b) - 2 * n) * 0.5;
- double alpha = 0.0;
- if (a <= (n - 1) || b <= n) { // If we are close from contact switch
- alpha = v_max; // Only trust IMU data
- } else {
- alpha = v_min + (v_max - v_min) * std::abs(c - (a - n)) / c;
- // self.alpha = 0.997
- }
+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));
- // Use cascade of complementary filters
+ return basePosition;
+}
- // Base velocity estimated by FK, estimated by motion capture
- if (solo3D) {
- FK_lin_vel_ = b_baseVel_perfect;
+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;
+}
- // Rotation matrix to go from base frame to world frame
- Matrix3 oRb = IMU_ang_pos_.toRotationMatrix();
-
- // Get FK estimated velocity at IMU location (base frame)
- Vector3 cross_product = (_1Mi_.translation()).cross(IMU_ang_vel_);
- Vector3 i_FK_lin_vel = FK_lin_vel_ + cross_product;
-
- // Get FK estimated velocity at IMU location (world frame)
- Vector3 oi_FK_lin_vel = oRb * i_FK_lin_vel;
-
- // Integration of IMU acc at IMU location (world frame)
- Vector3 oi_filt_lin_vel = filter_xyz_vel_.compute(oi_FK_lin_vel, oRb * IMU_lin_acc_, alpha * Vector3::Ones());
+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;
+}
- // Filtered estimated velocity at IMU location (base frame)
- Vector3 i_filt_lin_vel = oRb.transpose() * oi_filt_lin_vel;
+void Estimator::estimateVelocity(Vector3 const& b_perfectVelocity) {
+ Vector3 alpha = Vector3::Ones() * computeAlphaVelocity();
+ Matrix3 oRb = IMUQuat_.toRotationMatrix();
+ Vector3 bTi = (b_M_IMU_.translation()).cross(IMUAngularVelocity_);
- // Filtered estimated velocity at center base (base frame)
- b_filt_lin_vel_ = i_filt_lin_vel - cross_product;
+ // 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);
- // Filtered estimated velocity at center base (world frame)
- Vector3 ob_filt_lin_vel = oRb * b_filt_lin_vel_;
+ // At base location in base frame
+ b_baseVelocity_ = oRb.transpose() * oi_baseVelocity - bTi;
- // Position of the center of the base from FGeometry and filtered velocity (world frame)
- Vector3 filt_lin_pos = Vector3::Zero();
+ vEstimate_.head(3) = perfectEstimator_ ? b_perfectVelocity : b_baseVelocity_;
+ vEstimate_.segment(3, 3) = IMUAngularVelocity_;
+ vEstimate_.tail(12) = vActuators_;
+}
- if (solo3D) {
- Vector3 offset_ = Vector3::Zero();
- // offset_.tail(3) << -0.0155;
- filt_lin_pos = filter_xyz_pos_.compute(q_perfect.head(3) - offset_, ob_filt_lin_vel, Vector3(0.995, 0.995, 0.9));
- } else {
- filt_lin_pos = filter_xyz_pos_.compute(FK_xyz_ + xyz_mean_feet_, ob_filt_lin_vel, Vector3(0.995, 0.995, 0.9));
- }
+void Estimator::estimatePosition(Vector3 const& perfectPosition) {
+ Matrix3 oRb = IMUQuat_.toRotationMatrix();
- // Output filtered position vector (19 x 1)
- q_filt_.head(3) = filt_lin_pos;
- if (perfect_estimator || solo3D) {
- q_filt_(2, 0) = q_perfect(2, 0); // Minus feet radius
- }
- q_filt_.block(3, 0, 4, 1) = filt_ang_pos;
- q_filt_.tail(12) = actuators_pos_; // Actuators pos are already directly from PyBullet
+ Vector3 basePosition = solo3D_ ? perfectPosition : (basePositionFK_ + feetPositionBarycenter_);
+ qEstimate_.head(3) = positionFilter_.compute(basePosition, oRb * b_baseVelocity_, alphaPos_);
- // Output filtered velocity vector (18 x 1)
- // Linear velocities directly from PyBullet if perfect estimator
- v_filt_.head(3) = perfect_estimator ? b_baseVel_perfect : 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
+ 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(perfect_estimator ? b_baseVel_perfect(0) : b_filt_lin_vel_(0));
- vy_queue_.push_front(perfect_estimator ? b_baseVel_perfect(1) : b_filt_lin_vel_(1));
- vz_queue_.push_front(perfect_estimator ? b_baseVel_perfect(2) : 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_;
- /*
- wR_queue_.pop_back();
- wP_queue_.pop_back();
- wY_queue_.pop_back();
- wR_queue_.push_front(filt_ang_vel(0));
- wP_queue_.push_front(filt_ang_vel(1));
- wY_queue_.push_front(filt_ang_vel(2));
- v_filt_bis_(3) = std::accumulate(wR_queue_.begin(), wR_queue_.end(), 0.0) / N_queue_;
- v_filt_bis_(4) = std::accumulate(wP_queue_.begin(), wP_queue_.end(), 0.0) / N_queue_;
- v_filt_bis_(5) = std::accumulate(wY_queue_.begin(), wY_queue_.end(), 0.0) / N_queue_;*/
- v_filt_bis_.tail(3) = filt_ang_vel; // No filtering for angular velocity
- //////
-
- // Update model used for the forward kinematics
- /*pin.forwardKinematics(self.model, self.data, q_up__filt, self.v_filt)
- pin.updateFramePlacements(self.model, self.data)
-
- z_min = 100
- for i in (np.where(feet_status == 1))[0]: // Consider only feet in contact
- // Estimated position of the base using the considered foot
- framePlacement = pin.updateFramePlacement(self.model, self.data, self.indexes[i])
- z_min = np.min((framePlacement.translation[2], z_min))
- q_up__filt[2, 0] -= z_min*/
-
- //////
-
- // 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_++;
-}
-
-int Estimator::security_check(VectorN const& tau_ff) {
- if (((q_filt_.tail(12).cwiseAbs()).array() > q_security_.array()).any()) { // Test position limits
- std::cout << "Position limit error " << ((q_filt_.tail(12).cwiseAbs()).array() > q_security_.array()).transpose() << std::endl;
- return 1;
- } else if (((v_secu_.cwiseAbs()).array() > 100.0).any()) { // Test velocity limits
- std::cout << "Velocity limit error " << ((v_secu_.cwiseAbs()).array() > 100.0).transpose() << std::endl;
- return 2;
- } else if (((tau_ff.cwiseAbs()).array() > 8.0).any()) { // Test feedforward torques limits
- std::cout << "feedforward limit error " << ((tau_ff.cwiseAbs()).array() > 8.0).transpose() << std::endl;
- return 3;
- }
- return 0;
-}
-
-void Estimator::updateState(VectorN const& joystick_v_ref, Gait& gait) {
- // TODO: Joystick velocity given in base frame and not in horizontal frame (case of non flat ground)
-
- // Update reference acceleration vector
- a_ref_.head(3) =
- (joystick_v_ref.head(3) - pinocchio::rpy::rpyToMatrix(0.0, 0.0, -v_ref_[5] * dt_wbc) * v_ref_.head(3)) / dt_wbc;
- a_ref_.tail(3) =
- (joystick_v_ref.tail(3) - pinocchio::rpy::rpyToMatrix(0.0, 0.0, -v_ref_[5] * dt_wbc) * v_ref_.tail(3)) / dt_wbc;
-
- // Update reference velocity vector
- v_ref_.head(3) = joystick_v_ref.head(3);
- v_ref_.tail(3) = joystick_v_ref.tail(3);
-
- // Update position and velocity state vectors
-
- // Integration to get evolution of perfect x, y and yaw
- Matrix2 Ryaw;
- Ryaw << cos(yaw_estim_), -sin(yaw_estim_), sin(yaw_estim_), cos(yaw_estim_);
- v_up_.head(2) = Ryaw * v_ref_.head(2);
- q_up_.head(2) = q_up_.head(2) + v_up_.head(2) * dt_wbc;
-
- // Mix perfect x and y with height measurement
- q_up_[2] = q_filt_dyn_[2];
-
- // Mix perfect yaw with pitch and roll measurements
- v_up_[5] = v_ref_[5];
- yaw_estim_ += v_ref_[5] * dt_wbc;
- q_up_.block(3, 0, 3, 1) << IMU_RPY_[0], IMU_RPY_[1], yaw_estim_;
-
- // Transformation matrices between world and base frames
- oRb_ = pinocchio::rpy::rpyToMatrix(IMU_RPY_(0, 0), IMU_RPY_(1, 0), yaw_estim_);
-
- // Actuators measurements
- q_up_.tail(12) = q_filt_dyn_.tail(12);
- v_up_.tail(12) = v_filt_dyn_.tail(12);
-
- // Velocities are the one estimated by the estimator
- hRb_ = pinocchio::rpy::rpyToMatrix(IMU_RPY_[0], IMU_RPY_[1], 0.0);
-
- // Express estimated velocity and filtered estimated velocity in horizontal frame
- h_v_.head(3) = hRb_ * v_filt_.block(0, 0, 3, 1);
- h_v_.tail(3) = hRb_ * v_filt_.block(3, 0, 3, 1);
- h_v_windowed_.head(3) = hRb_ * v_filt_bis_.block(0, 0, 3, 1);
- h_v_windowed_.tail(3) = hRb_ * v_filt_bis_.block(3, 0, 3, 1);
-
- // Transformation matrices between world and horizontal frames
- oRh_ = Matrix3::Identity();
- oRh_.block(0, 0, 2, 2) << cos(yaw_estim_), -sin(yaw_estim_), sin(yaw_estim_), cos(yaw_estim_);
- oTh_ << q_up_[0], q_up_[1], 0.0;
+ 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_;
}