add imu estimator

controllers/unitree_guide_controller/CMakeLists.txt

@@ -37,6 +37,7 @@ add_library(${PROJECT_NAME} SHARED
         src/robot/RobotLeg.cpp
 
         src/control/Estimator.cpp
+        src/control/LowPassFilter.cpp
 
         src/quadProgpp/Array.cc
         src/quadProgpp/QuadProg++.cc

controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h

@@ -76,6 +76,9 @@ namespace unitree_guide_controller {
         std::vector<std::string> command_interface_types_;
         std::vector<std::string> state_interface_types_;
 
+        std::string imu_name_;
+        std::vector<std::string> imu_interface_types_;
+
         rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
         rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
 

controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h

@@ -4,6 +4,7 @@
 
 #ifndef MATHTOOLS_H
 #define MATHTOOLS_H
+#include <iostream>
 
 template<typename T0, typename T1, typename T2>
 T1 invNormalize(const T0 value, const T1 min, const T2 max,
@@ -11,4 +12,26 @@ T1 invNormalize(const T0 value, const T1 min, const T2 max,
     return (value - minLim) * (max - min) / (maxLim - minLim) + min;
 }
 
+template<typename T>
+T windowFunc(const T x, const T windowRatio, const T xRange = 1.0,
+             const T yRange = 1.0) {
+    if (x < 0 || x > xRange) {
+        std::cout << "[ERROR][windowFunc] The x=" << x
+                << ", which should between [0, xRange]" << std::endl;
+    }
+    if (windowRatio <= 0 || windowRatio >= 0.5) {
+        std::cout << "[ERROR][windowFunc] The windowRatio=" << windowRatio
+                << ", which should between [0, 0.5]" << std::endl;
+    }
+
+    if (x / xRange < windowRatio) {
+        return x * yRange / (xRange * windowRatio);
+    }
+    if (x / xRange > 1 - windowRatio) {
+        return yRange * (xRange - x) / (xRange * windowRatio);
+    }
+    return yRange;
+}
+
+
 #endif //MATHTOOLS_H

controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h

@@ -31,6 +31,9 @@ struct CtrlComponent {
     std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
     joint_velocity_state_interface_;
 
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    imu_state_interface_;
+
     control_input_msgs::msg::Inputs default_inputs_;
     std::reference_wrapper<control_input_msgs::msg::Inputs> control_inputs_;
     int frequency_{};
@@ -51,6 +54,8 @@ struct CtrlComponent {
         joint_effort_state_interface_.clear();
         joint_position_state_interface_.clear();
         joint_velocity_state_interface_.clear();
+
+        imu_state_interface_.clear();
     }
 };
 

controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h

@@ -4,13 +4,87 @@
 
 #ifndef ESTIMATOR_H
 #define ESTIMATOR_H
+#include <eigen3/Eigen/Dense>
+#include <kdl/frames.hpp>
 
-
+struct CtrlComponent;
 
 class Estimator {
+public:
+    Estimator();
 
+    ~Estimator() = default;
+
+    /**
+     * Get the estimated robot central position
+     * @return robot central position
+     */
+    Eigen::Matrix<double, 3, 1> getPosition() {
+        return _xhat.segment(0, 3);
+    }
+
+    /**
+     * Get the estimated robot central velocity
+     * @return robot central velocity
+     */
+    Eigen::Matrix<double, 3, 1> getVelocity() {
+        return _xhat.segment(3, 3);
+    }
+
+    void run(
+        const CtrlComponent &ctrlComp,
+        std::vector<KDL::Frame> feet_poses_,
+        std::vector<KDL::Vector> feet_vels_,
+        const std::vector<int> &contact,
+        const std::vector<double> &phase
+    );
+
+private:
+    void init();
+
+    Eigen::Matrix<double, 18, 1> _xhat; // The state of estimator, position(3)+velocity(3)+feet position(3x4)
+
+    Eigen::Matrix<double, 3, 1> _u; // The input of estimator
+
+    Eigen::Matrix<double, 28, 1> _y; // The measurement value of output y
+    Eigen::Matrix<double, 28, 1> _yhat; // The prediction of output y
+    Eigen::Matrix<double, 18, 18> _A; // The transtion matrix of estimator
+    Eigen::Matrix<double, 18, 3> _B; // The input matrix
+    Eigen::Matrix<double, 28, 18> _C; // The output matrix
+    // Covariance Matrix
+    Eigen::Matrix<double, 18, 18> _P; // Prediction covariance
+    Eigen::Matrix<double, 18, 18> _Ppriori; // Priori prediction covariance
+    Eigen::Matrix<double, 18, 18> _Q; // Dynamic simulation covariance
+    Eigen::Matrix<double, 28, 28> _R; // Measurement covariance
+    Eigen::Matrix<double, 18, 18>
+    _QInit; // Initial value of Dynamic simulation covariance
+    Eigen::Matrix<double, 28, 28>
+    _RInit; // Initial value of Measurement covariance
+    Eigen::Matrix<double, 18, 1> _Qdig; // adjustable process noise covariance
+    Eigen::Matrix<double, 3, 3> _Cu; // The covariance of system input u
+    // Output Measurement
+    Eigen::Matrix<double, 12, 1>
+    _feetPos2Body; // The feet positions to body, in the global coordinate
+    Eigen::Matrix<double, 12, 1>
+    _feetVel2Body; // The feet velocity to body, in the global coordinate
+    Eigen::Matrix<double, 4, 1>
+    _feetH; // The Height of each foot, in the global coordinate
+    Eigen::Matrix<double, 28, 28> _S; // _S = C*P*C.T + R
+    Eigen::PartialPivLU<Eigen::Matrix<double, 28, 28> > _Slu; // _S.lu()
+    Eigen::Matrix<double, 28, 1> _Sy; // _Sy = _S.inv() * (y - yhat)
+    Eigen::Matrix<double, 28, 18> _Sc; // _Sc = _S.inv() * C
+    Eigen::Matrix<double, 28, 28> _SR; // _SR = _S.inv() * R
+    Eigen::Matrix<double, 28, 18> _STC; // _STC = (_S.transpose()).inv() * C
+    Eigen::Matrix<double, 18, 18> _IKC; // _IKC = I - KC
+
+    KDL::Vector g_;
+
+    std::vector<KDL::Frame> feet_poses_;
+    std::vector<KDL::Vector> feet_vels_;
+
+    double _trust;
+    double _largeVariance;
 };
 
 
-
 #endif //ESTIMATOR_H

controllers/unitree_guide_controller/include/unitree_guide_controller/control/LowPassFilter.h

@@ -0,0 +1,29 @@
+//
+// Created by biao on 24-9-16.
+//
+
+
+#ifndef LOWPASSFILTER_H
+#define LOWPASSFILTER_H
+
+
+class LowPassFilter {
+public:
+    LowPassFilter(double samplePeriod, double cutFrequency);
+
+    ~LowPassFilter() = default;
+
+    void addValue(double newValue);
+
+    [[nodiscard]] double getValue() const;
+
+    void clear();
+
+private:
+    double weight_;
+    double pass_value_{};
+    bool start_;
+};
+
+
+#endif //LOWPASSFILTER_H

controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h

@@ -58,6 +58,19 @@ public:
     [[nodiscard]] KDL::JntArray getTorque(
         const KDL::Wrenches &force, int index) const;
 
+    /**
+     * Calculate the foot end velocity
+     * @param index leg index
+     * @return velocity vector
+     */
+    [[nodiscard]] KDL::Vector getFeet2BVelocities (int index) const;
+
+    /**
+     * Calculate all foot end velocity
+     * @return list of foot end velocity
+     */
+    [[nodiscard]] std::vector<KDL::Vector> getFeet2BVelocities () const;
+
     std::vector<KDL::JntArray> current_joint_pos_;
     std::vector<KDL::JntArray> current_joint_vel_;
 

controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp

@@ -92,14 +92,10 @@ void StateSwingTest::positionCtrl() {
 
 void StateSwingTest::torqueCtrl() const {
     const KDL::Frame fr_current_pos = ctrlComp_.robot_model_.get().getFeet2BPositions(0);
-    KDL::Jacobian fr_jaco = ctrlComp_.robot_model_.get().getJacobian(0);
 
     const KDL::Vector pos_goal = fr_goal_pos_.p;
     const KDL::Vector pos0 = fr_current_pos.p;
-    const Eigen::Matrix<double, 3, Eigen::Dynamic> linear_jacobian = fr_jaco.data.topRows(3);
+    const KDL::Vector vel0 = ctrlComp_.robot_model_.get().getFeet2BVelocities(0);
-    Eigen::Product<Eigen::Matrix<double, 3, -1>, Eigen::Matrix<double, -1, 1> > vel_eigen =
-            linear_jacobian * ctrlComp_.robot_model_.get().current_joint_vel_[0].data;
-    const KDL::Vector vel0(vel_eigen(0), vel_eigen(1), vel_eigen(2));
 
     const KDL::Vector force0 = Kp * (pos_goal - pos0) + Kd * (-vel0);
     const KDL::Wrench wrench0(force0, KDL::Vector::Zero()); // 假设力矩为零

controllers/unitree_guide_controller/src/UnitreeGuideController.cpp

@@ -32,6 +32,10 @@ namespace unitree_guide_controller {
             }
         }
 
+        for (const auto &interface_type: imu_interface_types_) {
+            conf.names.push_back(imu_name_ + "/" += interface_type);
+        }
+
         return conf;
     }
 
@@ -65,7 +69,10 @@ namespace unitree_guide_controller {
             state_interface_types_ =
                     auto_declare<std::vector<std::string> >("state_interfaces", state_interface_types_);
 
-            // Initialize variables and pointers
+            // imu sensor
+            imu_name_ = auto_declare<std::string>("imu_name", imu_name_);
+            imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_);
+
         } catch (const std::exception &e) {
             fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
             return controller_interface::CallbackReturn::ERROR;
@@ -111,8 +118,12 @@ namespace unitree_guide_controller {
 
         // assign state interfaces
         for (auto &interface: state_interfaces_) {
+            if (interface.get_prefix_name() == imu_name_) {
+                ctrl_comp_.imu_state_interface_.emplace_back(interface);
+            } else {
                 state_interface_map_[interface.get_interface_name()]->push_back(interface);
             }
+        }
 
         state_list_.passive = std::make_shared<StatePassive>(ctrl_comp_);
         state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_);

controllers/unitree_guide_controller/src/control/Estimator.cpp

@@ -3,3 +3,49 @@
 //
 
 #include "unitree_guide_controller/control/Estimator.h"
+#include "unitree_guide_controller/control/CtrlComponent.h"
+#include <unitree_guide_controller/common/mathTools.h>
+
+Estimator::Estimator() {
+    g_ = KDL::Vector(0, 0, -9.81);
+}
+
+void Estimator::run(const CtrlComponent &ctrlComp, std::vector<KDL::Frame> feet_poses_,
+                    std::vector<KDL::Vector> feet_vels_,
+                    const std::vector<int> &contact, const std::vector<double> &phase) {
+    _Q = _QInit;
+    _R = _RInit;
+
+    // Adjust the covariance based on foot contact and phase.
+    for (int i(0); i < 4; ++i) {
+        if (contact[i] == 0) {
+            _Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3);
+            _R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3);
+            _R(24 + i, 24 + i) = _largeVariance;
+        } else {
+            _trust = windowFunc(phase[i], 0.2);
+            _Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) =
+                    (1 + (1 - _trust) * _largeVariance) *
+                    _QInit.block(6 + 3 * i, 6 + 3 * i, 3, 3);
+            _R.block(12 + 3 * i, 12 + 3 * i, 3, 3) =
+                    (1 + (1 - _trust) * _largeVariance) *
+                    _RInit.block(12 + 3 * i, 12 + 3 * i, 3, 3);
+            _R(24 + i, 24 + i) =
+                    (1 + (1 - _trust) * _largeVariance) * _RInit(24 + i, 24 + i);
+        }
+        _feetPos2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(feet_poses_[i].p.data);
+        _feetVel2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(feet_vels_[i].data);
+    }
+
+    // Acceleration from imu as system input
+    const KDL::Rotation rotation = KDL::Rotation::Quaternion(ctrlComp.imu_state_interface_[7].get().get_value(),
+                                                             ctrlComp.imu_state_interface_[8].get().get_value(),
+                                                             ctrlComp.imu_state_interface_[9].get().get_value(),
+                                                             ctrlComp.imu_state_interface_[6].get().get_value());
+    const KDL::Vector acc(ctrlComp.imu_state_interface_[3].get().get_value(),
+                          ctrlComp.imu_state_interface_[4].get().get_value(),
+                          ctrlComp.imu_state_interface_[5].get().get_value());
+    _u = Eigen::Map<Eigen::Vector3d>((rotation * acc + g_).data);
+    _xhat = _A * _xhat + _B * _u;
+    _yhat = _C * _xhat;
+}

