tried to add balance test

controllers/unitree_guide_controller/CMakeLists.txt

@@ -5,6 +5,7 @@ if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
     add_compile_options(-Wall -Wextra -Wpedantic)
 endif ()
 
+set(CMAKE_BUILD_TYPE Release)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
 set(CONTROLLER_INCLUDE_DEPENDS
@@ -32,12 +33,14 @@ add_library(${PROJECT_NAME} SHARED
         src/FSM/StateFixedStand.cpp
         src/FSM/StateSwingTest.cpp
         src/FSM/StateFreeStand.cpp
+        src/FSM/StateBalanceTest.cpp
 
         src/robot/QuadrupedRobot.cpp
         src/robot/RobotLeg.cpp
 
         src/control/Estimator.cpp
         src/control/LowPassFilter.cpp
+        src/control/BalanceCtrl.cpp
 
         src/quadProgpp/Array.cc
         src/quadProgpp/QuadProg++.cc

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h

@@ -0,0 +1,43 @@
+//
+// Created by tlab-uav on 24-9-16.
+//
+
+#ifndef STATEBALANCETEST_H
+#define STATEBALANCETEST_H
+#include "FSMState.h"
+
+
+class StateBalanceTest final : public FSMState {
+public:
+    explicit StateBalanceTest(CtrlComponent ctrlComp);
+
+    void enter() override;
+
+    void run() override;
+
+    void exit() override;
+
+    FSMStateName checkChange() override;
+
+private:
+    void calcTorque();
+
+    KDL::Vector pcd_;
+    KDL::Vector pcdInit_;
+    KDL::Rotation Rd_;
+    KDL::Rotation RdInit_;
+
+    KDL::Vector pose_body_, vel_body_;
+
+    double kp_w_;
+    Mat3 Kp_p_, Kd_p_, Kd_w_;
+    Vec3 _ddPcd, _dWbd;
+
+    float _xMax, _xMin;
+    float _yMax, _yMin;
+    float _zMax, _zMin;
+    float _yawMax, _yawMin;
+};
+
+
+#endif //STATEBALANCETEST_H

controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h

@@ -10,6 +10,7 @@
 #include <unitree_guide_controller/FSM/FSMState.h>
 #include <unitree_guide_controller/common/enumClass.h>
 
+#include "FSM/StateBalanceTest.h"
 #include "FSM/StateFixedDown.h"
 #include "FSM/StateFixedStand.h"
 #include "FSM/StateFreeStand.h"
@@ -25,6 +26,7 @@ namespace unitree_guide_controller {
         std::shared_ptr<StateFreeStand> freeStand;
 
         std::shared_ptr<StateSwingTest> swingTest;
+        std::shared_ptr<StateBalanceTest> balanceTest;
     };
 
     class UnitreeGuideController final : public controller_interface::ControllerInterface {

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

@@ -33,5 +33,37 @@ T windowFunc(const T x, const T windowRatio, const T xRange = 1.0,
     return yRange;
 }
 
+inline Eigen::Matrix3d skew(const KDL::Vector &vec) {
+    Eigen::Matrix3d skewMat;
+    skewMat << 0, -vec.z(), vec.y(),
+            vec.z(), 0, -vec.x(),
+            -vec.y(), vec.x(), 0;
+    return skewMat;
+}
+
+
+inline Vec3 rotationToExp(const KDL::Rotation &rotation) {
+    auto rm = Eigen::Matrix3d(rotation.data);
+    double cosValue = rm.trace() / 2.0 - 1 / 2.0;
+    if (cosValue > 1.0f) {
+        cosValue = 1.0f;
+    } else if (cosValue < -1.0f) {
+        cosValue = -1.0f;
+    }
+
+    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;
+}
+
 
 #endif //MATHTOOLS_H

controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTypes.h

@@ -10,58 +10,58 @@
 /******** Vector ********/
 /************************/
 // 2x1 Vector
-using Vec2 = typename Eigen::Matrix<double, 2, 1>;
+using Vec2 = Eigen::Matrix<double, 2, 1>;
 
 // 3x1 Vector
-using Vec3 = typename Eigen::Matrix<double, 3, 1>;
+using Vec3 = Eigen::Matrix<double, 3, 1>;
 
 // 4x1 Vector
-using Vec4 = typename Eigen::Matrix<double, 4, 1>;
+using Vec4 = Eigen::Matrix<double, 4, 1>;
 
 // 6x1 Vector
-using Vec6 = typename Eigen::Matrix<double, 6, 1>;
+using Vec6 = Eigen::Matrix<double, 6, 1>;
 
 // Quaternion
-using Quat = typename Eigen::Matrix<double, 4, 1>;
+using Quat = Eigen::Matrix<double, 4, 1>;
 
 // 4x1 Integer Vector
-using VecInt4 = typename Eigen::Matrix<int, 4, 1>;
+using VecInt4 = Eigen::Matrix<int, 4, 1>;
 
 // 12x1 Vector
-using Vec12 = typename Eigen::Matrix<double, 12, 1>;
+using Vec12 = Eigen::Matrix<double, 12, 1>;
 
 // 18x1 Vector
-using Vec18 = typename Eigen::Matrix<double, 18, 1>;
+using Vec18 = Eigen::Matrix<double, 18, 1>;
 
 // Dynamic Length Vector
-using VecX = typename Eigen::Matrix<double, Eigen::Dynamic, 1>;
+using VecX = Eigen::Matrix<double, Eigen::Dynamic, 1>;
 
 /************************/
 /******** Matrix ********/
 /************************/
 // Rotation Matrix
-using RotMat = typename Eigen::Matrix<double, 3, 3>;
+using RotMat = Eigen::Matrix<double, 3, 3>;
 
 // Homogenous Matrix
-using HomoMat = typename Eigen::Matrix<double, 4, 4>;
+using HomoMat = Eigen::Matrix<double, 4, 4>;
 
 // 2x2 Matrix
-using Mat2 = typename Eigen::Matrix<double, 2, 2>;
+using Mat2 = Eigen::Matrix<double, 2, 2>;
 
 // 3x3 Matrix
-using Mat3 = typename Eigen::Matrix<double, 3, 3>;
+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 = typename Eigen::Matrix<double, 3, 4>;
+using Vec34 = Eigen::Matrix<double, 3, 4>;
 
 // 6x6 Matrix
-using Mat6 = typename Eigen::Matrix<double, 6, 6>;
+using Mat6 = Eigen::Matrix<double, 6, 6>;
 
 // 12x12 Matrix
-using Mat12 = typename Eigen::Matrix<double, 12, 12>;
+using Mat12 = Eigen::Matrix<double, 12, 12>;
 
 // 12x12 Identity Matrix
 #define I12 Eigen::MatrixXd::Identity(12, 12)
@@ -70,7 +70,7 @@ using Mat12 = typename Eigen::Matrix<double, 12, 12>;
 #define I18 Eigen::MatrixXd::Identity(18, 18)
 
 // Dynamic Size Matrix
-using MatX = typename Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
+using MatX = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
 
 /************************/
 /****** Functions *******/

controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h

@@ -0,0 +1,48 @@
+//
+// Created by tlab-uav on 24-9-16.
+//
+
+#ifndef BALANCECTRL_H
+#define BALANCECTRL_H
+
+#include <kdl/frames.hpp>
+
+#include "unitree_guide_controller/common/mathTypes.h"
+class QuadrupedRobot;
+
+class BalanceCtrl {
+public:
+    explicit BalanceCtrl();
+
+    ~BalanceCtrl() = default;
+
+    void init(const QuadrupedRobot &robot);
+
+    std::vector<KDL::Vector> calF(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM,
+                                  const std::vector<KDL::Vector> &feetPos2B, const std::vector<int> &contact);
+
+private:
+    void calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const KDL::Rotation &rotM);
+
+    void calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM);
+
+    void calConstraints(const std::vector<int> &contact);
+
+    void solveQP();
+
+    Mat12 _G, _W, _U;
+    Mat6 _S;
+    Mat3 _Ib;
+    Vec6 _bd;
+    KDL::Vector _pcb;
+    Vec3 _g;
+    Vec12 _F, _Fprev, _g0T;
+    double _mass, _alpha, _beta, _fricRatio;
+    Eigen::MatrixXd _CE, _CI;
+    Eigen::VectorXd _ce0, _ci0;
+    Eigen::Matrix<double, 6, 12> _A;
+    Eigen::Matrix<double, 5, 3> _fricMat;
+};
+
+
+#endif //BALANCECTRL_H

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

@@ -11,6 +11,9 @@
 #include <control_input_msgs/msg/inputs.hpp>
 #include <unitree_guide_controller/robot/QuadrupedRobot.h>
 
+#include "BalanceCtrl.h"
+#include "Estimator.h"
+
 struct CtrlComponent {
     std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
     joint_effort_command_interface_;
@@ -41,7 +44,14 @@ struct CtrlComponent {
     QuadrupedRobot default_robot_model_;
     std::reference_wrapper<QuadrupedRobot> robot_model_;
 
-    CtrlComponent() : control_inputs_(default_inputs_), robot_model_(default_robot_model_) {
+    Estimator default_estimator_;
+    std::reference_wrapper<Estimator> estimator_;
+
+    BalanceCtrl default_balance_ctrl_;
+    std::reference_wrapper<BalanceCtrl> balance_ctrl_;
+
+    CtrlComponent() : control_inputs_(default_inputs_), robot_model_(default_robot_model_),
+                      estimator_(default_estimator_), balance_ctrl_(default_balance_ctrl_) {
     }
 
     void clear() {

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

@@ -4,9 +4,12 @@
 
 #ifndef ESTIMATOR_H
 #define ESTIMATOR_H
+#include <memory>
 #include <eigen3/Eigen/Dense>
 #include <kdl/frames.hpp>
 
+#include "LowPassFilter.h"
+
 struct CtrlComponent;
 
 class Estimator {
@@ -19,49 +22,89 @@ public:
      * Get the estimated robot central position
      * @return robot central position
      */
-    Eigen::Matrix<double, 3, 1> getPosition() {
+    KDL::Vector getPosition() {
-        return _xhat.segment(0, 3);
+        return {_xhat(0), _xhat(1), _xhat(2)};
     }
 
     /**
      * Get the estimated robot central velocity
      * @return robot central velocity
      */
-    Eigen::Matrix<double, 3, 1> getVelocity() {
+    KDL::Vector getVelocity() {
-        return _xhat.segment(3, 3);
+        return {_xhat(3), _xhat(4), _xhat(5)};
     }
 
-    void run(
+    /**
-        const CtrlComponent &ctrlComp,
+     * Get the estimated foot position in world frame
-        std::vector<KDL::Frame> feet_poses_,
+     * @param index leg index
-        std::vector<KDL::Vector> feet_vels_,
+     * @return foot position in world frame
-        const std::vector<int> &contact,
+     */
-        const std::vector<double> &phase
+    KDL::Vector getFootPos(const int index) {
-    );
+        return getPosition() + rotation_ * foot_poses_[index].p;
+    }
+
+    /**
+     * Get all estimated foot positions in world frame
+     * @return all foot positions in world frame
+     */
+    std::vector<KDL::Vector> getFootPos() {
+        std::vector<KDL::Vector> foot_pos;
+        foot_pos.resize(4);
+        for (int i = 0; i < 4; i++) {
+            foot_pos[i] = getFootPos(i);
+        }
+        return foot_pos;
+    }
+
+    /**
+     * Get the estimated foot position in body frame
+     * @return
+     */
+    std::vector<KDL::Vector> getFootPos2Body() {
+        std::vector<KDL::Vector> foot_pos;
+        foot_pos.resize(4);
+        const KDL::Vector body_pos = getPosition();
+        for (int i = 0; i < 4; i++) {
+            foot_pos[i] = getFootPos(i) - body_pos;
+        }
+        return foot_pos;
+    }
+
+    KDL::Rotation getRotation() {
+        return rotation_;
+    }
+
+    KDL::Vector getGyro() {
+        return gyro_;
+    }
+
+    [[nodiscard]] KDL::Vector getGlobalGyro() const {
+        return rotation_ * gyro_;
+    }
+
+    void update(const CtrlComponent &ctrlComp);
 
 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, 18> A; // The transtion matrix of estimator
-    Eigen::Matrix<double, 18, 3> _B; // The input matrix
+    Eigen::Matrix<double, 18, 3> B; // The input matrix
-    Eigen::Matrix<double, 28, 18> _C; // The output 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> P; // Prediction covariance
-    Eigen::Matrix<double, 18, 18> _Ppriori; // Priori prediction covariance
+    Eigen::Matrix<double, 18, 18> Ppriori; // Priori prediction covariance
-    Eigen::Matrix<double, 18, 18> _Q; // Dynamic simulation covariance
+    Eigen::Matrix<double, 18, 18> Q; // Dynamic simulation covariance
-    Eigen::Matrix<double, 28, 28> _R; // Measurement covariance
+    Eigen::Matrix<double, 28, 28> R; // Measurement covariance
     Eigen::Matrix<double, 18, 18>
-    _QInit; // Initial value of Dynamic simulation covariance
+    QInit; // Initial value of Dynamic simulation covariance
     Eigen::Matrix<double, 28, 28>
-    _RInit; // Initial value of Measurement covariance
+    RInit; // Initial value of Measurement covariance
-    Eigen::Matrix<double, 18, 1> _Qdig; // adjustable process noise covariance
+    Eigen::Matrix<double, 18, 1> Qdig; // adjustable process noise covariance
-    Eigen::Matrix<double, 3, 3> _Cu; // The covariance of system input u
+    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
@@ -69,20 +112,25 @@ private:
     _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::Matrix<double, 28, 28> S; // _S = C*P*C.T + R
-    Eigen::PartialPivLU<Eigen::Matrix<double, 28, 28> > _Slu; // _S.lu()
+    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, 1> Sy; // _Sy = _S.inv() * (y - yhat)
-    Eigen::Matrix<double, 28, 18> _Sc; // _Sc = _S.inv() * C
+    Eigen::Matrix<double, 28, 18> Sc; // _Sc = _S.inv() * C
-    Eigen::Matrix<double, 28, 28> _SR; // _SR = _S.inv() * R
+    Eigen::Matrix<double, 28, 28> SR; // _SR = _S.inv() * R
-    Eigen::Matrix<double, 28, 18> _STC; // _STC = (_S.transpose()).inv() * C
+    Eigen::Matrix<double, 28, 18> STC; // _STC = (_S.transpose()).inv() * C
-    Eigen::Matrix<double, 18, 18> _IKC; // _IKC = I - KC
+    Eigen::Matrix<double, 18, 18> IKC; // _IKC = I - KC
 
     KDL::Vector g_;
+    double _dt;
 
-    std::vector<KDL::Frame> feet_poses_;
+    KDL::Rotation rotation_;
-    std::vector<KDL::Vector> feet_vels_;
+    KDL::Vector acceleration_;
+    KDL::Vector gyro_;
+
+    std::vector<KDL::Frame> foot_poses_;
+    std::vector<KDL::Vector> foot_vels_;
+    std::vector<std::shared_ptr<LowPassFilter> > low_pass_filters_;
 
-    double _trust;
     double _largeVariance;
 };
 

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

@@ -30,10 +30,9 @@ public:
 
     /**
      * Calculate the foot end position based on joint positions
-     * @param joint_positions vector of joint positions
      * @return vector of foot-end position
      */
-    [[nodiscard]] std::vector<KDL::Frame> getFeet2BPositions(const std::vector<KDL::JntArray> &joint_positions) const;
+    [[nodiscard]] std::vector<KDL::Frame> getFeet2BPositions() const;
 
     /**
      * Calculate the foot end position based on joint positions
@@ -63,22 +62,21 @@ public:
      * @param index leg index
      * @return velocity vector
      */
-    [[nodiscard]] KDL::Vector getFeet2BVelocities (int index) const;
+    [[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;
+    [[nodiscard]] std::vector<KDL::Vector> getFeet2BVelocities() const;
 
+    double mass_ = 0;
     std::vector<KDL::JntArray> current_joint_pos_;
     std::vector<KDL::JntArray> current_joint_vel_;
 
     void update(const CtrlComponent &ctrlComp);
 
 private:
-    double mass_ = 0;
-
     std::vector<std::shared_ptr<RobotLeg> > robot_legs_;
 
     KDL::Chain fr_chain_;

controllers/unitree_guide_controller/readme.md

@@ -1,6 +1,6 @@
 # Unitree Guide Controller
 
-This is a ros2-control controller based on unitree guide.
+This is a ros2-control controller based on unitree guide. The original unitree guide project could be found [here](https://github.com/unitreerobotics/unitree_guide)
 
 ```bash
 cd ~/ros2_ws

controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp

@@ -0,0 +1,97 @@
+//
+// Created by tlab-uav on 24-9-16.
+//
+
+#include "unitree_guide_controller/FSM/StateBalanceTest.h"
+
+#include <unitree_guide_controller/common/mathTools.h>
+
+StateBalanceTest::StateBalanceTest(CtrlComponent ctrlComp) : FSMState(FSMStateName::BALANCETEST, "balance test",
+                                                                      std::move(ctrlComp)) {
+    _xMax = 0.05;
+    _xMin = -_xMax;
+    _yMax = 0.05;
+    _yMin = -_yMax;
+    _zMax = 0.04;
+    _zMin = -_zMax;
+    _yawMax = 20 * M_PI / 180;
+    _yawMin = -_yawMax;
+
+    Kp_p_ = Vec3(150, 150, 150).asDiagonal();
+    Kd_p_ = Vec3(25, 25, 25).asDiagonal();
+
+    kp_w_ = 200;
+    Kd_w_ = Vec3(30, 30, 30).asDiagonal();
+}
+
+void StateBalanceTest::enter() {
+    pcd_ = ctrlComp_.estimator_.get().getPosition();
+    pcdInit_ = pcd_;
+    RdInit_ = ctrlComp_.estimator_.get().getRotation();
+}
+
+void StateBalanceTest::run() {
+    pcd_(0) = pcdInit_(0) + invNormalize(ctrlComp_.control_inputs_.get().ly, _xMin, _xMax);
+    pcd_(1) = pcdInit_(1) - invNormalize(ctrlComp_.control_inputs_.get().lx, _yMin, _yMax);
+    pcd_(2) = pcdInit_(2) + invNormalize(ctrlComp_.control_inputs_.get().ry, _zMin, _zMax);
+    const float yaw = invNormalize(ctrlComp_.control_inputs_.get().rx, _yawMin, _yawMax);
+    Rd_ = KDL::Rotation::RPY(0, 0, yaw) * RdInit_;
+
+    pose_body_ = ctrlComp_.estimator_.get().getPosition();
+    vel_body_ = ctrlComp_.estimator_.get().getVelocity();
+
+    calcTorque();
+
+    for (int i = 0; i < 12; i++) {
+        ctrlComp_.joint_kp_command_interface_[i].get().set_value(0.8);
+        ctrlComp_.joint_kd_command_interface_[i].get().set_value(0.8);
+    }
+}
+
+void StateBalanceTest::exit() {
+}
+
+FSMStateName StateBalanceTest::checkChange() {
+    switch (ctrlComp_.control_inputs_.get().command) {
+        case 1:
+            return FSMStateName::FIXEDDOWN;
+        case 2:
+            return FSMStateName::FIXEDSTAND;
+        default:
+            return FSMStateName::BALANCETEST;
+    }
+}
+
+void StateBalanceTest::calcTorque() {
+    // expected body acceleration
+    _ddPcd = Kp_p_ * Vec3((pcd_ - pose_body_).data) + Kd_p_ * Vec3(
+                 (KDL::Vector(0, 0, 0) - vel_body_).data);
+
+    std::cout << "ddPcd: " << Vec3(vel_body_.data) << std::endl;
+
+    // expected body angular acceleration
+    const KDL::Rotation B2G_RotMat = ctrlComp_.estimator_.get().getRotation();
+    const KDL::Rotation G2B_RotMat = B2G_RotMat.Inverse();
+    _dWbd = kp_w_ * rotationToExp(Rd_ * G2B_RotMat) +
+            Kd_w_ * (Vec3(0, 0, 0) - Vec3(ctrlComp_.estimator_.get().getGlobalGyro().data));
+
+    // calculate foot force
+    const std::vector contact(4, 1);
+    const std::vector<KDL::Vector> foot_force = ctrlComp_.balance_ctrl_.get().calF(_ddPcd, _dWbd, B2G_RotMat,
+        ctrlComp_.estimator_.get().
+        getFootPos2Body(), contact);
+
+    std::vector<KDL::JntArray> current_joints = ctrlComp_.robot_model_.get().current_joint_pos_;
+    for (int i = 0; i < 4; i++) {
+        const auto &foot = foot_force[i];
+        std::cout << "foot_force: " << foot.x() << "," << foot.y() << "," << foot.z() << std::endl;
+        const KDL::Wrench wrench0(G2B_RotMat * foot, KDL::Vector::Zero());
+        const KDL::Wrenches wrenches(1, wrench0); // 创建一个包含 wrench0 的容器
+        KDL::JntArray torque0 = ctrlComp_.robot_model_.get().getTorque(wrenches, i);
+
+        for (int j = 0; j < 3; j++) {
+            ctrlComp_.joint_effort_command_interface_[i * 3 + j].get().set_value(torque0(j));
+            ctrlComp_.joint_position_command_interface_[i * 3 + j].get().set_value(current_joints[i](j));
+        }
+    }
+}

controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp

@@ -46,6 +46,8 @@ FSMStateName StateFixedStand::checkChange() {
             return FSMStateName::FREESTAND;
         case 9:
             return FSMStateName::SWINGTEST;
+        case 10:
+            return FSMStateName::BALANCETEST;
         default:
             return FSMStateName::FIXEDSTAND;
     }

controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp

@@ -22,10 +22,9 @@ void StateFreeStand::enter() {
         ctrlComp_.joint_kp_command_interface_[i].get().set_value(180);
         ctrlComp_.joint_kd_command_interface_[i].get().set_value(5);
     }
-    ctrlComp_.robot_model_.get().update(ctrlComp_);
 
     init_joint_pos_ = ctrlComp_.robot_model_.get().current_joint_pos_;
-    init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions(init_joint_pos_);
+    init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions();
 
 
     fr_init_pos_ = init_foot_pos_[0];

controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp

@@ -29,10 +29,9 @@ void StateSwingTest::enter() {
     Kp = KDL::Vector(20, 20, 50);
     Kd = KDL::Vector(5, 5, 20);
 
-    ctrlComp_.robot_model_.get().update(ctrlComp_);
     init_joint_pos_ = ctrlComp_.robot_model_.get().current_joint_pos_;
+    init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions();
 
-    init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions(init_joint_pos_);
     target_foot_pos_ = init_foot_pos_;
     fr_init_pos_ = init_foot_pos_[0];
     fr_goal_pos_ = fr_init_pos_;

controllers/unitree_guide_controller/src/UnitreeGuideController.cpp

@@ -41,6 +41,19 @@ namespace unitree_guide_controller {
 
     controller_interface::return_type UnitreeGuideController::
     update(const rclcpp::Time &time, const rclcpp::Duration &period) {
+
+        // auto now = std::chrono::steady_clock::now();
+        // std::chrono::duration<double> time_diff = now - last_update_time_;
+        // last_update_time_ = now;
+        //
+        // // Calculate the frequency
+        // update_frequency_ = 1.0 / time_diff.count();
+        // RCLCPP_INFO(get_node()->get_logger(), "Update frequency: %f Hz", update_frequency_);
+
+
+        ctrl_comp_.robot_model_.get().update(ctrl_comp_);
+        ctrl_comp_.estimator_.get().update(ctrl_comp_);
+
         if (mode_ == FSMMode::NORMAL) {
             current_state_->run();
             next_state_name_ = current_state_->checkChange();
@@ -72,7 +85,6 @@ namespace unitree_guide_controller {
             // 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;
@@ -96,8 +108,8 @@ namespace unitree_guide_controller {
         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) {
-                // Handle message
                 ctrl_comp_.robot_model_.get().init(msg->data);
+                ctrl_comp_.balance_ctrl_.get().init(ctrl_comp_.robot_model_.get());
             });
 
         get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
@@ -130,6 +142,7 @@ namespace unitree_guide_controller {
         state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_);
         state_list_.swingTest = std::make_shared<StateSwingTest>(ctrl_comp_);
         state_list_.freeStand = std::make_shared<StateFreeStand>(ctrl_comp_);
+        state_list_.balanceTest = std::make_shared<StateBalanceTest>(ctrl_comp_);
 
         // Initialize FSM
         current_state_ = state_list_.passive;
@@ -178,8 +191,8 @@ namespace unitree_guide_controller {
             //     return state_list_.trotting;
             case FSMStateName::SWINGTEST:
                 return state_list_.swingTest;
-            // case FSMStateName::BALANCETEST:
+            case FSMStateName::BALANCETEST:
-            //     return state_list_.balanceTest;
+                return state_list_.balanceTest;
             default:
                 return state_list_.invalid;
         }

controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp

@@ -0,0 +1,153 @@
+//
+// Created by tlab-uav on 24-9-16.
+//
+
+#include "unitree_guide_controller/control/BalanceCtrl.h"
+
+#include <unitree_guide_controller/common/mathTools.h>
+#include <unitree_guide_controller/quadProgpp/QuadProg++.hh>
+#include <unitree_guide_controller/robot/QuadrupedRobot.h>
+
+BalanceCtrl::BalanceCtrl() {
+    _mass = 15;
+    _alpha = 0.001;
+    _beta = 0.1;
+    _fricRatio = 0.4;
+    _g << 0, 0, -9.81;
+}
+
+void BalanceCtrl::init(const QuadrupedRobot &robot) {
+    _mass = robot.mass_;
+    _pcb = KDL::Vector(0.0, 0.0, 0.0);
+    _Ib = Vec3(0.0792, 0.2085, 0.2265).asDiagonal();
+
+    Vec6 s;
+    Vec12 w, u;
+    w << 10, 10, 4, 10, 10, 4, 10, 10, 4, 10, 10, 4;
+    u << 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3;
+
+    s << 20, 20, 50, 450, 450, 450;
+    _S = s.asDiagonal();
+    _W = w.asDiagonal();
+    _U = u.asDiagonal();
+
+    _Fprev.setZero();
+    _fricMat << 1, 0, _fricRatio, -1, 0, _fricRatio, 0, 1, _fricRatio, 0, -1,
+            _fricRatio, 0, 0, 1;
+}
+
+std::vector<KDL::Vector> BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM,
+                                           const std::vector<KDL::Vector> &feetPos2B, const std::vector<int> &contact) {
+    calMatrixA(feetPos2B, rotM);
+    calVectorBd(ddPcd, dWbd, rotM);
+    calConstraints(contact);
+
+    _G = _A.transpose() * _S * _A + _alpha * _W + _beta * _U;
+    _g0T = -_bd.transpose() * _S * _A - _beta * _Fprev.transpose() * _U;
+
+    solveQP();
+    _Fprev = _F;
+    std::vector<KDL::Vector> res;
+    res.resize(4);
+    for (int i = 0; i < 4; ++i) {
+        res[i] = KDL::Vector(_F(i * 3), _F(i * 3 + 1), _F(i * 3 + 2));
+    }
+    return res;
+}
+
+void BalanceCtrl::calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const KDL::Rotation &rotM) {
+    for (int i = 0; i < 4; ++i) {
+        _A.block(0, 3 * i, 3, 3) = I3;
+        KDL::Vector tempVec = feetPos2B[i] - rotM * _pcb;
+        _A.block(3, 3 * i, 3, 3) = skew(tempVec);
+    }
+}
+
+void BalanceCtrl::calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM) {
+    _bd.head(3) = _mass * (ddPcd - _g);
+    _bd.tail(3) = Eigen::Matrix3d(rotM.data) * _Ib * Eigen::Matrix3d(rotM.data).transpose() *
+                  dWbd;
+}
+
+void BalanceCtrl::calConstraints(const std::vector<int> &contact) {
+    int contactLegNum = 0;
+    for (int i(0); i < 4; ++i) {
+        if (contact[i] == 1) {
+            contactLegNum += 1;
+        }
+    }
+    _CI.resize(5 * contactLegNum, 12);
+    _ci0.resize(5 * contactLegNum);
+    _CE.resize(3 * (4 - contactLegNum), 12);
+    _ce0.resize(3 * (4 - contactLegNum));
+
+    _CI.setZero();
+    _ci0.setZero();
+    _CE.setZero();
+    _ce0.setZero();
+
+    int ceID = 0;
+    int ciID = 0;
+    for (int i(0); i < 4; ++i) {
+        if (contact[i] == 1) {
+            _CI.block(5 * ciID, 3 * i, 5, 3) = _fricMat;
+            ++ciID;
+        } else {
+            _CE.block(3 * ceID, 3 * i, 3, 3) = I3;
+            ++ceID;
+        }
+    }
+}
+
+void BalanceCtrl::solveQP() {
+    const int n = _F.size();
+    const int m = _ce0.size();
+    const int p = _ci0.size();
+
+    quadprogpp::Matrix<double> G, CE, CI;
+    quadprogpp::Vector<double> g0, ce0, ci0, x;
+
+    G.resize(n, n);
+    CE.resize(n, m);
+    CI.resize(n, p);
+    g0.resize(n);
+    ce0.resize(m);
+    ci0.resize(p);
+    x.resize(n);
+
+    for (int i = 0; i < n; ++i) {
+        for (int j = 0; j < n; ++j) {
+            G[i][j] = _G(i, j);
+        }
+    }
+
+    for (int i = 0; i < n; ++i) {
+        for (int j = 0; j < m; ++j) {
+            CE[i][j] = (_CE.transpose())(i, j);
+        }
+    }
+
+    for (int i = 0; i < n; ++i) {
+        for (int j = 0; j < p; ++j) {
+            CI[i][j] = (_CI.transpose())(i, j);
+        }
+    }
+
+    for (int i = 0; i < n; ++i) {
+        g0[i] = _g0T[i];
+    }
+
+    for (int i = 0; i < m; ++i) {
+        ce0[i] = _ce0[i];
+    }
+
+    for (int i = 0; i < p; ++i) {
+        ci0[i] = _ci0[i];
+    }
+
+    solve_quadprog(G, g0, CE, ce0, CI, ci0, x);
+
+    for (int i = 0; i < n; ++i) {
+        _F[i] = x[i];
+    }
+}

controllers/unitree_guide_controller/src/control/Estimator.cpp

@@ -4,48 +4,228 @@
 
 #include "unitree_guide_controller/control/Estimator.h"
 #include "unitree_guide_controller/control/CtrlComponent.h"
-#include <unitree_guide_controller/common/mathTools.h>
+#include "unitree_guide_controller/common/mathTools.h"
+
+#define I3 Eigen::MatrixXd::Identity(3, 3)
+
+// 12x12 Identity Matrix
+#define I12 Eigen::MatrixXd::Identity(12, 12)
+
+// 18x18 Identity Matrix
+#define I18 Eigen::MatrixXd::Identity(18, 18)
 
 Estimator::Estimator() {
     g_ = KDL::Vector(0, 0, -9.81);
+    _dt = 0.002;
+    _largeVariance = 100;
+
+    _xhat.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 = _largeVariance * 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>(0.02, 3.0);
+    low_pass_filters_[1] = std::make_shared<LowPassFilter>(0.02, 3.0);
+    low_pass_filters_[2] = std::make_shared<LowPassFilter>(0.02, 3.0);
 }
 
-void Estimator::run(const CtrlComponent &ctrlComp, std::vector<KDL::Frame> feet_poses_,
+void Estimator::update(const CtrlComponent &ctrlComp) {
-                    std::vector<KDL::Vector> feet_vels_,
+    if (ctrlComp.robot_model_.get().mass_ == 0) return;
-                    const std::vector<int> &contact, const std::vector<double> &phase) {
+
-    _Q = _QInit;
+    Q = QInit;
-    _R = _RInit;
+    R = RInit;
+
+    foot_poses_ = ctrlComp.robot_model_.get().getFeet2BPositions();
+    foot_vels_ = ctrlComp.robot_model_.get().getFeet2BVelocities();
+    _feetH.setZero();
+
+    const std::vector contact(4, 1);
+    const std::vector phase(4, 0.5);
 
     // 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);
+            // foot not contact
-            _R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3);
+            Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3);
-            _R(24 + i, 24 + i) = _largeVariance;
+            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);
+            // foot contact
-            _Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) =
+            const double trust = windowFunc(phase[i], 0.2);
-                    (1 + (1 - _trust) * _largeVariance) *
+            Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) =
-                    _QInit.block(6 + 3 * i, 6 + 3 * i, 3, 3);
+                    (1 + (1 - trust) * _largeVariance) *
-            _R.block(12 + 3 * i, 12 + 3 * i, 3, 3) =
+                    QInit.block(6 + 3 * i, 6 + 3 * i, 3, 3);
-                    (1 + (1 - _trust) * _largeVariance) *
+            R.block(12 + 3 * i, 12 + 3 * i, 3, 3) =
-                    _RInit.block(12 + 3 * i, 12 + 3 * i, 3, 3);
+                    (1 + (1 - trust) * _largeVariance) *
-            _R(24 + i, 24 + i) =
+                    RInit.block(12 + 3 * i, 12 + 3 * i, 3, 3);
-                    (1 + (1 - _trust) * _largeVariance) * _RInit(24 + i, 24 + i);
+            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);
+        _feetPos2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(foot_poses_[i].p.data);
-        _feetVel2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(feet_vels_[i].data);
+        _feetVel2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(foot_vels_[i].data);
     }
 
     // Acceleration from imu as system input
-    const KDL::Rotation rotation = KDL::Rotation::Quaternion(ctrlComp.imu_state_interface_[7].get().get_value(),
+    rotation_ = KDL::Rotation::Quaternion(ctrlComp.imu_state_interface_[0].get().get_value(),
-                                                             ctrlComp.imu_state_interface_[8].get().get_value(),
+                                          ctrlComp.imu_state_interface_[1].get().get_value(),
-                                                             ctrlComp.imu_state_interface_[9].get().get_value(),
+                                          ctrlComp.imu_state_interface_[2].get().get_value(),
-                                                             ctrlComp.imu_state_interface_[6].get().get_value());
+                                          ctrlComp.imu_state_interface_[3].get().get_value());
-    const KDL::Vector acc(ctrlComp.imu_state_interface_[3].get().get_value(),
+
-                          ctrlComp.imu_state_interface_[4].get().get_value(),
+    gyro_ = KDL::Vector(ctrlComp.imu_state_interface_[4].get().get_value(),
-                          ctrlComp.imu_state_interface_[5].get().get_value());
+                        ctrlComp.imu_state_interface_[5].get().get_value(),
-    _u = Eigen::Map<Eigen::Vector3d>((rotation * acc + g_).data);
+                        ctrlComp.imu_state_interface_[6].get().get_value());
-    _xhat = _A * _xhat + _B * _u;
+
-    _yhat = _C * _xhat;
+    acceleration_ = KDL::Vector(ctrlComp.imu_state_interface_[7].get().get_value(),
+                                ctrlComp.imu_state_interface_[8].get().get_value(),
+                                ctrlComp.imu_state_interface_[9].get().get_value());
+
+    _u = Vec3((rotation_ * acceleration_ + g_).data);
+
+    const double *rot_mat = rotation_.data;
+    std::cout << "Rotation Matrix: " << std::endl;
+    for (int i = 0; i < 3; ++i) {
+        for (int j = 0; j < 3; ++j) {
+            std::cout << rot_mat[i * 3 + j] << " ";
+        }
+        std::cout << std::endl;
+    }
+
+    _xhat = A * _xhat + B * _u;
+    _yhat = C * _xhat;
+
+    // Update the measurement value
+    _y << _feetPos2Body, _feetVel2Body, _feetH;
+
+    // Update the covariance matrix
+    Ppriori = A * P * A.transpose() + Q;
+    S = R + C * Ppriori * C.transpose();
+    Slu = S.lu();
+    Sy = Slu.solve(_y - _yhat);
+    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
+    _xhat += 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(_xhat(3));
+    low_pass_filters_[1]->addValue(_xhat(4));
+    low_pass_filters_[2]->addValue(_xhat(5));
+    _xhat(3) = low_pass_filters_[0]->getValue();
+    _xhat(4) = low_pass_filters_[1]->getValue();
+    _xhat(5) = low_pass_filters_[2]->getValue();
 }

controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp

@@ -42,11 +42,11 @@ std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &p
     return result;
 }
 
-std::vector<KDL::Frame> QuadrupedRobot::getFeet2BPositions(const std::vector<KDL::JntArray> &joint_positions) const {
+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(joint_positions[i]);
+        result[i] = robot_legs_[i]->calcPEe2B(current_joint_pos_[i]);
     }
     return result;
 }
@@ -80,6 +80,7 @@ std::vector<KDL::Vector> QuadrupedRobot::getFeet2BVelocities() const {
 }
 
 void QuadrupedRobot::update(const CtrlComponent &ctrlComp) {
+    if (mass_ == 0) return;
     for (int i = 0; i < 4; i++) {
         KDL::JntArray pos_array(3);
         pos_array(0) = ctrlComp.joint_position_state_interface_[i * 3].get().get_value();

descriptions/go2_description/config/robot_control.yaml

@@ -50,13 +50,13 @@ unitree_guide_controller:
     imu_name: "imu_sensor"
 
     imu_interfaces:
+      - orientation.x
+      - orientation.y
+      - orientation.z
+      - orientation.w
       - angular_velocity.x
       - angular_velocity.y
       - angular_velocity.z
       - linear_acceleration.x
       - linear_acceleration.y
-      - linear_acceleration.z
+      - linear_acceleration.z
-      - orientation.w
-      - orientation.x
-      - orientation.y
-      - orientation.z