EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add real go2 test

This commit is contained in:
Huang Zhenbiao 2025-03-30 11:04:48 +08:00
parent 8175ae4b47
commit 30e1dd627b
29 changed files with 821 additions and 349 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
README.md
View File

@ -12,16 +12,11 @@ Todo List:
- [x] **[2025-02-23]** Add Gazebo Playground - [x] **[2025-02-23]** Add Gazebo Playground
- [x] OCS2 controller for Gazebo Simulation - [x] OCS2 controller for Gazebo Simulation
- [x] Refactor FSM and Unitree Guide Controller - [x] Refactor FSM and Unitree Guide Controller
- [x] **[2025-03-30]** Add Real Go2 Robot Support
- [ ] OCS2 Perceptive locomotion demo - [ ] OCS2 Perceptive locomotion demo
Video for Unitree Guide Controller: Video on Real Unitree Go2 Robot:
[![](http://i1.hdslb.com/bfs/archive/310e6208920985ac43015b2da31c01ec15e2c5f9.jpg)](https://www.bilibili.com/video/BV1aJbAeZEuo/) [![](http://i0.hdslb.com/bfs/archive/7d3856b3c5e5040f24990d3eab760cf8ba4cf80d.jpg)](https://www.bilibili.com/video/BV1QpZaY8EYV/)
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/)
## 1. Quick Start ## 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 colcon build --packages-up-to unitree_guide_controller go2_description keyboard_input --symlink-install
``` ```
### 1.1 Mujoco Simulator ### 1.1 Mujoco Simulator or Real Unitree Robot
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)
> **Warning:** CycloneDDS ROS2 RMW may conflict with unitree_sdk2. If you cannot launch unitree mujoco simulation > **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 > without `sudo`, then you cannot used `unitree_mujoco_hardware`. This conflict could be solved by one of below two
> methods: > 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 > 2. Follow the guide in [unitree_ros2](https://github.com/unitreerobotics/unitree_ros2) to configure the ROS2 RMW by
compiling cyclone dds. compiling cyclone dds.
@ -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 * [RL Quadruped Controller](controllers/rl_quadruped_controller): Reinforcement learning controller for quadruped robot
* **Simulate with more sensors** * **Simulate with more sensors**
* [Gazebo Quadruped Playground](libraries/gz_quadruped_playground): Provide gazebo simulation with lidar or depth camera. * [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 ## Reference

2
commands/keyboard_input/README.md
View File

@ -8,11 +8,13 @@ Tested environment:
* Ubuntu 22.04 * Ubuntu 22.04
* ROS2 Humble * ROS2 Humble
### Build Command
```bash ```bash
cd ~/ros2_ws cd ~/ros2_ws
colcon build --packages-up-to keyboard_input colcon build --packages-up-to keyboard_input
``` ```
### Launch Command
```bash ```bash
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash
ros2 run keyboard_input keyboard_input ros2 run keyboard_input keyboard_input

52
commands/unitree_joystick_input/CMakeLists.txt Normal file
View File

@ -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
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
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()

46
commands/unitree_joystick_input/README.md Normal file
View File

@ -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

55
commands/unitree_joystick_input/include/unitree_joystick_input/JoystickInput.h Normal file
View File

@ -0,0 +1,55 @@
//
// Created by tlab-uav on 24-9-13.
//
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <unitree/idl/go2/WirelessController_.hpp>
#include <control_input_msgs/msg/inputs.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>
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<control_input_msgs::msg::Inputs>::SharedPtr publisher_;
unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::WirelessController_> subscriber_;
};

17
commands/unitree_joystick_input/launch/joystick.launch.py Normal file
View File

@ -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',
}
],
)
])

19
commands/unitree_joystick_input/package.xml Normal file
View File

@ -0,0 +1,19 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>unitree_joystick_input</name>
<version>0.0.0</version>
<description>A package to convert joystick signal to control input</description>
<maintainer email="biao876990970@hotmail.com">Huang Zhenbiao</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>control_input_msgs</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

97
commands/unitree_joystick_input/src/JoystickInput.cpp Normal file
View File