controllers/unitree_guide_controller/src/control/LowPassFilter.cpp

@@ -0,0 +1,31 @@
+//
+// Created by biao on 24-9-16.
+//
+#include <cmath>
+#include "unitree_guide_controller/control/LowPassFilter.h"
+
+/**
+ * Low Pass Filter to prevent high frequency signal
+ * @param samplePeriod sample period
+ * @param cutFrequency cut frequency
+ */
+LowPassFilter::LowPassFilter(const double samplePeriod, const double cutFrequency) {
+    weight_ = 1.0 / (1.0 + 1.0 / (2.0 * M_PI * samplePeriod * cutFrequency));
+    start_ = false;
+}
+
+void LowPassFilter::addValue(const double newValue) {
+    if (!start_) {
+        start_ = true;
+        pass_value_ = newValue;
+    }
+    pass_value_ = weight_ * newValue + (1 - weight_) * pass_value_;
+}
+
+double LowPassFilter::getValue() const {
+    return pass_value_;
+}
+
+void LowPassFilter::clear() {
+    start_ = false;
+}

controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp

@@ -64,6 +64,21 @@ KDL::JntArray QuadrupedRobot::getTorque(
     return robot_legs_[index]->calcTorque(current_joint_pos_[index], current_joint_vel_[index], force);
 }
 
