From 07e3cf086e79326c74955c2bf34acf3eebc683d9 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Tue, 15 Oct 2024 20:17:03 +0800 Subject: [PATCH] add estimator --- .../rl_quadruped_controller/CMakeLists.txt | 10 +- .../rl_quadruped_controller/FSM/StateRL.h | 2 +- .../common/mathTools.h | 116 +++++++++ .../common/mathTypes.h | 94 ++++++++ .../control/CtrlComponent.h | 8 +- .../control/Estimator.h | 158 ++++++++++++ .../control/LowPassFilter.h | 29 +++ .../robot/QuadrupedRobot.h | 104 ++++++++ .../rl_quadruped_controller/robot/RobotLeg.h | 59 +++++ .../src/FSM/StateRL.cpp | 25 +- ...ntroller.cpp => RlQuadrupedController.cpp} | 25 +- ...ymController.h => RlQuadrupedController.h} | 13 +- .../src/control/Estimator.cpp | 226 ++++++++++++++++++ .../src/control/LowPassFilter.cpp | 31 +++ .../src/robot/QuadrupedRobot.cpp | 128 ++++++++++ .../src/robot/RobotLeg.cpp | 44 ++++ .../src/control/Estimator.cpp | 27 +-- .../config/issacgym/config.yaml | 2 +- .../config/issacgym/config.yaml | 48 ++-- 19 files changed, 1087 insertions(+), 62 deletions(-) create mode 100644 controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTools.h create mode 100644 controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTypes.h create mode 100644 controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h create mode 100644 controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/LowPassFilter.h create mode 100644 controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/QuadrupedRobot.h create mode 100644 controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/RobotLeg.h rename controllers/rl_quadruped_controller/src/{LeggedGymController.cpp => RlQuadrupedController.cpp} (88%) rename controllers/rl_quadruped_controller/src/{LeggedGymController.h => RlQuadrupedController.h} (93%) create mode 100644 controllers/rl_quadruped_controller/src/control/Estimator.cpp create mode 100644 controllers/rl_quadruped_controller/src/control/LowPassFilter.cpp create mode 100644 controllers/rl_quadruped_controller/src/robot/QuadrupedRobot.cpp create mode 100644 controllers/rl_quadruped_controller/src/robot/RobotLeg.cpp diff --git a/controllers/rl_quadruped_controller/CMakeLists.txt b/controllers/rl_quadruped_controller/CMakeLists.txt index c071149..3466652 100644 --- a/controllers/rl_quadruped_controller/CMakeLists.txt +++ b/controllers/rl_quadruped_controller/CMakeLists.txt @@ -25,6 +25,7 @@ set(dependencies controller_interface realtime_tools control_input_msgs + kdl_parser ) # find dependencies @@ -33,7 +34,7 @@ foreach (Dependency IN ITEMS ${dependencies}) endforeach () add_library(${PROJECT_NAME} SHARED - src/LeggedGymController.cpp + src/RlQuadrupedController.cpp src/common/ObservationBuffer.cpp @@ -41,6 +42,13 @@ add_library(${PROJECT_NAME} SHARED src/FSM/StateFixedStand.cpp src/FSM/StateFixedDown.cpp src/FSM/StateRL.cpp + + src/control/LowPassFilter.cpp + src/control/Estimator.cpp + + src/robot/QuadrupedRobot.cpp + src/robot/RobotLeg.cpp + ) target_include_directories(${PROJECT_NAME} PUBLIC diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h index fcfe07b..98cd52b 100644 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h @@ -65,10 +65,10 @@ struct Control struct ModelParams { std::string model_name; std::string framework; - bool use_history; int decimation; int num_observations; std::vector observations; + std::vector observations_history; double damping; double stiffness; double action_scale; diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTools.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTools.h new file mode 100644 index 0000000..32ce1e4 --- /dev/null +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTools.h @@ -0,0 +1,116 @@ +// +// Created by tlab-uav on 24-9-12. +// + +#ifndef MATHTOOLS_H +#define MATHTOOLS_H +#include + +template +T1 invNormalize(const T0 value, const T1 min, const T2 max, + const double minLim = -1, const double maxLim = 1) { + return (value - minLim) * (max - min) / (maxLim - minLim) + min; +} + +template +T windowFunc(const T x, const T windowRatio, const T xRange = 1.0, + const T yRange = 1.0) { + if (x < 0 || x > xRange) { + std::cout << "[ERROR][windowFunc] The x=" << x + << ", which should between [0, xRange]" << std::endl; + } + if (windowRatio <= 0 || windowRatio >= 0.5) { + std::cout << "[ERROR][windowFunc] The windowRatio=" << windowRatio + << ", which should between [0, 0.5]" << std::endl; + } + + if (x / xRange < windowRatio) { + return x * yRange / (xRange * windowRatio); + } + if (x / xRange > 1 - windowRatio) { + return yRange * (xRange - x) / (xRange * windowRatio); + } + return yRange; +} + +inline Mat3 skew(const Vec3 &v) { + Mat3 m; + m << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0; + return m; +} + +inline Vec3 rotMatToExp(const RotMat &rm) { + double cosValue = rm.trace() / 2.0 - 1 / 2.0; + if (cosValue > 1.0f) { + cosValue = 1.0f; + } else if (cosValue < -1.0f) { + cosValue = -1.0f; + } + + const double angle = acos(cosValue); + Vec3 exp; + if (fabs(angle) < 1e-5) { + exp = Vec3(0, 0, 0); + } else if (fabs(angle - M_PI) < 1e-5) { + exp = angle * Vec3(rm(0, 0) + 1, rm(0, 1), rm(0, 2)) / + sqrt(2 * (1 + rm(0, 0))); + } else { + exp = angle / (2.0f * sin(angle)) * + Vec3(rm(2, 1) - rm(1, 2), rm(0, 2) - rm(2, 0), rm(1, 0) - rm(0, 1)); + } + return exp; +} + +template +T saturation(const T a, Vec2 limits) { + T lowLim, highLim; + if (limits(0) > limits(1)) { + lowLim = limits(1); + highLim = limits(0); + } else { + lowLim = limits(0); + highLim = limits(1); + } + + if (a < lowLim) { + return lowLim; + } + if (a > highLim) { + return highLim; + } + return a; +} + +inline RotMat quatToRotMat(const Quat &q) { + double e0 = q(0); + double e1 = q(1); + double e2 = q(2); + double e3 = q(3); + + RotMat R; + R << 1 - 2 * (e2 * e2 + e3 * e3), 2 * (e1 * e2 - e0 * e3), + 2 * (e1 * e3 + e0 * e2), 2 * (e1 * e2 + e0 * e3), + 1 - 2 * (e1 * e1 + e3 * e3), 2 * (e2 * e3 - e0 * e1), + 2 * (e1 * e3 - e0 * e2), 2 * (e2 * e3 + e0 * e1), + 1 - 2 * (e1 * e1 + e2 * e2); + return R; +} + +inline Vec3 rotMatToRPY(const Mat3 &R) { + Vec3 rpy; + rpy(0) = atan2(R(2, 1), R(2, 2)); + rpy(1) = asin(-R(2, 0)); + rpy(2) = atan2(R(1, 0), R(0, 0)); + return rpy; +} + +inline RotMat rotz(const double &theta) { + const double s = std::sin(theta); + const double c = std::cos(theta); + + RotMat R; + R << c, -s, 0, s, c, 0, 0, 0, 1; + return R; +} + +#endif //MATHTOOLS_H diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTypes.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTypes.h new file mode 100644 index 0000000..d7f18ad --- /dev/null +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTypes.h @@ -0,0 +1,94 @@ +/********************************************************************** + Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. +***********************************************************************/ +#ifndef MATHTYPES_H +#define MATHTYPES_H + +#include + +/************************/ +/******** Vector ********/ +/************************/ +// 2x1 Vector +using Vec2 = Eigen::Matrix; + +// 3x1 Vector +using Vec3 = Eigen::Matrix; + +// 4x1 Vector +using Vec4 = Eigen::Matrix; + +// 6x1 Vector +using Vec6 = Eigen::Matrix; + +// Quaternion +using Quat = Eigen::Matrix; + +// 4x1 Integer Vector +using VecInt4 = Eigen::Matrix; + +// 12x1 Vector +using Vec12 = Eigen::Matrix; + +// 18x1 Vector +using Vec18 = Eigen::Matrix; + +// Dynamic Length Vector +using VecX = Eigen::Matrix; + +/************************/ +/******** Matrix ********/ +/************************/ +// Rotation Matrix +using RotMat = Eigen::Matrix; + +// Homogenous Matrix +using HomoMat = Eigen::Matrix; + +// 2x2 Matrix +using Mat2 = Eigen::Matrix; + +// 3x3 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 = Eigen::Matrix; + +// 6x6 Matrix +using Mat6 = Eigen::Matrix; + +// 12x12 Matrix +using Mat12 = Eigen::Matrix; + +// 12x12 Identity Matrix +#define I12 Eigen::MatrixXd::Identity(12, 12) + +// 18x18 Identity Matrix +#define I18 Eigen::MatrixXd::Identity(18, 18) + +// Dynamic Size Matrix +using MatX = Eigen::Matrix; + +/************************/ +/****** Functions *******/ +/************************/ +inline Vec34 vec12ToVec34(Vec12 vec12) { + Vec34 vec34; + for (int i(0); i < 4; ++i) { + vec34.col(i) = vec12.segment(3 * i, 3); + } + return vec34; +} + +inline Vec12 vec34ToVec12(Vec34 vec34) { + Vec12 vec12; + for (int i(0); i < 4; ++i) { + vec12.segment(3 * i, 3) = vec34.col(i); + } + return vec12; +} + +#endif // MATHTYPES_H \ No newline at end of file diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h index 7ca3092..7e92601 100644 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h @@ -10,6 +10,8 @@ #include #include +#include "Estimator.h" + struct CtrlComponent { std::vector > joint_torque_command_interface_; @@ -39,8 +41,10 @@ struct CtrlComponent { control_input_msgs::msg::Inputs control_inputs_; int frequency_{}; - CtrlComponent() { - } + std::shared_ptr robot_model_; + std::shared_ptr estimator_; + + CtrlComponent() = default; void clear() { joint_torque_command_interface_.clear(); diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h new file mode 100644 index 0000000..66b6704 --- /dev/null +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h @@ -0,0 +1,158 @@ +// +// Created by biao on 24-9-14. +// + +#ifndef ESTIMATOR_H +#define ESTIMATOR_H +#include +#include +#include +#include + +#include "LowPassFilter.h" + +class QuadrupedRobot; +struct CtrlComponent; + +class Estimator { +public: + explicit Estimator(CtrlComponent &ctrl_component); + + ~Estimator() = default; + + /** + * Get the estimated robot central position + * @return robot central position + */ + Vec3 getPosition() { + return x_hat_.segment(0, 3); + } + + /** + * Get the estimated robot central velocity + * @return robot central velocity + */ + Vec3 getVelocity() { + return x_hat_.segment(3, 3); + } + + /** + * Get the estimated foot position in world frame + * @param index leg index + * @return foot position in world frame + */ + Vec3 getFootPos(const int index) { + return getPosition() + rotation_ * Vec3(foot_poses_[index].p.data); + } + + /** + * Get the estimated feet velocity in world frame + * @return feet velocity in world frame + */ + Vec34 getFeetPos() { + Vec34 feet_pos; + for (int i(0); i < 4; ++i) { + feet_pos.col(i) = getFootPos(i); + } + return feet_pos; + } + + /** + * Get the estimated feet velocity in world frame + * @return feet velocity in world frame + */ + Vec34 getFeetVel() { + const std::vector feet_vel = robot_model_->getFeet2BVelocities(); + Vec34 result; + for (int i(0); i < 4; ++i) { + result.col(i) = Vec3(feet_vel[i].data) + getVelocity(); + } + return result; + } + + /** + * Get the estimated foot position in body frame + * @return + */ + Vec34 getFeetPos2Body() { + Vec34 foot_pos; + const Vec3 body_pos = getPosition(); + for (int i = 0; i < 4; i++) { + foot_pos.col(i) = getFootPos(i) - body_pos; + } + return foot_pos; + } + + RotMat getRotation() { + return rotation_; + } + + Vec3 getGyro() { + return gyro_; + } + + [[nodiscard]] Vec3 getGyroGlobal() const { + return rotation_ * gyro_; + } + + [[nodiscard]] double getYaw() const; + + [[nodiscard]] double getDYaw() const { + return getGyroGlobal()(2); + } + + void update(); + +private: + CtrlComponent &ctrl_component_; + std::shared_ptr &robot_model_; + + Eigen::Matrix x_hat_; // 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 y_hat_; // 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 feet_pos_body_; // The feet positions to body, in the global coordinate + Eigen::Matrix feet_vel_body_; // The feet velocity to body, in the global coordinate + Eigen::Matrix feet_h_; // 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 + + Vec3 g_; + double dt_; + + RotMat rotation_; + Vec3 acceleration_; + Vec3 gyro_; + + std::vector foot_poses_; + std::vector foot_vels_; + std::vector > low_pass_filters_; + + double large_variance_; +}; + + +#endif //ESTIMATOR_H diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/LowPassFilter.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/LowPassFilter.h new file mode 100644 index 0000000..8c80822 --- /dev/null +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/LowPassFilter.h @@ -0,0 +1,29 @@ +// +// Created by biao on 24-9-16. +// + + +#ifndef LOWPASSFILTER_H +#define LOWPASSFILTER_H + + +class LowPassFilter { +public: + LowPassFilter(double samplePeriod, double cutFrequency); + + ~LowPassFilter() = default; + + void addValue(double newValue); + + [[nodiscard]] double getValue() const; + + void clear(); + +private: + double weight_; + double pass_value_{}; + bool start_; +}; + + +#endif //LOWPASSFILTER_H diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/QuadrupedRobot.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/QuadrupedRobot.h new file mode 100644 index 0000000..db62b3f --- /dev/null +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/QuadrupedRobot.h @@ -0,0 +1,104 @@ +// +// Created by biao on 24-9-12. +// + + +#ifndef QUADRUPEDROBOT_H +#define QUADRUPEDROBOT_H +#include +#include +#include + +#include "RobotLeg.h" + + +struct CtrlComponent; + +class QuadrupedRobot { +public: + explicit QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description, + const std::vector &feet_names, const std::string &base_name); + + ~QuadrupedRobot() = default; + + /** + * Calculate the joint positions based on the foot end position + * @param pEe_list vector of foot-end position + * @return + */ + [[nodiscard]] std::vector getQ(const std::vector &pEe_list) const; + + [[nodiscard]] Vec12 getQ(const Vec34 &vecP) const; + + Vec12 getQd(const std::vector &pos, const Vec34 &vel); + + /** + * Calculate the foot end position based on joint positions + * @return vector of foot-end position + */ + [[nodiscard]] std::vector getFeet2BPositions() const; + + /** + * Calculate the foot end position based on joint positions + * @param index leg index + * @return foot-end position + */ + [[nodiscard]] KDL::Frame getFeet2BPositions(int index) const; + + /** + * Calculate the Jacobian matrix based on joint positions + * @param index leg index + * @return Jacobian matrix + */ + [[nodiscard]] KDL::Jacobian getJacobian(int index) const; + + /** + * Calculate the torque based on joint positions, joint velocities and external force + * @param force external force + * @param index leg index + * @return torque + */ + [[nodiscard]] KDL::JntArray getTorque( + const Vec3 &force, int index) const; + + /** + * Calculate the torque based on joint positions, joint velocities and external force + * @param force external force + * @param index leg index + * @return torque + */ + [[nodiscard]] KDL::JntArray getTorque( + const KDL::Vector &force, int index) const; + + /** + * Calculate the foot end velocity + * @param index leg index + * @return velocity vector + */ + [[nodiscard]] KDL::Vector getFeet2BVelocities(int index) const; + + /** + * Calculate all foot end velocity + * @return list of foot end velocity + */ + [[nodiscard]] std::vector getFeet2BVelocities() const; + + double mass_ = 0; + Vec34 feet_pos_normal_stand_; + std::vector current_joint_pos_; + std::vector current_joint_vel_; + + void update(); + +private: + CtrlComponent &ctrl_component_; + std::vector > robot_legs_; + + KDL::Chain fr_chain_; + KDL::Chain fl_chain_; + KDL::Chain rr_chain_; + KDL::Chain rl_chain_; +}; + + +#endif //QUADRUPEDROBOT_H diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/RobotLeg.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/RobotLeg.h new file mode 100644 index 0000000..891d882 --- /dev/null +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/RobotLeg.h @@ -0,0 +1,59 @@ +// +// Created by biao on 24-9-12. +// + + +#ifndef ROBOTLEG_H +#define ROBOTLEG_H + +#include +#include +#include +#include +#include + +class RobotLeg { +public: + explicit RobotLeg(const KDL::Chain &chain); + + ~RobotLeg() = default; + + /** + * Use forward kinematic to calculate the Pose of End effector to Body frame. + * @param joint_positions Leg joint positions + * @return Pose of End effector to Body frame + */ + [[nodiscard]] KDL::Frame calcPEe2B(const KDL::JntArray &joint_positions) const; + + /** + * Use inverse kinematic to calculate the joint positions. + * @param pEe target position of end effector + * @param q_init current joint positions + * @return target joint positions + */ + [[nodiscard]] KDL::JntArray calcQ(const KDL::Frame &pEe, const KDL::JntArray &q_init) const; + + /** + * Calculate the current jacobian matrix. + * @param joint_positions Leg joint positions + * @return jacobian matrix + */ + [[nodiscard]] KDL::Jacobian calcJaco(const KDL::JntArray &joint_positions) const; + + /** + * Calculate the torque based on joint positions and end force + * @param joint_positions current joint positions + * @param force foot end force + * @return joint torque + */ + [[nodiscard]] KDL::JntArray calcTorque(const KDL::JntArray &joint_positions, const Vec3 &force) const; + +protected: + KDL::Chain chain_; + std::shared_ptr fk_pose_solver_; + std::shared_ptr jac_solver_; + std::shared_ptr ik_pose_solver_; +}; + + +#endif //ROBOTLEG_H diff --git a/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp index 2692276..8cbc5cc 100644 --- a/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp +++ b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp @@ -45,8 +45,9 @@ StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path) loadYaml(config_path); // history - if (params_.use_history) { - history_obs_buf_ = std::make_shared(1, params_.num_observations, 6); + if (!params_.observations_history.empty()) + { + history_obs_buf_ = std::make_shared(1, params_.num_observations, params_.observations_history.size()); } model_ = torch::jit::load(config_path + "/" + params_.model_name); @@ -120,7 +121,6 @@ torch::Tensor StateRL::computeObservation() { if (observation == "lin_vel") { obs_list.push_back(obs_.lin_vel * params_.lin_vel_scale); } else if (observation == "ang_vel") { - // obs_list.push_back(obs.ang_vel * params_.ang_vel_scale); // TODO is QuatRotateInverse necessery? obs_list.push_back( quatRotateInverse(obs_.base_quat, obs_.ang_vel, params_.framework) * params_.ang_vel_scale); } else if (observation == "gravity_vec") { @@ -156,7 +156,14 @@ void StateRL::loadYaml(const std::string &config_path) { params_.framework = config["framework"].as(); const int rows = config["rows"].as(); const int cols = config["cols"].as(); - params_.use_history = config["use_history"].as(); + if (config["observations_history"].IsNull()) + { + params_.observations_history = {}; + } + else + { + params_.observations_history = ReadVectorFromYaml(config["observations_history"]); + } params_.decimation = config["decimation"].as(); params_.num_observations = config["num_observations"].as(); params_.observations = ReadVectorFromYaml(config["observations"]); @@ -219,11 +226,14 @@ torch::Tensor StateRL::forward() { torch::Tensor clamped_obs = computeObservation(); torch::Tensor actions; - if (params_.use_history) { + if (!params_.observations_history.empty()) + { history_obs_buf_->insert(clamped_obs); - history_obs_ = history_obs_buf_->getObsVec({0, 1, 2, 3, 4, 5}); + history_obs_ = history_obs_buf_->getObsVec(params_.observations_history); actions = model_.forward({history_obs_}).toTensor(); - } else { + } + else + { actions = model_.forward({clamped_obs}).toTensor(); } @@ -268,6 +278,7 @@ void StateRL::getState() { } void StateRL::runModel() { + obs_.lin_vel = torch::tensor(ctrl_comp_.estimator_->getVelocity().data()).unsqueeze(0); obs_.ang_vel = torch::tensor(robot_state_.imu.gyroscope).unsqueeze(0); obs_.commands = torch::tensor({{control_.x, control_.y, control_.yaw}}); obs_.base_quat = torch::tensor(robot_state_.imu.quaternion).unsqueeze(0); diff --git a/controllers/rl_quadruped_controller/src/LeggedGymController.cpp b/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp similarity index 88% rename from controllers/rl_quadruped_controller/src/LeggedGymController.cpp rename to controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp index e6232f5..f25d2fa 100644 --- a/controllers/rl_quadruped_controller/src/LeggedGymController.cpp +++ b/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp @@ -2,7 +2,7 @@ // Created by tlab-uav on 24-10-4. // -#include "LeggedGymController.h" +#include "RlQuadrupedController.h" namespace rl_quadruped_controller { using config_type = controller_interface::interface_configuration_type; @@ -47,6 +47,13 @@ namespace rl_quadruped_controller { controller_interface::return_type LeggedGymController:: update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + if (ctrl_comp_.robot_model_ == nullptr) { + return controller_interface::return_type::OK; + } + + ctrl_comp_.robot_model_->update(); + ctrl_comp_.estimator_->update(); + if (mode_ == FSMMode::NORMAL) { current_state_->run(); next_state_name_ = current_state_->checkChange(); @@ -77,16 +84,24 @@ namespace rl_quadruped_controller { auto_declare >("state_interfaces", state_interface_types_); command_prefix_ = auto_declare("command_prefix", command_prefix_); + base_name_ = auto_declare("base_name", base_name_); // imu sensor imu_name_ = auto_declare("imu_name", imu_name_); imu_interface_types_ = auto_declare >("imu_interfaces", state_interface_types_); + // foot_force_sensor + foot_force_name_ = auto_declare("foot_force_name", foot_force_name_); + foot_force_interface_types_ = + auto_declare >("foot_force_interfaces", state_interface_types_); + // rl config folder rl_config_folder_ = auto_declare("config_folder", rl_config_folder_); get_node()->get_parameter("update_rate", ctrl_comp_.frequency_); RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", ctrl_comp_.frequency_); + + ctrl_comp_.estimator_ = std::make_shared(ctrl_comp_); } catch (const std::exception &e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; @@ -97,6 +112,14 @@ namespace rl_quadruped_controller { controller_interface::CallbackReturn LeggedGymController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { + robot_description_subscription_ = get_node()->create_subscription( + "/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(), + [this](const std_msgs::msg::String::SharedPtr msg) { + ctrl_comp_.robot_model_ = std::make_shared( + ctrl_comp_, msg->data, feet_names_, base_name_); + }); + + control_input_subscription_ = get_node()->create_subscription( "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) { // Handle message diff --git a/controllers/rl_quadruped_controller/src/LeggedGymController.h b/controllers/rl_quadruped_controller/src/RlQuadrupedController.h similarity index 93% rename from controllers/rl_quadruped_controller/src/LeggedGymController.h rename to controllers/rl_quadruped_controller/src/RlQuadrupedController.h index 083cb47..421f116 100644 --- a/controllers/rl_quadruped_controller/src/LeggedGymController.h +++ b/controllers/rl_quadruped_controller/src/RlQuadrupedController.h @@ -6,11 +6,12 @@ #define LEGGEDGYMCONTROLLER_H #include #include +#include -#include "rl_quadruped_controller/FSM/StateFixedStand.h" -#include "rl_quadruped_controller/FSM/StateFixedDown.h" -#include "rl_quadruped_controller/FSM/StatePassive.h" #include "rl_quadruped_controller/control/CtrlComponent.h" +#include "rl_quadruped_controller/FSM/StateFixedDown.h" +#include "rl_quadruped_controller/FSM/StateFixedStand.h" +#include "rl_quadruped_controller/FSM/StatePassive.h" namespace rl_quadruped_controller { struct FSMStateList { @@ -68,6 +69,7 @@ namespace rl_quadruped_controller { CtrlComponent ctrl_comp_; std::vector joint_names_; + std::string base_name_ = "base"; std::vector feet_names_; std::vector command_interface_types_; std::vector state_interface_types_; @@ -78,8 +80,8 @@ namespace rl_quadruped_controller { std::string imu_name_; std::vector imu_interface_types_; // Foot Force Sensor - std::string foot_force_name_; - std::vector foot_force_interface_types_; + std::string foot_force_name_ = "foot_force"; + std::vector foot_force_interface_types_ = {"force", "torque"}; std::string rl_config_folder_; @@ -102,6 +104,7 @@ namespace rl_quadruped_controller { }; rclcpp::Subscription::SharedPtr control_input_subscription_; + rclcpp::Subscription::SharedPtr robot_description_subscription_; FSMMode mode_ = FSMMode::NORMAL; std::string state_name_; diff --git a/controllers/rl_quadruped_controller/src/control/Estimator.cpp b/controllers/rl_quadruped_controller/src/control/Estimator.cpp new file mode 100644 index 0000000..af2e624 --- /dev/null +++ b/controllers/rl_quadruped_controller/src/control/Estimator.cpp @@ -0,0 +1,226 @@ +// +// Created by biao on 24-9-14. +// + +#include "rl_quadruped_controller/control/Estimator.h" + +#include + +#include "rl_quadruped_controller/control/CtrlComponent.h" + +Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component), + robot_model_(ctrl_component.robot_model_) { + g_ << 0, 0, -9.81; + dt_ = 1.0 / ctrl_component_.frequency_; + + std::cout << "dt: " << dt_ << std::endl; + large_variance_ = 100; + for (int i(0); i < Qdig.rows(); ++i) { + Qdig(i) = i < 6 ? 0.0003 : 0.01; + } + + x_hat_.setZero(); + u_.setZero(); + + A.setZero(); + A.block(0, 0, 3, 3) = I3; + A.block(0, 3, 3, 3) = I3 * dt_; + A.block(3, 3, 3, 3) = I3; + A.block(6, 6, 12, 12) = I12; + + B.setZero(); + B.block(3, 0, 3, 3) = I3 * dt_; + + C.setZero(); + C.block(0, 0, 3, 3) = -I3; + C.block(3, 0, 3, 3) = -I3; + C.block(6, 0, 3, 3) = -I3; + C.block(9, 0, 3, 3) = -I3; + C.block(12, 3, 3, 3) = -I3; + C.block(15, 3, 3, 3) = -I3; + C.block(18, 3, 3, 3) = -I3; + C.block(21, 3, 3, 3) = -I3; + C.block(0, 6, 12, 12) = I12; + C(24, 8) = 1; + C(25, 11) = 1; + C(26, 14) = 1; + C(27, 17) = 1; + + P.setIdentity(); + P = large_variance_ * P; + + RInit_ << 0.008, 0.012, -0.000, -0.009, 0.012, 0.000, 0.009, -0.009, -0.000, + -0.009, -0.009, 0.000, -0.000, 0.000, -0.000, 0.000, -0.000, -0.001, + -0.002, 0.000, -0.000, -0.003, -0.000, -0.001, 0.000, 0.000, 0.000, 0.000, + 0.012, 0.019, -0.001, -0.014, 0.018, -0.000, 0.014, -0.013, -0.000, + -0.014, -0.014, 0.001, -0.001, 0.001, -0.001, 0.000, 0.000, -0.001, + -0.003, 0.000, -0.001, -0.004, -0.000, -0.001, 0.000, 0.000, 0.000, 0.000, + -0.000, -0.001, 0.001, 0.001, -0.001, 0.000, -0.000, 0.000, -0.000, 0.001, + 0.000, -0.000, 0.000, -0.000, 0.000, 0.000, -0.000, -0.000, 0.000, -0.000, + -0.000, -0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, -0.009, -0.014, + 0.001, 0.010, -0.013, 0.000, -0.010, 0.010, 0.000, 0.010, 0.010, -0.000, + 0.001, 0.000, 0.000, 0.001, -0.000, 0.001, 0.002, -0.000, 0.000, 0.003, + 0.000, 0.001, 0.000, 0.000, 0.000, 0.000, 0.012, 0.018, -0.001, -0.013, + 0.018, -0.000, 0.013, -0.013, -0.000, -0.013, -0.013, 0.001, -0.001, + 0.000, -0.001, 0.000, 0.001, -0.001, -0.003, 0.000, -0.001, -0.004, + -0.000, -0.001, 0.000, 0.000, 0.000, 0.000, 0.000, -0.000, 0.000, 0.000, + -0.000, 0.001, 0.000, 0.000, -0.000, 0.000, 0.000, -0.000, -0.000, 0.000, + -0.000, 0.000, 0.000, 0.000, -0.000, -0.000, -0.000, -0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, 0.000, 0.009, 0.014, -0.000, -0.010, 0.013, 0.000, + 0.010, -0.010, -0.000, -0.010, -0.010, 0.000, -0.001, 0.000, -0.001, + 0.000, -0.000, -0.001, -0.001, 0.000, -0.000, -0.003, -0.000, -0.001, + 0.000, 0.000, 0.000, 0.000, -0.009, -0.013, 0.000, 0.010, -0.013, 0.000, + -0.010, 0.009, 0.000, 0.010, 0.010, -0.000, 0.001, -0.000, 0.000, -0.000, + 0.000, 0.001, 0.002, 0.000, 0.000, 0.003, 0.000, 0.001, 0.000, 0.000, + 0.000, 0.000, -0.000, -0.000, -0.000, 0.000, -0.000, -0.000, -0.000, + 0.000, 0.001, 0.000, 0.000, 0.000, 0.000, -0.000, 0.000, -0.000, 0.000, + -0.000, 0.000, -0.000, 0.000, 0.000, -0.000, -0.000, 0.000, 0.000, 0.000, + 0.000, -0.009, -0.014, 0.001, 0.010, -0.013, 0.000, -0.010, 0.010, 0.000, + 0.010, 0.010, -0.000, 0.001, 0.000, 0.000, -0.000, -0.000, 0.001, 0.002, + -0.000, 0.000, 0.003, 0.000, 0.001, 0.000, 0.000, 0.000, 0.000, -0.009, + -0.014, 0.000, 0.010, -0.013, 0.000, -0.010, 0.010, 0.000, 0.010, 0.010, + -0.000, 0.001, -0.000, 0.000, -0.000, 0.000, 0.001, 0.002, -0.000, 0.000, + 0.003, 0.001, 0.001, 0.000, 0.000, 0.000, 0.000, 0.000, 0.001, -0.000, + -0.000, 0.001, -0.000, 0.000, -0.000, 0.000, -0.000, -0.000, 0.001, 0.000, + -0.000, -0.000, -0.000, 0.000, 0.000, -0.000, 0.000, 0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, 0.000, 0.000, -0.000, -0.001, 0.000, 0.001, -0.001, + -0.000, -0.001, 0.001, 0.000, 0.001, 0.001, 0.000, 1.708, 0.048, 0.784, + 0.062, 0.042, 0.053, 0.077, 0.001, -0.061, 0.046, -0.019, -0.029, 0.000, + 0.000, 0.000, 0.000, 0.000, 0.001, -0.000, 0.000, 0.000, 0.000, 0.000, + -0.000, -0.000, 0.000, -0.000, -0.000, 0.048, 5.001, -1.631, -0.036, + 0.144, 0.040, 0.036, 0.016, -0.051, -0.067, -0.024, -0.005, 0.000, 0.000, + 0.000, 0.000, -0.000, -0.001, 0.000, 0.000, -0.001, -0.000, -0.001, 0.000, + 0.000, 0.000, 0.000, -0.000, 0.784, -1.631, 1.242, 0.057, -0.037, 0.018, + 0.034, -0.017, -0.015, 0.058, -0.021, -0.029, 0.000, 0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, 0.001, 0.000, 0.000, 0.000, -0.000, -0.000, -0.000, + -0.000, -0.000, 0.062, -0.036, 0.057, 6.228, -0.014, 0.932, 0.059, 0.053, + -0.069, 0.148, 0.015, -0.031, 0.000, 0.000, 0.000, 0.000, -0.000, 0.000, + -0.000, -0.000, 0.001, 0.000, -0.000, 0.000, 0.000, -0.000, 0.000, 0.000, + 0.042, 0.144, -0.037, -0.014, 3.011, 0.986, 0.076, 0.030, -0.052, -0.027, + 0.057, 0.051, 0.000, 0.000, 0.000, 0.000, -0.001, -0.001, -0.000, 0.001, + -0.001, 0.000, -0.001, 0.001, -0.000, 0.001, 0.001, 0.000, 0.053, 0.040, + 0.018, 0.932, 0.986, 0.885, 0.090, 0.044, -0.055, 0.057, 0.051, -0.003, + 0.000, 0.000, 0.000, 0.000, -0.002, -0.003, 0.000, 0.002, -0.003, -0.000, + -0.001, 0.002, 0.000, 0.002, 0.002, -0.000, 0.077, 0.036, 0.034, 0.059, + 0.076, 0.090, 6.230, 0.139, 0.763, 0.013, -0.019, -0.024, 0.000, 0.000, + 0.000, 0.000, 0.000, 0.000, -0.000, -0.000, 0.000, -0.000, 0.000, 0.000, + -0.000, -0.000, -0.000, 0.000, 0.001, 0.016, -0.017, 0.053, 0.030, 0.044, + 0.139, 3.130, -1.128, -0.010, 0.131, 0.018, 0.000, 0.000, 0.000, 0.000, + -0.000, -0.001, -0.000, 0.000, -0.001, -0.000, -0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, -0.061, -0.051, -0.015, -0.069, -0.052, -0.055, + 0.763, -1.128, 0.866, -0.022, -0.053, 0.007, 0.000, 0.000, 0.000, 0.000, + -0.003, -0.004, -0.000, 0.003, -0.004, -0.000, -0.003, 0.003, 0.000, + 0.003, 0.003, 0.000, 0.046, -0.067, 0.058, 0.148, -0.027, 0.057, 0.013, + -0.010, -0.022, 2.437, -0.102, 0.938, 0.000, 0.000, 0.000, 0.000, -0.000, + -0.000, 0.000, 0.000, -0.000, 0.000, -0.000, 0.000, -0.000, 0.000, 0.001, + 0.000, -0.019, -0.024, -0.021, 0.015, 0.057, 0.051, -0.019, 0.131, -0.053, + -0.102, 4.944, 1.724, 0.000, 0.000, 0.000, 0.000, -0.001, -0.001, 0.000, + 0.001, -0.001, 0.000, -0.001, 0.001, -0.000, 0.001, 0.001, 0.000, -0.029, + -0.005, -0.029, -0.031, 0.051, -0.003, -0.024, 0.018, 0.007, 0.938, 1.724, + 1.569, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 1.0, 0.000, + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 1.0, 0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, 0.000, 0.000, 1.0, 0.000, 0.000, 0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, + 0.000, 0.000, 0.000, 1.0; + + Cu << 268.573, -43.819, -147.211, -43.819, 92.949, 58.082, -147.211, 58.082, + 302.120; + + QInit_ = Qdig.asDiagonal(); + QInit_ += B * Cu * B.transpose(); + + // low_pass_filters_.resize(3); + // low_pass_filters_[0] = std::make_shared(dt_, 3.0); + // low_pass_filters_[1] = std::make_shared(dt_, 3.0); + // low_pass_filters_[2] = std::make_shared(dt_, 3.0); +} + +double Estimator::getYaw() const { + return rotMatToRPY(rotation_)(2); +} + +void Estimator::update() { + if (robot_model_->mass_ == 0) return; + + Q = QInit_; + R = RInit_; + + foot_poses_ = robot_model_->getFeet2BPositions(); + foot_vels_ = robot_model_->getFeet2BVelocities(); + feet_h_.setZero(); + + // Adjust the covariance based on foot contact and phase. + for (int i(0); i < 4; ++i) { + if (ctrl_component_.foot_force_state_interface_[i].get().get_value() < 1) { + // foot not contact + Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3); + R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3); + R(24 + i, 24 + i) = large_variance_; + } else { + // foot contact + const double trust = windowFunc(0.5, 0.2); + Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = + (1 + (1 - trust) * large_variance_) * + QInit_.block(6 + 3 * i, 6 + 3 * i, 3, 3); + R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = + (1 + (1 - trust) * large_variance_) * + RInit_.block(12 + 3 * i, 12 + 3 * i, 3, 3); + R(24 + i, 24 + i) = + (1 + (1 - trust) * large_variance_) * RInit_(24 + i, 24 + i); + } + feet_pos_body_.segment(3 * i, 3) = Vec3(foot_poses_[i].p.data); + feet_vel_body_.segment(3 * i, 3) = Vec3(foot_vels_[i].data); + } + + Quat quat; + quat << ctrl_component_.imu_state_interface_[0].get().get_value(), + ctrl_component_.imu_state_interface_[1].get().get_value(), + ctrl_component_.imu_state_interface_[2].get().get_value(), + ctrl_component_.imu_state_interface_[3].get().get_value(); + rotation_ = quatToRotMat(quat); + + gyro_ << ctrl_component_.imu_state_interface_[4].get().get_value(), + ctrl_component_.imu_state_interface_[5].get().get_value(), + ctrl_component_.imu_state_interface_[6].get().get_value(); + + acceleration_ << ctrl_component_.imu_state_interface_[7].get().get_value(), + ctrl_component_.imu_state_interface_[8].get().get_value(), + ctrl_component_.imu_state_interface_[9].get().get_value(); + + u_ = rotation_ * acceleration_ + g_; + x_hat_ = A * x_hat_ + B * u_; + y_hat_ = C * x_hat_; + + // Update the measurement value + y_ << feet_pos_body_, feet_vel_body_, feet_h_; + + // Update the covariance matrix + Ppriori = A * P * A.transpose() + Q; + S = R + C * Ppriori * C.transpose(); + Slu = S.lu(); + Sy = Slu.solve(y_ - y_hat_); + Sc = Slu.solve(C); + SR = Slu.solve(R); + STC = S.transpose().lu().solve(C); + IKC = Eigen::MatrixXd::Identity(18, 18) - Ppriori * C.transpose() * Sc; + + // Update the state and covariance matrix + x_hat_ += Ppriori * C.transpose() * Sy; + P = IKC * Ppriori * IKC.transpose() + + Ppriori * C.transpose() * SR * STC * Ppriori.transpose(); + + // // Using low pass filter to smooth the velocity + low_pass_filters_[0]->addValue(x_hat_(3)); + low_pass_filters_[1]->addValue(x_hat_(4)); + low_pass_filters_[2]->addValue(x_hat_(5)); + x_hat_(3) = low_pass_filters_[0]->getValue(); + x_hat_(4) = low_pass_filters_[1]->getValue(); + x_hat_(5) = low_pass_filters_[2]->getValue(); +} diff --git a/controllers/rl_quadruped_controller/src/control/LowPassFilter.cpp b/controllers/rl_quadruped_controller/src/control/LowPassFilter.cpp new file mode 100644 index 0000000..047cde6 --- /dev/null +++ b/controllers/rl_quadruped_controller/src/control/LowPassFilter.cpp @@ -0,0 +1,31 @@ +// +// Created by biao on 24-9-16. +// +#include +#include "rl_quadruped_controller/control/LowPassFilter.h" + +/** + * Low Pass Filter to prevent high frequency signal + * @param samplePeriod sample period + * @param cutFrequency cut frequency + */ +LowPassFilter::LowPassFilter(const double samplePeriod, const double cutFrequency) { + weight_ = 1.0 / (1.0 + 1.0 / (2.0 * M_PI * samplePeriod * cutFrequency)); + start_ = false; +} + +void LowPassFilter::addValue(const double newValue) { + if (!start_) { + start_ = true; + pass_value_ = newValue; + } + pass_value_ = weight_ * newValue + (1 - weight_) * pass_value_; +} + +double LowPassFilter::getValue() const { + return pass_value_; +} + +void LowPassFilter::clear() { + start_ = false; +} diff --git a/controllers/rl_quadruped_controller/src/robot/QuadrupedRobot.cpp b/controllers/rl_quadruped_controller/src/robot/QuadrupedRobot.cpp new file mode 100644 index 0000000..616aca7 --- /dev/null +++ b/controllers/rl_quadruped_controller/src/robot/QuadrupedRobot.cpp @@ -0,0 +1,128 @@ +// +// Created by biao on 24-9-12. +// + +#include +#include "rl_quadruped_controller/control/CtrlComponent.h" +#include "rl_quadruped_controller/robot/QuadrupedRobot.h" + +QuadrupedRobot::QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description, + const std::vector &feet_names, + const std::string &base_name) : ctrl_component_(ctrl_component) { + KDL::Tree robot_tree; + kdl_parser::treeFromString(robot_description, robot_tree); + + robot_tree.getChain(base_name, feet_names[0], fr_chain_); + robot_tree.getChain(base_name, feet_names[1], fl_chain_); + robot_tree.getChain(base_name, feet_names[2], rr_chain_); + robot_tree.getChain(base_name, feet_names[3], rl_chain_); + + + robot_legs_.resize(4); + robot_legs_[0] = std::make_shared(fr_chain_); + robot_legs_[1] = std::make_shared(fl_chain_); + robot_legs_[2] = std::make_shared(rr_chain_); + robot_legs_[3] = std::make_shared(rl_chain_); + + current_joint_pos_.resize(4); + current_joint_vel_.resize(4); + + std::cout << "robot_legs_.size(): " << robot_legs_.size() << std::endl; + + // calculate total mass from urdf + mass_ = 0; + for (const auto &[fst, snd]: robot_tree.getSegments()) { + mass_ += snd.segment.getInertia().getMass(); + } + feet_pos_normal_stand_ << 0.1881, 0.1881, -0.1881, -0.1881, -0.1300, 0.1300, + -0.1300, 0.1300, -0.3200, -0.3200, -0.3200, -0.3200; +} + +std::vector QuadrupedRobot::getQ(const std::vector &pEe_list) const { + std::vector result; + result.resize(4); + for (int i(0); i < 4; ++i) { + result[i] = robot_legs_[i]->calcQ(pEe_list[i], current_joint_pos_[i]); + } + return result; +} + +Vec12 QuadrupedRobot::getQ(const Vec34 &vecP) const { + Vec12 q; + for (int i(0); i < 4; ++i) { + KDL::Frame frame; + frame.p = KDL::Vector(vecP.col(i)[0], vecP.col(i)[1], vecP.col(i)[2]); + frame.M = KDL::Rotation::Identity(); + q.segment(3 * i, 3) = robot_legs_[i]->calcQ(frame, current_joint_pos_[i]).data; + } + return q; +} + +Vec12 QuadrupedRobot::getQd(const std::vector &pos, const Vec34 &vel) { + Vec12 qd; + const std::vector q = getQ(pos); + for (int i(0); i < 4; ++i) { + Mat3 jacobian = robot_legs_[i]->calcJaco(q[i]).data.topRows(3); + qd.segment(3 * i, 3) = jacobian.inverse() * vel.col(i); + } + return qd; +} + +std::vector QuadrupedRobot::getFeet2BPositions() const { + std::vector result; + result.resize(4); + for (int i = 0; i < 4; i++) { + result[i] = robot_legs_[i]->calcPEe2B(current_joint_pos_[i]); + result[i].M = KDL::Rotation::Identity(); + } + return result; +} + +KDL::Frame QuadrupedRobot::getFeet2BPositions(const int index) const { + return robot_legs_[index]->calcPEe2B(current_joint_pos_[index]); +} + +KDL::Jacobian QuadrupedRobot::getJacobian(const int index) const { + return robot_legs_[index]->calcJaco(current_joint_pos_[index]); +} + +KDL::JntArray QuadrupedRobot::getTorque( + const Vec3 &force, const int index) const { + return robot_legs_[index]->calcTorque(current_joint_pos_[index], force); +} + +KDL::JntArray QuadrupedRobot::getTorque(const KDL::Vector &force, int index) const { + return robot_legs_[index]->calcTorque(current_joint_pos_[index], Vec3(force.data)); +} + +KDL::Vector QuadrupedRobot::getFeet2BVelocities(const int index) const { + const Mat3 jacobian = getJacobian(index).data.topRows(3); + Vec3 foot_velocity = jacobian * current_joint_vel_[index].data; + return {foot_velocity(0), foot_velocity(1), foot_velocity(2)}; +} + +std::vector 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() { + if (mass_ == 0) return; + for (int i = 0; i < 4; i++) { + KDL::JntArray pos_array(3); + pos_array(0) = ctrl_component_.joint_position_state_interface_[i * 3].get().get_value(); + pos_array(1) = ctrl_component_.joint_position_state_interface_[i * 3 + 1].get().get_value(); + pos_array(2) = ctrl_component_.joint_position_state_interface_[i * 3 + 2].get().get_value(); + current_joint_pos_[i] = pos_array; + + KDL::JntArray vel_array(3); + vel_array(0) = ctrl_component_.joint_velocity_state_interface_[i * 3].get().get_value(); + vel_array(1) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 1].get().get_value(); + vel_array(2) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 2].get().get_value(); + current_joint_vel_[i] = vel_array; + } +} diff --git a/controllers/rl_quadruped_controller/src/robot/RobotLeg.cpp b/controllers/rl_quadruped_controller/src/robot/RobotLeg.cpp new file mode 100644 index 0000000..134472c --- /dev/null +++ b/controllers/rl_quadruped_controller/src/robot/RobotLeg.cpp @@ -0,0 +1,44 @@ +// +// Created by biao on 24-9-12. +// + +#include +#include "rl_quadruped_controller/robot/RobotLeg.h" + +#include + +RobotLeg::RobotLeg(const KDL::Chain &chain) { + chain_ = chain; + + fk_pose_solver_ = std::make_shared(chain); + ik_pose_solver_ = std::make_shared(chain); + jac_solver_ = std::make_shared(chain); +} + +KDL::Frame RobotLeg::calcPEe2B(const KDL::JntArray &joint_positions) const { + KDL::Frame pEe; + fk_pose_solver_->JntToCart(joint_positions, pEe); + return pEe; +} + +KDL::JntArray RobotLeg::calcQ(const KDL::Frame &pEe, const KDL::JntArray &q_init) const { + KDL::JntArray q(chain_.getNrOfJoints()); + ik_pose_solver_->CartToJnt(q_init, pEe, q); + return q; +} + +KDL::Jacobian RobotLeg::calcJaco(const KDL::JntArray &joint_positions) const { + KDL::Jacobian jacobian(chain_.getNrOfJoints()); + jac_solver_->JntToJac(joint_positions, jacobian); + return jacobian; +} + +KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const Vec3 &force) const { + const Eigen::Matrix jacobian = calcJaco(joint_positions).data.topRows(3); + Eigen::VectorXd torque_eigen = jacobian.transpose() * force; + KDL::JntArray torque(chain_.getNrOfJoints()); + for (unsigned int i = 0; i < chain_.getNrOfJoints(); ++i) { + torque(i) = torque_eigen(i); + } + return torque; +} diff --git a/controllers/unitree_guide_controller/src/control/Estimator.cpp b/controllers/unitree_guide_controller/src/control/Estimator.cpp index 88419a3..568be9f 100644 --- a/controllers/unitree_guide_controller/src/control/Estimator.cpp +++ b/controllers/unitree_guide_controller/src/control/Estimator.cpp @@ -14,7 +14,7 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo g_ << 0, 0, -9.81; dt_ = 1.0 / ctrl_component_.frequency_; - std::cout<<"dt: "<addValue(x_hat_(3)); - // low_pass_filters_[1]->addValue(x_hat_(4)); - // low_pass_filters_[2]->addValue(x_hat_(5)); - // x_hat_(3) = low_pass_filters_[0]->getValue(); - // x_hat_(4) = low_pass_filters_[1]->getValue(); - // x_hat_(5) = low_pass_filters_[2]->getValue(); + low_pass_filters_[0]->addValue(x_hat_(3)); + low_pass_filters_[1]->addValue(x_hat_(4)); + low_pass_filters_[2]->addValue(x_hat_(5)); + x_hat_(3) = low_pass_filters_[0]->getValue(); + x_hat_(4) = low_pass_filters_[1]->getValue(); + x_hat_(5) = low_pass_filters_[2]->getValue(); } diff --git a/descriptions/unitree/a1_description/config/issacgym/config.yaml b/descriptions/unitree/a1_description/config/issacgym/config.yaml index 4ac2b9b..8b46e4f 100644 --- a/descriptions/unitree/a1_description/config/issacgym/config.yaml +++ b/descriptions/unitree/a1_description/config/issacgym/config.yaml @@ -2,10 +2,10 @@ model_name: "model_0702.pt" framework: "isaacgym" rows: 4 cols: 3 -use_history: True decimation: 4 num_observations: 45 observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] +observations_history: [0, 1, 2, 3, 4, 5] clip_obs: 100.0 clip_actions_lower: [-100, -100, -100, -100, -100, -100, diff --git a/descriptions/unitree/go2_description/config/issacgym/config.yaml b/descriptions/unitree/go2_description/config/issacgym/config.yaml index 10be7ba..279a417 100644 --- a/descriptions/unitree/go2_description/config/issacgym/config.yaml +++ b/descriptions/unitree/go2_description/config/issacgym/config.yaml @@ -1,11 +1,11 @@ -model_name: "policy.pt" +model_name: "himloco.pt" framework: "isaacgym" rows: 4 cols: 3 -use_history: True decimation: 4 -num_observations: 45 -observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] +num_observations: 48 +observations: ["lin_vel", "ang_vel", "gravity_vec", "dof_pos", "dof_vel", "actions"] +observations_history: [5, 4, 3, 2, 1, 0] clip_obs: 100.0 clip_actions_lower: [-100, -100, -100, -100, -100, -100, @@ -15,23 +15,23 @@ clip_actions_upper: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] -rl_kp: [20.1, 20.1, 20.1, - 20.1, 20.1, 20.1, - 20.1, 20.1, 20.1, - 20.1, 20.1, 20.1] -rl_kd: [0.61, 0.61, 0.61, - 0.61, 0.61, 0.61, - 0.61, 0.61, 0.61, - 0.61, 0.61, 0.61] -fixed_kp: [80, 80, 80, - 80, 80, 80, - 80, 80, 80, - 80, 80, 80] -fixed_kd: [3, 3, 3, - 3, 3, 3, - 3, 3, 3, - 3, 3, 3] -hip_scale_reduction: 0.5 +rl_kp: [40, 40, 40, + 40, 40, 40, + 40, 40, 40, + 40, 40, 40] +rl_kd: [1, 1, 1, + 1, 1, 1, + 1, 1, 1, + 1, 1, 1] +fixed_kp: [60, 60, 60, + 60, 60, 60, + 60, 60, 60, + 60, 60, 60] +fixed_kd: [5, 5, 5, + 5, 5, 5, + 5, 5, 5, + 5, 5, 5] +hip_scale_reduction: 1.0 hip_scale_reduction_indices: [0, 3, 6, 9] num_of_dofs: 12 action_scale: 0.25 @@ -39,12 +39,12 @@ lin_vel_scale: 2.0 ang_vel_scale: 0.25 dof_pos_scale: 1.0 dof_vel_scale: 0.05 -commands_scale: [2.0, 2.0, 1.0] +commands_scale: [2.0, 2.0, 0.25] torque_limits: [33.5, 33.5, 33.5, 33.5, 33.5, 33.5, 33.5, 33.5, 33.5, 33.5, 33.5, 33.5] default_dof_pos: [ 0.1000, 0.8000, -1.5000, - -0.1000, 0.8000, -1.5000, + -0.1000, 0.8000, -1.5000, 0.1000, 1.0000, -1.5000, - -0.1000, 1.0000, -1.5000] + -0.1000, 1.0000, -1.5000] \ No newline at end of file