add estimator

2024-10-15 20:17:03 +08:00 · 2024-10-15 20:17:03 +08:00 · 07e3cf086e
parent 45fde17425
commit 07e3cf086e
19 changed files with 1087 additions and 62 deletions
--- a/controllers/rl_quadruped_controller/CMakeLists.txt
+++ b/controllers/rl_quadruped_controller/CMakeLists.txt
@ -25,6 +25,7 @@ set(dependencies
        controller_interface
        realtime_tools
        control_input_msgs
+        kdl_parser
 )

 # find dependencies
@ -33,7 +34,7 @@ foreach (Dependency IN ITEMS ${dependencies})
 endforeach ()

 add_library(${PROJECT_NAME} SHARED
-        src/LeggedGymController.cpp
+        src/RlQuadrupedController.cpp

        src/common/ObservationBuffer.cpp

@ -41,6 +42,13 @@ add_library(${PROJECT_NAME} SHARED
        src/FSM/StateFixedStand.cpp
        src/FSM/StateFixedDown.cpp
        src/FSM/StateRL.cpp
+
+        src/control/LowPassFilter.cpp
+        src/control/Estimator.cpp
+
+        src/robot/QuadrupedRobot.cpp
+        src/robot/RobotLeg.cpp
+
 )
 target_include_directories(${PROJECT_NAME}
        PUBLIC
--- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h
+++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h
@ -65,10 +65,10 @@ struct Control
 struct ModelParams {
    std::string model_name;
    std::string framework;
-    bool use_history;
    int decimation;
    int num_observations;
    std::vector<std::string> observations;
+    std::vector<int> observations_history;
    double damping;
    double stiffness;
    double action_scale;
--- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTools.h
+++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTools.h
@ -0,0 +1,116 @@
+//
+// Created by tlab-uav on 24-9-12.
+//
+
+#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,
+                const double minLim = -1, const double maxLim = 1) {
+    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;
+}
+
+inline Mat3 skew(const Vec3 &v) {
+    Mat3 m;
+    m << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
+    return m;
+}
+
+inline Vec3 rotMatToExp(const RotMat &rm) {
+    double cosValue = rm.trace() / 2.0 - 1 / 2.0;
+    if (cosValue > 1.0f) {
+        cosValue = 1.0f;
+    } else if (cosValue < -1.0f) {
+        cosValue = -1.0f;
+    }
+
+    const double angle = acos(cosValue);
+    Vec3 exp;
+    if (fabs(angle) < 1e-5) {
+        exp = Vec3(0, 0, 0);
+    } else if (fabs(angle - M_PI) < 1e-5) {
+        exp = angle * Vec3(rm(0, 0) + 1, rm(0, 1), rm(0, 2)) /
+              sqrt(2 * (1 + rm(0, 0)));
+    } else {
+        exp = angle / (2.0f * sin(angle)) *
+              Vec3(rm(2, 1) - rm(1, 2), rm(0, 2) - rm(2, 0), rm(1, 0) - rm(0, 1));
+    }
+    return exp;
+}
+
+template<typename T>
+T saturation(const T a, Vec2 limits) {
+    T lowLim, highLim;
+    if (limits(0) > limits(1)) {
+        lowLim = limits(1);
+        highLim = limits(0);
+    } else {
+        lowLim = limits(0);
+        highLim = limits(1);
+    }
+
+    if (a < lowLim) {
+        return lowLim;
+    }
+    if (a > highLim) {
+        return highLim;
+    }
+    return a;
+}
+
+inline RotMat quatToRotMat(const Quat &q) {
+    double e0 = q(0);
+    double e1 = q(1);
+    double e2 = q(2);
+    double e3 = q(3);
+
+    RotMat R;
+    R << 1 - 2 * (e2 * e2 + e3 * e3), 2 * (e1 * e2 - e0 * e3),
+            2 * (e1 * e3 + e0 * e2), 2 * (e1 * e2 + e0 * e3),
+            1 - 2 * (e1 * e1 + e3 * e3), 2 * (e2 * e3 - e0 * e1),
+            2 * (e1 * e3 - e0 * e2), 2 * (e2 * e3 + e0 * e1),
+            1 - 2 * (e1 * e1 + e2 * e2);
+    return R;
+}
+
+inline Vec3 rotMatToRPY(const Mat3 &R) {
+    Vec3 rpy;
+    rpy(0) = atan2(R(2, 1), R(2, 2));
+    rpy(1) = asin(-R(2, 0));
+    rpy(2) = atan2(R(1, 0), R(0, 0));
+    return rpy;
+}
+
+inline RotMat rotz(const double &theta) {
+    const double s = std::sin(theta);
+    const double c = std::cos(theta);
+
+    RotMat R;
+    R << c, -s, 0, s, c, 0, 0, 0, 1;
+    return R;
+}
+
+#endif //MATHTOOLS_H
--- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTypes.h
+++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTypes.h
@ -0,0 +1,94 @@
+/**********************************************************************
+ Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved.
+***********************************************************************/
+#ifndef MATHTYPES_H
+#define MATHTYPES_H
+
+#include <eigen3/Eigen/Dense>
+
+/************************/
+/******** Vector ********/
+/************************/
+// 2x1 Vector
+using Vec2 = Eigen::Matrix<double, 2, 1>;
+
+// 3x1 Vector
+using Vec3 = Eigen::Matrix<double, 3, 1>;
+
+// 4x1 Vector
+using Vec4 = Eigen::Matrix<double, 4, 1>;
+
+// 6x1 Vector
+using Vec6 = Eigen::Matrix<double, 6, 1>;
+
+// Quaternion
+using Quat = Eigen::Matrix<double, 4, 1>;
+
+// 4x1 Integer Vector
+using VecInt4 = Eigen::Matrix<int, 4, 1>;
+
+// 12x1 Vector
+using Vec12 = Eigen::Matrix<double, 12, 1>;
+
+// 18x1 Vector
+using Vec18 = Eigen::Matrix<double, 18, 1>;
+
+// Dynamic Length Vector
+using VecX = Eigen::Matrix<double, Eigen::Dynamic, 1>;
+
+/************************/
+/******** Matrix ********/
+/************************/
+// Rotation Matrix
+using RotMat = Eigen::Matrix<double, 3, 3>;
+
+// Homogenous Matrix
+using HomoMat = Eigen::Matrix<double, 4, 4>;
+
+// 2x2 Matrix
+using Mat2 = Eigen::Matrix<double, 2, 2>;
+
+// 3x3 Matrix
+using Mat3 = Eigen::Matrix<double, 3, 3>;
+
+// 3x3 Identity Matrix
+#define I3 Eigen::MatrixXd::Identity(3, 3)
+
+// 3x4 Matrix, each column is a 3x1 vector
+using Vec34 = Eigen::Matrix<double, 3, 4>;
+
+// 6x6 Matrix
+using Mat6 = Eigen::Matrix<double, 6, 6>;
+
+// 12x12 Matrix
+using Mat12 = Eigen::Matrix<double, 12, 12>;
+
+// 12x12 Identity Matrix
+#define I12 Eigen::MatrixXd::Identity(12, 12)
+
+// 18x18 Identity Matrix
+#define I18 Eigen::MatrixXd::Identity(18, 18)
+
+// Dynamic Size Matrix
+using MatX = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
+
+/************************/
+/****** Functions *******/
+/************************/
+inline Vec34 vec12ToVec34(Vec12 vec12) {
+  Vec34 vec34;
+  for (int i(0); i < 4; ++i) {
+    vec34.col(i) = vec12.segment(3 * i, 3);
+  }
+  return vec34;
+}
+
+inline Vec12 vec34ToVec12(Vec34 vec34) {
+  Vec12 vec12;
+  for (int i(0); i < 4; ++i) {
+    vec12.segment(3 * i, 3) = vec34.col(i);
+  }
+  return vec12;
+}
+
+#endif  // MATHTYPES_H
--- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h
+++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h
@ -10,6 +10,8 @@
 #include <hardware_interface/loaned_state_interface.hpp>
 #include <control_input_msgs/msg/inputs.hpp>

+#include "Estimator.h"
+
 struct CtrlComponent {
    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
    joint_torque_command_interface_;
@ -39,8 +41,10 @@ struct CtrlComponent {
    control_input_msgs::msg::Inputs control_inputs_;
    int frequency_{};

-    CtrlComponent() {
-    }
+    std::shared_ptr<QuadrupedRobot> robot_model_;
+    std::shared_ptr<Estimator> estimator_;
+
+    CtrlComponent() = default;

    void clear() {
        joint_torque_command_interface_.clear();
--- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h
+++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h
@ -0,0 +1,158 @@
+//
+// Created by biao on 24-9-14.
+//
+
+#ifndef ESTIMATOR_H
+#define ESTIMATOR_H
+#include <memory>
+#include <kdl/frames.hpp>
+#include <rl_quadruped_controller/common/mathTypes.h>
+#include <rl_quadruped_controller/robot/QuadrupedRobot.h>
+
+#include "LowPassFilter.h"
+
+class QuadrupedRobot;
+struct CtrlComponent;
+
+class Estimator {
+public:
+    explicit Estimator(CtrlComponent &ctrl_component);
+
+    ~Estimator() = default;
+
+    /**
+     * Get the estimated robot central position
+     * @return robot central position
+     */
+    Vec3 getPosition() {
+        return x_hat_.segment(0, 3);
+    }
+
+    /**
+     * Get the estimated robot central velocity
+     * @return robot central velocity
+     */
+    Vec3 getVelocity() {
+        return x_hat_.segment(3, 3);
+    }
+
+    /**
+     * Get the estimated foot position in world frame
+     * @param index leg index
+     * @return foot position in world frame
+     */
+    Vec3 getFootPos(const int index) {
+        return getPosition() + rotation_ * Vec3(foot_poses_[index].p.data);
+    }
+
+    /**
+     * Get the estimated feet velocity in world frame
+     * @return feet velocity in world frame
+     */
+    Vec34 getFeetPos() {
+        Vec34 feet_pos;
+        for (int i(0); i < 4; ++i) {
+            feet_pos.col(i) = getFootPos(i);
+        }
+        return feet_pos;
+    }
+
+    /**
+     * Get the estimated feet velocity in world frame
+     * @return feet velocity in world frame
+     */
+    Vec34 getFeetVel() {
+        const std::vector<KDL::Vector> feet_vel = robot_model_->getFeet2BVelocities();
+        Vec34 result;
+        for (int i(0); i < 4; ++i) {
+            result.col(i) = Vec3(feet_vel[i].data) + getVelocity();
+        }
+        return result;
+    }
+
+    /**
+     * Get the estimated foot position in body frame
+     * @return
+     */
+    Vec34 getFeetPos2Body() {
+        Vec34 foot_pos;
+        const Vec3 body_pos = getPosition();
+        for (int i = 0; i < 4; i++) {
+            foot_pos.col(i) = getFootPos(i) - body_pos;
+        }
+        return foot_pos;
+    }
+
+    RotMat getRotation() {
+        return rotation_;
+    }
+
+    Vec3 getGyro() {
+        return gyro_;
+    }
+
+    [[nodiscard]] Vec3 getGyroGlobal() const {
+        return rotation_ * gyro_;
+    }
+
+    [[nodiscard]] double getYaw() const;
+
+    [[nodiscard]] double getDYaw() const {
+        return getGyroGlobal()(2);
+    }
+
+    void update();
+
+private:
+    CtrlComponent &ctrl_component_;
+    std::shared_ptr<QuadrupedRobot> &robot_model_;
+
+    Eigen::Matrix<double, 18, 1> x_hat_; // 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> y_hat_; // 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> feet_pos_body_; // The feet positions to body, in the global coordinate
+    Eigen::Matrix<double, 12, 1> feet_vel_body_; // The feet velocity to body, in the global coordinate
+    Eigen::Matrix<double, 4, 1> feet_h_; // 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
+
+    Vec3 g_;
+    double dt_;
+
+    RotMat rotation_;
+    Vec3 acceleration_;
+    Vec3 gyro_;
+
+    std::vector<KDL::Frame> foot_poses_;
+    std::vector<KDL::Vector> foot_vels_;
+    std::vector<std::shared_ptr<LowPassFilter> > low_pass_filters_;
+
+    double large_variance_;
+};
+
+
+#endif //ESTIMATOR_H
--- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/LowPassFilter.h
+++ b/controllers/rl_quadruped_controller/include/rl_quadruped_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
--- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/QuadrupedRobot.h
+++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/QuadrupedRobot.h
@ -0,0 +1,104 @@
+//
+// Created by biao on 24-9-12.
+//
+
+
+#ifndef QUADRUPEDROBOT_H
+#define QUADRUPEDROBOT_H
+#include <string>
+#include <kdl_parser/kdl_parser.hpp>
+#include <rl_quadruped_controller/common/mathTypes.h>
+
+#include "RobotLeg.h"
+
+
+struct CtrlComponent;
+
+class QuadrupedRobot {
+public:
+    explicit QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description,
+                            const std::vector<std::string> &feet_names, const std::string &base_name);
+
+    ~QuadrupedRobot() = default;
+
+    /**
+     * Calculate the joint positions based on the foot end position
+     * @param pEe_list vector of foot-end position
+     * @return
+     */
+    [[nodiscard]] std::vector<KDL::JntArray> getQ(const std::vector<KDL::Frame> &pEe_list) const;
+
+    [[nodiscard]] Vec12 getQ(const Vec34 &vecP) const;
+
+    Vec12 getQd(const std::vector<KDL::Frame> &pos, const Vec34 &vel);
+
+    /**
+     * Calculate the foot end position based on joint positions
+     * @return vector of foot-end position
+     */
+    [[nodiscard]] std::vector<KDL::Frame> getFeet2BPositions() const;
+
+    /**
+     * Calculate the foot end position based on joint positions
+     * @param index leg index
+     * @return foot-end position
+     */
+    [[nodiscard]] KDL::Frame getFeet2BPositions(int index) const;
+
+    /**
+     * Calculate the Jacobian matrix based on joint positions
+     * @param index leg index
+     * @return Jacobian matrix
+     */
+    [[nodiscard]] KDL::Jacobian getJacobian(int index) const;
+
+    /**
+     * Calculate the torque based on joint positions, joint velocities and external force
+     * @param force external force
+     * @param index leg index
+     * @return torque
+     */
+    [[nodiscard]] KDL::JntArray getTorque(
+        const Vec3 &force, int index) const;
+
+    /**
+    * Calculate the torque based on joint positions, joint velocities and external force
+    * @param force external force
+    * @param index leg index
+    * @return torque
+    */
+    [[nodiscard]] KDL::JntArray getTorque(
+        const KDL::Vector &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;
+
+    double mass_ = 0;
+    Vec34 feet_pos_normal_stand_;
+    std::vector<KDL::JntArray> current_joint_pos_;
+    std::vector<KDL::JntArray> current_joint_vel_;
+
+    void update();
+
+private:
+    CtrlComponent &ctrl_component_;
+    std::vector<std::shared_ptr<RobotLeg> > robot_legs_;
+
+    KDL::Chain fr_chain_;
+    KDL::Chain fl_chain_;
+    KDL::Chain rr_chain_;
+    KDL::Chain rl_chain_;
+};
+
+
+#endif //QUADRUPEDROBOT_H
--- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/RobotLeg.h
+++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/RobotLeg.h
@ -0,0 +1,59 @@
+//
+// Created by biao on 24-9-12.
+//
+
+
+#ifndef ROBOTLEG_H
+#define ROBOTLEG_H
+
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainiksolverpos_lma.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
+#include <kdl_parser/kdl_parser.hpp>
+#include <rl_quadruped_controller/common/mathTypes.h>
+
+class RobotLeg {
+public:
+    explicit RobotLeg(const KDL::Chain &chain);
+
+    ~RobotLeg() = default;
+
+    /**
+     * Use forward kinematic to calculate the Pose of End effector to Body frame.
+     * @param joint_positions Leg joint positions
+     * @return Pose of End effector to Body frame
+     */
+    [[nodiscard]] KDL::Frame calcPEe2B(const KDL::JntArray &joint_positions) const;
+
+    /**
+     * Use inverse kinematic to calculate the joint positions.
+     * @param pEe target position of end effector
+     * @param q_init current joint positions
+     * @return target joint positions
+     */
+    [[nodiscard]] KDL::JntArray calcQ(const KDL::Frame &pEe, const KDL::JntArray &q_init) const;
+
+    /**
+     * Calculate the current jacobian matrix.
+     * @param joint_positions Leg joint positions
+     * @return jacobian matrix
+     */
+    [[nodiscard]] KDL::Jacobian calcJaco(const KDL::JntArray &joint_positions) const;
+
+    /**
+     * Calculate the torque based on joint positions and end force
+     * @param joint_positions current joint positions
+     * @param force foot end force
+     * @return joint torque
+     */
+    [[nodiscard]] KDL::JntArray calcTorque(const KDL::JntArray &joint_positions, const Vec3 &force) const;
+
+protected:
+    KDL::Chain chain_;
+    std::shared_ptr<KDL::ChainFkSolverPos_recursive> fk_pose_solver_;
+    std::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_;
+    std::shared_ptr<KDL::ChainIkSolverPos_LMA> ik_pose_solver_;
+};
+
+
+#endif //ROBOTLEG_H
--- a/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp
+++ b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp
@ -45,8 +45,9 @@ StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path)
    loadYaml(config_path);

    // history
-    if (params_.use_history) {
-        history_obs_buf_ = std::make_shared<ObservationBuffer>(1, params_.num_observations, 6);
+    if (!params_.observations_history.empty())
+    {
+        history_obs_buf_ = std::make_shared<ObservationBuffer>(1, params_.num_observations, params_.observations_history.size());
    }

    model_ = torch::jit::load(config_path + "/" + params_.model_name);
@ -120,7 +121,6 @@ torch::Tensor StateRL::computeObservation() {
        if (observation == "lin_vel") {
            obs_list.push_back(obs_.lin_vel * params_.lin_vel_scale);
        } else if (observation == "ang_vel") {
-            // obs_list.push_back(obs.ang_vel * params_.ang_vel_scale); // TODO is QuatRotateInverse necessery?
            obs_list.push_back(
                quatRotateInverse(obs_.base_quat, obs_.ang_vel, params_.framework) * params_.ang_vel_scale);
        } else if (observation == "gravity_vec") {
@ -156,7 +156,14 @@ void StateRL::loadYaml(const std::string &config_path) {
    params_.framework = config["framework"].as<std::string>();
    const int rows = config["rows"].as<int>();
    const int cols = config["cols"].as<int>();
-    params_.use_history = config["use_history"].as<bool>();
+    if (config["observations_history"].IsNull())
+    {
+        params_.observations_history = {};
+    }
+    else
+    {
+        params_.observations_history = ReadVectorFromYaml<int>(config["observations_history"]);
+    }
    params_.decimation = config["decimation"].as<int>();
    params_.num_observations = config["num_observations"].as<int>();
    params_.observations = ReadVectorFromYaml<std::string>(config["observations"]);
@ -219,11 +226,14 @@ torch::Tensor StateRL::forward() {
    torch::Tensor clamped_obs = computeObservation();
    torch::Tensor actions;

-    if (params_.use_history) {
+    if (!params_.observations_history.empty())
+    {
        history_obs_buf_->insert(clamped_obs);
-        history_obs_ = history_obs_buf_->getObsVec({0, 1, 2, 3, 4, 5});
+        history_obs_ = history_obs_buf_->getObsVec(params_.observations_history);
        actions = model_.forward({history_obs_}).toTensor();
-    } else {
+    }
+    else
+    {
        actions = model_.forward({clamped_obs}).toTensor();
    }

@ -268,6 +278,7 @@ void StateRL::getState() {
 }

 void StateRL::runModel() {
+    obs_.lin_vel = torch::tensor(ctrl_comp_.estimator_->getVelocity().data()).unsqueeze(0);
    obs_.ang_vel = torch::tensor(robot_state_.imu.gyroscope).unsqueeze(0);
    obs_.commands = torch::tensor({{control_.x, control_.y, control_.yaw}});
    obs_.base_quat = torch::tensor(robot_state_.imu.quaternion).unsqueeze(0);
--- a/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp
+++ b/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp
@ -2,7 +2,7 @@
 // Created by tlab-uav on 24-10-4.
 //

-#include "LeggedGymController.h"
+#include "RlQuadrupedController.h"

 namespace rl_quadruped_controller {
    using config_type = controller_interface::interface_configuration_type;
@ -47,6 +47,13 @@ namespace rl_quadruped_controller {

    controller_interface::return_type LeggedGymController::
    update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
+        if (ctrl_comp_.robot_model_ == nullptr) {
+            return controller_interface::return_type::OK;
+        }
+
+        ctrl_comp_.robot_model_->update();
+        ctrl_comp_.estimator_->update();
+
        if (mode_ == FSMMode::NORMAL) {
            current_state_->run();
            next_state_name_ = current_state_->checkChange();
@ -77,16 +84,24 @@ namespace rl_quadruped_controller {
                    auto_declare<std::vector<std::string> >("state_interfaces", state_interface_types_);

            command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
+            base_name_ = auto_declare<std::string>("base_name", base_name_);

            // 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_);

+            // foot_force_sensor
+            foot_force_name_ = auto_declare<std::string>("foot_force_name", foot_force_name_);
+            foot_force_interface_types_ =
+                    auto_declare<std::vector<std::string> >("foot_force_interfaces", state_interface_types_);
+
            // rl config folder
            rl_config_folder_ = auto_declare<std::string>("config_folder", rl_config_folder_);

            get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
            RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", ctrl_comp_.frequency_);
+
+            ctrl_comp_.estimator_ = std::make_shared<Estimator>(ctrl_comp_);
        } catch (const std::exception &e) {
            fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
            return controller_interface::CallbackReturn::ERROR;
@ -97,6 +112,14 @@ namespace rl_quadruped_controller {

    controller_interface::CallbackReturn LeggedGymController::on_configure(
        const rclcpp_lifecycle::State & /*previous_state*/) {
+        robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(
+            "/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
+            [this](const std_msgs::msg::String::SharedPtr msg) {
+                ctrl_comp_.robot_model_ = std::make_shared<QuadrupedRobot>(
+                    ctrl_comp_, msg->data, feet_names_, base_name_);
+            });
+
+
        control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
            "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
                // Handle message
--- a/controllers/rl_quadruped_controller/src/RlQuadrupedController.h
+++ b/controllers/rl_quadruped_controller/src/RlQuadrupedController.h
@ -6,11 +6,12 @@
 #define LEGGEDGYMCONTROLLER_H
 #include <controller_interface/controller_interface.hpp>
 #include <rl_quadruped_controller/FSM/StateRL.h>
+#include <std_msgs/msg/string.hpp>

-#include "rl_quadruped_controller/FSM/StateFixedStand.h"
-#include "rl_quadruped_controller/FSM/StateFixedDown.h"
-#include "rl_quadruped_controller/FSM/StatePassive.h"
 #include "rl_quadruped_controller/control/CtrlComponent.h"
+#include "rl_quadruped_controller/FSM/StateFixedDown.h"
+#include "rl_quadruped_controller/FSM/StateFixedStand.h"
+#include "rl_quadruped_controller/FSM/StatePassive.h"

 namespace rl_quadruped_controller {
    struct FSMStateList {
@ -68,6 +69,7 @@ namespace rl_quadruped_controller {

        CtrlComponent ctrl_comp_;
        std::vector<std::string> joint_names_;
+        std::string base_name_ = "base";
        std::vector<std::string> feet_names_;
        std::vector<std::string> command_interface_types_;
        std::vector<std::string> state_interface_types_;
@ -78,8 +80,8 @@ namespace rl_quadruped_controller {
        std::string imu_name_;
        std::vector<std::string> imu_interface_types_;
        // Foot Force Sensor
-        std::string foot_force_name_;
-        std::vector<std::string> foot_force_interface_types_;
+        std::string foot_force_name_ = "foot_force";
+        std::vector<std::string> foot_force_interface_types_ = {"force", "torque"};

        std::string rl_config_folder_;

@ -102,6 +104,7 @@ namespace rl_quadruped_controller {
        };

        rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
+        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;

        FSMMode mode_ = FSMMode::NORMAL;
        std::string state_name_;
--- a/controllers/rl_quadruped_controller/src/control/Estimator.cpp
+++ b/controllers/rl_quadruped_controller/src/control/Estimator.cpp
@ -0,0 +1,226 @@
+//
+// Created by biao on 24-9-14.
+//
+
+#include "rl_quadruped_controller/control/Estimator.h"
+
+#include <rl_quadruped_controller/common/mathTools.h>
+
+#include "rl_quadruped_controller/control/CtrlComponent.h"
+
+Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component),
+                                                      robot_model_(ctrl_component.robot_model_) {
+    g_ << 0, 0, -9.81;
+    dt_ = 1.0 / ctrl_component_.frequency_;
+
+    std::cout << "dt: " << dt_ << std::endl;
+    large_variance_ = 100;
+    for (int i(0); i < Qdig.rows(); ++i) {
+        Qdig(i) = i < 6 ? 0.0003 : 0.01;
+    }
+
+    x_hat_.setZero();
+    u_.setZero();
+
+    A.setZero();
+    A.block(0, 0, 3, 3) = I3;
+    A.block(0, 3, 3, 3) = I3 * dt_;
+    A.block(3, 3, 3, 3) = I3;
+    A.block(6, 6, 12, 12) = I12;
+
+    B.setZero();
+    B.block(3, 0, 3, 3) = I3 * dt_;
+
+    C.setZero();
+    C.block(0, 0, 3, 3) = -I3;
+    C.block(3, 0, 3, 3) = -I3;
+    C.block(6, 0, 3, 3) = -I3;
+    C.block(9, 0, 3, 3) = -I3;
+    C.block(12, 3, 3, 3) = -I3;
+    C.block(15, 3, 3, 3) = -I3;
+    C.block(18, 3, 3, 3) = -I3;
+    C.block(21, 3, 3, 3) = -I3;
+    C.block(0, 6, 12, 12) = I12;
+    C(24, 8) = 1;
+    C(25, 11) = 1;
+    C(26, 14) = 1;
+    C(27, 17) = 1;
+
+    P.setIdentity();
+    P = large_variance_ * P;
+
+    RInit_ << 0.008, 0.012, -0.000, -0.009, 0.012, 0.000, 0.009, -0.009, -0.000,
+            -0.009, -0.009, 0.000, -0.000, 0.000, -0.000, 0.000, -0.000, -0.001,
+            -0.002, 0.000, -0.000, -0.003, -0.000, -0.001, 0.000, 0.000, 0.000, 0.000,
+            0.012, 0.019, -0.001, -0.014, 0.018, -0.000, 0.014, -0.013, -0.000,
+            -0.014, -0.014, 0.001, -0.001, 0.001, -0.001, 0.000, 0.000, -0.001,
+            -0.003, 0.000, -0.001, -0.004, -0.000, -0.001, 0.000, 0.000, 0.000, 0.000,
+            -0.000, -0.001, 0.001, 0.001, -0.001, 0.000, -0.000, 0.000, -0.000, 0.001,
+            0.000, -0.000, 0.000, -0.000, 0.000, 0.000, -0.000, -0.000, 0.000, -0.000,
+            -0.000, -0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, -0.009, -0.014,
+            0.001, 0.010, -0.013, 0.000, -0.010, 0.010, 0.000, 0.010, 0.010, -0.000,
+            0.001, 0.000, 0.000, 0.001, -0.000, 0.001, 0.002, -0.000, 0.000, 0.003,
+            0.000, 0.001, 0.000, 0.000, 0.000, 0.000, 0.012, 0.018, -0.001, -0.013,
+            0.018, -0.000, 0.013, -0.013, -0.000, -0.013, -0.013, 0.001, -0.001,
+            0.000, -0.001, 0.000, 0.001, -0.001, -0.003, 0.000, -0.001, -0.004,
+            -0.000, -0.001, 0.000, 0.000, 0.000, 0.000, 0.000, -0.000, 0.000, 0.000,
+            -0.000, 0.001, 0.000, 0.000, -0.000, 0.000, 0.000, -0.000, -0.000, 0.000,
+            -0.000, 0.000, 0.000, 0.000, -0.000, -0.000, -0.000, -0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, 0.000, 0.009, 0.014, -0.000, -0.010, 0.013, 0.000,
+            0.010, -0.010, -0.000, -0.010, -0.010, 0.000, -0.001, 0.000, -0.001,
+            0.000, -0.000, -0.001, -0.001, 0.000, -0.000, -0.003, -0.000, -0.001,
+            0.000, 0.000, 0.000, 0.000, -0.009, -0.013, 0.000, 0.010, -0.013, 0.000,
+            -0.010, 0.009, 0.000, 0.010, 0.010, -0.000, 0.001, -0.000, 0.000, -0.000,
+            0.000, 0.001, 0.002, 0.000, 0.000, 0.003, 0.000, 0.001, 0.000, 0.000,
+            0.000, 0.000, -0.000, -0.000, -0.000, 0.000, -0.000, -0.000, -0.000,
+            0.000, 0.001, 0.000, 0.000, 0.000, 0.000, -0.000, 0.000, -0.000, 0.000,
+            -0.000, 0.000, -0.000, 0.000, 0.000, -0.000, -0.000, 0.000, 0.000, 0.000,
+            0.000, -0.009, -0.014, 0.001, 0.010, -0.013, 0.000, -0.010, 0.010, 0.000,
+            0.010, 0.010, -0.000, 0.001, 0.000, 0.000, -0.000, -0.000, 0.001, 0.002,
+            -0.000, 0.000, 0.003, 0.000, 0.001, 0.000, 0.000, 0.000, 0.000, -0.009,
+            -0.014, 0.000, 0.010, -0.013, 0.000, -0.010, 0.010, 0.000, 0.010, 0.010,
+            -0.000, 0.001, -0.000, 0.000, -0.000, 0.000, 0.001, 0.002, -0.000, 0.000,
+            0.003, 0.001, 0.001, 0.000, 0.000, 0.000, 0.000, 0.000, 0.001, -0.000,
+            -0.000, 0.001, -0.000, 0.000, -0.000, 0.000, -0.000, -0.000, 0.001, 0.000,
+            -0.000, -0.000, -0.000, 0.000, 0.000, -0.000, 0.000, 0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, 0.000, 0.000, -0.000, -0.001, 0.000, 0.001, -0.001,
+            -0.000, -0.001, 0.001, 0.000, 0.001, 0.001, 0.000, 1.708, 0.048, 0.784,
+            0.062, 0.042, 0.053, 0.077, 0.001, -0.061, 0.046, -0.019, -0.029, 0.000,
+            0.000, 0.000, 0.000, 0.000, 0.001, -0.000, 0.000, 0.000, 0.000, 0.000,
+            -0.000, -0.000, 0.000, -0.000, -0.000, 0.048, 5.001, -1.631, -0.036,
+            0.144, 0.040, 0.036, 0.016, -0.051, -0.067, -0.024, -0.005, 0.000, 0.000,
+            0.000, 0.000, -0.000, -0.001, 0.000, 0.000, -0.001, -0.000, -0.001, 0.000,
+            0.000, 0.000, 0.000, -0.000, 0.784, -1.631, 1.242, 0.057, -0.037, 0.018,
+            0.034, -0.017, -0.015, 0.058, -0.021, -0.029, 0.000, 0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, 0.001, 0.000, 0.000, 0.000, -0.000, -0.000, -0.000,
+            -0.000, -0.000, 0.062, -0.036, 0.057, 6.228, -0.014, 0.932, 0.059, 0.053,
+            -0.069, 0.148, 0.015, -0.031, 0.000, 0.000, 0.000, 0.000, -0.000, 0.000,
+            -0.000, -0.000, 0.001, 0.000, -0.000, 0.000, 0.000, -0.000, 0.000, 0.000,
+            0.042, 0.144, -0.037, -0.014, 3.011, 0.986, 0.076, 0.030, -0.052, -0.027,
+            0.057, 0.051, 0.000, 0.000, 0.000, 0.000, -0.001, -0.001, -0.000, 0.001,
+            -0.001, 0.000, -0.001, 0.001, -0.000, 0.001, 0.001, 0.000, 0.053, 0.040,
+            0.018, 0.932, 0.986, 0.885, 0.090, 0.044, -0.055, 0.057, 0.051, -0.003,
+            0.000, 0.000, 0.000, 0.000, -0.002, -0.003, 0.000, 0.002, -0.003, -0.000,
+            -0.001, 0.002, 0.000, 0.002, 0.002, -0.000, 0.077, 0.036, 0.034, 0.059,
+            0.076, 0.090, 6.230, 0.139, 0.763, 0.013, -0.019, -0.024, 0.000, 0.000,
+            0.000, 0.000, 0.000, 0.000, -0.000, -0.000, 0.000, -0.000, 0.000, 0.000,
+            -0.000, -0.000, -0.000, 0.000, 0.001, 0.016, -0.017, 0.053, 0.030, 0.044,
+            0.139, 3.130, -1.128, -0.010, 0.131, 0.018, 0.000, 0.000, 0.000, 0.000,
+            -0.000, -0.001, -0.000, 0.000, -0.001, -0.000, -0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, -0.061, -0.051, -0.015, -0.069, -0.052, -0.055,
+            0.763, -1.128, 0.866, -0.022, -0.053, 0.007, 0.000, 0.000, 0.000, 0.000,
+            -0.003, -0.004, -0.000, 0.003, -0.004, -0.000, -0.003, 0.003, 0.000,
+            0.003, 0.003, 0.000, 0.046, -0.067, 0.058, 0.148, -0.027, 0.057, 0.013,
+            -0.010, -0.022, 2.437, -0.102, 0.938, 0.000, 0.000, 0.000, 0.000, -0.000,
+            -0.000, 0.000, 0.000, -0.000, 0.000, -0.000, 0.000, -0.000, 0.000, 0.001,
+            0.000, -0.019, -0.024, -0.021, 0.015, 0.057, 0.051, -0.019, 0.131, -0.053,
+            -0.102, 4.944, 1.724, 0.000, 0.000, 0.000, 0.000, -0.001, -0.001, 0.000,
+            0.001, -0.001, 0.000, -0.001, 0.001, -0.000, 0.001, 0.001, 0.000, -0.029,
+            -0.005, -0.029, -0.031, 0.051, -0.003, -0.024, 0.018, 0.007, 0.938, 1.724,
+            1.569, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 1.0, 0.000,
+            0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 1.0, 0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, 0.000, 0.000, 1.0, 0.000, 0.000, 0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
+            0.000, 0.000, 0.000, 1.0;
+
+    Cu << 268.573, -43.819, -147.211, -43.819, 92.949, 58.082, -147.211, 58.082,
+            302.120;
+
+    QInit_ = Qdig.asDiagonal();
+    QInit_ += B * Cu * B.transpose();
+
+    // low_pass_filters_.resize(3);
+    // low_pass_filters_[0] = std::make_shared<LowPassFilter>(dt_, 3.0);
+    // low_pass_filters_[1] = std::make_shared<LowPassFilter>(dt_, 3.0);
+    // low_pass_filters_[2] = std::make_shared<LowPassFilter>(dt_, 3.0);
+}
+
+double Estimator::getYaw() const {
+    return rotMatToRPY(rotation_)(2);
+}
+
+void Estimator::update() {
+    if (robot_model_->mass_ == 0) return;
+
+    Q = QInit_;
+    R = RInit_;
+
+    foot_poses_ = robot_model_->getFeet2BPositions();
+    foot_vels_ = robot_model_->getFeet2BVelocities();
+    feet_h_.setZero();
+
+    // Adjust the covariance based on foot contact and phase.
+    for (int i(0); i < 4; ++i) {
+        if (ctrl_component_.foot_force_state_interface_[i].get().get_value() < 1) {
+            // foot not contact
+            Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3);
+            R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3);
+            R(24 + i, 24 + i) = large_variance_;
+        } else {
+            // foot contact
+            const double trust = windowFunc(0.5, 0.2);
+            Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) =
+                    (1 + (1 - trust) * large_variance_) *
+                    QInit_.block(6 + 3 * i, 6 + 3 * i, 3, 3);
+            R.block(12 + 3 * i, 12 + 3 * i, 3, 3) =
+                    (1 + (1 - trust) * large_variance_) *
+                    RInit_.block(12 + 3 * i, 12 + 3 * i, 3, 3);
+            R(24 + i, 24 + i) =
+                    (1 + (1 - trust) * large_variance_) * RInit_(24 + i, 24 + i);
+        }
+        feet_pos_body_.segment(3 * i, 3) = Vec3(foot_poses_[i].p.data);
+        feet_vel_body_.segment(3 * i, 3) = Vec3(foot_vels_[i].data);
+    }
+
+    Quat quat;
+    quat << ctrl_component_.imu_state_interface_[0].get().get_value(),
+            ctrl_component_.imu_state_interface_[1].get().get_value(),
+            ctrl_component_.imu_state_interface_[2].get().get_value(),
+            ctrl_component_.imu_state_interface_[3].get().get_value();
+    rotation_ = quatToRotMat(quat);
+
+    gyro_ << ctrl_component_.imu_state_interface_[4].get().get_value(),
+            ctrl_component_.imu_state_interface_[5].get().get_value(),
+            ctrl_component_.imu_state_interface_[6].get().get_value();
+
+    acceleration_ << ctrl_component_.imu_state_interface_[7].get().get_value(),
+            ctrl_component_.imu_state_interface_[8].get().get_value(),
+            ctrl_component_.imu_state_interface_[9].get().get_value();
+
+    u_ = rotation_ * acceleration_ + g_;
+    x_hat_ = A * x_hat_ + B * u_;
+    y_hat_ = C * x_hat_;
+
+    // Update the measurement value
+    y_ << feet_pos_body_, feet_vel_body_, feet_h_;
+
+    // Update the covariance matrix
+    Ppriori = A * P * A.transpose() + Q;
+    S = R + C * Ppriori * C.transpose();
+    Slu = S.lu();
+    Sy = Slu.solve(y_ - y_hat_);
+    Sc = Slu.solve(C);
+    SR = Slu.solve(R);
+    STC = S.transpose().lu().solve(C);
+    IKC = Eigen::MatrixXd::Identity(18, 18) - Ppriori * C.transpose() * Sc;
+
+    // Update the state and covariance matrix
+    x_hat_ += Ppriori * C.transpose() * Sy;
+    P = IKC * Ppriori * IKC.transpose() +
+        Ppriori * C.transpose() * SR * STC * Ppriori.transpose();
+
+    // // Using low pass filter to smooth the velocity
+    low_pass_filters_[0]->addValue(x_hat_(3));
+    low_pass_filters_[1]->addValue(x_hat_(4));
+    low_pass_filters_[2]->addValue(x_hat_(5));
+    x_hat_(3) = low_pass_filters_[0]->getValue();
+    x_hat_(4) = low_pass_filters_[1]->getValue();
+    x_hat_(5) = low_pass_filters_[2]->getValue();
+}
--- a/controllers/rl_quadruped_controller/src/control/LowPassFilter.cpp
+++ b/controllers/rl_quadruped_controller/src/control/LowPassFilter.cpp
@ -0,0 +1,31 @@
+//
+// Created by biao on 24-9-16.
+//
+#include <cmath>
+#include "rl_quadruped_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;
+}
--- a/controllers/rl_quadruped_controller/src/robot/QuadrupedRobot.cpp
+++ b/controllers/rl_quadruped_controller/src/robot/QuadrupedRobot.cpp
@ -0,0 +1,128 @@
+//
+// Created by biao on 24-9-12.
+//
+
+#include <iostream>
+#include "rl_quadruped_controller/control/CtrlComponent.h"
+#include "rl_quadruped_controller/robot/QuadrupedRobot.h"
+
+QuadrupedRobot::QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description,
+                               const std::vector<std::string> &feet_names,
+                               const std::string &base_name) : ctrl_component_(ctrl_component) {
+    KDL::Tree robot_tree;
+    kdl_parser::treeFromString(robot_description, robot_tree);
+
+    robot_tree.getChain(base_name, feet_names[0], fr_chain_);
+    robot_tree.getChain(base_name, feet_names[1], fl_chain_);
+    robot_tree.getChain(base_name, feet_names[2], rr_chain_);
+    robot_tree.getChain(base_name, feet_names[3], rl_chain_);
+
+
+    robot_legs_.resize(4);
+    robot_legs_[0] = std::make_shared<RobotLeg>(fr_chain_);
+    robot_legs_[1] = std::make_shared<RobotLeg>(fl_chain_);
+    robot_legs_[2] = std::make_shared<RobotLeg>(rr_chain_);
+    robot_legs_[3] = std::make_shared<RobotLeg>(rl_chain_);
+
+    current_joint_pos_.resize(4);
+    current_joint_vel_.resize(4);
+
+    std::cout << "robot_legs_.size(): " << robot_legs_.size() << std::endl;
+
+    // calculate total mass from urdf
+    mass_ = 0;
+    for (const auto &[fst, snd]: robot_tree.getSegments()) {
+        mass_ += snd.segment.getInertia().getMass();
+    }
+    feet_pos_normal_stand_ << 0.1881, 0.1881, -0.1881, -0.1881, -0.1300, 0.1300,
+            -0.1300, 0.1300, -0.3200, -0.3200, -0.3200, -0.3200;
+}
+
+std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &pEe_list) const {
+    std::vector<KDL::JntArray> result;
+    result.resize(4);
+    for (int i(0); i < 4; ++i) {
+        result[i] = robot_legs_[i]->calcQ(pEe_list[i], current_joint_pos_[i]);
+    }
+    return result;
+}
+
+Vec12 QuadrupedRobot::getQ(const Vec34 &vecP) const {
+    Vec12 q;
+    for (int i(0); i < 4; ++i) {
+        KDL::Frame frame;
+        frame.p = KDL::Vector(vecP.col(i)[0], vecP.col(i)[1], vecP.col(i)[2]);
+        frame.M = KDL::Rotation::Identity();
+        q.segment(3 * i, 3) = robot_legs_[i]->calcQ(frame, current_joint_pos_[i]).data;
+    }
+    return q;
+}
+
+Vec12 QuadrupedRobot::getQd(const std::vector<KDL::Frame> &pos, const Vec34 &vel) {
+    Vec12 qd;
+    const std::vector<KDL::JntArray> q = getQ(pos);
+    for (int i(0); i < 4; ++i) {
+        Mat3 jacobian = robot_legs_[i]->calcJaco(q[i]).data.topRows(3);
+        qd.segment(3 * i, 3) = jacobian.inverse() * vel.col(i);
+    }
+    return qd;
+}
+
+std::vector<KDL::Frame> QuadrupedRobot::getFeet2BPositions() const {
+    std::vector<KDL::Frame> result;
+    result.resize(4);
+    for (int i = 0; i < 4; i++) {
+        result[i] = robot_legs_[i]->calcPEe2B(current_joint_pos_[i]);
+        result[i].M = KDL::Rotation::Identity();
+    }
+    return result;
+}
+
+KDL::Frame QuadrupedRobot::getFeet2BPositions(const int index) const {
+    return robot_legs_[index]->calcPEe2B(current_joint_pos_[index]);
+}
+
+KDL::Jacobian QuadrupedRobot::getJacobian(const int index) const {
+    return robot_legs_[index]->calcJaco(current_joint_pos_[index]);
+}
+
+KDL::JntArray QuadrupedRobot::getTorque(
+    const Vec3 &force, const int index) const {
+    return robot_legs_[index]->calcTorque(current_joint_pos_[index], force);
+}
+
+KDL::JntArray QuadrupedRobot::getTorque(const KDL::Vector &force, int index) const {
+    return robot_legs_[index]->calcTorque(current_joint_pos_[index], Vec3(force.data));
+}
+
+KDL::Vector QuadrupedRobot::getFeet2BVelocities(const int index) const {
+    const Mat3 jacobian = getJacobian(index).data.topRows(3);
+    Vec3 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() {
+    if (mass_ == 0) return;
+    for (int i = 0; i < 4; i++) {
+        KDL::JntArray pos_array(3);
+        pos_array(0) = ctrl_component_.joint_position_state_interface_[i * 3].get().get_value();
+        pos_array(1) = ctrl_component_.joint_position_state_interface_[i * 3 + 1].get().get_value();
+        pos_array(2) = ctrl_component_.joint_position_state_interface_[i * 3 + 2].get().get_value();
+        current_joint_pos_[i] = pos_array;
+
+        KDL::JntArray vel_array(3);
+        vel_array(0) = ctrl_component_.joint_velocity_state_interface_[i * 3].get().get_value();
+        vel_array(1) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 1].get().get_value();
+        vel_array(2) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 2].get().get_value();
+        current_joint_vel_[i] = vel_array;
+    }
+}
--- a/controllers/rl_quadruped_controller/src/robot/RobotLeg.cpp
+++ b/controllers/rl_quadruped_controller/src/robot/RobotLeg.cpp
@ -0,0 +1,44 @@
+//
+// Created by biao on 24-9-12.
+//
+
+#include <memory>
+#include "rl_quadruped_controller/robot/RobotLeg.h"
+
+#include <rl_quadruped_controller/common/mathTypes.h>
+
+RobotLeg::RobotLeg(const KDL::Chain &chain) {
+    chain_ = chain;
+
+    fk_pose_solver_ = std::make_shared<KDL::ChainFkSolverPos_recursive>(chain);
+    ik_pose_solver_ = std::make_shared<KDL::ChainIkSolverPos_LMA>(chain);
+    jac_solver_ = std::make_shared<KDL::ChainJntToJacSolver>(chain);
+}
+
+KDL::Frame RobotLeg::calcPEe2B(const KDL::JntArray &joint_positions) const {
+    KDL::Frame pEe;
+    fk_pose_solver_->JntToCart(joint_positions, pEe);
+    return pEe;
+}
+
+KDL::JntArray RobotLeg::calcQ(const KDL::Frame &pEe, const KDL::JntArray &q_init) const {
+    KDL::JntArray q(chain_.getNrOfJoints());
+    ik_pose_solver_->CartToJnt(q_init, pEe, q);
+    return q;
+}
+
+KDL::Jacobian RobotLeg::calcJaco(const KDL::JntArray &joint_positions) const {
+    KDL::Jacobian jacobian(chain_.getNrOfJoints());
+    jac_solver_->JntToJac(joint_positions, jacobian);
+    return jacobian;
+}
+
+KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const Vec3 &force) const {
+    const Eigen::Matrix<double, 3, Eigen::Dynamic> jacobian = calcJaco(joint_positions).data.topRows(3);
+    Eigen::VectorXd torque_eigen = jacobian.transpose() * force;
+    KDL::JntArray torque(chain_.getNrOfJoints());
+    for (unsigned int i = 0; i < chain_.getNrOfJoints(); ++i) {
+        torque(i) = torque_eigen(i);
+    }
+    return torque;
+}
--- a/controllers/unitree_guide_controller/src/control/Estimator.cpp
+++ b/controllers/unitree_guide_controller/src/control/Estimator.cpp
@ -14,7 +14,7 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo
    g_ << 0, 0, -9.81;
    dt_ = 1.0 / ctrl_component_.frequency_;

-    std::cout<<"dt: "<<dt_<<std::endl;
+    std::cout << "dt: " << dt_ << std::endl;
    large_variance_ = 100;
    for (int i(0); i < Qdig.rows(); ++i) {
        Qdig(i) = i < 6 ? 0.0003 : 0.01;
@ -180,12 +180,6 @@ void Estimator::update() {
        feet_vel_body_.segment(3 * i, 3) = Vec3(foot_vels_[i].data);
    }

-    // Acceleration from imu as system input
-    // rotation_ = KDL::Rotation::Quaternion(ctrl_component_.imu_state_interface_[1].get().get_value(),
-    //                                       ctrl_component_.imu_state_interface_[2].get().get_value(),
-    //                                       ctrl_component_.imu_state_interface_[3].get().get_value(),
-    //                                       ctrl_component_.imu_state_interface_[0].get().get_value());
-
    Quat quat;
    quat << ctrl_component_.imu_state_interface_[0].get().get_value(),
            ctrl_component_.imu_state_interface_[1].get().get_value(),
@ -197,16 +191,9 @@ void Estimator::update() {
            ctrl_component_.imu_state_interface_[5].get().get_value(),
            ctrl_component_.imu_state_interface_[6].get().get_value();

-    // gyro_ = KDL::Vector(ctrl_component_.imu_state_interface_[4].get().get_value(),
-    //                     ctrl_component_.imu_state_interface_[5].get().get_value(),
-    //                     ctrl_component_.imu_state_interface_[6].get().get_value());
-
    acceleration_ << ctrl_component_.imu_state_interface_[7].get().get_value(),
            ctrl_component_.imu_state_interface_[8].get().get_value(),
            ctrl_component_.imu_state_interface_[9].get().get_value();
-    // acceleration_ = KDL::Vector(ctrl_component_.imu_state_interface_[7].get().get_value(),
-    //                             ctrl_component_.imu_state_interface_[8].get().get_value(),
-    //                             ctrl_component_.imu_state_interface_[9].get().get_value());

    u_ = rotation_ * acceleration_ + g_;
    x_hat_ = A * x_hat_ + B * u_;
@ -231,10 +218,10 @@ void Estimator::update() {
        Ppriori * C.transpose() * SR * STC * Ppriori.transpose();

    // // Using low pass filter to smooth the velocity
-    // low_pass_filters_[0]->addValue(x_hat_(3));
-    // low_pass_filters_[1]->addValue(x_hat_(4));
-    // low_pass_filters_[2]->addValue(x_hat_(5));
-    // x_hat_(3) = low_pass_filters_[0]->getValue();
-    // x_hat_(4) = low_pass_filters_[1]->getValue();
-    // x_hat_(5) = low_pass_filters_[2]->getValue();
+    low_pass_filters_[0]->addValue(x_hat_(3));
+    low_pass_filters_[1]->addValue(x_hat_(4));
+    low_pass_filters_[2]->addValue(x_hat_(5));
+    x_hat_(3) = low_pass_filters_[0]->getValue();
+    x_hat_(4) = low_pass_filters_[1]->getValue();
+    x_hat_(5) = low_pass_filters_[2]->getValue();
 }
--- a/descriptions/unitree/a1_description/config/issacgym/config.yaml
+++ b/descriptions/unitree/a1_description/config/issacgym/config.yaml
@ -2,10 +2,10 @@ model_name: "model_0702.pt"
 framework: "isaacgym"
 rows: 4
 cols: 3
-use_history: True
 decimation: 4
 num_observations: 45
 observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"]
+observations_history: [0, 1, 2, 3, 4, 5]
 clip_obs: 100.0
 clip_actions_lower: [-100, -100, -100,
                     -100, -100, -100,
--- a/descriptions/unitree/go2_description/config/issacgym/config.yaml
+++ b/descriptions/unitree/go2_description/config/issacgym/config.yaml
@ -1,11 +1,11 @@
-model_name: "policy.pt"
+model_name: "himloco.pt"
 framework: "isaacgym"
 rows: 4
 cols: 3
-use_history: True
 decimation: 4
-num_observations: 45
-observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"]
+num_observations: 48
+observations: ["lin_vel", "ang_vel", "gravity_vec", "dof_pos", "dof_vel", "actions"]
+observations_history: [5, 4, 3, 2, 1, 0]
 clip_obs: 100.0
 clip_actions_lower: [-100, -100, -100,
                     -100, -100, -100,
@ -15,23 +15,23 @@ clip_actions_upper: [100, 100, 100,
                     100, 100, 100,
                     100, 100, 100,
                     100, 100, 100]
-rl_kp: [20.1, 20.1, 20.1,
-        20.1, 20.1, 20.1,
-        20.1, 20.1, 20.1,
-        20.1, 20.1, 20.1]
-rl_kd: [0.61, 0.61, 0.61,
-        0.61, 0.61, 0.61,
-        0.61, 0.61, 0.61,
-        0.61, 0.61, 0.61]
-fixed_kp: [80, 80, 80,
-           80, 80, 80,
-           80, 80, 80,
-           80, 80, 80]
-fixed_kd: [3, 3, 3,
-           3, 3, 3,
-           3, 3, 3,
-           3, 3, 3]
-hip_scale_reduction: 0.5
+rl_kp: [40, 40, 40,
+        40, 40, 40,
+        40, 40, 40,
+        40, 40, 40]
+rl_kd: [1, 1, 1,
+        1, 1, 1,
+        1, 1, 1,
+        1, 1, 1]
+fixed_kp: [60, 60, 60,
+           60, 60, 60,
+           60, 60, 60,
+           60, 60, 60]
+fixed_kd: [5, 5, 5,
+           5, 5, 5,
+           5, 5, 5,
+           5, 5, 5]
+hip_scale_reduction: 1.0
 hip_scale_reduction_indices: [0, 3, 6, 9]
 num_of_dofs: 12
 action_scale: 0.25
@ -39,12 +39,12 @@ lin_vel_scale: 2.0
 ang_vel_scale: 0.25
 dof_pos_scale: 1.0
 dof_vel_scale: 0.05
-commands_scale: [2.0, 2.0, 1.0]
+commands_scale: [2.0, 2.0, 0.25]
 torque_limits: [33.5, 33.5, 33.5,
                33.5, 33.5, 33.5,
                33.5, 33.5, 33.5,
                33.5, 33.5, 33.5]
 default_dof_pos: [ 0.1000, 0.8000, -1.5000,
-                  -0.1000, 0.8000, -1.5000,
+                   -0.1000, 0.8000, -1.5000,
                   0.1000, 1.0000, -1.5000,
-                  -0.1000, 1.0000, -1.5000]
+                   -0.1000, 1.0000, -1.5000]