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 default pos for rl controller

This commit is contained in:
Zhenbiao Huang 2024-10-21 17:40:42 +08:00
parent ca616a8c87
commit dda29e8a1f
15 changed files with 362 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
controllers/rl_quadruped_controller/CMakeLists.txt
View File

@ -81,6 +81,11 @@ install(
RUNTIME DESTINATION bin
)
install(
DIRECTORY launch
DESTINATION share/${PROJECT_NAME}/
)
ament_export_dependencies(${dependencies})
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)

15
controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedDown.h
View File

@ -9,7 +9,11 @@
class StateFixedDown final : public FSMState {
public:
explicit StateFixedDown(CtrlComponent &ctrlComp);
explicit StateFixedDown(CtrlComponent &ctrlComp,
const std::vector<double> &target_pos,
double kp,
double kd
);
void enter() override;
@ -19,15 +23,12 @@ public:
FSMStateName checkChange() override;
private:
double target_pos_[12] = {
0.0473455, 1.22187, -2.44375, -0.0473455,
1.22187, -2.44375, 0.0473455, 1.22187,
-2.44375, -0.0473455, 1.22187, -2.44375
};
double target_pos_[12] = {};
double start_pos_[12] = {};
rclcpp::Time start_time_;
double kp_, kd_;
double duration_ = 600; // steps
double percent_ = 0; //%
double phase = 0.0;

15
controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedStand.h
View File

@ -9,7 +9,10 @@
class StateFixedStand final : public FSMState {
public:
explicit StateFixedStand(CtrlComponent &ctrlComp);
explicit StateFixedStand(CtrlComponent &ctrlComp,
const std::vector<double> &target_pos,
double kp,
double kd);
void enter() override;
@ -20,16 +23,12 @@ public:
FSMStateName checkChange() override;
private:
double target_pos_[12] = {
0.00571868, 0.608813, -1.21763,
-0.00571868, 0.608813, -1.21763,
0.00571868, 0.608813, -1.21763,
-0.00571868, 0.608813, -1.21763
};
double target_pos_[12] = {};
double start_pos_[12] = {};
rclcpp::Time start_time_;
double kp_, kd_;
double duration_ = 600; // steps
double percent_ = 0; //%
double phase = 0.0;

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

@ -105,7 +105,7 @@ struct Observations
class StateRL final : public FSMState {
public:
explicit StateRL(CtrlComponent &ctrl_component, const std::string &config_path);
explicit StateRL(CtrlComponent &ctrl_component, const std::string &config_path, const std::vector<double> &target_pos);
void enter() override;
@ -137,6 +137,7 @@ private:
ModelParams params_;
Observations obs_;
Control control_;
double init_pos_[12] = {};
RobotState<double> robot_state_;
RobotCommand<double> robot_command_;

124
controllers/rl_quadruped_controller/launch/mujoco.launch.py Normal file
View File

@ -0,0 +1,124 @@
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']
model_path = context.launch_configurations['model_folder']
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,
{
'config_folder': os.path.join(get_package_share_directory(package_description), 'config',
model_path),
}],
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"],
)
controller = Node(
package="controller_manager",
executable="spawner",
arguments=["rl_quadruped_controller", "--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],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=imu_sensor_broadcaster,
on_exit=[controller],
)
),
]
def generate_launch_description():
pkg_description = DeclareLaunchArgument(
'pkg_description',
default_value='go2_description',
description='package for robot description'
)
model_folder = DeclareLaunchArgument(
'model_folder',
default_value='issacgym',
description='folder name for RL model'
)
return LaunchDescription([
pkg_description,
model_folder,
OpaqueFunction(function=launch_setup),
])

21
controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp
View File