@ -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_msgs::msg::Inputs>("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<ChannelSubscriber<unitree_go::msg::dds_::WirelessController_>>(
TOPIC_JOYSTICK);
subscriber_->InitChannel(
[this](auto&& PH1)
{
remoteWirelessHandle(std::forward<decltype(PH1)>(PH1));
},
1);
}
void JoystickInput::remoteWirelessHandle(const void* messages)
{
wireless_controller_ = *static_cast<const unitree_go::msg::dds_::WirelessController_*>(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<JoystickInput>();
spin(node);
rclcpp::shutdown();
return 0;
}

7
controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h
View File

@ -132,7 +132,7 @@ class StateRL final : public FSMState
{ {
public: public:
explicit StateRL(CtrlInterfaces& ctrl_interfaces, explicit StateRL(CtrlInterfaces& ctrl_interfaces,
CtrlComponent& ctrl_component, const std::string& config_path, CtrlComponent& ctrl_component,
const std::vector<double>& target_pos); const std::vector<double>& target_pos);
void enter() override; void enter() override;
@ -163,6 +163,10 @@ private:
void setCommand() const; void setCommand() const;
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
std::string robot_pkg_ = "go2_description";
std::string model_folder_ = "legged_gym";
bool enable_estimator_; bool enable_estimator_;
std::shared_ptr<Estimator>& estimator_; std::shared_ptr<Estimator>& estimator_;
@ -181,6 +185,7 @@ private:
// rl module // rl module
torch::jit::script::Module model_; torch::jit::script::Module model_;
bool use_rl_thread_ = true;
std::thread rl_thread_; std::thread rl_thread_;
bool running_ = false; bool running_ = false;
bool updated_ = false; bool updated_ = false;

2
controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h
View File

@ -6,12 +6,14 @@
#define CtrlComponent_H #define CtrlComponent_H
#include "Estimator.h" #include "Estimator.h"
#include <rclcpp_lifecycle/lifecycle_node.hpp>
struct CtrlComponent { struct CtrlComponent {
bool enable_estimator_ = false; bool enable_estimator_ = false;
std::shared_ptr<QuadrupedRobot> robot_model_; std::shared_ptr<QuadrupedRobot> robot_model_;
std::shared_ptr<Estimator> estimator_; std::shared_ptr<Estimator> estimator_;
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
CtrlComponent() = default; CtrlComponent() = default;
}; };

5
controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h
View File

@ -16,7 +16,7 @@ struct CtrlComponent;
class Estimator { class Estimator {
public: public:
explicit Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component); explicit Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component, int feet_force_threshold = 1);
~Estimator() = default; ~Estimator() = default;
@ -103,9 +103,12 @@ public:
void update(); void update();
bool safety() const;
private: private:
CtrlInterfaces &ctrl_interfaces_; CtrlInterfaces &ctrl_interfaces_;
std::shared_ptr<QuadrupedRobot> &robot_model_; std::shared_ptr<QuadrupedRobot> &robot_model_;
double feet_force_threshold_;
Eigen::Matrix<double, 18, 1> x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4) Eigen::Matrix<double, 18, 1> x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4)

121
controllers/rl_quadruped_controller/src/FSM/StateRL.cpp
View File

@ -3,7 +3,7 @@
// //
#include "rl_quadruped_controller/FSM/StateRL.h" #include "rl_quadruped_controller/FSM/StateRL.h"
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <rclcpp/logging.hpp> #include <rclcpp/logging.hpp>
#include <yaml-cpp/yaml.h> #include <yaml-cpp/yaml.h>
@ -48,29 +48,40 @@ std::vector<T> ReadVectorFromYaml(const YAML::Node& node, const std::string& fra
} }
StateRL::StateRL(CtrlInterfaces& ctrl_interfaces, StateRL::StateRL(CtrlInterfaces& ctrl_interfaces,
CtrlComponent& ctrl_component, const std::string& config_path, CtrlComponent& ctrl_component,
const std::vector<double>& target_pos) : FSMState( const std::vector<double>& target_pos) :
FSMStateName::RL, "rl", ctrl_interfaces), FSMState(FSMStateName::RL, "rl", ctrl_interfaces),
estimator_(ctrl_component.estimator_), node_(ctrl_component.node_),
enable_estimator_(ctrl_component.enable_estimator_) 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++) for (int i = 0; i < 12; i++)
{ {
init_pos_[i] = target_pos[i]; init_pos_[i] = target_pos[i];
} }
// read params from yaml // read params from yaml
loadYaml(config_path); loadYaml(model_path);
// history
if (!params_.observations_history.empty()) if (!params_.observations_history.empty())
{ {
history_obs_buf_ = std::make_shared<ObservationBuffer>(1, params_.num_observations, history_obs_buf_ = std::make_shared<ObservationBuffer>(1, params_.num_observations,
params_.observations_history.size()); params_.observations_history.size());
} }
std::cout << "Model loading: " << config_path + "/" + params_.model_name << std::endl; RCLCPP_INFO(node_->get_logger(), "Model loading: %s", params_.model_name.c_str());
model_ = torch::jit::load(config_path + "/" + params_.model_name); model_ = torch::jit::load(model_path + "/" + params_.model_name);
// for (const auto &param: model_.parameters()) { // for (const auto &param: model_.parameters()) {
@ -78,30 +89,32 @@ StateRL::StateRL(CtrlInterfaces& ctrl_interfaces,
// } // }
rl_thread_ = std::thread([&] if (use_rl_thread_)
{ {
while (true) rl_thread_ = std::thread([&]{
{ while (true)
try
{ {
executeAndSleep( try
[&] {
{ executeAndSleep(
if (running_) [&]
{ {
runModel(); if (running_)
} {
}, runModel();
ctrl_interfaces_.frequency_ / params_.decimation); }
},
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) });
{ setThreadPriority(60, rl_thread_);
running_ = false; }
RCLCPP_ERROR(rclcpp::get_logger("StateRL"), "Error in RL thread: %s", e.what());
}
}
});
setThreadPriority(50, rl_thread_);
} }
void StateRL::enter() void StateRL::enter()
@ -125,12 +138,19 @@ void StateRL::enter()
control_.y = 0.0; control_.y = 0.0;
control_.yaw = 0.0; control_.yaw = 0.0;
// history
history_obs_buf_->clear();
running_ = true; running_ = true;
} }
void StateRL::run(const rclcpp::Time&/*time*/, const rclcpp::Duration&/*period*/) void StateRL::run(const rclcpp::Time&/*time*/, const rclcpp::Duration&/*period*/)
{ {
getState(); getState();
if (!use_rl_thread_)
{
runModel();
}
setCommand(); setCommand();
} }
@ -141,6 +161,10 @@ void StateRL::exit()
FSMStateName StateRL::checkChange() FSMStateName StateRL::checkChange()
{ {
if (!estimator_->safety())
{
return FSMStateName::PASSIVE;
}
switch (ctrl_interfaces_.control_inputs_.command) switch (ctrl_interfaces_.control_inputs_.command)
{ {
case 1: case 1:
@ -314,32 +338,35 @@ void StateRL::getState()
{ {
if (params_.framework == "isaacgym") if (params_.framework == "isaacgym")
{ {
robot_state_.imu.quaternion[3] = ctrl_interfaces_.imu_state_interface_[0].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_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_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_value(); robot_state_.imu.quaternion[2] = ctrl_interfaces_.imu_state_interface_[3].get().get_optional().value();
} }
else if (params_.framework == "isaacsim") else if (params_.framework == "isaacsim")
{ {
robot_state_.imu.quaternion[0] = ctrl_interfaces_.imu_state_interface_[0].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_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_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_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[0] = ctrl_interfaces_.imu_state_interface_[4].get().get_optional().value();
robot_state_.imu.gyroscope[1] = ctrl_interfaces_.imu_state_interface_[5].get().get_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_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[0] = ctrl_interfaces_.imu_state_interface_[7].get().get_optional().value();
robot_state_.imu.accelerometer[1] = ctrl_interfaces_.imu_state_interface_[8].get().get_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_value(); robot_state_.imu.accelerometer[2] = ctrl_interfaces_.imu_state_interface_[9].get().get_optional().value();
for (int i = 0; i < 12; i++) 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.q[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_optional().
robot_state_.motor_state.dq[i] = ctrl_interfaces_.joint_velocity_state_interface_[i].get().get_value(); value();
robot_state_.motor_state.tauEst[i] = ctrl_interfaces_.joint_effort_state_interface_[i].get().get_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; control_.x = ctrl_interfaces_.control_inputs_.ly;

177
controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp
View File

@ -3,20 +3,26 @@
// //
#include "RlQuadrupedController.h" #include "RlQuadrupedController.h"
#include <ament_index_cpp/get_package_share_directory.hpp>
namespace rl_quadruped_controller { namespace rl_quadruped_controller
{
using config_type = controller_interface::interface_configuration_type; 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, {}}; controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
conf.names.reserve(joint_names_.size() * command_interface_types_.size()); conf.names.reserve(joint_names_.size() * command_interface_types_.size());
for (const auto &joint_name: joint_names_) { for (const auto& joint_name : joint_names_)
for (const auto &interface_type: command_interface_types_) { {
if (!command_prefix_.empty()) { for (const auto& interface_type : command_interface_types_)
{
if (!command_prefix_.empty())
{
conf.names.push_back(command_prefix_ + "/" + joint_name + "/" += interface_type); conf.names.push_back(command_prefix_ + "/" + joint_name + "/" += interface_type);
} else { }
else
{
conf.names.push_back(joint_name + "/" += interface_type); conf.names.push_back(joint_name + "/" += interface_type);
} }
} }
@ -25,21 +31,26 @@ namespace rl_quadruped_controller {
return conf; 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, {}}; controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
conf.names.reserve(joint_names_.size() * state_interface_types_.size()); conf.names.reserve(joint_names_.size() * state_interface_types_.size());
for (const auto &joint_name: joint_names_) { for (const auto& joint_name : joint_names_)
for (const auto &interface_type: state_interface_types_) { {
for (const auto& interface_type : state_interface_types_)
{
conf.names.push_back(joint_name + "/" += interface_type); 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); 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); conf.names.push_back(foot_force_name_ + "/" += interface_type);
} }
@ -47,9 +58,12 @@ namespace rl_quadruped_controller {
} }
controller_interface::return_type LeggedGymController:: controller_interface::return_type LeggedGymController::
update(const rclcpp::Time & time, const rclcpp::Duration & period) { update(const rclcpp::Time& time, const rclcpp::Duration& period)
if (ctrl_component_.enable_estimator_) { {
if (ctrl_component_.robot_model_ == nullptr) { if (ctrl_component_.enable_estimator_)
{
if (ctrl_component_.robot_model_ == nullptr)
{
return controller_interface::return_type::OK; return controller_interface::return_type::OK;
} }
@ -57,16 +71,20 @@ namespace rl_quadruped_controller {
ctrl_component_.estimator_->update(); ctrl_component_.estimator_->update();
} }
if (mode_ == FSMMode::NORMAL) { if (mode_ == FSMMode::NORMAL)
{
current_state_->run(time, period); current_state_->run(time, period);
next_state_name_ = current_state_->checkChange(); 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; mode_ = FSMMode::CHANGE;
next_state_ = getNextState(next_state_name_); next_state_ = getNextState(next_state_name_);
RCLCPP_INFO(get_node()->get_logger(), "Switched from %s to %s", 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()); 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_->exit();
current_state_ = next_state_; current_state_ = next_state_;
@ -77,46 +95,49 @@ namespace rl_quadruped_controller {
return controller_interface::return_type::OK; return controller_interface::return_type::OK;
} }
controller_interface::CallbackReturn LeggedGymController::on_init() { controller_interface::CallbackReturn LeggedGymController::on_init()
try { {
joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_); try
feet_names_ = auto_declare<std::vector<std::string> >("feet_names", feet_names_); {
joint_names_ = auto_declare<std::vector<std::string>>("joints", joint_names_);
feet_names_ = auto_declare<std::vector<std::string>>("feet_names", feet_names_);
command_interface_types_ = command_interface_types_ =
auto_declare<std::vector<std::string> >("command_interfaces", command_interface_types_); auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
state_interface_types_ = state_interface_types_ =
auto_declare<std::vector<std::string> >("state_interfaces", state_interface_types_); auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_); command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
base_name_ = auto_declare<std::string>("base_name", base_name_); base_name_ = auto_declare<std::string>("base_name", base_name_);
// imu sensor // imu sensor
imu_name_ = auto_declare<std::string>("imu_name", imu_name_); imu_name_ = auto_declare<std::string>("imu_name", imu_name_);
imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_); imu_interface_types_ = auto_declare<std::vector<std::string>>("imu_interfaces", state_interface_types_);
// foot_force_sensor // foot_force_sensor
foot_force_name_ = auto_declare<std::string>("foot_force_name", foot_force_name_); foot_force_name_ = auto_declare<std::string>("foot_force_name", foot_force_name_);
foot_force_interface_types_ = foot_force_interface_types_ =
auto_declare<std::vector<std::string> >("foot_force_interfaces", foot_force_interface_types_); auto_declare<std::vector<std::string>>("foot_force_interfaces", foot_force_interface_types_);
feet_force_threshold_ = auto_declare<double>("feet_force_threshold", feet_force_threshold_);
// rl config folder
robot_pkg_ = auto_declare<std::string>("robot_pkg", robot_pkg_);
model_folder_ = auto_declare<std::string>("model_folder", model_folder_);
// pose parameters // pose parameters
down_pos_ = auto_declare<std::vector<double> >("down_pos", down_pos_); down_pos_ = auto_declare<std::vector<double>>("down_pos", down_pos_);
stand_pos_ = auto_declare<std::vector<double> >("stand_pos", stand_pos_); stand_pos_ = auto_declare<std::vector<double>>("stand_pos", stand_pos_);
stand_kp_ = auto_declare<double>("stand_kp", stand_kp_); stand_kp_ = auto_declare<double>("stand_kp", stand_kp_);
stand_kd_ = auto_declare<double>("stand_kd", stand_kd_); stand_kd_ = auto_declare<double>("stand_kd", stand_kd_);
get_node()->get_parameter("update_rate", ctrl_interfaces_.frequency_); get_node()->get_parameter("update_rate", ctrl_interfaces_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", 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"); RCLCPP_INFO(get_node()->get_logger(), "Enable Estimator");
ctrl_component_.enable_estimator_ = true; ctrl_component_.enable_estimator_ = true;
ctrl_component_.estimator_ = std::make_shared<Estimator>(ctrl_interfaces_, ctrl_component_); ctrl_component_.estimator_ = std::make_shared<Estimator>(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()); fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
return controller_interface::CallbackReturn::ERROR; return controller_interface::CallbackReturn::ERROR;
} }
@ -125,11 +146,14 @@ namespace rl_quadruped_controller {
} }
controller_interface::CallbackReturn LeggedGymController::on_configure( 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<std_msgs::msg::String>( robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(
"/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(), "/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
[this](const std_msgs::msg::String::SharedPtr msg) { [this](const std_msgs::msg::String::SharedPtr msg)
if (ctrl_component_.enable_estimator_) { {
if (ctrl_component_.enable_estimator_)
{
ctrl_component_.robot_model_ = std::make_shared<QuadrupedRobot>( ctrl_component_.robot_model_ = std::make_shared<QuadrupedRobot>(
ctrl_interfaces_, msg->data, feet_names_, base_name_); 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_msgs::msg::Inputs>( control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
"/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 // Handle message
ctrl_interfaces_.control_inputs_.command = msg->command; ctrl_interfaces_.control_inputs_.command = msg->command;
ctrl_interfaces_.control_inputs_.lx = msg->lx; ctrl_interfaces_.control_inputs_.lx = msg->lx;
@ -150,27 +175,38 @@ namespace rl_quadruped_controller {
} }
controller_interface::CallbackReturn LeggedGymController::on_activate( 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 // clear out vectors in case of restart
ctrl_interfaces_.clear(); ctrl_interfaces_.clear();
// assign command interfaces // assign command interfaces
for (auto &interface: command_interfaces_) { for (auto& interface : command_interfaces_)
{
std::string interface_name = interface.get_interface_name(); 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); command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
} else { }
else
{
command_interface_map_[interface_name]->push_back(interface); command_interface_map_[interface_name]->push_back(interface);
} }
} }
// assign state interfaces // assign state interfaces
for (auto &interface: state_interfaces_) { for (auto& interface : state_interfaces_)
if (interface.get_prefix_name() == imu_name_) { {
if (interface.get_prefix_name() == imu_name_)
{
ctrl_interfaces_.imu_state_interface_.emplace_back(interface); 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); ctrl_interfaces_.foot_force_state_interface_.emplace_back(interface);
} else { }
else
{
state_interface_map_[interface.get_interface_name()]->push_back(interface); state_interface_map_[interface.get_interface_name()]->push_back(interface);
} }
} }
@ -179,12 +215,7 @@ namespace rl_quadruped_controller {
state_list_.passive = std::make_shared<StatePassive>(ctrl_interfaces_); state_list_.passive = std::make_shared<StatePassive>(ctrl_interfaces_);
state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_interfaces_, down_pos_, stand_kp_, stand_kd_); state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_interfaces_, down_pos_, stand_kp_, stand_kd_);
state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_interfaces_, stand_pos_, stand_kp_, stand_kd_); state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_interfaces_, stand_pos_, stand_kp_, stand_kd_);
state_list_.rl = std::make_shared<StateRL>(ctrl_interfaces_, ctrl_component_, stand_pos_);
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<StateRL>(ctrl_interfaces_, ctrl_component_, model_path, stand_pos_);
// Initialize FSM // Initialize FSM
current_state_ = state_list_.passive; current_state_ = state_list_.passive;
@ -197,39 +228,45 @@ namespace rl_quadruped_controller {
} }
controller_interface::CallbackReturn LeggedGymController::on_deactivate( controller_interface::CallbackReturn LeggedGymController::on_deactivate(
const rclcpp_lifecycle::State & /*previous_state*/) { const rclcpp_lifecycle::State& /*previous_state*/)
{
release_interfaces(); release_interfaces();
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
controller_interface::CallbackReturn 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); return ControllerInterface::on_cleanup(previous_state);
} }
controller_interface::CallbackReturn 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); 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); return ControllerInterface::on_error(previous_state);
} }
std::shared_ptr<FSMState> LeggedGymController::getNextState(const FSMStateName stateName) const { std::shared_ptr<FSMState> LeggedGymController::getNextState(const FSMStateName stateName) const
switch (stateName) { {
case FSMStateName::INVALID: switch (stateName)
return state_list_.invalid; {
case FSMStateName::PASSIVE: case FSMStateName::INVALID:
return state_list_.passive; return state_list_.invalid;
case FSMStateName::FIXEDDOWN: case FSMStateName::PASSIVE:
return state_list_.fixedDown; return state_list_.passive;
case FSMStateName::FIXEDSTAND: case FSMStateName::FIXEDDOWN:
return state_list_.fixedStand; return state_list_.fixedDown;
case FSMStateName::RL: case FSMStateName::FIXEDSTAND:
return state_list_.rl; return state_list_.fixedStand;
default: case FSMStateName::RL:
return state_list_.invalid; return state_list_.rl;
default:
return state_list_.invalid;
} }
} }
} }

33
controllers/rl_quadruped_controller/src/RlQuadrupedController.h
View File

@ -13,8 +13,10 @@
#include "rl_quadruped_controller/FSM/StateFixedStand.h" #include "rl_quadruped_controller/FSM/StateFixedStand.h"
#include "controller_common//FSM/StatePassive.h" #include "controller_common//FSM/StatePassive.h"
namespace rl_quadruped_controller { namespace rl_quadruped_controller
struct FSMStateList { {
struct FSMStateList
{
std::shared_ptr<FSMState> invalid; std::shared_ptr<FSMState> invalid;
std::shared_ptr<StatePassive> passive; std::shared_ptr<StatePassive> passive;
std::shared_ptr<StateFixedDown> fixedDown; std::shared_ptr<StateFixedDown> fixedDown;
@ -22,7 +24,8 @@ namespace rl_quadruped_controller {
std::shared_ptr<StateRL> rl; std::shared_ptr<StateRL> rl;
}; };
class LeggedGymController final : public controller_interface::ControllerInterface { class LeggedGymController final : public controller_interface::ControllerInterface
{
public: public:
LeggedGymController() = default; LeggedGymController() = default;
@ -31,27 +34,27 @@ namespace rl_quadruped_controller {
controller_interface::InterfaceConfiguration state_interface_configuration() const override; controller_interface::InterfaceConfiguration state_interface_configuration() const override;
controller_interface::return_type update( 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_init() override;
controller_interface::CallbackReturn on_configure( controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State &previous_state) override; const rclcpp_lifecycle::State& previous_state) override;
controller_interface::CallbackReturn on_activate( controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State &previous_state) override; const rclcpp_lifecycle::State& previous_state) override;
controller_interface::CallbackReturn on_deactivate( controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State &previous_state) override; const rclcpp_lifecycle::State& previous_state) override;
controller_interface::CallbackReturn on_cleanup( controller_interface::CallbackReturn on_cleanup(
const rclcpp_lifecycle::State &previous_state) override; const rclcpp_lifecycle::State& previous_state) override;
controller_interface::CallbackReturn on_shutdown( controller_interface::CallbackReturn on_shutdown(
const rclcpp_lifecycle::State &previous_state) override; const rclcpp_lifecycle::State& previous_state) override;
controller_interface::CallbackReturn on_error( controller_interface::CallbackReturn on_error(
const rclcpp_lifecycle::State &previous_state) override; const rclcpp_lifecycle::State& previous_state) override;
private: private:
std::shared_ptr<FSMState> getNextState(FSMStateName stateName) const; std::shared_ptr<FSMState> getNextState(FSMStateName stateName) const;
@ -90,12 +93,10 @@ namespace rl_quadruped_controller {
double stand_kp_ = 80.0; double stand_kp_ = 80.0;
double stand_kd_ = 3.5; double stand_kd_ = 3.5;
double feet_force_threshold_ = 20.0;
std::string robot_pkg_;
std::string model_folder_;
std::unordered_map< std::unordered_map<
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *> std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>*>
command_interface_map_ = { command_interface_map_ = {
{"effort", &ctrl_interfaces_.joint_torque_command_interface_}, {"effort", &ctrl_interfaces_.joint_torque_command_interface_},
{"position", &ctrl_interfaces_.joint_position_command_interface_}, {"position", &ctrl_interfaces_.joint_position_command_interface_},
@ -105,7 +106,7 @@ namespace rl_quadruped_controller {
}; };
std::unordered_map< std::unordered_map<
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *> std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>*>
state_interface_map_ = { state_interface_map_ = {
{"position", &ctrl_interfaces_.joint_position_state_interface_}, {"position", &ctrl_interfaces_.joint_position_state_interface_},
{"effort", &ctrl_interfaces_.joint_effort_state_interface_}, {"effort", &ctrl_interfaces_.joint_effort_state_interface_},

9
controllers/rl_quadruped_controller/src/common/ObservationBuffer.cpp
View File

@ -10,8 +10,8 @@ ObservationBuffer::ObservationBuffer(int num_envs,
: num_envs_(num_envs), : num_envs_(num_envs),
num_obs_(num_obs), num_obs_(num_obs),
include_history_steps_(include_history_steps) { include_history_steps_(include_history_steps) {
num_obs_total_ = num_obs * include_history_steps; num_obs_total_ = num_obs_ * include_history_steps_;
obs_buffer_ = torch::zeros({num_envs, num_obs_total_}, dtype(torch::kFloat32)); obs_buffer_ = torch::zeros({num_envs_, num_obs_total_}, dtype(torch::kFloat32));
} }
void ObservationBuffer::reset(const std::vector<int> &reset_index, const torch::Tensor &new_obs) { void ObservationBuffer::reset(const std::vector<int> &reset_index, const torch::Tensor &new_obs) {
@ -22,6 +22,11 @@ void ObservationBuffer::reset(const std::vector<int> &reset_index, const torch::
obs_buffer_.index_put_(indices, new_obs.repeat({1, include_history_steps_})); 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) { void ObservationBuffer::insert(const torch::Tensor &new_obs) {
// Shift observations back. // Shift observations back.
const torch::Tensor shifted_obs = obs_buffer_.index({ const torch::Tensor shifted_obs = obs_buffer_.index({

11
controllers/rl_quadruped_controller/src/common/ObservationBuffer.h
View File

@ -8,17 +8,20 @@
#include <torch/torch.h> #include <torch/torch.h>
#include <vector> #include <vector>
class ObservationBuffer { class ObservationBuffer
{
public: public:
ObservationBuffer(int num_envs, int num_obs, int include_history_steps); ObservationBuffer(int num_envs, int num_obs, int include_history_steps);
~ObservationBuffer() = default; ~ObservationBuffer() = default;
void reset(const std::vector<int>& reset_index, const torch::Tensor &new_obs); void reset(const std::vector<int>& reset_index, const torch::Tensor& new_obs);
void insert(const torch::Tensor &new_obs); void clear();
[[nodiscard]] torch::Tensor getObsVec(const std::vector<int> &obs_ids) const; void insert(const torch::Tensor& new_obs);
[[nodiscard]] torch::Tensor getObsVec(const std::vector<int>& obs_ids) const;
private: private:
int num_envs_; int num_envs_;

230
controllers/rl_quadruped_controller/src/control/Estimator.cpp
View File

@ -9,14 +9,18 @@
#include "rl_quadruped_controller/control/CtrlComponent.h" #include "rl_quadruped_controller/control/CtrlComponent.h"
Estimator::Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component) : ctrl_interfaces_(ctrl_interfaces), Estimator::Estimator(CtrlInterfaces& ctrl_interfaces, CtrlComponent& ctrl_component, int feet_force_threshold) :
robot_model_(ctrl_component.robot_model_){ ctrl_interfaces_(ctrl_interfaces),
robot_model_(ctrl_component.robot_model_),
feet_force_threshold_(feet_force_threshold)
{
g_ << 0, 0, -9.81; g_ << 0, 0, -9.81;
dt_ = 1.0 / ctrl_interfaces_.frequency_; dt_ = 1.0 / ctrl_interfaces_.frequency_;
std::cout << "dt: " << dt_ << std::endl; std::cout << "dt: " << dt_ << std::endl;
large_variance_ = 100; 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; Qdig(i) = i < 6 ? 0.0003 : 0.01;
} }
@ -51,88 +55,88 @@ Estimator::Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_compon
P = large_variance_ * P; P = large_variance_ * P;
RInit_ << 0.008, 0.012, -0.000, -0.009, 0.012, 0.000, 0.009, -0.009, -0.000, 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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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.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, -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, 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, 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, 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, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 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, 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, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 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, 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, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 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, 1.0;
Cu << 268.573, -43.819, -147.211, -43.819, 92.949, 58.082, -147.211, 58.082, 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_ = Qdig.asDiagonal();
QInit_ += B * Cu * B.transpose(); QInit_ += B * Cu * B.transpose();
@ -143,11 +147,13 @@ Estimator::Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_compon
// low_pass_filters_[2] = std::make_shared<LowPassFilter>(dt_, 3.0); // low_pass_filters_[2] = std::make_shared<LowPassFilter>(dt_, 3.0);
} }
double Estimator::getYaw() const { double Estimator::getYaw() const
{
return rotMatToRPY(rotation_)(2); return rotMatToRPY(rotation_)(2);
} }
void Estimator::update() { void Estimator::update()
{
if (robot_model_->mass_ == 0) return; if (robot_model_->mass_ == 0) return;
Q = QInit_; Q = QInit_;
@ -158,42 +164,46 @@ void Estimator::update() {
feet_h_.setZero(); feet_h_.setZero();
// Adjust the covariance based on foot contact and phase. // Adjust the covariance based on foot contact and phase.
for (int i(0); i < 4; ++i) { for (int i(0); i < 4; ++i)
if (ctrl_interfaces_.foot_force_state_interface_[i].get().get_value() < 1) { {
if (ctrl_interfaces_.foot_force_state_interface_[i].get().get_optional().value() < feet_force_threshold_)
{
// foot not contact // foot not contact
Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3); 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.block(12 + 3 * i, 12 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3);
R(24 + i, 24 + i) = large_variance_; R(24 + i, 24 + i) = large_variance_;
} else { }
else
{
// foot contact // foot contact
const double trust = windowFunc(0.5, 0.2); const double trust = windowFunc(0.5, 0.2);
Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) =
(1 + (1 - trust) * large_variance_) * (1 + (1 - trust) * large_variance_) *
QInit_.block(6 + 3 * i, 6 + 3 * i, 3, 3); QInit_.block(6 + 3 * i, 6 + 3 * i, 3, 3);
R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = R.block(12 + 3 * i, 12 + 3 * i, 3, 3) =
(1 + (1 - trust) * large_variance_) * (1 + (1 - trust) * large_variance_) *
RInit_.block(12 + 3 * i, 12 + 3 * i, 3, 3); RInit_.block(12 + 3 * i, 12 + 3 * i, 3, 3);
R(24 + i, 24 + i) = 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_pos_body_.segment(3 * i, 3) = Vec3(foot_poses_[i].p.data);
feet_vel_body_.segment(3 * i, 3) = Vec3(foot_vels_[i].data); feet_vel_body_.segment(3 * i, 3) = Vec3(foot_vels_[i].data);
} }
Quat quat; Quat quat;
quat << ctrl_interfaces_.imu_state_interface_[0].get().get_value(), quat << ctrl_interfaces_.imu_state_interface_[0].get().get_optional().value(),
ctrl_interfaces_.imu_state_interface_[1].get().get_value(), ctrl_interfaces_.imu_state_interface_[1].get().get_optional().value(),
ctrl_interfaces_.imu_state_interface_[2].get().get_value(), ctrl_interfaces_.imu_state_interface_[2].get().get_optional().value(),
ctrl_interfaces_.imu_state_interface_[3].get().get_value(); ctrl_interfaces_.imu_state_interface_[3].get().get_optional().value();
rotation_ = quatToRotMat(quat); rotation_ = quatToRotMat(quat);
gyro_ <<ctrl_interfaces_.imu_state_interface_[4].get().get_value(), gyro_ << ctrl_interfaces_.imu_state_interface_[4].get().get_optional().value(),
ctrl_interfaces_.imu_state_interface_[5].get().get_value(), ctrl_interfaces_.imu_state_interface_[5].get().get_optional().value(),
ctrl_interfaces_.imu_state_interface_[6].get().get_value(); ctrl_interfaces_.imu_state_interface_[6].get().get_optional().value();
acceleration_ <<ctrl_interfaces_.imu_state_interface_[7].get().get_value(), acceleration_ << ctrl_interfaces_.imu_state_interface_[7].get().get_optional().value(),
ctrl_interfaces_.imu_state_interface_[8].get().get_value(), ctrl_interfaces_.imu_state_interface_[8].get().get_optional().value(),
ctrl_interfaces_.imu_state_interface_[9].get().get_value(); ctrl_interfaces_.imu_state_interface_[9].get().get_optional().value();
u_ = rotation_ * acceleration_ + g_; u_ = rotation_ * acceleration_ + g_;
x_hat_ = A * x_hat_ + B * u_; x_hat_ = A * x_hat_ + B * u_;
@ -225,3 +235,17 @@ void Estimator::update() {
// x_hat_(4) = low_pass_filters_[1]->getValue(); // x_hat_(4) = low_pass_filters_[1]->getValue();
// x_hat_(5) = low_pass_filters_[2]->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;
}

15
controllers/rl_quadruped_controller/src/control/LowPassFilter.cpp
View File

@ -9,23 +9,28 @@
* @param samplePeriod sample period * @param samplePeriod sample period
* @param cutFrequency cut frequency * @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)); weight_ = 1.0 / (1.0 + 1.0 / (2.0 * M_PI * samplePeriod * cutFrequency));
start_ = false; start_ = false;
} }
void LowPassFilter::addValue(const double newValue) { void LowPassFilter::addValue(const double newValue)
if (!start_) { {
if (!start_)
{
start_ = true; start_ = true;
pass_value_ = newValue; pass_value_ = newValue;
} }
pass_value_ = weight_ * newValue + (1 - weight_) * pass_value_; pass_value_ = weight_ * newValue + (1 - weight_) * pass_value_;
} }
double LowPassFilter::getValue() const { double LowPassFilter::getValue() const
{
return pass_value_; return pass_value_;
} }
void LowPassFilter::clear() { void LowPassFilter::clear()
{
start_ = false; start_ = false;
} }

8
descriptions/unitree/go2_description/README.md
View File

@ -29,8 +29,12 @@ ros2 launch go2_description visualize.launch.py
## 3. Launch ROS2 Control ## 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 * Unitree Guide Controller
```bash ```bash
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash

0
descriptions/unitree/go2_description/config/legged_gym/config_himloco.yaml → descriptions/unitree/go2_description/config/himloco/config.yaml
View File

0
descriptions/unitree/go2_description/config/legged_gym/himloco.pt → descriptions/unitree/go2_description/config/himloco/himloco.pt
View File

50
descriptions/unitree/go2_description/config/legged_gym/config.yaml
View File

@ -2,29 +2,29 @@ model_name: "policy.pt"
framework: "isaacgym" framework: "isaacgym"
rows: 4 rows: 4
cols: 3 cols: 3
decimation: 4 decimation: 1
num_observations: 48 num_observations: 45
observations: ["lin_vel", "ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] observations: [ "commands", "ang_vel", "gravity_vec", "dof_pos", "dof_vel", "actions" ]
#observations_history: [6, 5, 4, 3, 2, 1, 0] #observations_history: [6, 5, 4, 3, 2, 1, 0]
clip_obs: 100.0 clip_obs: 100.0
clip_actions_lower: [-100, -100, -100, clip_actions_lower: [ -100, -100, -100,
-100, -100, -100, -100, -100, -100,
-100, -100, -100, -100, -100, -100,
-100, -100, -100] -100, -100, -100 ]
clip_actions_upper: [100, 100, 100, clip_actions_upper: [ 100, 100, 100,
100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100,
100, 100, 100] 100, 100, 100 ]
rl_kp: [40, 40, 40, rl_kp: [ 20, 20, 20,
40, 40, 40, 20, 20, 20,
40, 40, 40, 20, 20, 20,
40, 40, 40] 20, 20, 20 ]
rl_kd: [1, 1, 1, rl_kd: [ 0.5, 0.5, 0.5,
1, 1, 1, 0.5, 0.5, 0.5,
1, 1, 1, 0.5, 0.5, 0.5,
1, 1, 1] 0.5, 0.5, 0.5 ]
hip_scale_reduction: 1.0 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 num_of_dofs: 12
action_scale: 0.25 action_scale: 0.25
@ -33,9 +33,9 @@ ang_vel_scale: 0.25
dof_pos_scale: 1.0 dof_pos_scale: 1.0
dof_vel_scale: 0.05 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, 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, 33.5, 33.5, 33.5,
33.5, 33.5, 33.5] 33.5, 33.5, 33.5 ]

BIN
descriptions/unitree/go2_description/config/legged_gym/policy.pt
View File

Binary file not shown.

8
descriptions/unitree/go2_description/config/ocs2/reference.info
View File

@ -5,16 +5,16 @@ comHeight 0.35
defaultJointState defaultJointState
{ {
(0,0) -0.20 ; FL_hip_joint (0,0) -0.0 ; FL_hip_joint
(1,0) 0.72 ; FL_thigh_joint (1,0) 0.72 ; FL_thigh_joint
(2,0) -1.44 ; FL_calf_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 (4,0) 0.72 ; FR_thigh_joint
(5,0) -1.44 ; FR_calf_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 (7,0) 0.72 ; RL_thigh_joint
(8,0) -1.44 ; RL_calf_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 (10,0) 0.72 ; RR_thigh_joint
(11,0) -1.44 ; RR_calf_joint (11,0) -1.44 ; RR_calf_joint
} }

12
descriptions/unitree/go2_description/config/ocs2/task.info
View File

@ -85,16 +85,16 @@ initialState
(11,0) 0.0 ; theta_base_x (11,0) 0.0 ; theta_base_x
;; Leg Joint Positions: [FL, RL, FR, RR] ;; ;; 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 (13,0) 0.72 ; FL_thigh_joint
(14,0) -1.44 ; FL_calf_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 (16,0) 0.72 ; RL_thigh_joint
(17,0) -1.44 ; RL_calf_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 (19,0) 0.72 ; FR_thigh_joint
(20,0) -1.44 ; FR_calf_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 (22,0) 0.72 ; RR_thigh_joint
(23,0) -1.44 ; RR_calf_joint (23,0) -1.44 ; RR_calf_joint
} }
@ -220,8 +220,8 @@ frictionConeTask
swingLegTask swingLegTask
{ {
kp 250 kp 350
kd 25 kd 37
} }
weight weight

34
descriptions/unitree/go2_description/config/robot_control.yaml
View File

@ -27,6 +27,19 @@ imu_sensor_broadcaster:
unitree_guide_controller: unitree_guide_controller:
ros__parameters: ros__parameters:
update_rate: 200 # Hz 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: joints:
- FR_hip_joint - FR_hip_joint
- FR_thigh_joint - FR_thigh_joint
@ -76,8 +89,10 @@ unitree_guide_controller:
ocs2_quadruped_controller: ocs2_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 100 # Hz update_rate: 500 # Hz
robot_pkg: "go2_description" robot_pkg: "go2_description"
feet_force_threshold: 20.0
# default_kd: 3.5
joints: joints:
- FL_hip_joint - FL_hip_joint
- FL_thigh_joint - FL_thigh_joint
@ -136,7 +151,8 @@ rl_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 200 # Hz update_rate: 200 # Hz
robot_pkg: "go2_description" robot_pkg: "go2_description"
model_folder: "legged_gym" # use_rl_thread: false
model_folder: "himloco"
joints: joints:
- FL_hip_joint - FL_hip_joint
- FL_thigh_joint - FL_thigh_joint
@ -165,6 +181,20 @@ rl_quadruped_controller:
- 0.8000 - 0.8000
- -1.5000 - -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: command_interfaces:
- effort - effort
- position - position

3
descriptions/unitree/go2_description/xacro/ros2_control.xacro
View File

@ -5,6 +5,9 @@
<hardware> <hardware>
<plugin>hardware_unitree_mujoco/HardwareUnitree</plugin> <plugin>hardware_unitree_mujoco/HardwareUnitree</plugin>
<!-- <param name="domain">0</param>-->
<!-- <param name="network_interface">enp46s0</param>-->
<!-- <param name="show_foot_force">true</param>-->
</hardware> </hardware>
<joint name="FR_hip_joint"> <joint name="FR_hip_joint">

7
hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h
View File

@ -7,7 +7,7 @@
#define HARDWAREUNITREE_H #define HARDWAREUNITREE_H
#include "hardware_interface/system_interface.hpp" #include "hardware_interface/system_interface.hpp"
#include <unitree/idl/go2/WirelessController_.hpp>
#include <unitree/idl/go2/LowState_.hpp> #include <unitree/idl/go2/LowState_.hpp>
#include <unitree/idl/go2/LowCmd_.hpp> #include <unitree/idl/go2/LowCmd_.hpp>
#include <unitree/idl/go2/SportModeState_.hpp> #include <unitree/idl/go2/SportModeState_.hpp>
@ -54,12 +54,16 @@ protected:
void highStateMessageHandle(const void *messages); void highStateMessageHandle(const void *messages);
void remoteWirelessHandle(const void *messages);
unitree_go::msg::dds_::LowCmd_ low_cmd_{}; // default init unitree_go::msg::dds_::LowCmd_ low_cmd_{}; // default init
unitree_go::msg::dds_::LowState_ low_state_{}; // default init unitree_go::msg::dds_::LowState_ low_state_{}; // default init
unitree_go::msg::dds_::SportModeState_ high_state_{}; // default init unitree_go::msg::dds_::SportModeState_ high_state_{}; // default init
std::string network_interface_ = "lo"; std::string network_interface_ = "lo";
int domain_ = 1; int domain_ = 1;
bool show_foot_force_ = false;
/*publisher*/ /*publisher*/
unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> low_cmd_publisher_; unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> low_cmd_publisher_;
/*subscriber*/ /*subscriber*/
@ -67,5 +71,4 @@ protected:
unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::SportModeState_> high_state_subscriber_; unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::SportModeState_> high_state_subscriber_;
}; };
#endif //HARDWAREUNITREE_H #endif //HARDWAREUNITREE_H

126
hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp
View File

@ -13,8 +13,10 @@ using namespace unitree::robot;
using hardware_interface::return_type; using hardware_interface::return_type;
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn HardwareUnitree::on_init( rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn HardwareUnitree::on_init(
const hardware_interface::HardwareInfo &info) { const hardware_interface::HardwareInfo& info)
if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { {
if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
{
return CallbackReturn::ERROR; return CallbackReturn::ERROR;
} }
@ -32,43 +34,56 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
foot_force_.assign(4, 0); foot_force_.assign(4, 0);
high_states_.assign(6, 0); high_states_.assign(6, 0);
for (const auto &joint: info_.joints) { for (const auto& joint : info_.joints)
for (const auto &interface: joint.state_interfaces) { {
for (const auto& interface : joint.state_interfaces)
{
joint_interfaces[interface.name].push_back(joint.name); joint_interfaces[interface.name].push_back(joint.name);
} }
} }
if (const auto network_interface_param = info.hardware_parameters.find("network_interface"); network_interface_param != info.hardware_parameters.end()) { 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; network_interface_ = network_interface_param->second;
} }
if (const auto domain_param = info.hardware_parameters.find("domain"); domain_param != info.hardware_parameters.end()) { if (const auto domain_param = info.hardware_parameters.find("domain"); domain_param != info.hardware_parameters.
end())
{
domain_ = std::stoi(domain_param->second); 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_); RCLCPP_INFO(get_logger(), " network_interface: %s, domain: %d", network_interface_.c_str(), domain_);
ChannelFactory::Instance()->Init(domain_, network_interface_); ChannelFactory::Instance()->Init(domain_, network_interface_);
low_cmd_publisher_ = low_cmd_publisher_ =
std::make_shared<ChannelPublisher<unitree_go::msg::dds_::LowCmd_> >( std::make_shared<ChannelPublisher<unitree_go::msg::dds_::LowCmd_>>(
TOPIC_LOWCMD); TOPIC_LOWCMD);
low_cmd_publisher_->InitChannel(); low_cmd_publisher_->InitChannel();
lows_tate_subscriber_ = lows_tate_subscriber_ =
std::make_shared<ChannelSubscriber<unitree_go::msg::dds_::LowState_> >( std::make_shared<ChannelSubscriber<unitree_go::msg::dds_::LowState_>>(
TOPIC_LOWSTATE); TOPIC_LOWSTATE);
lows_tate_subscriber_->InitChannel( lows_tate_subscriber_->InitChannel(
[this](auto &&PH1) { [this](auto&& PH1)
{
lowStateMessageHandle(std::forward<decltype(PH1)>(PH1)); lowStateMessageHandle(std::forward<decltype(PH1)>(PH1));
}, },
1); 1);
initLowCmd(); initLowCmd();
high_state_subscriber_ = high_state_subscriber_ =
std::make_shared<ChannelSubscriber<unitree_go::msg::dds_::SportModeState_> >( std::make_shared<ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>>(
TOPIC_HIGHSTATE); TOPIC_HIGHSTATE);
high_state_subscriber_->InitChannel( high_state_subscriber_->InitChannel(
[this](auto &&PH1) { [this](auto&& PH1)
{
highStateMessageHandle(std::forward<decltype(PH1)>(PH1)); highStateMessageHandle(std::forward<decltype(PH1)>(PH1));
}, },
1); 1);
@ -77,42 +92,51 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
return SystemInterface::on_init(info); return SystemInterface::on_init(info);
} }
std::vector<hardware_interface::StateInterface> HardwareUnitree::export_state_interfaces() { std::vector<hardware_interface::StateInterface> HardwareUnitree::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces; std::vector<hardware_interface::StateInterface> state_interfaces;
int ind = 0; 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++]); state_interfaces.emplace_back(joint_name, "position", &joint_position_[ind++]);
} }
ind = 0; 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++]); state_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_[ind++]);
} }
ind = 0; ind = 0;
for (const auto &joint_name: joint_interfaces["effort"]) { for (const auto& joint_name : joint_interfaces["effort"])
state_interfaces.emplace_back(joint_name, "effort", &joint_velocities_[ind++]); {
state_interfaces.emplace_back(joint_name, "effort", &joint_effort_[ind++]);
} }
// export imu sensor state interface // 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( state_interfaces.emplace_back(
info_.sensors[0].name, info_.sensors[0].state_interfaces[i].name, &imu_states_[i]); info_.sensors[0].name, info_.sensors[0].state_interfaces[i].name, &imu_states_[i]);
} }
// export foot force sensor state interface // export foot force sensor state interface
if (info_.sensors.size() > 1) { if (info_.sensors.size() > 1)
for (uint i = 0; i < info_.sensors[1].state_interfaces.size(); i++) { {
for (uint i = 0; i < info_.sensors[1].state_interfaces.size(); i++)
{
state_interfaces.emplace_back( state_interfaces.emplace_back(
info_.sensors[1].name, info_.sensors[1].state_interfaces[i].name, &foot_force_[i]); info_.sensors[1].name, info_.sensors[1].state_interfaces[i].name, &foot_force_[i]);
} }
} }
// export odometer state interface // export odometer state interface
if (info_.sensors.size() > 2) { if (info_.sensors.size() > 2)
{
// export high state interface // 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( state_interfaces.emplace_back(
info_.sensors[2].name, info_.sensors[2].state_interfaces[i].name, &high_states_[i]); info_.sensors[2].name, info_.sensors[2].state_interfaces[i].name, &high_states_[i]);
} }
@ -120,24 +144,28 @@ std::vector<hardware_interface::StateInterface> HardwareUnitree::export_state_in
return return
state_interfaces; state_interfaces;
} }
std::vector<hardware_interface::CommandInterface> HardwareUnitree::export_command_interfaces() { std::vector<hardware_interface::CommandInterface> HardwareUnitree::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces; std::vector<hardware_interface::CommandInterface> command_interfaces;
int ind = 0; 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++]); command_interfaces.emplace_back(joint_name, "position", &joint_position_command_[ind++]);
} }
ind = 0; 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++]); command_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_command_[ind++]);
} }
ind = 0; 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, "effort", &joint_torque_command_[ind]);
command_interfaces.emplace_back(joint_name, "kp", &joint_kp_command_[ind]); command_interfaces.emplace_back(joint_name, "kp", &joint_kp_command_[ind]);
command_interfaces.emplace_back(joint_name, "kd", &joint_kd_command_[ind]); command_interfaces.emplace_back(joint_name, "kd", &joint_kd_command_[ind]);
@ -146,9 +174,11 @@ std::vector<hardware_interface::CommandInterface> HardwareUnitree::export_comman
return command_interfaces; 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 // 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_position_[i] = low_state_.motor_state()[i].q();
joint_velocities_[i] = low_state_.motor_state()[i].dq(); joint_velocities_[i] = low_state_.motor_state()[i].dq();
joint_effort_[i] = low_state_.motor_state()[i].tau_est(); joint_effort_[i] = low_state_.motor_state()[i].tau_est();
@ -172,6 +202,12 @@ return_type HardwareUnitree::read(const rclcpp::Time & /*time*/, const rclcpp::D
foot_force_[2] = low_state_.foot_force()[2]; foot_force_[2] = low_state_.foot_force()[2];
foot_force_[3] = low_state_.foot_force()[3]; 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
high_states_[0] = high_state_.position()[0]; high_states_[0] = high_state_.position()[0];
high_states_[1] = high_state_.position()[1]; high_states_[1] = high_state_.position()[1];
@ -186,9 +222,11 @@ return_type HardwareUnitree::read(const rclcpp::Time & /*time*/, const rclcpp::D
return return_type::OK; 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 // 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].mode() = 0x01;
low_cmd_.motor_cmd()[i].q() = static_cast<float>(joint_position_command_[i]); low_cmd_.motor_cmd()[i].q() = static_cast<float>(joint_position_command_[i]);
low_cmd_.motor_cmd()[i].dq() = static_cast<float>(joint_velocities_command_[i]); low_cmd_.motor_cmd()[i].dq() = static_cast<float>(joint_velocities_command_[i]);
@ -197,21 +235,23 @@ return_type HardwareUnitree::write(const rclcpp::Time & /*time*/, const rclcpp::
low_cmd_.motor_cmd()[i].tau() = static_cast<float>(joint_torque_command_[i]); low_cmd_.motor_cmd()[i].tau() = static_cast<float>(joint_torque_command_[i]);
} }
low_cmd_.crc() = crc32_core(reinterpret_cast<uint32_t *>(&low_cmd_), low_cmd_.crc() = crc32_core(reinterpret_cast<uint32_t*>(&low_cmd_),
(sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1); (sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1);
low_cmd_publisher_->Write(low_cmd_); low_cmd_publisher_->Write(low_cmd_);
return return_type::OK; return return_type::OK;
} }
void HardwareUnitree::initLowCmd() { void HardwareUnitree::initLowCmd()
{
low_cmd_.head()[0] = 0xFE; low_cmd_.head()[0] = 0xFE;
low_cmd_.head()[1] = 0xEF; low_cmd_.head()[1] = 0xEF;
low_cmd_.level_flag() = 0xFF; low_cmd_.level_flag() = 0xFF;
low_cmd_.gpio() = 0; low_cmd_.gpio() = 0;
for (int i = 0; i < 20; i++) { for (int i = 0; i < 20; i++)
{
low_cmd_.motor_cmd()[i].mode() = 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].q() = 0;
low_cmd_.motor_cmd()[i].kp() = 0; low_cmd_.motor_cmd()[i].kp() = 0;
low_cmd_.motor_cmd()[i].dq() = 0; low_cmd_.motor_cmd()[i].dq() = 0;
@ -220,12 +260,14 @@ void HardwareUnitree::initLowCmd() {
} }
} }
void HardwareUnitree::lowStateMessageHandle(const void *messages) { void HardwareUnitree::lowStateMessageHandle(const void* messages)
low_state_ = *static_cast<const unitree_go::msg::dds_::LowState_ *>(messages); {
low_state_ = *static_cast<const unitree_go::msg::dds_::LowState_*>(messages);
} }
void HardwareUnitree::highStateMessageHandle(const void *messages) { void HardwareUnitree::highStateMessageHandle(const void* messages)
high_state_ = *static_cast<const unitree_go::msg::dds_::SportModeState_ *>(messages); {
high_state_ = *static_cast<const unitree_go::msg::dds_::SportModeState_*>(messages);
} }
#include "pluginlib/class_list_macros.hpp" #include "pluginlib/class_list_macros.hpp"