From 5eaf8e1e17bd5bfc9faa84e60b68a186364ce9ec Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Mon, 16 Sep 2024 21:15:34 +0800 Subject: [PATCH] tried to add balance test --- .../unitree_guide_controller/CMakeLists.txt | 3 + .../FSM/StateBalanceTest.h | 43 ++++ .../UnitreeGuideController.h | 2 + .../common/mathTools.h | 32 +++ .../common/mathTypes.h | 34 +-- .../control/BalanceCtrl.h | 48 ++++ .../control/CtrlComponent.h | 12 +- .../control/Estimator.h | 116 ++++++--- .../robot/QuadrupedRobot.h | 10 +- .../unitree_guide_controller/readme.md | 2 +- .../src/FSM/StateBalanceTest.cpp | 97 +++++++ .../src/FSM/StateFixedStand.cpp | 2 + .../src/FSM/StateFreeStand.cpp | 3 +- .../src/FSM/StateSwingTest.cpp | 3 +- .../src/UnitreeGuideController.cpp | 21 +- .../src/control/BalanceCtrl.cpp | 153 +++++++++++ .../src/control/Estimator.cpp | 240 +++++++++++++++--- .../src/robot/QuadrupedRobot.cpp | 5 +- .../go2_description/config/robot_control.yaml | 10 +- 19 files changed, 732 insertions(+), 104 deletions(-) create mode 100644 controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h create mode 100644 controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h create mode 100644 controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp create mode 100644 controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp diff --git a/controllers/unitree_guide_controller/CMakeLists.txt b/controllers/unitree_guide_controller/CMakeLists.txt index 417fc9f..e308953 100644 --- a/controllers/unitree_guide_controller/CMakeLists.txt +++ b/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 diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h new file mode 100644 index 0000000..b2b786b --- /dev/null +++ b/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 diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h index e309090..a6c481b 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h @@ -10,6 +10,7 @@ #include #include +#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 freeStand; std::shared_ptr swingTest; + std::shared_ptr balanceTest; }; class UnitreeGuideController final : public controller_interface::ControllerInterface { diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h index cab50f7..5e630f2 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h +++ b/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 diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTypes.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTypes.h index 0770512..d7f18ad 100755 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTypes.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTypes.h @@ -10,58 +10,58 @@ /******** Vector ********/ /************************/ // 2x1 Vector -using Vec2 = typename Eigen::Matrix; +using Vec2 = Eigen::Matrix; // 3x1 Vector -using Vec3 = typename Eigen::Matrix; +using Vec3 = Eigen::Matrix; // 4x1 Vector -using Vec4 = typename Eigen::Matrix; +using Vec4 = Eigen::Matrix; // 6x1 Vector -using Vec6 = typename Eigen::Matrix; +using Vec6 = Eigen::Matrix; // Quaternion -using Quat = typename Eigen::Matrix; +using Quat = Eigen::Matrix; // 4x1 Integer Vector -using VecInt4 = typename Eigen::Matrix; +using VecInt4 = Eigen::Matrix; // 12x1 Vector -using Vec12 = typename Eigen::Matrix; +using Vec12 = Eigen::Matrix; // 18x1 Vector -using Vec18 = typename Eigen::Matrix; +using Vec18 = Eigen::Matrix; // Dynamic Length Vector -using VecX = typename Eigen::Matrix; +using VecX = Eigen::Matrix; /************************/ /******** Matrix ********/ /************************/ // Rotation Matrix -using RotMat = typename Eigen::Matrix; +using RotMat = Eigen::Matrix; // Homogenous Matrix -using HomoMat = typename Eigen::Matrix; +using HomoMat = Eigen::Matrix; // 2x2 Matrix -using Mat2 = typename Eigen::Matrix; +using Mat2 = Eigen::Matrix; // 3x3 Matrix -using Mat3 = typename Eigen::Matrix; +using Mat3 = Eigen::Matrix; // 3x3 Identity Matrix #define I3 Eigen::MatrixXd::Identity(3, 3) // 3x4 Matrix, each column is a 3x1 vector -using Vec34 = typename Eigen::Matrix; +using Vec34 = Eigen::Matrix; // 6x6 Matrix -using Mat6 = typename Eigen::Matrix; +using Mat6 = Eigen::Matrix; // 12x12 Matrix -using Mat12 = typename Eigen::Matrix; +using Mat12 = Eigen::Matrix; // 12x12 Identity Matrix #define I12 Eigen::MatrixXd::Identity(12, 12) @@ -70,7 +70,7 @@ using Mat12 = typename Eigen::Matrix; #define I18 Eigen::MatrixXd::Identity(18, 18) // Dynamic Size Matrix -using MatX = typename Eigen::Matrix; +using MatX = Eigen::Matrix; /************************/ /****** Functions *******/ diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h new file mode 100644 index 0000000..307e09e --- /dev/null +++ b/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 + +#include "unitree_guide_controller/common/mathTypes.h" +class QuadrupedRobot; + +class BalanceCtrl { +public: + explicit BalanceCtrl(); + + ~BalanceCtrl() = default; + + void init(const QuadrupedRobot &robot); + + std::vector calF(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM, + const std::vector &feetPos2B, const std::vector &contact); + +private: + void calMatrixA(const std::vector &feetPos2B, const KDL::Rotation &rotM); + + void calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM); + + void calConstraints(const std::vector &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 _A; + Eigen::Matrix _fricMat; +}; + + +#endif //BALANCECTRL_H diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h index b204731..d393fde 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h @@ -11,6 +11,9 @@ #include #include +#include "BalanceCtrl.h" +#include "Estimator.h" + struct CtrlComponent { std::vector > joint_effort_command_interface_; @@ -41,7 +44,14 @@ struct CtrlComponent { QuadrupedRobot default_robot_model_; std::reference_wrapper robot_model_; - CtrlComponent() : control_inputs_(default_inputs_), robot_model_(default_robot_model_) { + Estimator default_estimator_; + std::reference_wrapper estimator_; + + BalanceCtrl default_balance_ctrl_; + std::reference_wrapper balance_ctrl_; + + CtrlComponent() : control_inputs_(default_inputs_), robot_model_(default_robot_model_), + estimator_(default_estimator_), balance_ctrl_(default_balance_ctrl_) { } void clear() { diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h index 85323ba..5082a23 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h @@ -4,9 +4,12 @@ #ifndef ESTIMATOR_H #define ESTIMATOR_H +#include #include #include +#include "LowPassFilter.h" + struct CtrlComponent; class Estimator { @@ -19,49 +22,89 @@ public: * Get the estimated robot central position * @return robot central position */ - Eigen::Matrix getPosition() { - return _xhat.segment(0, 3); + KDL::Vector getPosition() { + return {_xhat(0), _xhat(1), _xhat(2)}; } /** * Get the estimated robot central velocity * @return robot central velocity */ - Eigen::Matrix getVelocity() { - return _xhat.segment(3, 3); + KDL::Vector getVelocity() { + return {_xhat(3), _xhat(4), _xhat(5)}; } - void run( - const CtrlComponent &ctrlComp, - std::vector feet_poses_, - std::vector feet_vels_, - const std::vector &contact, - const std::vector &phase - ); + /** + * Get the estimated foot position in world frame + * @param index leg index + * @return foot position in world frame + */ + 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 getFootPos() { + std::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 getFootPos2Body() { + std::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 _xhat; // The state of estimator, position(3)+velocity(3)+feet position(3x4) Eigen::Matrix _u; // The input of estimator Eigen::Matrix _y; // The measurement value of output y Eigen::Matrix _yhat; // The prediction of output y - Eigen::Matrix _A; // The transtion matrix of estimator - Eigen::Matrix _B; // The input matrix - Eigen::Matrix _C; // The output matrix + Eigen::Matrix A; // The transtion matrix of estimator + Eigen::Matrix B; // The input matrix + Eigen::Matrix C; // The output matrix // Covariance Matrix - Eigen::Matrix _P; // Prediction covariance - Eigen::Matrix _Ppriori; // Priori prediction covariance - Eigen::Matrix _Q; // Dynamic simulation covariance - Eigen::Matrix _R; // Measurement covariance + Eigen::Matrix P; // Prediction covariance + Eigen::Matrix Ppriori; // Priori prediction covariance + Eigen::Matrix Q; // Dynamic simulation covariance + Eigen::Matrix R; // Measurement covariance Eigen::Matrix - _QInit; // Initial value of Dynamic simulation covariance + QInit; // Initial value of Dynamic simulation covariance Eigen::Matrix - _RInit; // Initial value of Measurement covariance - Eigen::Matrix _Qdig; // adjustable process noise covariance - Eigen::Matrix _Cu; // The covariance of system input u + RInit; // Initial value of Measurement covariance + Eigen::Matrix Qdig; // adjustable process noise covariance + Eigen::Matrix Cu; // The covariance of system input u // Output Measurement Eigen::Matrix _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 _feetH; // The Height of each foot, in the global coordinate - Eigen::Matrix _S; // _S = C*P*C.T + R - Eigen::PartialPivLU > _Slu; // _S.lu() - Eigen::Matrix _Sy; // _Sy = _S.inv() * (y - yhat) - Eigen::Matrix _Sc; // _Sc = _S.inv() * C - Eigen::Matrix _SR; // _SR = _S.inv() * R - Eigen::Matrix _STC; // _STC = (_S.transpose()).inv() * C - Eigen::Matrix _IKC; // _IKC = I - KC + Eigen::Matrix S; // _S = C*P*C.T + R + Eigen::PartialPivLU > Slu; // _S.lu() + Eigen::Matrix Sy; // _Sy = _S.inv() * (y - yhat) + Eigen::Matrix Sc; // _Sc = _S.inv() * C + Eigen::Matrix SR; // _SR = _S.inv() * R + Eigen::Matrix STC; // _STC = (_S.transpose()).inv() * C + Eigen::Matrix IKC; // _IKC = I - KC KDL::Vector g_; + double _dt; - std::vector feet_poses_; - std::vector feet_vels_; + KDL::Rotation rotation_; + KDL::Vector acceleration_; + KDL::Vector gyro_; + + std::vector foot_poses_; + std::vector foot_vels_; + std::vector > low_pass_filters_; - double _trust; double _largeVariance; }; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h index 719139c..466b08f 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h +++ b/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 getFeet2BPositions(const std::vector &joint_positions) const; + [[nodiscard]] std::vector 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 getFeet2BVelocities () const; + [[nodiscard]] std::vector getFeet2BVelocities() const; + double mass_ = 0; std::vector current_joint_pos_; std::vector current_joint_vel_; void update(const CtrlComponent &ctrlComp); private: - double mass_ = 0; - std::vector > robot_legs_; KDL::Chain fr_chain_; diff --git a/controllers/unitree_guide_controller/readme.md b/controllers/unitree_guide_controller/readme.md index df95a92..6ad2457 100644 --- a/controllers/unitree_guide_controller/readme.md +++ b/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 diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp new file mode 100644 index 0000000..98ca3d8 --- /dev/null +++ b/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 + +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 foot_force = ctrlComp_.balance_ctrl_.get().calF(_ddPcd, _dWbd, B2G_RotMat, + ctrlComp_.estimator_.get(). + getFootPos2Body(), contact); + + std::vector 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)); + } + } +} diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp index 525a084..5cf74ae 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp +++ b/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; } diff --git a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp index 9b65dc7..aabc1fe 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp +++ b/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]; diff --git a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp index f1824d1..9998e2f 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp +++ b/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_; diff --git a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp index 1f6a620..c672749 100644 --- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp +++ b/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 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("imu_name", imu_name_); imu_interface_types_ = auto_declare >("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( "~/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(ctrl_comp_); state_list_.swingTest = std::make_shared(ctrl_comp_); state_list_.freeStand = std::make_shared(ctrl_comp_); + state_list_.balanceTest = std::make_shared(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: - // return state_list_.balanceTest; + case FSMStateName::BALANCETEST: + return state_list_.balanceTest; default: return state_list_.invalid; } diff --git a/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp b/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp new file mode 100644 index 0000000..7520aba --- /dev/null +++ b/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 +#include +#include + +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 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM, + const std::vector &feetPos2B, const std::vector &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 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 &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 &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 G, CE, CI; + quadprogpp::Vector 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]; + } +} diff --git a/controllers/unitree_guide_controller/src/control/Estimator.cpp b/controllers/unitree_guide_controller/src/control/Estimator.cpp index d33fb64..2b74268 100644 --- a/controllers/unitree_guide_controller/src/control/Estimator.cpp +++ b/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 +#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(0.02, 3.0); + low_pass_filters_[1] = std::make_shared(0.02, 3.0); + low_pass_filters_[2] = std::make_shared(0.02, 3.0); } -void Estimator::run(const CtrlComponent &ctrlComp, std::vector feet_poses_, - std::vector feet_vels_, - const std::vector &contact, const std::vector &phase) { - _Q = _QInit; - _R = _RInit; +void Estimator::update(const CtrlComponent &ctrlComp) { + if (ctrlComp.robot_model_.get().mass_ == 0) return; + + Q = QInit; + 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); - _R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3); - _R(24 + i, 24 + i) = _largeVariance; + // foot not contact + Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3); + R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3); + R(24 + i, 24 + i) = _largeVariance; } else { - _trust = windowFunc(phase[i], 0.2); - _Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = - (1 + (1 - _trust) * _largeVariance) * - _QInit.block(6 + 3 * i, 6 + 3 * i, 3, 3); - _R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = - (1 + (1 - _trust) * _largeVariance) * - _RInit.block(12 + 3 * i, 12 + 3 * i, 3, 3); - _R(24 + i, 24 + i) = - (1 + (1 - _trust) * _largeVariance) * _RInit(24 + i, 24 + i); + // foot contact + const double trust = windowFunc(phase[i], 0.2); + Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = + (1 + (1 - trust) * _largeVariance) * + QInit.block(6 + 3 * i, 6 + 3 * i, 3, 3); + R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = + (1 + (1 - trust) * _largeVariance) * + RInit.block(12 + 3 * i, 12 + 3 * i, 3, 3); + R(24 + i, 24 + i) = + (1 + (1 - trust) * _largeVariance) * RInit(24 + i, 24 + i); } - _feetPos2Body.segment(3 * i, 3) = Eigen::Map(feet_poses_[i].p.data); - _feetVel2Body.segment(3 * i, 3) = Eigen::Map(feet_vels_[i].data); + _feetPos2Body.segment(3 * i, 3) = Eigen::Map(foot_poses_[i].p.data); + _feetVel2Body.segment(3 * i, 3) = Eigen::Map(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(), - ctrlComp.imu_state_interface_[8].get().get_value(), - ctrlComp.imu_state_interface_[9].get().get_value(), - ctrlComp.imu_state_interface_[6].get().get_value()); - const KDL::Vector acc(ctrlComp.imu_state_interface_[3].get().get_value(), - ctrlComp.imu_state_interface_[4].get().get_value(), - ctrlComp.imu_state_interface_[5].get().get_value()); - _u = Eigen::Map((rotation * acc + g_).data); - _xhat = _A * _xhat + _B * _u; - _yhat = _C * _xhat; + rotation_ = KDL::Rotation::Quaternion(ctrlComp.imu_state_interface_[0].get().get_value(), + ctrlComp.imu_state_interface_[1].get().get_value(), + ctrlComp.imu_state_interface_[2].get().get_value(), + ctrlComp.imu_state_interface_[3].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_[6].get().get_value()); + + 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(); } diff --git a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp index 4831cce..581a370 100644 --- a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp +++ b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp @@ -42,11 +42,11 @@ std::vector QuadrupedRobot::getQ(const std::vector &p return result; } -std::vector QuadrupedRobot::getFeet2BPositions(const std::vector &joint_positions) const { +std::vector QuadrupedRobot::getFeet2BPositions() const { std::vector 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 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(); diff --git a/descriptions/go2_description/config/robot_control.yaml b/descriptions/go2_description/config/robot_control.yaml index 91e5dfb..711b2aa 100644 --- a/descriptions/go2_description/config/robot_control.yaml +++ b/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 - - orientation.w - - orientation.x - - orientation.y - - orientation.z \ No newline at end of file + - linear_acceleration.z \ No newline at end of file