From 028b7b52b1e6ea44887f687c29e781f764ad3090 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Mon, 16 Sep 2024 13:44:08 +0800 Subject: [PATCH] add imu estimator --- .../unitree_guide_controller/CMakeLists.txt | 1 + .../UnitreeGuideController.h | 3 + .../common/mathTools.h | 23 ++++++ .../control/CtrlComponent.h | 5 ++ .../control/Estimator.h | 78 ++++++++++++++++++- .../control/LowPassFilter.h | 29 +++++++ .../robot/QuadrupedRobot.h | 13 ++++ .../src/FSM/StateSwingTest.cpp | 6 +- .../src/UnitreeGuideController.cpp | 15 +++- .../src/control/Estimator.cpp | 46 +++++++++++ .../src/control/LowPassFilter.cpp | 31 ++++++++ .../src/robot/QuadrupedRobot.cpp | 15 ++++ .../go2_description/config/robot_control.yaml | 17 +++- 13 files changed, 272 insertions(+), 10 deletions(-) create mode 100644 controllers/unitree_guide_controller/include/unitree_guide_controller/control/LowPassFilter.h create mode 100644 controllers/unitree_guide_controller/src/control/LowPassFilter.cpp diff --git a/controllers/unitree_guide_controller/CMakeLists.txt b/controllers/unitree_guide_controller/CMakeLists.txt index c595cb1..417fc9f 100644 --- a/controllers/unitree_guide_controller/CMakeLists.txt +++ b/controllers/unitree_guide_controller/CMakeLists.txt @@ -37,6 +37,7 @@ add_library(${PROJECT_NAME} SHARED src/robot/RobotLeg.cpp src/control/Estimator.cpp + src/control/LowPassFilter.cpp src/quadProgpp/Array.cc src/quadProgpp/QuadProg++.cc 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 b2b1390..e309090 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h @@ -76,6 +76,9 @@ namespace unitree_guide_controller { std::vector command_interface_types_; std::vector state_interface_types_; + std::string imu_name_; + std::vector imu_interface_types_; + rclcpp::Subscription::SharedPtr control_input_subscription_; rclcpp::Subscription::SharedPtr robot_description_subscription_; 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 d91f726..cab50f7 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 @@ -4,6 +4,7 @@ #ifndef MATHTOOLS_H #define MATHTOOLS_H +#include template T1 invNormalize(const T0 value, const T1 min, const T2 max, @@ -11,4 +12,26 @@ T1 invNormalize(const T0 value, const T1 min, const T2 max, return (value - minLim) * (max - min) / (maxLim - minLim) + min; } +template +T windowFunc(const T x, const T windowRatio, const T xRange = 1.0, + const T yRange = 1.0) { + if (x < 0 || x > xRange) { + std::cout << "[ERROR][windowFunc] The x=" << x + << ", which should between [0, xRange]" << std::endl; + } + if (windowRatio <= 0 || windowRatio >= 0.5) { + std::cout << "[ERROR][windowFunc] The windowRatio=" << windowRatio + << ", which should between [0, 0.5]" << std::endl; + } + + if (x / xRange < windowRatio) { + return x * yRange / (xRange * windowRatio); + } + if (x / xRange > 1 - windowRatio) { + return yRange * (xRange - x) / (xRange * windowRatio); + } + return yRange; +} + + #endif //MATHTOOLS_H 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 a02b478..b204731 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 @@ -31,6 +31,9 @@ struct CtrlComponent { std::vector > joint_velocity_state_interface_; + std::vector > + imu_state_interface_; + control_input_msgs::msg::Inputs default_inputs_; std::reference_wrapper control_inputs_; int frequency_{}; @@ -51,6 +54,8 @@ struct CtrlComponent { joint_effort_state_interface_.clear(); joint_position_state_interface_.clear(); joint_velocity_state_interface_.clear(); + + imu_state_interface_.clear(); } }; 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 8187df6..85323ba 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,13 +4,87 @@ #ifndef ESTIMATOR_H #define ESTIMATOR_H +#include +#include - +struct CtrlComponent; class Estimator { +public: + Estimator(); + ~Estimator() = default; + + /** + * Get the estimated robot central position + * @return robot central position + */ + Eigen::Matrix getPosition() { + return _xhat.segment(0, 3); + } + + /** + * Get the estimated robot central velocity + * @return robot central velocity + */ + Eigen::Matrix getVelocity() { + return _xhat.segment(3, 3); + } + + void run( + const CtrlComponent &ctrlComp, + std::vector feet_poses_, + std::vector feet_vels_, + const std::vector &contact, + const std::vector &phase + ); + +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 + // 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 + _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 + // Output Measurement + Eigen::Matrix + _feetPos2Body; // The feet positions to body, in the global coordinate + Eigen::Matrix + _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 + + KDL::Vector g_; + + std::vector feet_poses_; + std::vector feet_vels_; + + double _trust; + double _largeVariance; }; - #endif //ESTIMATOR_H diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/LowPassFilter.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/LowPassFilter.h new file mode 100644 index 0000000..8c80822 --- /dev/null +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/LowPassFilter.h @@ -0,0 +1,29 @@ +// +// Created by biao on 24-9-16. +// + + +#ifndef LOWPASSFILTER_H +#define LOWPASSFILTER_H + + +class LowPassFilter { +public: + LowPassFilter(double samplePeriod, double cutFrequency); + + ~LowPassFilter() = default; + + void addValue(double newValue); + + [[nodiscard]] double getValue() const; + + void clear(); + +private: + double weight_; + double pass_value_{}; + bool start_; +}; + + +#endif //LOWPASSFILTER_H 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 8eb1b0f..719139c 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 @@ -58,6 +58,19 @@ public: [[nodiscard]] KDL::JntArray getTorque( const KDL::Wrenches &force, int index) const; + /** + * Calculate the foot end velocity + * @param index leg index + * @return velocity vector + */ + [[nodiscard]] KDL::Vector getFeet2BVelocities (int index) const; + + /** + * Calculate all foot end velocity + * @return list of foot end velocity + */ + [[nodiscard]] std::vector getFeet2BVelocities () const; + std::vector current_joint_pos_; std::vector current_joint_vel_; diff --git a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp index 1623595..f1824d1 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp @@ -92,14 +92,10 @@ void StateSwingTest::positionCtrl() { void StateSwingTest::torqueCtrl() const { const KDL::Frame fr_current_pos = ctrlComp_.robot_model_.get().getFeet2BPositions(0); - KDL::Jacobian fr_jaco = ctrlComp_.robot_model_.get().getJacobian(0); const KDL::Vector pos_goal = fr_goal_pos_.p; const KDL::Vector pos0 = fr_current_pos.p; - const Eigen::Matrix linear_jacobian = fr_jaco.data.topRows(3); - Eigen::Product, Eigen::Matrix > vel_eigen = - linear_jacobian * ctrlComp_.robot_model_.get().current_joint_vel_[0].data; - const KDL::Vector vel0(vel_eigen(0), vel_eigen(1), vel_eigen(2)); + const KDL::Vector vel0 = ctrlComp_.robot_model_.get().getFeet2BVelocities(0); const KDL::Vector force0 = Kp * (pos_goal - pos0) + Kd * (-vel0); const KDL::Wrench wrench0(force0, KDL::Vector::Zero()); // 假设力矩为零 diff --git a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp index 6d57fbe..1f6a620 100644 --- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp +++ b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp @@ -32,6 +32,10 @@ namespace unitree_guide_controller { } } + for (const auto &interface_type: imu_interface_types_) { + conf.names.push_back(imu_name_ + "/" += interface_type); + } + return conf; } @@ -65,7 +69,10 @@ namespace unitree_guide_controller { state_interface_types_ = auto_declare >("state_interfaces", state_interface_types_); - // Initialize variables and pointers + // 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; @@ -111,7 +118,11 @@ namespace unitree_guide_controller { // assign state interfaces for (auto &interface: state_interfaces_) { - state_interface_map_[interface.get_interface_name()]->push_back(interface); + if (interface.get_prefix_name() == imu_name_) { + ctrl_comp_.imu_state_interface_.emplace_back(interface); + } else { + state_interface_map_[interface.get_interface_name()]->push_back(interface); + } } state_list_.passive = std::make_shared(ctrl_comp_); diff --git a/controllers/unitree_guide_controller/src/control/Estimator.cpp b/controllers/unitree_guide_controller/src/control/Estimator.cpp index c8b1bdc..d33fb64 100644 --- a/controllers/unitree_guide_controller/src/control/Estimator.cpp +++ b/controllers/unitree_guide_controller/src/control/Estimator.cpp @@ -3,3 +3,49 @@ // #include "unitree_guide_controller/control/Estimator.h" +#include "unitree_guide_controller/control/CtrlComponent.h" +#include + +Estimator::Estimator() { + g_ = KDL::Vector(0, 0, -9.81); +} + +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; + + // Adjust the covariance based on foot contact and phase. + for (int i(0); i < 4; ++i) { + if (contact[i] == 0) { + _Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3); + _R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3); + _R(24 + i, 24 + i) = _largeVariance; + } else { + _trust = windowFunc(phase[i], 0.2); + _Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = + (1 + (1 - _trust) * _largeVariance) * + _QInit.block(6 + 3 * i, 6 + 3 * i, 3, 3); + _R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = + (1 + (1 - _trust) * _largeVariance) * + _RInit.block(12 + 3 * i, 12 + 3 * i, 3, 3); + _R(24 + i, 24 + i) = + (1 + (1 - _trust) * _largeVariance) * _RInit(24 + i, 24 + i); + } + _feetPos2Body.segment(3 * i, 3) = Eigen::Map(feet_poses_[i].p.data); + _feetVel2Body.segment(3 * i, 3) = Eigen::Map(feet_vels_[i].data); + } + + // Acceleration from imu as system input + const KDL::Rotation rotation = KDL::Rotation::Quaternion(ctrlComp.imu_state_interface_[7].get().get_value(), + ctrlComp.imu_state_interface_[8].get().get_value(), + ctrlComp.imu_state_interface_[9].get().get_value(), + ctrlComp.imu_state_interface_[6].get().get_value()); + const KDL::Vector acc(ctrlComp.imu_state_interface_[3].get().get_value(), + ctrlComp.imu_state_interface_[4].get().get_value(), + ctrlComp.imu_state_interface_[5].get().get_value()); + _u = Eigen::Map((rotation * acc + g_).data); + _xhat = _A * _xhat + _B * _u; + _yhat = _C * _xhat; +} diff --git a/controllers/unitree_guide_controller/src/control/LowPassFilter.cpp b/controllers/unitree_guide_controller/src/control/LowPassFilter.cpp new file mode 100644 index 0000000..391b5df --- /dev/null +++ b/controllers/unitree_guide_controller/src/control/LowPassFilter.cpp @@ -0,0 +1,31 @@ +// +// Created by biao on 24-9-16. +// +#include +#include "unitree_guide_controller/control/LowPassFilter.h" + +/** + * Low Pass Filter to prevent high frequency signal + * @param samplePeriod sample period + * @param cutFrequency cut frequency + */ +LowPassFilter::LowPassFilter(const double samplePeriod, const double cutFrequency) { + weight_ = 1.0 / (1.0 + 1.0 / (2.0 * M_PI * samplePeriod * cutFrequency)); + start_ = false; +} + +void LowPassFilter::addValue(const double newValue) { + if (!start_) { + start_ = true; + pass_value_ = newValue; + } + pass_value_ = weight_ * newValue + (1 - weight_) * pass_value_; +} + +double LowPassFilter::getValue() const { + return pass_value_; +} + +void LowPassFilter::clear() { + start_ = false; +} diff --git a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp index 76e674f..4831cce 100644 --- a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp +++ b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp @@ -64,6 +64,21 @@ KDL::JntArray QuadrupedRobot::getTorque( return robot_legs_[index]->calcTorque(current_joint_pos_[index], current_joint_vel_[index], force); } +KDL::Vector QuadrupedRobot::getFeet2BVelocities(const int index) const { + const Eigen::Matrix jacobian = getJacobian(index).data.topRows(3); + Eigen::VectorXd foot_velocity = jacobian * current_joint_vel_[index].data; + return {foot_velocity(0), foot_velocity(1), foot_velocity(2)}; +} + +std::vector QuadrupedRobot::getFeet2BVelocities() const { + std::vector result; + result.resize(4); + for (int i = 0; i < 4; i++) { + result[i] = getFeet2BVelocities(i); + } + return result; +} + void QuadrupedRobot::update(const CtrlComponent &ctrlComp) { for (int i = 0; i < 4; i++) { KDL::JntArray pos_array(3); diff --git a/descriptions/go2_description/config/robot_control.yaml b/descriptions/go2_description/config/robot_control.yaml index 3817005..91e5dfb 100644 --- a/descriptions/go2_description/config/robot_control.yaml +++ b/descriptions/go2_description/config/robot_control.yaml @@ -20,6 +20,7 @@ imu_sensor_broadcaster: unitree_guide_controller: ros__parameters: + update_rate: 500 # Hz joints: - FR_hip_joint - FR_thigh_joint @@ -44,4 +45,18 @@ unitree_guide_controller: state_interfaces: - effort - position - - velocity \ No newline at end of file + - velocity + + imu_name: "imu_sensor" + + imu_interfaces: + - angular_velocity.x + - angular_velocity.y + - angular_velocity.z + - linear_acceleration.x + - linear_acceleration.y + - linear_acceleration.z + - orientation.w + - orientation.x + - orientation.y + - orientation.z \ No newline at end of file