@ -6,9 +6,16 @@
#include <cmath>
StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp): FSMState(
FSMStateName::FIXEDDOWN, "fixed down", ctrlComp) {
StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp,
const std::vector<double> &target_pos,
const double kp,
const double kd)
: FSMState(FSMStateName::FIXEDDOWN, "fixed down", ctrlComp),
kp_(kp), kd_(kd) {
duration_ = ctrl_comp_.frequency_ * 1.2;
for (int i = 0; i < 12; i++) {
target_pos_[i] = target_pos[i];
}
}
void StateFixedDown::enter() {
@ -16,6 +23,12 @@ void StateFixedDown::enter() {
start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
}
ctrl_comp_.control_inputs_.command = 0;
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_*0.5);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(kd_*0.5);
}
}
void StateFixedDown::run() {
@ -24,10 +37,6 @@ void StateFixedDown::run() {
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(
phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(30.0);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(1.5);
}
}

21
controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp
View File

@ -6,15 +6,27 @@
#include <cmath>
StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp): FSMState(
FSMStateName::FIXEDSTAND, "fixed stand", ctrlComp) {
StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp, const std::vector<double> &target_pos,
const double kp,
const double kd)
: FSMState(FSMStateName::FIXEDSTAND, "fixed stand", ctrlComp),
kp_(kp), kd_(kd) {
duration_ = ctrl_comp_.frequency_ * 1.2;
for (int i = 0; i < 12; i++) {
target_pos_[i] = target_pos[i];
}
}
void StateFixedStand::enter() {
for (int i = 0; i < 12; i++) {
start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
}
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(kd_);
}
ctrl_comp_.control_inputs_.command = 0;
}
@ -24,11 +36,6 @@ void StateFixedStand::run() {
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(
phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(
phase * 60.0 + (1 - phase) * 20.0);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(3.5);
}
}

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

@ -39,8 +39,13 @@ std::vector<T> ReadVectorFromYaml(const YAML::Node &node, const std::string &fra
throw std::invalid_argument("Unsupported framework: " + framework);
}
StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path) : FSMState(
StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path,
const std::vector<double> &target_pos) : FSMState(
FSMStateName::RL, "rl", ctrl_component) {
for (int i = 0; i < 12; i++) {
init_pos_[i] = target_pos[i];
}
// read params from yaml
loadYaml(config_path);
@ -204,8 +209,11 @@ void StateRL::loadYaml(const std::string &config_path) {
view({1, -1});
params_.torque_limits = torch::tensor(
ReadVectorFromYaml<double>(config["torque_limits"], params_.framework, rows, cols)).view({1, -1});
params_.default_dof_pos = torch::tensor(
ReadVectorFromYaml<double>(config["default_dof_pos"], params_.framework, rows, cols)).view({1, -1});
params_.default_dof_pos = torch::from_blob(init_pos_, {12}, torch::kDouble).clone().to(torch::kFloat).unsqueeze(0);
// params_.default_dof_pos = torch::tensor(
// ReadVectorFromYaml<double>(config["default_dof_pos"], params_.framework, rows, cols)).view({1, -1});
}
torch::Tensor StateRL::quatRotateInverse(const torch::Tensor &q, const torch::Tensor &v, const std::string &framework) {

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

@ -98,6 +98,12 @@ namespace rl_quadruped_controller {
// rl config folder
rl_config_folder_ = auto_declare<std::string>("config_folder", rl_config_folder_);
// pose parameters
down_pos_ = auto_declare<std::vector<double> >("down_pos", down_pos_);
stand_pos_ = auto_declare<std::vector<double> >("stand_pos", stand_pos_);
stand_kp_ = auto_declare<double>("stand_kp", stand_kp_);
stand_kd_ = auto_declare<double>("stand_kd", stand_kd_);
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", ctrl_comp_.frequency_);
@ -161,9 +167,9 @@ namespace rl_quadruped_controller {
// Create FSM List
state_list_.passive = std::make_shared<StatePassive>(ctrl_comp_);
state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_);
state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_);
state_list_.rl = std::make_shared<StateRL>(ctrl_comp_, rl_config_folder_);
state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_, down_pos_, stand_kp_, stand_kd_);
state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_, stand_pos_, stand_kp_, stand_kd_);
state_list_.rl = std::make_shared<StateRL>(ctrl_comp_, rl_config_folder_, stand_pos_);
// Initialize FSM
current_state_ = state_list_.passive;

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

