diff --git a/README.md b/README.md index 8b6190e..bf6c577 100644 --- a/README.md +++ b/README.md @@ -12,16 +12,11 @@ Todo List: - [x] **[2025-02-23]** Add Gazebo Playground - [x] OCS2 controller for Gazebo Simulation - [x] Refactor FSM and Unitree Guide Controller +- [x] **[2025-03-30]** Add Real Go2 Robot Support - [ ] OCS2 Perceptive locomotion demo -Video for Unitree Guide Controller: -[![](http://i1.hdslb.com/bfs/archive/310e6208920985ac43015b2da31c01ec15e2c5f9.jpg)](https://www.bilibili.com/video/BV1aJbAeZEuo/) - -Video for OCS2 Quadruped Controller: -[![](http://i0.hdslb.com/bfs/archive/e758ce019587032449a153cf897a543443b64bba.jpg)](https://www.bilibili.com/video/BV1UcxieuEmH/) - -Video for RL Quadruped Controller: -[![](http://i0.hdslb.com/bfs/archive/9886e7f9ed06d7f880b5614cb2f4c3ec1d7bf85f.jpg)](https://www.bilibili.com/video/BV1QP1pYBE47/) +Video on Real Unitree Go2 Robot: +[![](http://i0.hdslb.com/bfs/archive/7d3856b3c5e5040f24990d3eab760cf8ba4cf80d.jpg)](https://www.bilibili.com/video/BV1QpZaY8EYV/) ## 1. Quick Start @@ -35,18 +30,11 @@ Video for RL Quadruped Controller: colcon build --packages-up-to unitree_guide_controller go2_description keyboard_input --symlink-install ``` -### 1.1 Mujoco Simulator - -Please use **C++ Simulation** in this [Mujoco Simulation](https://github.com/legubiao/unitree_mujoco) for more robot -models and contact sensor. - -* **Python Simulation** is also supported, but you still need to - install [unitree_sdk2](https://github.com/unitreerobotics/unitree_sdk2) - +### 1.1 Mujoco Simulator or Real Unitree Robot > **Warning:** CycloneDDS ROS2 RMW may conflict with unitree_sdk2. If you cannot launch unitree mujoco simulation > without `sudo`, then you cannot used `unitree_mujoco_hardware`. This conflict could be solved by one of below two > methods: -> 1. Uninstall CycloneDDS ROS2 RMW, or +> 1. Uninstall CycloneDDS ROS2 RMW, used another ROS2 RMW, such as FastDDS **[Recommended]**. > 2. Follow the guide in [unitree_ros2](https://github.com/unitreerobotics/unitree_ros2) to configure the ROS2 RMW by compiling cyclone dds. @@ -97,7 +85,7 @@ models and contact sensor. * Install Gazebo ```bash - sudo apt-get install ros-jazzy-ros-gz ros-jazzy-gz-ros2-control + sudo apt-get install ros-jazzy-ros-gz ``` * Compile Gazebo Playground @@ -128,6 +116,8 @@ Congratulations! You have successfully launched the quadruped robot in the simul * [RL Quadruped Controller](controllers/rl_quadruped_controller): Reinforcement learning controller for quadruped robot * **Simulate with more sensors** * [Gazebo Quadruped Playground](libraries/gz_quadruped_playground): Provide gazebo simulation with lidar or depth camera. +* **Real Robot Deploy** + * [Unitree Go2 Robot](descriptions/unitree/go2_description): Check here about how to deploy on go2 robot. ## Reference diff --git a/commands/keyboard_input/README.md b/commands/keyboard_input/README.md index 05a1368..215a1b0 100644 --- a/commands/keyboard_input/README.md +++ b/commands/keyboard_input/README.md @@ -8,11 +8,13 @@ Tested environment: * Ubuntu 22.04 * ROS2 Humble +### Build Command ```bash cd ~/ros2_ws colcon build --packages-up-to keyboard_input ``` +### Launch Command ```bash source ~/ros2_ws/install/setup.bash ros2 run keyboard_input keyboard_input diff --git a/commands/unitree_joystick_input/CMakeLists.txt b/commands/unitree_joystick_input/CMakeLists.txt new file mode 100644 index 0000000..6fbda5d --- /dev/null +++ b/commands/unitree_joystick_input/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.8) +project(unitree_joystick_input) + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(control_input_msgs REQUIRED) +find_package(unitree_sdk2 REQUIRED) + +include(GNUInstallDirs) + +add_executable(joystick_input src/JoystickInput.cpp) +target_include_directories(joystick_input + PUBLIC + "$" + "$") +target_link_libraries(joystick_input unitree_sdk2) +ament_target_dependencies( + joystick_input + rclcpp + unitree_sdk2 + control_input_msgs +) + +install(TARGETS + joystick_input + DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) + +if (BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif () + +ament_package() diff --git a/commands/unitree_joystick_input/README.md b/commands/unitree_joystick_input/README.md new file mode 100644 index 0000000..8b193c0 --- /dev/null +++ b/commands/unitree_joystick_input/README.md @@ -0,0 +1,46 @@ +# Unitree Joystick Input Node + +This node will listen to the wireless remote topic and publish a `unitree_go::msg::dds_::WirelessController_` message by using `unitree_sdk2`. + +> Before use this node, please use `ifconfig` command to check the network interface to connect to the robot, then change the parameter in launch file. + +Tested environment: +* Ubuntu 24.04 + * ROS2 Jazzy + +### Build Command +```bash +cd ~/ros2_ws +colcon build --packages-up-to unitree_joystick_input --symlink-install +``` + +### Launch Command +```bash +source ~/ros2_ws/install/setup.bash +ros2 launch unitree_joystick_input joystick.launch.py +``` + +## 1. Use Instructions for Controllers + +### 1.1 Unitree Guide Controller +* Passive Mode: select +* Fixed Down: start +* Fixed Stand: start + * Free Stand: right + X + * Trot: right + Y + * SwingTest: right + B + * Balance: right + A + +### 1.2 OCS2 Quadruped Controller +* Passive Mode: select +* OCS2 Mode: start + * Stance: start + * trot: right + X + * standing trot: right + Y + * flying_trot: right + B + +### 1.3 RL Quadruped Controller +* Passive Mode: select +* Fixed Down: start +* Fixed Stand: start + * RL Mode: right + X \ No newline at end of file diff --git a/commands/unitree_joystick_input/include/unitree_joystick_input/JoystickInput.h b/commands/unitree_joystick_input/include/unitree_joystick_input/JoystickInput.h new file mode 100644 index 0000000..d833097 --- /dev/null +++ b/commands/unitree_joystick_input/include/unitree_joystick_input/JoystickInput.h @@ -0,0 +1,55 @@ +// +// Created by tlab-uav on 24-9-13. +// +#pragma once + + +#include +#include +#include +#include + + +typedef union +{ + struct + { + uint8_t R1 : 1; + uint8_t L1 : 1; + uint8_t start : 1; + uint8_t select : 1; + uint8_t R2 : 1; + uint8_t L2 : 1; + uint8_t F1 : 1; + uint8_t F2 : 1; + uint8_t A : 1; + uint8_t B : 1; + uint8_t X : 1; + uint8_t Y : 1; + uint8_t up : 1; + uint8_t right : 1; + uint8_t down : 1; + uint8_t left : 1; + } components; + uint16_t value; +} xKeySwitchUnion; + +class JoystickInput final : public rclcpp::Node { +public: + JoystickInput(); + + ~JoystickInput() override = default; + +private: + void remoteWirelessHandle(const void *messages); + + std::string network_interface_ = "lo"; + int domain_ = 0; + + unitree_go::msg::dds_::WirelessController_ wireless_controller_{}; + xKeySwitchUnion xKeySwitchUnion_{}; + + control_input_msgs::msg::Inputs inputs_; + rclcpp::Publisher::SharedPtr publisher_; + unitree::robot::ChannelSubscriberPtr subscriber_; +}; diff --git a/commands/unitree_joystick_input/launch/joystick.launch.py b/commands/unitree_joystick_input/launch/joystick.launch.py new file mode 100644 index 0000000..4b5c0f8 --- /dev/null +++ b/commands/unitree_joystick_input/launch/joystick.launch.py @@ -0,0 +1,17 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='unitree_joystick_input', + executable='joystick_input', + name='joystick_input_node', + parameters=[ + { + 'network_interface': 'enp46s0', + } + ], + ) + ]) diff --git a/commands/unitree_joystick_input/package.xml b/commands/unitree_joystick_input/package.xml new file mode 100644 index 0000000..a9ab00a --- /dev/null +++ b/commands/unitree_joystick_input/package.xml @@ -0,0 +1,19 @@ + + + + unitree_joystick_input + 0.0.0 + A package to convert joystick signal to control input + Huang Zhenbiao + Apache-2.0 + + ament_cmake + + rclcpp + sensor_msgs + control_input_msgs + + + ament_cmake + + diff --git a/commands/unitree_joystick_input/src/JoystickInput.cpp b/commands/unitree_joystick_input/src/JoystickInput.cpp new file mode 100644 index 0000000..1ea5d90 --- /dev/null +++ b/commands/unitree_joystick_input/src/JoystickInput.cpp @@ -0,0 +1,97 @@ +// +// Created by tlab-uav on 24-9-13. +// + +#include "unitree_joystick_input/JoystickInput.h" +#define TOPIC_JOYSTICK "rt/wirelesscontroller" + +using std::placeholders::_1; + +using namespace unitree::robot; + +JoystickInput::JoystickInput() : Node("unitree_joysick_node") +{ + publisher_ = create_publisher("control_input", 10); + + declare_parameter("network_interface", network_interface_); + declare_parameter("domain", domain_); + + network_interface_ = get_parameter("network_interface").as_string(); + domain_ = get_parameter("domain").as_int(); + RCLCPP_INFO(get_logger(), " network_interface: %s, domain: %d", network_interface_.c_str(), domain_); + ChannelFactory::Instance()->Init(domain_, network_interface_); + + subscriber_ = std::make_shared>( + TOPIC_JOYSTICK); + subscriber_->InitChannel( + [this](auto&& PH1) + { + remoteWirelessHandle(std::forward(PH1)); + }, + 1); +} + +void JoystickInput::remoteWirelessHandle(const void* messages) +{ + wireless_controller_ = *static_cast(messages); + xKeySwitchUnion_.value = wireless_controller_.keys(); + + if (xKeySwitchUnion_.components.select) + { + inputs_.command = 1; + } + else if (xKeySwitchUnion_.components.start) + { + inputs_.command = 2; + } + else if (xKeySwitchUnion_.components.right and xKeySwitchUnion_.components.B) + { + inputs_.command = 3; + } + else if (xKeySwitchUnion_.components.right and xKeySwitchUnion_.components.A) + { + inputs_.command = 4; + } + else if (xKeySwitchUnion_.components.right and xKeySwitchUnion_.components.X) + { + inputs_.command = 5; + } + else if (xKeySwitchUnion_.components.right and xKeySwitchUnion_.components.Y) + { + inputs_.command = 6; + } + else if (xKeySwitchUnion_.components.left and xKeySwitchUnion_.components.B) + { + inputs_.command = 7; + } + else if (xKeySwitchUnion_.components.left and xKeySwitchUnion_.components.A) + { + inputs_.command = 8; + } + else if (xKeySwitchUnion_.components.left and xKeySwitchUnion_.components.X) + { + inputs_.command = 9; + } + else if (xKeySwitchUnion_.components.left and xKeySwitchUnion_.components.Y) + { + inputs_.command = 10; + } + else + { + inputs_.command = 0; + inputs_.lx = wireless_controller_.lx(); + inputs_.ly = wireless_controller_.ly(); + inputs_.rx = wireless_controller_.rx(); + inputs_.ry = wireless_controller_.ry(); + } + publisher_->publish(inputs_); +} + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/controllers/rl_quadruped_controller/README.md b/controllers/rl_quadruped_controller/README.md index 7109145..1450e73 100644 --- a/controllers/rl_quadruped_controller/README.md +++ b/controllers/rl_quadruped_controller/README.md @@ -15,17 +15,19 @@ Tested environment: ### 2.1 Installing libtorch -```bash -cd ~/CLionProjects/ -wget https://download.pytorch.org/libtorch/cpu/libtorch-cxx11-abi-shared-with-deps-2.5.0%2Bcpu.zip -unzip unzip libtorch-cxx11-abi-shared-with-deps-2.5.0+cpu.zip -``` +> You can also choose `libtorch` with cuda. Just remember to download for c++ 11 ABI version. The position to place `libtorch` is also not fixed, just need to config the `.bashrc`. ```bash cd ~/CLionProjects/ +wget https://download.pytorch.org/libtorch/cpu/libtorch-cxx11-abi-shared-with-deps-2.5.0%2Bcpu.zip +unzip libtorch-cxx11-abi-shared-with-deps-2.5.0+cpu.zip +``` + +```bash +cd ~ rm -rf libtorch-cxx11-abi-shared-with-deps-2.5.0+cpu.zip -echo 'export Torch_DIR=~/CLionProjects/libtorch' >> ~/.bashrc -echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:~/CLionProjects/libtorch/lib' >> ~/.bashrc +echo 'export Torch_DIR=~/libtorch' >> ~/.bashrc +echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:~/libtorch/lib' >> ~/.bashrc ``` ### 2.2 Build Controller 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 aa43e14..ac69055 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 @@ -132,7 +132,7 @@ class StateRL final : public FSMState { public: explicit StateRL(CtrlInterfaces& ctrl_interfaces, - CtrlComponent& ctrl_component, const std::string& config_path, + CtrlComponent& ctrl_component, const std::vector& target_pos); void enter() override; @@ -163,6 +163,10 @@ private: void setCommand() const; + std::shared_ptr node_; + std::string robot_pkg_ = "go2_description"; + std::string model_folder_ = "legged_gym"; + bool enable_estimator_; std::shared_ptr& estimator_; @@ -181,6 +185,7 @@ private: // rl module torch::jit::script::Module model_; + bool use_rl_thread_ = true; std::thread rl_thread_; bool running_ = false; bool updated_ = false; 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 6544160..3c94f8b 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 @@ -6,12 +6,14 @@ #define CtrlComponent_H #include "Estimator.h" +#include struct CtrlComponent { bool enable_estimator_ = false; std::shared_ptr robot_model_; std::shared_ptr estimator_; + std::shared_ptr node_; CtrlComponent() = default; }; 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 index 353785b..936bb8a 100644 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h @@ -16,7 +16,7 @@ struct CtrlComponent; class Estimator { public: - explicit Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component); + explicit Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component, int feet_force_threshold = 1); ~Estimator() = default; @@ -103,9 +103,12 @@ public: void update(); + bool safety() const; + private: CtrlInterfaces &ctrl_interfaces_; std::shared_ptr &robot_model_; + double feet_force_threshold_; Eigen::Matrix x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4) diff --git a/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp index b7bf2fb..e109938 100644 --- a/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp +++ b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp @@ -3,7 +3,7 @@ // #include "rl_quadruped_controller/FSM/StateRL.h" - +#include #include #include @@ -48,29 +48,40 @@ std::vector ReadVectorFromYaml(const YAML::Node& node, const std::string& fra } StateRL::StateRL(CtrlInterfaces& ctrl_interfaces, - CtrlComponent& ctrl_component, const std::string& config_path, - const std::vector& target_pos) : FSMState( - FSMStateName::RL, "rl", ctrl_interfaces), - estimator_(ctrl_component.estimator_), - enable_estimator_(ctrl_component.enable_estimator_) + CtrlComponent& ctrl_component, + const std::vector& target_pos) : + FSMState(FSMStateName::RL, "rl", ctrl_interfaces), + node_(ctrl_component.node_), + enable_estimator_(ctrl_component.enable_estimator_), + estimator_(ctrl_component.estimator_) { + node_->declare_parameter("robot_pkg", robot_pkg_); + node_->declare_parameter("model_folder", model_folder_); + node_->declare_parameter("use_rl_thread", use_rl_thread_); + robot_pkg_ = node_->get_parameter("robot_pkg").as_string(); + model_folder_ = node_->get_parameter("model_folder").as_string(); + use_rl_thread_ = node_->get_parameter("use_rl_thread").as_bool(); + + RCLCPP_INFO(node_->get_logger(), "Using robot model from %s", robot_pkg_.c_str()); + const std::string package_share_directory = ament_index_cpp::get_package_share_directory(robot_pkg_); + const std::string model_path = package_share_directory + "/config/" + model_folder_; + for (int i = 0; i < 12; i++) { init_pos_[i] = target_pos[i]; } // read params from yaml - loadYaml(config_path); + loadYaml(model_path); - // history if (!params_.observations_history.empty()) { history_obs_buf_ = std::make_shared(1, params_.num_observations, params_.observations_history.size()); } - std::cout << "Model loading: " << config_path + "/" + params_.model_name << std::endl; - model_ = torch::jit::load(config_path + "/" + params_.model_name); + RCLCPP_INFO(node_->get_logger(), "Model loading: %s", params_.model_name.c_str()); + model_ = torch::jit::load(model_path + "/" + params_.model_name); // for (const auto ¶m: model_.parameters()) { @@ -78,30 +89,32 @@ StateRL::StateRL(CtrlInterfaces& ctrl_interfaces, // } - rl_thread_ = std::thread([&] + if (use_rl_thread_) { - while (true) - { - try + rl_thread_ = std::thread([&]{ + while (true) { - executeAndSleep( - [&] - { - if (running_) + try + { + executeAndSleep( + [&] { - runModel(); - } - }, - ctrl_interfaces_.frequency_ / params_.decimation); + if (running_) + { + runModel(); + } + }, + ctrl_interfaces_.frequency_ / params_.decimation); + } + catch (const std::exception& e) + { + running_ = false; + RCLCPP_ERROR(rclcpp::get_logger("StateRL"), "Error in RL thread: %s", e.what()); + } } - catch (const std::exception& e) - { - running_ = false; - RCLCPP_ERROR(rclcpp::get_logger("StateRL"), "Error in RL thread: %s", e.what()); - } - } - }); - setThreadPriority(50, rl_thread_); + }); + setThreadPriority(60, rl_thread_); + } } void StateRL::enter() @@ -125,12 +138,21 @@ void StateRL::enter() control_.y = 0.0; control_.yaw = 0.0; + // history + if (!params_.observations_history.empty()) { + history_obs_buf_->clear(); + } + running_ = true; } void StateRL::run(const rclcpp::Time&/*time*/, const rclcpp::Duration&/*period*/) { getState(); + if (!use_rl_thread_) + { + runModel(); + } setCommand(); } @@ -141,6 +163,10 @@ void StateRL::exit() FSMStateName StateRL::checkChange() { + if (enable_estimator_ and !estimator_->safety()) + { + return FSMStateName::PASSIVE; + } switch (ctrl_interfaces_.control_inputs_.command) { case 1: @@ -314,32 +340,35 @@ void StateRL::getState() { if (params_.framework == "isaacgym") { - robot_state_.imu.quaternion[3] = ctrl_interfaces_.imu_state_interface_[0].get().get_value(); - robot_state_.imu.quaternion[0] = ctrl_interfaces_.imu_state_interface_[1].get().get_value(); - robot_state_.imu.quaternion[1] = ctrl_interfaces_.imu_state_interface_[2].get().get_value(); - robot_state_.imu.quaternion[2] = ctrl_interfaces_.imu_state_interface_[3].get().get_value(); + robot_state_.imu.quaternion[3] = ctrl_interfaces_.imu_state_interface_[0].get().get_optional().value(); + robot_state_.imu.quaternion[0] = ctrl_interfaces_.imu_state_interface_[1].get().get_optional().value(); + robot_state_.imu.quaternion[1] = ctrl_interfaces_.imu_state_interface_[2].get().get_optional().value(); + robot_state_.imu.quaternion[2] = ctrl_interfaces_.imu_state_interface_[3].get().get_optional().value(); } else if (params_.framework == "isaacsim") { - robot_state_.imu.quaternion[0] = ctrl_interfaces_.imu_state_interface_[0].get().get_value(); - robot_state_.imu.quaternion[1] = ctrl_interfaces_.imu_state_interface_[1].get().get_value(); - robot_state_.imu.quaternion[2] = ctrl_interfaces_.imu_state_interface_[2].get().get_value(); - robot_state_.imu.quaternion[3] = ctrl_interfaces_.imu_state_interface_[3].get().get_value(); + robot_state_.imu.quaternion[0] = ctrl_interfaces_.imu_state_interface_[0].get().get_optional().value(); + robot_state_.imu.quaternion[1] = ctrl_interfaces_.imu_state_interface_[1].get().get_optional().value(); + robot_state_.imu.quaternion[2] = ctrl_interfaces_.imu_state_interface_[2].get().get_optional().value(); + robot_state_.imu.quaternion[3] = ctrl_interfaces_.imu_state_interface_[3].get().get_optional().value(); } - robot_state_.imu.gyroscope[0] = ctrl_interfaces_.imu_state_interface_[4].get().get_value(); - robot_state_.imu.gyroscope[1] = ctrl_interfaces_.imu_state_interface_[5].get().get_value(); - robot_state_.imu.gyroscope[2] = ctrl_interfaces_.imu_state_interface_[6].get().get_value(); + robot_state_.imu.gyroscope[0] = ctrl_interfaces_.imu_state_interface_[4].get().get_optional().value(); + robot_state_.imu.gyroscope[1] = ctrl_interfaces_.imu_state_interface_[5].get().get_optional().value(); + robot_state_.imu.gyroscope[2] = ctrl_interfaces_.imu_state_interface_[6].get().get_optional().value(); - robot_state_.imu.accelerometer[0] = ctrl_interfaces_.imu_state_interface_[7].get().get_value(); - robot_state_.imu.accelerometer[1] = ctrl_interfaces_.imu_state_interface_[8].get().get_value(); - robot_state_.imu.accelerometer[2] = ctrl_interfaces_.imu_state_interface_[9].get().get_value(); + robot_state_.imu.accelerometer[0] = ctrl_interfaces_.imu_state_interface_[7].get().get_optional().value(); + robot_state_.imu.accelerometer[1] = ctrl_interfaces_.imu_state_interface_[8].get().get_optional().value(); + robot_state_.imu.accelerometer[2] = ctrl_interfaces_.imu_state_interface_[9].get().get_optional().value(); for (int i = 0; i < 12; i++) { - robot_state_.motor_state.q[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_value(); - robot_state_.motor_state.dq[i] = ctrl_interfaces_.joint_velocity_state_interface_[i].get().get_value(); - robot_state_.motor_state.tauEst[i] = ctrl_interfaces_.joint_effort_state_interface_[i].get().get_value(); + robot_state_.motor_state.q[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_optional(). + value(); + robot_state_.motor_state.dq[i] = ctrl_interfaces_.joint_velocity_state_interface_[i].get().get_optional(). + value(); + robot_state_.motor_state.tauEst[i] = ctrl_interfaces_.joint_effort_state_interface_[i].get().get_optional(). + value(); } control_.x = ctrl_interfaces_.control_inputs_.ly; diff --git a/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp b/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp index 6940d5e..0e9fe66 100644 --- a/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp +++ b/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp @@ -3,20 +3,26 @@ // #include "RlQuadrupedController.h" -#include -namespace rl_quadruped_controller { +namespace rl_quadruped_controller +{ using config_type = controller_interface::interface_configuration_type; - controller_interface::InterfaceConfiguration LeggedGymController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration LeggedGymController::command_interface_configuration() const + { controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}}; conf.names.reserve(joint_names_.size() * command_interface_types_.size()); - for (const auto &joint_name: joint_names_) { - for (const auto &interface_type: command_interface_types_) { - if (!command_prefix_.empty()) { + for (const auto& joint_name : joint_names_) + { + for (const auto& interface_type : command_interface_types_) + { + if (!command_prefix_.empty()) + { conf.names.push_back(command_prefix_ + "/" + joint_name + "/" += interface_type); - } else { + } + else + { conf.names.push_back(joint_name + "/" += interface_type); } } @@ -25,21 +31,26 @@ namespace rl_quadruped_controller { return conf; } - controller_interface::InterfaceConfiguration LeggedGymController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration LeggedGymController::state_interface_configuration() const + { controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}}; conf.names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto &joint_name: joint_names_) { - for (const auto &interface_type: state_interface_types_) { + for (const auto& joint_name : joint_names_) + { + for (const auto& interface_type : state_interface_types_) + { conf.names.push_back(joint_name + "/" += interface_type); } } - for (const auto &interface_type: imu_interface_types_) { + for (const auto& interface_type : imu_interface_types_) + { conf.names.push_back(imu_name_ + "/" += interface_type); } - for (const auto &interface_type: foot_force_interface_types_) { + for (const auto& interface_type : foot_force_interface_types_) + { conf.names.push_back(foot_force_name_ + "/" += interface_type); } @@ -47,9 +58,12 @@ namespace rl_quadruped_controller { } controller_interface::return_type LeggedGymController:: - update(const rclcpp::Time & time, const rclcpp::Duration & period) { - if (ctrl_component_.enable_estimator_) { - if (ctrl_component_.robot_model_ == nullptr) { + update(const rclcpp::Time& time, const rclcpp::Duration& period) + { + if (ctrl_component_.enable_estimator_) + { + if (ctrl_component_.robot_model_ == nullptr) + { return controller_interface::return_type::OK; } @@ -57,16 +71,20 @@ namespace rl_quadruped_controller { ctrl_component_.estimator_->update(); } - if (mode_ == FSMMode::NORMAL) { + if (mode_ == FSMMode::NORMAL) + { current_state_->run(time, period); next_state_name_ = current_state_->checkChange(); - if (next_state_name_ != current_state_->state_name) { + if (next_state_name_ != current_state_->state_name) + { mode_ = FSMMode::CHANGE; next_state_ = getNextState(next_state_name_); RCLCPP_INFO(get_node()->get_logger(), "Switched from %s to %s", current_state_->state_name_string.c_str(), next_state_->state_name_string.c_str()); } - } else if (mode_ == FSMMode::CHANGE) { + } + else if (mode_ == FSMMode::CHANGE) + { current_state_->exit(); current_state_ = next_state_; @@ -77,46 +95,49 @@ namespace rl_quadruped_controller { return controller_interface::return_type::OK; } - controller_interface::CallbackReturn LeggedGymController::on_init() { - try { - joint_names_ = auto_declare >("joints", joint_names_); - feet_names_ = auto_declare >("feet_names", feet_names_); + controller_interface::CallbackReturn LeggedGymController::on_init() + { + try + { + joint_names_ = auto_declare>("joints", joint_names_); + feet_names_ = auto_declare>("feet_names", feet_names_); command_interface_types_ = - auto_declare >("command_interfaces", command_interface_types_); + auto_declare>("command_interfaces", command_interface_types_); state_interface_types_ = - auto_declare >("state_interfaces", state_interface_types_); + 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_); + 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", foot_force_interface_types_); - - // rl config folder - robot_pkg_ = auto_declare("robot_pkg", robot_pkg_); - model_folder_ = auto_declare("model_folder", model_folder_); + auto_declare>("foot_force_interfaces", foot_force_interface_types_); + feet_force_threshold_ = auto_declare("feet_force_threshold", feet_force_threshold_); // pose parameters - down_pos_ = auto_declare >("down_pos", down_pos_); - stand_pos_ = auto_declare >("stand_pos", stand_pos_); + down_pos_ = auto_declare>("down_pos", down_pos_); + stand_pos_ = auto_declare>("stand_pos", stand_pos_); stand_kp_ = auto_declare("stand_kp", stand_kp_); stand_kd_ = auto_declare("stand_kd", stand_kd_); get_node()->get_parameter("update_rate", ctrl_interfaces_.frequency_); RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", ctrl_interfaces_.frequency_); - if (foot_force_interface_types_.size() == 4) { + if (foot_force_interface_types_.size() == 4) + { RCLCPP_INFO(get_node()->get_logger(), "Enable Estimator"); ctrl_component_.enable_estimator_ = true; ctrl_component_.estimator_ = std::make_shared(ctrl_interfaces_, ctrl_component_); } - } catch (const std::exception &e) { + ctrl_component_.node_ = get_node(); + } + catch (const std::exception& e) + { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } @@ -125,11 +146,14 @@ namespace rl_quadruped_controller { } controller_interface::CallbackReturn LeggedGymController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { + 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) { - if (ctrl_component_.enable_estimator_) { + [this](const std_msgs::msg::String::SharedPtr msg) + { + if (ctrl_component_.enable_estimator_) + { ctrl_component_.robot_model_ = std::make_shared( ctrl_interfaces_, msg->data, feet_names_, base_name_); } @@ -137,7 +161,8 @@ namespace rl_quadruped_controller { control_input_subscription_ = get_node()->create_subscription( - "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) { + "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) + { // Handle message ctrl_interfaces_.control_inputs_.command = msg->command; ctrl_interfaces_.control_inputs_.lx = msg->lx; @@ -150,27 +175,38 @@ namespace rl_quadruped_controller { } controller_interface::CallbackReturn LeggedGymController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) { + const rclcpp_lifecycle::State& /*previous_state*/) + { // clear out vectors in case of restart ctrl_interfaces_.clear(); // assign command interfaces - for (auto &interface: command_interfaces_) { + for (auto& interface : command_interfaces_) + { std::string interface_name = interface.get_interface_name(); - if (const size_t pos = interface_name.find('/'); pos != std::string::npos) { + if (const size_t pos = interface_name.find('/'); pos != std::string::npos) + { command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface); - } else { + } + else + { command_interface_map_[interface_name]->push_back(interface); } } // assign state interfaces - for (auto &interface: state_interfaces_) { - if (interface.get_prefix_name() == imu_name_) { + for (auto& interface : state_interfaces_) + { + if (interface.get_prefix_name() == imu_name_) + { ctrl_interfaces_.imu_state_interface_.emplace_back(interface); - } else if (interface.get_prefix_name() == foot_force_name_) { + } + else if (interface.get_prefix_name() == foot_force_name_) + { ctrl_interfaces_.foot_force_state_interface_.emplace_back(interface); - } else { + } + else + { state_interface_map_[interface.get_interface_name()]->push_back(interface); } } @@ -179,12 +215,7 @@ namespace rl_quadruped_controller { state_list_.passive = std::make_shared(ctrl_interfaces_); state_list_.fixedDown = std::make_shared(ctrl_interfaces_, down_pos_, stand_kp_, stand_kd_); state_list_.fixedStand = std::make_shared(ctrl_interfaces_, stand_pos_, stand_kp_, stand_kd_); - - - RCLCPP_INFO(get_node()->get_logger(), "Using robot model from %s", robot_pkg_.c_str()); - const std::string package_share_directory = ament_index_cpp::get_package_share_directory(robot_pkg_); - std::string model_path = package_share_directory + "/config/" + model_folder_; - state_list_.rl = std::make_shared(ctrl_interfaces_, ctrl_component_, model_path, stand_pos_); + state_list_.rl = std::make_shared(ctrl_interfaces_, ctrl_component_, stand_pos_); // Initialize FSM current_state_ = state_list_.passive; @@ -197,39 +228,45 @@ namespace rl_quadruped_controller { } controller_interface::CallbackReturn LeggedGymController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) { + const rclcpp_lifecycle::State& /*previous_state*/) + { release_interfaces(); return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn - LeggedGymController::on_cleanup(const rclcpp_lifecycle::State &previous_state) { + LeggedGymController::on_cleanup(const rclcpp_lifecycle::State& previous_state) + { return ControllerInterface::on_cleanup(previous_state); } controller_interface::CallbackReturn - LeggedGymController::on_shutdown(const rclcpp_lifecycle::State &previous_state) { + LeggedGymController::on_shutdown(const rclcpp_lifecycle::State& previous_state) + { return ControllerInterface::on_shutdown(previous_state); } - controller_interface::CallbackReturn LeggedGymController::on_error(const rclcpp_lifecycle::State &previous_state) { + controller_interface::CallbackReturn LeggedGymController::on_error(const rclcpp_lifecycle::State& previous_state) + { return ControllerInterface::on_error(previous_state); } - std::shared_ptr LeggedGymController::getNextState(const FSMStateName stateName) const { - switch (stateName) { - case FSMStateName::INVALID: - return state_list_.invalid; - case FSMStateName::PASSIVE: - return state_list_.passive; - case FSMStateName::FIXEDDOWN: - return state_list_.fixedDown; - case FSMStateName::FIXEDSTAND: - return state_list_.fixedStand; - case FSMStateName::RL: - return state_list_.rl; - default: - return state_list_.invalid; + std::shared_ptr LeggedGymController::getNextState(const FSMStateName stateName) const + { + switch (stateName) + { + case FSMStateName::INVALID: + return state_list_.invalid; + case FSMStateName::PASSIVE: + return state_list_.passive; + case FSMStateName::FIXEDDOWN: + return state_list_.fixedDown; + case FSMStateName::FIXEDSTAND: + return state_list_.fixedStand; + case FSMStateName::RL: + return state_list_.rl; + default: + return state_list_.invalid; } } } diff --git a/controllers/rl_quadruped_controller/src/RlQuadrupedController.h b/controllers/rl_quadruped_controller/src/RlQuadrupedController.h index acba961..4608152 100644 --- a/controllers/rl_quadruped_controller/src/RlQuadrupedController.h +++ b/controllers/rl_quadruped_controller/src/RlQuadrupedController.h @@ -13,8 +13,10 @@ #include "rl_quadruped_controller/FSM/StateFixedStand.h" #include "controller_common//FSM/StatePassive.h" -namespace rl_quadruped_controller { - struct FSMStateList { +namespace rl_quadruped_controller +{ + struct FSMStateList + { std::shared_ptr invalid; std::shared_ptr passive; std::shared_ptr fixedDown; @@ -22,7 +24,8 @@ namespace rl_quadruped_controller { std::shared_ptr rl; }; - class LeggedGymController final : public controller_interface::ControllerInterface { + class LeggedGymController final : public controller_interface::ControllerInterface + { public: LeggedGymController() = default; @@ -31,27 +34,27 @@ namespace rl_quadruped_controller { controller_interface::InterfaceConfiguration state_interface_configuration() const override; controller_interface::return_type update( - const rclcpp::Time &time, const rclcpp::Duration &period) override; + const rclcpp::Time& time, const rclcpp::Duration& period) override; controller_interface::CallbackReturn on_init() override; controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State &previous_state) override; + const rclcpp_lifecycle::State& previous_state) override; controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State &previous_state) override; + const rclcpp_lifecycle::State& previous_state) override; controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State &previous_state) override; + const rclcpp_lifecycle::State& previous_state) override; controller_interface::CallbackReturn on_cleanup( - const rclcpp_lifecycle::State &previous_state) override; + const rclcpp_lifecycle::State& previous_state) override; controller_interface::CallbackReturn on_shutdown( - const rclcpp_lifecycle::State &previous_state) override; - + const rclcpp_lifecycle::State& previous_state) override; + controller_interface::CallbackReturn on_error( - const rclcpp_lifecycle::State &previous_state) override; + const rclcpp_lifecycle::State& previous_state) override; private: std::shared_ptr getNextState(FSMStateName stateName) const; @@ -90,12 +93,10 @@ namespace rl_quadruped_controller { double stand_kp_ = 80.0; double stand_kd_ = 3.5; - - std::string robot_pkg_; - std::string model_folder_; + double feet_force_threshold_ = 20.0; std::unordered_map< - std::string, std::vector > *> + std::string, std::vector>*> command_interface_map_ = { {"effort", &ctrl_interfaces_.joint_torque_command_interface_}, {"position", &ctrl_interfaces_.joint_position_command_interface_}, @@ -105,7 +106,7 @@ namespace rl_quadruped_controller { }; std::unordered_map< - std::string, std::vector > *> + std::string, std::vector>*> state_interface_map_ = { {"position", &ctrl_interfaces_.joint_position_state_interface_}, {"effort", &ctrl_interfaces_.joint_effort_state_interface_}, diff --git a/controllers/rl_quadruped_controller/src/common/ObservationBuffer.cpp b/controllers/rl_quadruped_controller/src/common/ObservationBuffer.cpp index 4509942..49e63e0 100644 --- a/controllers/rl_quadruped_controller/src/common/ObservationBuffer.cpp +++ b/controllers/rl_quadruped_controller/src/common/ObservationBuffer.cpp @@ -10,8 +10,8 @@ ObservationBuffer::ObservationBuffer(int num_envs, : num_envs_(num_envs), num_obs_(num_obs), include_history_steps_(include_history_steps) { - num_obs_total_ = num_obs * include_history_steps; - obs_buffer_ = torch::zeros({num_envs, num_obs_total_}, dtype(torch::kFloat32)); + num_obs_total_ = num_obs_ * include_history_steps_; + obs_buffer_ = torch::zeros({num_envs_, num_obs_total_}, dtype(torch::kFloat32)); } void ObservationBuffer::reset(const std::vector &reset_index, const torch::Tensor &new_obs) { @@ -22,6 +22,11 @@ void ObservationBuffer::reset(const std::vector &reset_index, const torch:: obs_buffer_.index_put_(indices, new_obs.repeat({1, include_history_steps_})); } +void ObservationBuffer::clear() +{ + obs_buffer_ = torch::zeros_like(obs_buffer_); +} + void ObservationBuffer::insert(const torch::Tensor &new_obs) { // Shift observations back. const torch::Tensor shifted_obs = obs_buffer_.index({ diff --git a/controllers/rl_quadruped_controller/src/common/ObservationBuffer.h b/controllers/rl_quadruped_controller/src/common/ObservationBuffer.h index ebb5581..1c40d91 100644 --- a/controllers/rl_quadruped_controller/src/common/ObservationBuffer.h +++ b/controllers/rl_quadruped_controller/src/common/ObservationBuffer.h @@ -8,17 +8,20 @@ #include #include -class ObservationBuffer { +class ObservationBuffer +{ public: ObservationBuffer(int num_envs, int num_obs, int include_history_steps); ~ObservationBuffer() = default; - void reset(const std::vector& reset_index, const torch::Tensor &new_obs); + void reset(const std::vector& reset_index, const torch::Tensor& new_obs); - void insert(const torch::Tensor &new_obs); + void clear(); - [[nodiscard]] torch::Tensor getObsVec(const std::vector &obs_ids) const; + void insert(const torch::Tensor& new_obs); + + [[nodiscard]] torch::Tensor getObsVec(const std::vector& obs_ids) const; private: int num_envs_; diff --git a/controllers/rl_quadruped_controller/src/control/Estimator.cpp b/controllers/rl_quadruped_controller/src/control/Estimator.cpp index 5f05a5c..4cee8ea 100644 --- a/controllers/rl_quadruped_controller/src/control/Estimator.cpp +++ b/controllers/rl_quadruped_controller/src/control/Estimator.cpp @@ -9,14 +9,18 @@ #include "rl_quadruped_controller/control/CtrlComponent.h" -Estimator::Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component) : ctrl_interfaces_(ctrl_interfaces), - robot_model_(ctrl_component.robot_model_){ +Estimator::Estimator(CtrlInterfaces& ctrl_interfaces, CtrlComponent& ctrl_component, int feet_force_threshold) : + ctrl_interfaces_(ctrl_interfaces), + robot_model_(ctrl_component.robot_model_), + feet_force_threshold_(feet_force_threshold) +{ g_ << 0, 0, -9.81; dt_ = 1.0 / ctrl_interfaces_.frequency_; std::cout << "dt: " << dt_ << std::endl; large_variance_ = 100; - for (int i(0); i < Qdig.rows(); ++i) { + for (int i(0); i < Qdig.rows(); ++i) + { Qdig(i) = i < 6 ? 0.0003 : 0.01; } @@ -51,88 +55,88 @@ Estimator::Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_compon 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; + -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; + 302.120; QInit_ = Qdig.asDiagonal(); QInit_ += B * Cu * B.transpose(); @@ -143,11 +147,13 @@ Estimator::Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_compon // low_pass_filters_[2] = std::make_shared(dt_, 3.0); } -double Estimator::getYaw() const { +double Estimator::getYaw() const +{ return rotMatToRPY(rotation_)(2); } -void Estimator::update() { +void Estimator::update() +{ if (robot_model_->mass_ == 0) return; Q = QInit_; @@ -158,42 +164,46 @@ void Estimator::update() { feet_h_.setZero(); // Adjust the covariance based on foot contact and phase. - for (int i(0); i < 4; ++i) { - if (ctrl_interfaces_.foot_force_state_interface_[i].get().get_value() < 1) { + for (int i(0); i < 4; ++i) + { + if (ctrl_interfaces_.foot_force_state_interface_[i].get().get_optional().value() < feet_force_threshold_) + { // 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 { + } + 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); + (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); + (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); + (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_interfaces_.imu_state_interface_[0].get().get_value(), - ctrl_interfaces_.imu_state_interface_[1].get().get_value(), - ctrl_interfaces_.imu_state_interface_[2].get().get_value(), - ctrl_interfaces_.imu_state_interface_[3].get().get_value(); + quat << ctrl_interfaces_.imu_state_interface_[0].get().get_optional().value(), + ctrl_interfaces_.imu_state_interface_[1].get().get_optional().value(), + ctrl_interfaces_.imu_state_interface_[2].get().get_optional().value(), + ctrl_interfaces_.imu_state_interface_[3].get().get_optional().value(); rotation_ = quatToRotMat(quat); - gyro_ <getValue(); // x_hat_(5) = low_pass_filters_[2]->getValue(); } + +bool Estimator::safety() const +{ + Vec3 rpy = rotMatToRPY(rotation_); + if (rpy[0] > M_PI_2 or rpy[0] < -M_PI_2) + { + return false; + } + if (rpy[1] > M_PI_2 or rpy[1] < -M_PI_2) + { + return false; + } + return true; +} diff --git a/controllers/rl_quadruped_controller/src/control/LowPassFilter.cpp b/controllers/rl_quadruped_controller/src/control/LowPassFilter.cpp index 047cde6..68c7c83 100644 --- a/controllers/rl_quadruped_controller/src/control/LowPassFilter.cpp +++ b/controllers/rl_quadruped_controller/src/control/LowPassFilter.cpp @@ -9,23 +9,28 @@ * @param samplePeriod sample period * @param cutFrequency cut frequency */ -LowPassFilter::LowPassFilter(const double samplePeriod, const double cutFrequency) { +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_) { +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 { +double LowPassFilter::getValue() const +{ return pass_value_; } -void LowPassFilter::clear() { +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 index 353b933..7ffb45a 100644 --- a/controllers/rl_quadruped_controller/src/robot/QuadrupedRobot.cpp +++ b/controllers/rl_quadruped_controller/src/robot/QuadrupedRobot.cpp @@ -114,15 +114,15 @@ void QuadrupedRobot::update() { if (mass_ == 0) return; for (int i = 0; i < 4; i++) { KDL::JntArray pos_array(3); - pos_array(0) = ctrl_interfaces_.joint_position_state_interface_[i * 3].get().get_value(); - pos_array(1) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 1].get().get_value(); - pos_array(2) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 2].get().get_value(); + pos_array(0) = ctrl_interfaces_.joint_position_state_interface_[i * 3].get().get_optional().value(); + pos_array(1) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 1].get().get_optional().value(); + pos_array(2) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 2].get().get_optional().value(); current_joint_pos_[i] = pos_array; KDL::JntArray vel_array(3); - vel_array(0) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3].get().get_value(); - vel_array(1) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 1].get().get_value(); - vel_array(2) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 2].get().get_value(); + vel_array(0) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3].get().get_optional().value(); + vel_array(1) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 1].get().get_optional().value(); + vel_array(2) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 2].get().get_optional().value(); current_joint_vel_[i] = vel_array; } } diff --git a/controllers/unitree_guide_controller/src/control/Estimator.cpp b/controllers/unitree_guide_controller/src/control/Estimator.cpp index 4874368..8c9ffea 100644 --- a/controllers/unitree_guide_controller/src/control/Estimator.cpp +++ b/controllers/unitree_guide_controller/src/control/Estimator.cpp @@ -182,19 +182,19 @@ void Estimator::update() { } Quat quat; - quat << ctrl_interfaces_.imu_state_interface_[0].get().get_value(), - ctrl_interfaces_.imu_state_interface_[1].get().get_value(), - ctrl_interfaces_.imu_state_interface_[2].get().get_value(), - ctrl_interfaces_.imu_state_interface_[3].get().get_value(); + quat << ctrl_interfaces_.imu_state_interface_[0].get().get_optional().value(), + ctrl_interfaces_.imu_state_interface_[1].get().get_optional().value(), + ctrl_interfaces_.imu_state_interface_[2].get().get_optional().value(), + ctrl_interfaces_.imu_state_interface_[3].get().get_optional().value(); rotation_ = quatToRotMat(quat); - gyro_ << ctrl_interfaces_.imu_state_interface_[4].get().get_value(), - ctrl_interfaces_.imu_state_interface_[5].get().get_value(), - ctrl_interfaces_.imu_state_interface_[6].get().get_value(); + gyro_ << ctrl_interfaces_.imu_state_interface_[4].get().get_optional().value(), + ctrl_interfaces_.imu_state_interface_[5].get().get_optional().value(), + ctrl_interfaces_.imu_state_interface_[6].get().get_optional().value(); - acceleration_ << ctrl_interfaces_.imu_state_interface_[7].get().get_value(), - ctrl_interfaces_.imu_state_interface_[8].get().get_value(), - ctrl_interfaces_.imu_state_interface_[9].get().get_value(); + acceleration_ << ctrl_interfaces_.imu_state_interface_[7].get().get_optional().value(), + ctrl_interfaces_.imu_state_interface_[8].get().get_optional().value(), + ctrl_interfaces_.imu_state_interface_[9].get().get_optional().value(); u_ = rotation_ * acceleration_ + g_; x_hat_ = A * x_hat_ + B * u_; diff --git a/descriptions/unitree/go2_description/README.md b/descriptions/unitree/go2_description/README.md index d2af45e..6a6d47b 100644 --- a/descriptions/unitree/go2_description/README.md +++ b/descriptions/unitree/go2_description/README.md @@ -29,8 +29,12 @@ ros2 launch go2_description visualize.launch.py ## 3. Launch ROS2 Control -### 3.1 Mujoco Simulator - +### 3.1 Mujoco Simulator or Real Go2 Robot +> **About Real Go2 Robot**: To switch the Unitree Hardware Interface to the real robot, you need to change the config at `xacro/ros2_control.xacro`. +> * Config guide could be found at [UnitreeHardwareInterface](../../../hardwares/hardware_unitree_mujoco) +> * Don't forget to disable the **Official Locomotion Controller** in the robot before the test +> * To use Unitree's remote controller, see [UnitreeJoystickInput](../../../commands/unitree_joystick_input) +> * Always test on simulator first before deploying to the real robot. * Unitree Guide Controller ```bash source ~/ros2_ws/install/setup.bash diff --git a/descriptions/unitree/go2_description/config/gazebo.yaml b/descriptions/unitree/go2_description/config/gazebo.yaml index aeb11d9..5d51fd7 100644 --- a/descriptions/unitree/go2_description/config/gazebo.yaml +++ b/descriptions/unitree/go2_description/config/gazebo.yaml @@ -164,6 +164,20 @@ rl_quadruped_controller: - effort - position - velocity + + down_pos: + - 0.01 + - 1.27 + - -2.8 + - -0.01 + - 1.27 + - -2.8 + - 0.3 + - 1.31 + - -2.8 + - -0.3 + - 1.31 + - -2.8 imu_name: "imu_sensor" imu_interfaces: diff --git a/descriptions/unitree/go2_description/config/legged_gym/config_himloco.yaml b/descriptions/unitree/go2_description/config/himloco/config.yaml similarity index 100% rename from descriptions/unitree/go2_description/config/legged_gym/config_himloco.yaml rename to descriptions/unitree/go2_description/config/himloco/config.yaml diff --git a/descriptions/unitree/go2_description/config/legged_gym/himloco.pt b/descriptions/unitree/go2_description/config/himloco/himloco.pt similarity index 100% rename from descriptions/unitree/go2_description/config/legged_gym/himloco.pt rename to descriptions/unitree/go2_description/config/himloco/himloco.pt diff --git a/descriptions/unitree/go2_description/config/legged_gym/config.yaml b/descriptions/unitree/go2_description/config/legged_gym/config.yaml index ac43a20..454a735 100644 --- a/descriptions/unitree/go2_description/config/legged_gym/config.yaml +++ b/descriptions/unitree/go2_description/config/legged_gym/config.yaml @@ -3,28 +3,28 @@ framework: "isaacgym" rows: 4 cols: 3 decimation: 4 -num_observations: 48 -observations: ["lin_vel", "ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] +num_observations: 45 +observations: [ "commands", "ang_vel", "gravity_vec", "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, - -100, -100, -100, - -100, -100, -100] -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] +clip_actions_lower: [ -100, -100, -100, + -100, -100, -100, + -100, -100, -100, + -100, -100, -100 ] +clip_actions_upper: [ 100, 100, 100, + 100, 100, 100, + 100, 100, 100, + 100, 100, 100 ] +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 ] hip_scale_reduction: 1.0 -hip_scale_reduction_indices: [0, 3, 6, 9] +hip_scale_reduction_indices: [ 0, 3, 6, 9 ] num_of_dofs: 12 action_scale: 0.25 @@ -33,9 +33,9 @@ ang_vel_scale: 0.25 dof_pos_scale: 1.0 dof_vel_scale: 0.05 -commands_scale: [2.0, 2.0, 0.25] +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] \ No newline at end of file +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 ] \ No newline at end of file diff --git a/descriptions/unitree/go2_description/config/legged_gym/policy.pt b/descriptions/unitree/go2_description/config/legged_gym/policy.pt index 4bdacc9..f45bf31 100644 Binary files a/descriptions/unitree/go2_description/config/legged_gym/policy.pt and b/descriptions/unitree/go2_description/config/legged_gym/policy.pt differ diff --git a/descriptions/unitree/go2_description/config/ocs2/reference.info b/descriptions/unitree/go2_description/config/ocs2/reference.info index 107dea3..ed8715c 100644 --- a/descriptions/unitree/go2_description/config/ocs2/reference.info +++ b/descriptions/unitree/go2_description/config/ocs2/reference.info @@ -5,16 +5,16 @@ comHeight 0.35 defaultJointState { - (0,0) -0.20 ; FL_hip_joint + (0,0) -0.0 ; FL_hip_joint (1,0) 0.72 ; FL_thigh_joint (2,0) -1.44 ; FL_calf_joint - (3,0) 0.20 ; FR_hip_joint + (3,0) 0.0 ; FR_hip_joint (4,0) 0.72 ; FR_thigh_joint (5,0) -1.44 ; FR_calf_joint - (6,0) -0.20 ; RL_hip_joint + (6,0) -0.0 ; RL_hip_joint (7,0) 0.72 ; RL_thigh_joint (8,0) -1.44 ; RL_calf_joint - (9,0) 0.20 ; RR_hip_joint + (9,0) 0.0 ; RR_hip_joint (10,0) 0.72 ; RR_thigh_joint (11,0) -1.44 ; RR_calf_joint } diff --git a/descriptions/unitree/go2_description/config/ocs2/task.info b/descriptions/unitree/go2_description/config/ocs2/task.info index 8acb059..6389206 100644 --- a/descriptions/unitree/go2_description/config/ocs2/task.info +++ b/descriptions/unitree/go2_description/config/ocs2/task.info @@ -85,16 +85,16 @@ initialState (11,0) 0.0 ; theta_base_x ;; Leg Joint Positions: [FL, RL, FR, RR] ;; - (12,0) -0.20 ; FL_hip_joint + (12,0) -0.0 ; FL_hip_joint (13,0) 0.72 ; FL_thigh_joint (14,0) -1.44 ; FL_calf_joint - (15,0) -0.20 ; RL_hip_joint + (15,0) -0.0 ; RL_hip_joint (16,0) 0.72 ; RL_thigh_joint (17,0) -1.44 ; RL_calf_joint - (18,0) 0.20 ; FR_hip_joint + (18,0) 0.0 ; FR_hip_joint (19,0) 0.72 ; FR_thigh_joint (20,0) -1.44 ; FR_calf_joint - (21,0) 0.20 ; RR_hip_joint + (21,0) 0.0 ; RR_hip_joint (22,0) 0.72 ; RR_thigh_joint (23,0) -1.44 ; RR_calf_joint } @@ -220,8 +220,8 @@ frictionConeTask swingLegTask { - kp 250 - kd 25 + kp 350 + kd 37 } weight diff --git a/descriptions/unitree/go2_description/config/robot_control.yaml b/descriptions/unitree/go2_description/config/robot_control.yaml index 410b45a..d94b404 100644 --- a/descriptions/unitree/go2_description/config/robot_control.yaml +++ b/descriptions/unitree/go2_description/config/robot_control.yaml @@ -27,6 +27,19 @@ imu_sensor_broadcaster: unitree_guide_controller: ros__parameters: update_rate: 200 # Hz + down_pos: + - -0.01 + - 1.27 + - -2.8 + - 0.01 + - 1.27 + - -2.8 + - -0.3 + - 1.31 + - -2.8 + - 0.3 + - 1.31 + - -2.8 joints: - FR_hip_joint - FR_thigh_joint @@ -76,8 +89,10 @@ unitree_guide_controller: ocs2_quadruped_controller: ros__parameters: - update_rate: 100 # Hz + update_rate: 500 # Hz robot_pkg: "go2_description" + feet_force_threshold: 20.0 +# default_kd: 3.5 joints: - FL_hip_joint - FL_thigh_joint @@ -136,7 +151,8 @@ rl_quadruped_controller: ros__parameters: update_rate: 200 # Hz robot_pkg: "go2_description" - model_folder: "legged_gym" +# use_rl_thread: false + model_folder: "himloco" joints: - FL_hip_joint - FL_thigh_joint @@ -165,6 +181,20 @@ rl_quadruped_controller: - 0.8000 - -1.5000 + down_pos: + - 0.01 + - 1.27 + - -2.8 + - -0.01 + - 1.27 + - -2.8 + - 0.3 + - 1.31 + - -2.8 + - -0.3 + - 1.31 + - -2.8 + command_interfaces: - effort - position diff --git a/descriptions/unitree/go2_description/xacro/ros2_control.xacro b/descriptions/unitree/go2_description/xacro/ros2_control.xacro index 72cc1ec..1540ee6 100644 --- a/descriptions/unitree/go2_description/xacro/ros2_control.xacro +++ b/descriptions/unitree/go2_description/xacro/ros2_control.xacro @@ -5,6 +5,9 @@ hardware_unitree_mujoco/HardwareUnitree + + + diff --git a/hardwares/hardware_unitree_mujoco/CMakeLists.txt b/hardwares/hardware_unitree_mujoco/CMakeLists.txt index 7ff3324..16743b6 100644 --- a/hardwares/hardware_unitree_mujoco/CMakeLists.txt +++ b/hardwares/hardware_unitree_mujoco/CMakeLists.txt @@ -74,4 +74,10 @@ endif () ament_export_dependencies(${dependencies}) ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) + ament_package() diff --git a/hardwares/hardware_unitree_mujoco/README.md b/hardwares/hardware_unitree_mujoco/README.md index fba9834..8d50c2d 100644 --- a/hardwares/hardware_unitree_mujoco/README.md +++ b/hardwares/hardware_unitree_mujoco/README.md @@ -37,5 +37,21 @@ Tested environment: Build Command: ```bash cd ~/ros2_ws -colcon build --packages-up-to hardware_unitree_mujoco +colcon build --packages-up-to hardware_unitree_mujoco --symlink-install +``` + +## 3. Config network and domain +Since the real unitree robot has different network and domain name, you need to set the network and domain name in the xacro file. +```xml + + hardware_unitree_mujoco/HardwareUnitree + 1 + lo + +``` + +After modified the config, you can tried to visualize the robot info from real robot by following command: +```bash +source ~/ros2_ws/install/setup.bash +ros2 launch hardware_unitree_mujoco visualize.launch.py ``` \ No newline at end of file diff --git a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h b/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h index 7baad49..0db2de3 100644 --- a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h +++ b/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h @@ -7,7 +7,7 @@ #define HARDWAREUNITREE_H #include "hardware_interface/system_interface.hpp" - +#include #include #include #include @@ -54,10 +54,16 @@ protected: void highStateMessageHandle(const void *messages); + void remoteWirelessHandle(const void *messages); + unitree_go::msg::dds_::LowCmd_ low_cmd_{}; // default init unitree_go::msg::dds_::LowState_ low_state_{}; // default init unitree_go::msg::dds_::SportModeState_ high_state_{}; // default init + std::string network_interface_ = "lo"; + int domain_ = 1; + bool show_foot_force_ = false; + /*publisher*/ unitree::robot::ChannelPublisherPtr low_cmd_publisher_; /*subscriber*/ @@ -65,5 +71,4 @@ protected: unitree::robot::ChannelSubscriberPtr high_state_subscriber_; }; - #endif //HARDWAREUNITREE_H diff --git a/hardwares/hardware_unitree_mujoco/launch/visualize.launch.py b/hardwares/hardware_unitree_mujoco/launch/visualize.launch.py new file mode 100644 index 0000000..cc643a3 --- /dev/null +++ b/hardwares/hardware_unitree_mujoco/launch/visualize.launch.py @@ -0,0 +1,100 @@ +import os + +import xacro +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + package_description = context.launch_configurations['pkg_description'] + pkg_path = os.path.join(get_package_share_directory(package_description)) + + xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') + robot_description = xacro.process_file(xacro_file).toxml() + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare(package_description), + "config", + "robot_control.yaml", + ] + ) + + rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz") + + rviz = Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ) + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[ + { + 'publish_frequency': 20.0, + 'use_tf_static': True, + 'robot_description': robot_description, + 'ignore_timestamp': True + } + ], + ) + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + remappings=[ + ("~/robot_description", "/robot_description"), + ], + output="both", + ) + + joint_state_publisher = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", + "--controller-manager", "/controller_manager"], + ) + + imu_sensor_broadcaster = Node( + package="controller_manager", + executable="spawner", + arguments=["imu_sensor_broadcaster", + "--controller-manager", "/controller_manager"], + ) + + return [ + rviz, + robot_state_publisher, + controller_manager, + joint_state_publisher, + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_publisher, + on_exit=[imu_sensor_broadcaster], + ) + ) + ] + + +def generate_launch_description(): + pkg_description = DeclareLaunchArgument( + 'pkg_description', + default_value='go2_description', + description='package for robot description' + ) + + return LaunchDescription([ + pkg_description, + OpaqueFunction(function=launch_setup), + ]) diff --git a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp index 302e8b4..10360b7 100644 --- a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp +++ b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp @@ -3,7 +3,6 @@ // #include "hardware_unitree_mujoco/HardwareUnitree.h" -#include #include "crc32.h" #define TOPIC_LOWCMD "rt/lowcmd" @@ -14,8 +13,10 @@ using namespace unitree::robot; using hardware_interface::return_type; rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn HardwareUnitree::on_init( - const hardware_interface::HardwareInfo &info) { - if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + const hardware_interface::HardwareInfo& info) +{ + if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { return CallbackReturn::ERROR; } @@ -33,34 +34,56 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa foot_force_.assign(4, 0); high_states_.assign(6, 0); - for (const auto &joint: info_.joints) { - for (const auto &interface: joint.state_interfaces) { + for (const auto& joint : info_.joints) + { + for (const auto& interface : joint.state_interfaces) + { joint_interfaces[interface.name].push_back(joint.name); } } - ChannelFactory::Instance()->Init(1, "lo"); + + if (const auto network_interface_param = info.hardware_parameters.find("network_interface"); network_interface_param + != info.hardware_parameters.end()) + { + network_interface_ = network_interface_param->second; + } + if (const auto domain_param = info.hardware_parameters.find("domain"); domain_param != info.hardware_parameters. + end()) + { + domain_ = std::stoi(domain_param->second); + } + if (const auto show_foot_force_param = info.hardware_parameters.find("show_foot_force"); show_foot_force_param != + info.hardware_parameters.end()) + { + show_foot_force_ = show_foot_force_param->second == "true"; + } + + RCLCPP_INFO(get_logger(), " network_interface: %s, domain: %d", network_interface_.c_str(), domain_); + ChannelFactory::Instance()->Init(domain_, network_interface_); low_cmd_publisher_ = - std::make_shared >( - TOPIC_LOWCMD); + std::make_shared>( + TOPIC_LOWCMD); low_cmd_publisher_->InitChannel(); lows_tate_subscriber_ = - std::make_shared >( - TOPIC_LOWSTATE); + std::make_shared>( + TOPIC_LOWSTATE); lows_tate_subscriber_->InitChannel( - [this](auto &&PH1) { + [this](auto&& PH1) + { lowStateMessageHandle(std::forward(PH1)); }, 1); initLowCmd(); high_state_subscriber_ = - std::make_shared >( - TOPIC_HIGHSTATE); + std::make_shared>( + TOPIC_HIGHSTATE); high_state_subscriber_->InitChannel( - [this](auto &&PH1) { + [this](auto&& PH1) + { highStateMessageHandle(std::forward(PH1)); }, 1); @@ -69,42 +92,51 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa return SystemInterface::on_init(info); } -std::vector HardwareUnitree::export_state_interfaces() { +std::vector HardwareUnitree::export_state_interfaces() +{ std::vector state_interfaces; int ind = 0; - for (const auto &joint_name: joint_interfaces["position"]) { + for (const auto& joint_name : joint_interfaces["position"]) + { state_interfaces.emplace_back(joint_name, "position", &joint_position_[ind++]); } ind = 0; - for (const auto &joint_name: joint_interfaces["velocity"]) { + for (const auto& joint_name : joint_interfaces["velocity"]) + { state_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_[ind++]); } ind = 0; - for (const auto &joint_name: joint_interfaces["effort"]) { - state_interfaces.emplace_back(joint_name, "effort", &joint_velocities_[ind++]); + for (const auto& joint_name : joint_interfaces["effort"]) + { + state_interfaces.emplace_back(joint_name, "effort", &joint_effort_[ind++]); } // export imu sensor state interface - for (uint i = 0; i < info_.sensors[0].state_interfaces.size(); i++) { + for (uint i = 0; i < info_.sensors[0].state_interfaces.size(); i++) + { state_interfaces.emplace_back( info_.sensors[0].name, info_.sensors[0].state_interfaces[i].name, &imu_states_[i]); } // export foot force sensor state interface - if (info_.sensors.size() > 1) { - for (uint i = 0; i < info_.sensors[1].state_interfaces.size(); i++) { + if (info_.sensors.size() > 1) + { + for (uint i = 0; i < info_.sensors[1].state_interfaces.size(); i++) + { state_interfaces.emplace_back( info_.sensors[1].name, info_.sensors[1].state_interfaces[i].name, &foot_force_[i]); } } // export odometer state interface - if (info_.sensors.size() > 2) { + if (info_.sensors.size() > 2) + { // export high state interface - for (uint i = 0; i < info_.sensors[2].state_interfaces.size(); i++) { + for (uint i = 0; i < info_.sensors[2].state_interfaces.size(); i++) + { state_interfaces.emplace_back( info_.sensors[2].name, info_.sensors[2].state_interfaces[i].name, &high_states_[i]); } @@ -112,24 +144,28 @@ std::vector HardwareUnitree::export_state_in return - state_interfaces; + state_interfaces; } -std::vector HardwareUnitree::export_command_interfaces() { +std::vector HardwareUnitree::export_command_interfaces() +{ std::vector command_interfaces; int ind = 0; - for (const auto &joint_name: joint_interfaces["position"]) { + for (const auto& joint_name : joint_interfaces["position"]) + { command_interfaces.emplace_back(joint_name, "position", &joint_position_command_[ind++]); } ind = 0; - for (const auto &joint_name: joint_interfaces["velocity"]) { + for (const auto& joint_name : joint_interfaces["velocity"]) + { command_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_command_[ind++]); } ind = 0; - for (const auto &joint_name: joint_interfaces["effort"]) { + for (const auto& joint_name : joint_interfaces["effort"]) + { command_interfaces.emplace_back(joint_name, "effort", &joint_torque_command_[ind]); command_interfaces.emplace_back(joint_name, "kp", &joint_kp_command_[ind]); command_interfaces.emplace_back(joint_name, "kd", &joint_kd_command_[ind]); @@ -138,9 +174,11 @@ std::vector HardwareUnitree::export_comman return command_interfaces; } -return_type HardwareUnitree::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { +return_type HardwareUnitree::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) +{ // joint states - for (int i(0); i < 12; ++i) { + for (int i(0); i < 12; ++i) + { joint_position_[i] = low_state_.motor_state()[i].q(); joint_velocities_[i] = low_state_.motor_state()[i].dq(); joint_effort_[i] = low_state_.motor_state()[i].tau_est(); @@ -164,6 +202,12 @@ return_type HardwareUnitree::read(const rclcpp::Time & /*time*/, const rclcpp::D foot_force_[2] = low_state_.foot_force()[2]; foot_force_[3] = low_state_.foot_force()[3]; + if (show_foot_force_) + { + RCLCPP_INFO(get_logger(), "foot_force(): %f, %f, %f, %f", foot_force_[0], foot_force_[1], foot_force_[2], + foot_force_[3]); + } + // high states high_states_[0] = high_state_.position()[0]; high_states_[1] = high_state_.position()[1]; @@ -178,9 +222,11 @@ return_type HardwareUnitree::read(const rclcpp::Time & /*time*/, const rclcpp::D return return_type::OK; } -return_type HardwareUnitree::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { +return_type HardwareUnitree::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) +{ // send command - for (int i(0); i < 12; ++i) { + for (int i(0); i < 12; ++i) + { low_cmd_.motor_cmd()[i].mode() = 0x01; low_cmd_.motor_cmd()[i].q() = static_cast(joint_position_command_[i]); low_cmd_.motor_cmd()[i].dq() = static_cast(joint_velocities_command_[i]); @@ -189,21 +235,23 @@ return_type HardwareUnitree::write(const rclcpp::Time & /*time*/, const rclcpp:: low_cmd_.motor_cmd()[i].tau() = static_cast(joint_torque_command_[i]); } - low_cmd_.crc() = crc32_core(reinterpret_cast(&low_cmd_), + low_cmd_.crc() = crc32_core(reinterpret_cast(&low_cmd_), (sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1); low_cmd_publisher_->Write(low_cmd_); return return_type::OK; } -void HardwareUnitree::initLowCmd() { +void HardwareUnitree::initLowCmd() +{ low_cmd_.head()[0] = 0xFE; low_cmd_.head()[1] = 0xEF; low_cmd_.level_flag() = 0xFF; low_cmd_.gpio() = 0; - for (int i = 0; i < 20; i++) { + for (int i = 0; i < 20; i++) + { low_cmd_.motor_cmd()[i].mode() = - 0x01; // motor switch to servo (PMSM) mode + 0x01; // motor switch to servo (PMSM) mode low_cmd_.motor_cmd()[i].q() = 0; low_cmd_.motor_cmd()[i].kp() = 0; low_cmd_.motor_cmd()[i].dq() = 0; @@ -212,12 +260,14 @@ void HardwareUnitree::initLowCmd() { } } -void HardwareUnitree::lowStateMessageHandle(const void *messages) { - low_state_ = *static_cast(messages); +void HardwareUnitree::lowStateMessageHandle(const void* messages) +{ + low_state_ = *static_cast(messages); } -void HardwareUnitree::highStateMessageHandle(const void *messages) { - high_state_ = *static_cast(messages); +void HardwareUnitree::highStateMessageHandle(const void* messages) +{ + high_state_ = *static_cast(messages); } #include "pluginlib/class_list_macros.hpp"