+KDL::Vector QuadrupedRobot::getFeet2BVelocities(const int index) const {
+    const Eigen::Matrix<double, 3, Eigen::Dynamic> jacobian = getJacobian(index).data.topRows(3);
+    Eigen::VectorXd foot_velocity = jacobian * current_joint_vel_[index].data;
+    return {foot_velocity(0), foot_velocity(1), foot_velocity(2)};
+}
+
+std::vector<KDL::Vector> QuadrupedRobot::getFeet2BVelocities() const {
+    std::vector<KDL::Vector> result;
+    result.resize(4);
+    for (int i = 0; i < 4; i++) {
+        result[i] = getFeet2BVelocities(i);
+    }
+    return result;
+}
+
 void QuadrupedRobot::update(const CtrlComponent &ctrlComp) {
     for (int i = 0; i < 4; i++) {
         KDL::JntArray pos_array(3);

descriptions/go2_description/config/robot_control.yaml

@@ -20,6 +20,7 @@ imu_sensor_broadcaster:
 
 unitree_guide_controller:
   ros__parameters:
+    update_rate: 500  # Hz
     joints:
       - FR_hip_joint
       - FR_thigh_joint
@@ -45,3 +46,17 @@ unitree_guide_controller:
       - effort
       - position
       - velocity
+
+    imu_name: "imu_sensor"
+
+    imu_interfaces:
+      - angular_velocity.x
+      - angular_velocity.y
+      - angular_velocity.z
+      - linear_acceleration.x
+      - linear_acceleration.y
+      - linear_acceleration.z
+      - orientation.w
+      - orientation.x
+      - orientation.y
+      - orientation.z