From 0ff1b344e7849f69df0fc0bf286b167fa146abd1 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Tue, 15 Oct 2024 22:40:15 +0800 Subject: [PATCH] add estimator --- controllers/rl_quadruped_controller/README.md | 6 ++++ .../src/FSM/StateRL.cpp | 31 ++++++++++--------- .../src/RlQuadrupedController.cpp | 2 +- .../src/RlQuadrupedController.h | 2 +- .../src/control/Estimator.cpp | 12 +++---- .../src/control/Estimator.cpp | 8 ++--- .../unitree/a1_description/config/gazebo.yaml | 2 +- .../a1_description/config/robot_control.yaml | 8 ++--- .../config/issacgym/config.yaml | 25 ++++++++------- .../go2_description/config/robot_control.yaml | 7 +++++ 10 files changed, 61 insertions(+), 42 deletions(-) diff --git a/controllers/rl_quadruped_controller/README.md b/controllers/rl_quadruped_controller/README.md index 3a7f317..b21a677 100644 --- a/controllers/rl_quadruped_controller/README.md +++ b/controllers/rl_quadruped_controller/README.md @@ -31,4 +31,10 @@ colcon build --packages-up-to rl_quadruped_controller ```bash source ~/ros2_ws/install/setup.bash ros2 launch a1_description rl_control.launch.py + ``` + +* Unitree Go2 Robot + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch go2_description rl_control.launch.py ``` \ No newline at end of file diff --git a/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp index 8cbc5cc..3294b1b 100644 --- a/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp +++ b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp @@ -45,14 +45,20 @@ StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path) loadYaml(config_path); // history - if (!params_.observations_history.empty()) - { - history_obs_buf_ = std::make_shared(1, params_.num_observations, params_.observations_history.size()); + 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); std::cout << "Model loaded: " << config_path + "/" + params_.model_name << std::endl; + + // for (const auto ¶m: model_.parameters()) { + // std::cout << "Parameter dtype: " << param.dtype() << std::endl; + // } + + rl_thread_ = std::thread([&] { while (true) { try { @@ -137,6 +143,8 @@ torch::Tensor StateRL::computeObservation() { } const torch::Tensor obs = cat(obs_list, 1); + + std::cout << "Observation: " << obs << std::endl; torch::Tensor clamped_obs = clamp(obs, -params_.clip_obs, params_.clip_obs); return clamped_obs; } @@ -156,12 +164,9 @@ 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(); - if (config["observations_history"].IsNull()) - { + if (config["observations_history"].IsNull()) { params_.observations_history = {}; - } - else - { + } else { params_.observations_history = ReadVectorFromYaml(config["observations_history"]); } params_.decimation = config["decimation"].as(); @@ -226,14 +231,11 @@ torch::Tensor StateRL::forward() { torch::Tensor clamped_obs = computeObservation(); torch::Tensor actions; - if (!params_.observations_history.empty()) - { + if (!params_.observations_history.empty()) { history_obs_buf_->insert(clamped_obs); history_obs_ = history_obs_buf_->getObsVec(params_.observations_history); actions = model_.forward({history_obs_}).toTensor(); - } - else - { + } else { actions = model_.forward({clamped_obs}).toTensor(); } @@ -278,7 +280,8 @@ void StateRL::getState() { } void StateRL::runModel() { - obs_.lin_vel = torch::tensor(ctrl_comp_.estimator_->getVelocity().data()).unsqueeze(0); + obs_.lin_vel = torch::from_blob(ctrl_comp_.estimator_->getVelocity().data(), {3}, torch::kDouble).clone(). + to(torch::kFloat).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/RlQuadrupedController.cpp b/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp index f25d2fa..4702dbc 100644 --- a/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp +++ b/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp @@ -93,7 +93,7 @@ namespace rl_quadruped_controller { // 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_); + auto_declare >("foot_force_interfaces", foot_force_interface_types_); // rl config folder rl_config_folder_ = auto_declare("config_folder", rl_config_folder_); diff --git a/controllers/rl_quadruped_controller/src/RlQuadrupedController.h b/controllers/rl_quadruped_controller/src/RlQuadrupedController.h index 421f116..c0c9c18 100644 --- a/controllers/rl_quadruped_controller/src/RlQuadrupedController.h +++ b/controllers/rl_quadruped_controller/src/RlQuadrupedController.h @@ -81,7 +81,7 @@ namespace rl_quadruped_controller { std::vector imu_interface_types_; // Foot Force Sensor std::string foot_force_name_ = "foot_force"; - std::vector foot_force_interface_types_ = {"force", "torque"}; + std::vector foot_force_interface_types_ = {"FL", "FR", "RL", "RR"}; std::string rl_config_folder_; diff --git a/controllers/rl_quadruped_controller/src/control/Estimator.cpp b/controllers/rl_quadruped_controller/src/control/Estimator.cpp index af2e624..5c64ec8 100644 --- a/controllers/rl_quadruped_controller/src/control/Estimator.cpp +++ b/controllers/rl_quadruped_controller/src/control/Estimator.cpp @@ -217,10 +217,10 @@ void Estimator::update() { Ppriori * C.transpose() * SR * STC * Ppriori.transpose(); // // Using low pass filter to smooth the velocity - low_pass_filters_[0]->addValue(x_hat_(3)); - low_pass_filters_[1]->addValue(x_hat_(4)); - low_pass_filters_[2]->addValue(x_hat_(5)); - x_hat_(3) = low_pass_filters_[0]->getValue(); - x_hat_(4) = low_pass_filters_[1]->getValue(); - x_hat_(5) = low_pass_filters_[2]->getValue(); + // low_pass_filters_[0]->addValue(x_hat_(3)); + // low_pass_filters_[1]->addValue(x_hat_(4)); + // low_pass_filters_[2]->addValue(x_hat_(5)); + // x_hat_(3) = low_pass_filters_[0]->getValue(); + // x_hat_(4) = low_pass_filters_[1]->getValue(); + // x_hat_(5) = low_pass_filters_[2]->getValue(); } diff --git a/controllers/unitree_guide_controller/src/control/Estimator.cpp b/controllers/unitree_guide_controller/src/control/Estimator.cpp index 568be9f..c6453f8 100644 --- a/controllers/unitree_guide_controller/src/control/Estimator.cpp +++ b/controllers/unitree_guide_controller/src/control/Estimator.cpp @@ -137,10 +137,10 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo 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); + 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 { diff --git a/descriptions/unitree/a1_description/config/gazebo.yaml b/descriptions/unitree/a1_description/config/gazebo.yaml index 32bcfda..8756437 100644 --- a/descriptions/unitree/a1_description/config/gazebo.yaml +++ b/descriptions/unitree/a1_description/config/gazebo.yaml @@ -100,7 +100,7 @@ unitree_guide_controller: rl_quadruped_controller: ros__parameters: update_rate: 200 # Hz - config_folder: "/home/biao/ros2_ws/install/a1_description/share/a1_description/config/issacgym" + config_folder: "/home/tlab-uav/ros2_ws/install/a1_description/share/a1_description/config/issacgym" command_prefix: "leg_pd_controller" joints: - FL_hip_joint diff --git a/descriptions/unitree/a1_description/config/robot_control.yaml b/descriptions/unitree/a1_description/config/robot_control.yaml index c4180bc..80178f2 100644 --- a/descriptions/unitree/a1_description/config/robot_control.yaml +++ b/descriptions/unitree/a1_description/config/robot_control.yaml @@ -1,7 +1,7 @@ # Controller Manager configuration controller_manager: ros__parameters: - update_rate: 500 # Hz + update_rate: 1000 # Hz # Define the available controllers joint_state_broadcaster: @@ -135,7 +135,7 @@ ocs2_quadruped_controller: rl_quadruped_controller: ros__parameters: - update_rate: 100 # Hz + update_rate: 200 # Hz joints: - FL_hip_joint - FL_thigh_joint @@ -163,10 +163,10 @@ rl_quadruped_controller: - velocity feet_names: - - FR_foot - FL_foot - - RR_foot + - FR_foot - RL_foot + - RR_foot imu_name: "imu_sensor" base_name: "base" diff --git a/descriptions/unitree/go2_description/config/issacgym/config.yaml b/descriptions/unitree/go2_description/config/issacgym/config.yaml index 279a417..7a4ae22 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: "himloco.pt" +model_name: "policy.pt" framework: "isaacgym" rows: 4 cols: 3 decimation: 4 num_observations: 48 -observations: ["lin_vel", "ang_vel", "gravity_vec", "dof_pos", "dof_vel", "actions"] -observations_history: [5, 4, 3, 2, 1, 0] +observations: ["lin_vel", "ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] +#observations_history: [6, 5, 4, 3, 2, 1, 0] clip_obs: 100.0 clip_actions_lower: [-100, -100, -100, -100, -100, -100, @@ -15,14 +15,14 @@ clip_actions_upper: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] -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] +rl_kp: [20, 20, 20, + 20, 20, 20, + 20, 20, 20, + 20, 20, 20] +rl_kd: [0.5, 0.5, 0.5, + 0.5, 0.5, 0.5, + 0.5, 0.5, 0.5, + 0.5, 0.5, 0.5] fixed_kp: [60, 60, 60, 60, 60, 60, 60, 60, 60, @@ -35,11 +35,14 @@ hip_scale_reduction: 1.0 hip_scale_reduction_indices: [0, 3, 6, 9] num_of_dofs: 12 action_scale: 0.25 + 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, 0.25] + torque_limits: [33.5, 33.5, 33.5, 33.5, 33.5, 33.5, 33.5, 33.5, 33.5, diff --git a/descriptions/unitree/go2_description/config/robot_control.yaml b/descriptions/unitree/go2_description/config/robot_control.yaml index 7134243..082dec5 100644 --- a/descriptions/unitree/go2_description/config/robot_control.yaml +++ b/descriptions/unitree/go2_description/config/robot_control.yaml @@ -161,7 +161,14 @@ rl_quadruped_controller: - position - velocity + feet_names: + - FL_foot + - FR_foot + - RL_foot + - RR_foot + imu_name: "imu_sensor" + base_name: "base" imu_interfaces: - orientation.w