@ -83,6 +83,24 @@ namespace rl_quadruped_controller {
std::string foot_force_name_ = "foot_force";
std::vector<std::string> foot_force_interface_types_ = {"FL", "FR", "RL", "RR"};
// FR FL RR RL
std::vector<double> stand_pos_ = {
0.0, 0.67, -1.3,
0.0, 0.67, -1.3,
0.0, 0.67, -1.3,
0.0, 0.67, -1.3
};
std::vector<double> down_pos_ = {
0.0, 1.3, -2.4,
0.0, 1.3, -2.4,
0.0, 1.3, -2.4,
0.0, 1.3, -2.4
};
double stand_kp_ = 80.0;
double stand_kd_ = 3.5;
std::string rl_config_folder_;
std::unordered_map<

13
descriptions/deep_robotics/lite3_description/README.md
View File

@ -1,27 +1,34 @@
# DeepRobotics Lite3 Description
This repository contains the urdf model of lite3.
![lite3](../../../.images/lite3.png)
Tested environment:
* Ubuntu 24.04
* ROS2 Jazzy
## Build
```bash
cd ~/ros2_ws
colcon build --packages-up-to lite3_description --symlink-install
```
## Visualize the robot
To visualize and check the configuration of the robot in rviz, simply launch:
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch lite3_description visualize.launch.py
```
## Launch ROS2 Control
### Mujoco Simulator
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
@ -32,14 +39,14 @@ ros2 launch lite3_description visualize.launch.py
source ~/ros2_ws/install/setup.bash
ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=lite3_description
```
* Legged Gym Controller
* RL Quadruped Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch a1_description rl_control.launch.py
ros2 launch rl_quadruped_controller mujoco.launch.py pkg_description:=lite3_description model_folder:=legged_gym
```
### Gazebo Simulator
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash

53
descriptions/deep_robotics/lite3_description/config/legged_gym/config.yaml Normal file
View File

@ -0,0 +1,53 @@
model_name: "policy.pt"
framework: "isaacgym"
rows: 4
cols: 3
decimation: 4
num_observations: 48
observations: ["lin_vel", "ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"]
#observations_history: [6, 5, 4, 3, 2, 1, 0]
clip_obs: 100.0
clip_actions_lower: [-100, -100, -100,
-100, -100, -100,
-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]
fixed_kp: [60, 60, 60,
60, 60, 60,
60, 60, 60,
60, 60, 60]
fixed_kd: [5, 5, 5,
5, 5, 5,
5, 5, 5,
5, 5, 5]
hip_scale_reduction: 1.0
hip_scale_reduction_indices: [0, 3, 6, 9]
num_of_dofs: 12
action_scale: 0.25
lin_vel_scale: 2.0
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
commands_scale: [2.0, 2.0, 0.25]
torque_limits: [33.5, 33.5, 33.5,
33.5, 33.5, 33.5,
33.5, 33.5, 33.5,
33.5, 33.5, 33.5]
default_dof_pos: [ 0.1000, 0.8000, -1.5000,
-0.1000, 0.8000, -1.5000,
0.1000, 1.0000, -1.5000,
-0.1000, 1.0000, -1.5000]

BIN
descriptions/deep_robotics/lite3_description/config/legged_gym/policy.pt Normal file
View File

Binary file not shown.

85
descriptions/deep_robotics/lite3_description/config/robot_control.yaml
View File

@ -161,3 +161,88 @@ ocs2_quadruped_controller:
- HL
- FR
- HR
rl_quadruped_controller:
ros__parameters:
update_rate: 200 # Hz
joints:
- FL_HipX
- FL_HipY
- FL_Knee
- FR_HipX
- FR_HipY
- FR_Knee
- HL_HipX
- HL_HipY
- HL_Knee
- HR_HipX
- HR_HipY
- HR_Knee
down_pos:
- 0.0
- -1.22
- 2.61
- 0.0
- -1.22
- 2.61
- 0.0
- -1.22
- 2.61
- 0.0
- -1.22
- 2.61
stand_pos:
- 0.0
- -1.0
- 1.8
- 0.0
- -1.0
- 1.8
- 0.0
- -1.0
- 1.8
- 0.0
- -1.0
- 1.8
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
feet_names:
- FL_FOOT
- FR_FOOT
- HL_FOOT
- HR_FOOT
foot_force_name: "foot_force"
foot_force_interfaces:
- FL
- FR
- HL
- HR
imu_name: "imu_sensor"
base_name: "base"
imu_interfaces:
- orientation.w
- orientation.x
- orientation.y
- orientation.z
- angular_velocity.x
- angular_velocity.y
- angular_velocity.z
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z

2
descriptions/unitree/a1_description/README.md
View File

@ -42,7 +42,7 @@ ros2 launch a1_description visualize.launch.py
* RL Quadruped Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch a1_description rl_control.launch.py
ros2 launch rl_quadruped_controller mujoco.launch.py pkg_description:=a1_description
```
### Gazebo Classic 11 (ROS2 Humble)