rl controller enhanced

This commit is contained in:
Huang Zhenbiao 2024-10-21 21:47:25 +08:00
parent dda29e8a1f
commit ffa6c44184
35 changed files with 179 additions and 362 deletions

View File

@ -26,6 +26,7 @@ set(dependencies
realtime_tools realtime_tools
control_input_msgs control_input_msgs
kdl_parser kdl_parser
ament_index_cpp
) )
# find dependencies # find dependencies

View File

@ -85,8 +85,6 @@ struct ModelParams {
torch::Tensor torque_limits; torch::Tensor torque_limits;
torch::Tensor rl_kd; torch::Tensor rl_kd;
torch::Tensor rl_kp; torch::Tensor rl_kp;
torch::Tensor fixed_kp;
torch::Tensor fixed_kd;
torch::Tensor commands_scale; torch::Tensor commands_scale;
torch::Tensor default_dof_pos; torch::Tensor default_dof_pos;
}; };

View File

@ -40,6 +40,7 @@ struct CtrlComponent {
control_input_msgs::msg::Inputs control_inputs_; control_input_msgs::msg::Inputs control_inputs_;
int frequency_{}; int frequency_{};
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_;

View File

@ -9,7 +9,6 @@
#include <kdl/chainfksolverpos_recursive.hpp> #include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolverpos_lma.hpp> #include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainjnttojacsolver.hpp> #include <kdl/chainjnttojacsolver.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <rl_quadruped_controller/common/mathTypes.h> #include <rl_quadruped_controller/common/mathTypes.h>
class RobotLeg { class RobotLeg {

View File

@ -4,33 +4,37 @@ import xacro
from ament_index_python.packages import get_package_share_directory from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource
package_description = "a1_description"
def process_xacro(): def launch_setup(context, *args, **kwargs):
package_description = context.launch_configurations['pkg_description']
init_height = context.launch_configurations['height']
pkg_path = os.path.join(get_package_share_directory(package_description)) pkg_path = os.path.join(get_package_share_directory(package_description))
xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'}) robot_description = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'}).toxml()
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz") rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro() rviz = Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
)
gz_spawn_entity = Node( gz_spawn_entity = Node(
package='ros_gz_sim', package='ros_gz_sim',
executable='create', executable='create',
output='screen', output='screen',
arguments=['-topic', 'robot_description', '-name', arguments=['-topic', 'robot_description', '-name',
'a1', '-allow_renaming', 'true', '-z', '0.4'], 'robot', '-allow_renaming', 'true', '-z', init_height]
) )
robot_state_publisher = Node( robot_state_publisher = Node(
@ -71,22 +75,12 @@ def generate_launch_description():
controller = Node( controller = Node(
package="controller_manager", package="controller_manager",
executable="spawner", executable="spawner",
arguments=["rl_quadruped_controller", "--controller-manager", "/controller_manager"], arguments=["rl_quadruped_controller", "--controller-manager", "/controller_manager"]
parameters=[
{
'config_folder': os.path.join(get_package_share_directory(package_description), 'config',
'issacgym'),
}],
) )
return LaunchDescription([
Node( return [
package='rviz2', rviz,
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
Node( Node(
package='ros_gz_bridge', package='ros_gz_bridge',
executable='parameter_bridge', executable='parameter_bridge',
@ -105,7 +99,27 @@ def generate_launch_description():
RegisterEventHandler( RegisterEventHandler(
event_handler=OnProcessExit( event_handler=OnProcessExit(
target_action=leg_pd_controller, target_action=leg_pd_controller,
on_exit=[controller, imu_sensor_broadcaster, joint_state_publisher], on_exit=[imu_sensor_broadcaster, joint_state_publisher, controller],
) )
), ),
]
def generate_launch_description():
pkg_description = DeclareLaunchArgument(
'pkg_description',
default_value='go2_description',
description='package for robot description'
)
height = DeclareLaunchArgument(
'height',
default_value='0.5',
description='Init height in simulation'
)
return LaunchDescription([
pkg_description,
height,
OpaqueFunction(function=launch_setup),
]) ])

View File

@ -12,7 +12,6 @@ from launch_ros.substitutions import FindPackageShare
def launch_setup(context, *args, **kwargs): def launch_setup(context, *args, **kwargs):
package_description = context.launch_configurations['pkg_description'] 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)) pkg_path = os.path.join(get_package_share_directory(package_description))
xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
@ -53,11 +52,7 @@ def launch_setup(context, *args, **kwargs):
controller_manager = Node( controller_manager = Node(
package="controller_manager", package="controller_manager",
executable="ros2_control_node", executable="ros2_control_node",
parameters=[robot_controllers, parameters=[robot_controllers],
{
'config_folder': os.path.join(get_package_share_directory(package_description), 'config',
model_path),
}],
remappings=[ remappings=[
("~/robot_description", "/robot_description"), ("~/robot_description", "/robot_description"),
], ],
@ -111,14 +106,7 @@ def generate_launch_description():
description='package for robot description' description='package for robot description'
) )
model_folder = DeclareLaunchArgument(
'model_folder',
default_value='issacgym',
description='folder name for RL model'
)
return LaunchDescription([ return LaunchDescription([
pkg_description, pkg_description,
model_folder,
OpaqueFunction(function=launch_setup), OpaqueFunction(function=launch_setup),
]) ])

View File

@ -24,10 +24,11 @@ void StateFixedDown::enter() {
} }
ctrl_comp_.control_inputs_.command = 0; ctrl_comp_.control_inputs_.command = 0;
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0); 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_torque_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_*0.5); ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(kd_*0.5); ctrl_comp_.joint_kd_command_interface_[i].get().set_value(kd_);
} }
} }

View File

@ -22,6 +22,7 @@ void StateFixedStand::enter() {
start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value(); start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
} }
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0); 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_torque_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_); ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_);

View File

@ -55,8 +55,9 @@ StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path,
params_.observations_history.size()); params_.observations_history.size());
} }
std::cout << "Model loading: " << config_path + "/" + params_.model_name << std::endl;
model_ = torch::jit::load(config_path + "/" + params_.model_name); model_ = torch::jit::load(config_path + "/" + params_.model_name);
std::cout << "Model loaded: " << config_path + "/" + params_.model_name << std::endl;
// for (const auto &param: model_.parameters()) { // for (const auto &param: model_.parameters()) {
@ -149,7 +150,7 @@ torch::Tensor StateRL::computeObservation() {
const torch::Tensor obs = cat(obs_list, 1); const torch::Tensor obs = cat(obs_list, 1);
std::cout << "Observation: " << obs << std::endl; // std::cout << "Observation: " << obs << std::endl;
torch::Tensor clamped_obs = clamp(obs, -params_.clip_obs, params_.clip_obs); torch::Tensor clamped_obs = clamp(obs, -params_.clip_obs, params_.clip_obs);
return clamped_obs; return clamped_obs;
} }
@ -203,10 +204,6 @@ void StateRL::loadYaml(const std::string &config_path) {
params_.rl_kd = torch::tensor(ReadVectorFromYaml<double>(config["rl_kd"], params_.framework, rows, cols)).view({ params_.rl_kd = torch::tensor(ReadVectorFromYaml<double>(config["rl_kd"], params_.framework, rows, cols)).view({
1, -1 1, -1
}); });
params_.fixed_kp = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kp"], params_.framework, rows, cols)).
view({1, -1});
params_.fixed_kd = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kd"], params_.framework, rows, cols)).
view({1, -1});
params_.torque_limits = torch::tensor( params_.torque_limits = torch::tensor(
ReadVectorFromYaml<double>(config["torque_limits"], params_.framework, rows, cols)).view({1, -1}); ReadVectorFromYaml<double>(config["torque_limits"], params_.framework, rows, cols)).view({1, -1});
@ -288,8 +285,10 @@ void StateRL::getState() {
} }
void StateRL::runModel() { void StateRL::runModel() {
if (ctrl_comp_.enable_estimator_) {
obs_.lin_vel = torch::from_blob(ctrl_comp_.estimator_->getVelocity().data(), {3}, torch::kDouble).clone(). obs_.lin_vel = torch::from_blob(ctrl_comp_.estimator_->getVelocity().data(), {3}, torch::kDouble).clone().
to(torch::kFloat).unsqueeze(0); to(torch::kFloat).unsqueeze(0);
}
obs_.ang_vel = torch::tensor(robot_state_.imu.gyroscope).unsqueeze(0); obs_.ang_vel = torch::tensor(robot_state_.imu.gyroscope).unsqueeze(0);
obs_.commands = torch::tensor({{control_.x, control_.y, control_.yaw}}); obs_.commands = torch::tensor({{control_.x, control_.y, control_.yaw}});
obs_.base_quat = torch::tensor(robot_state_.imu.quaternion).unsqueeze(0); obs_.base_quat = torch::tensor(robot_state_.imu.quaternion).unsqueeze(0);

View File

@ -3,6 +3,7 @@
// //
#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;
@ -47,12 +48,14 @@ 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_comp_.enable_estimator_) {
if (ctrl_comp_.robot_model_ == nullptr) { if (ctrl_comp_.robot_model_ == nullptr) {
return controller_interface::return_type::OK; return controller_interface::return_type::OK;
} }
ctrl_comp_.robot_model_->update(); ctrl_comp_.robot_model_->update();
ctrl_comp_.estimator_->update(); ctrl_comp_.estimator_->update();
}
if (mode_ == FSMMode::NORMAL) { if (mode_ == FSMMode::NORMAL) {
current_state_->run(); current_state_->run();
@ -96,7 +99,8 @@ namespace rl_quadruped_controller {
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_);
// rl config folder // rl config folder
rl_config_folder_ = auto_declare<std::string>("config_folder", 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_);
@ -107,7 +111,10 @@ namespace rl_quadruped_controller {
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_); get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", ctrl_comp_.frequency_); RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", ctrl_comp_.frequency_);
if (foot_force_interface_types_.size() == 4) {
ctrl_comp_.enable_estimator_ = true;
ctrl_comp_.estimator_ = std::make_shared<Estimator>(ctrl_comp_); ctrl_comp_.estimator_ = std::make_shared<Estimator>(ctrl_comp_);
}
} catch (const std::exception &e) { } 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;
@ -121,8 +128,10 @@ namespace rl_quadruped_controller {
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_comp_.enable_estimator_) {
ctrl_comp_.robot_model_ = std::make_shared<QuadrupedRobot>( ctrl_comp_.robot_model_ = std::make_shared<QuadrupedRobot>(
ctrl_comp_, msg->data, feet_names_, base_name_); ctrl_comp_, msg->data, feet_names_, base_name_);
}
}); });
@ -169,7 +178,12 @@ namespace rl_quadruped_controller {
state_list_.passive = std::make_shared<StatePassive>(ctrl_comp_); state_list_.passive = std::make_shared<StatePassive>(ctrl_comp_);
state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_, down_pos_, stand_kp_, stand_kd_); 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_.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_);
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_comp_, model_path, stand_pos_);
// Initialize FSM // Initialize FSM
current_state_ = state_list_.passive; current_state_ = state_list_.passive;

View File

@ -80,8 +80,8 @@ namespace rl_quadruped_controller {
std::string imu_name_; std::string imu_name_;
std::vector<std::string> imu_interface_types_; std::vector<std::string> imu_interface_types_;
// Foot Force Sensor // Foot Force Sensor
std::string foot_force_name_ = "foot_force"; std::string foot_force_name_;
std::vector<std::string> foot_force_interface_types_ = {"FL", "FR", "RL", "RR"}; std::vector<std::string> foot_force_interface_types_;
// FR FL RR RL // FR FL RR RL
std::vector<double> stand_pos_ = { std::vector<double> stand_pos_ = {
@ -101,7 +101,8 @@ namespace rl_quadruped_controller {
double stand_kp_ = 80.0; double stand_kp_ = 80.0;
double stand_kd_ = 3.5; double stand_kd_ = 3.5;
std::string rl_config_folder_; 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> > *>

View File

@ -24,6 +24,7 @@ void StateFixedDown::enter() {
} }
ctrl_comp_.control_inputs_.command = 0; ctrl_comp_.control_inputs_.command = 0;
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0); 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_torque_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_); ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_);

View File

@ -22,6 +22,7 @@ void StateFixedStand::enter() {
start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value(); start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
} }
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0); 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_torque_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_); ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_);

View File

@ -52,8 +52,3 @@ ros2 launch lite3_description visualize.launch.py
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash
ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=lite3_description height:=0.43 ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=lite3_description height:=0.43
``` ```
* Legged Gym Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch lite3_description gazebo_rl_control.launch.py
```

View File

@ -65,6 +65,34 @@ unitree_guide_controller:
- HL_HipY - HL_HipY
- HL_Knee - HL_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
- -0.732
- 1.361
- 0.0
- -0.732
- 1.361
- 0.0
- -0.732
- 1.361
- 0.0
- -0.732
- 1.361
command_interfaces: command_interfaces:
- effort - effort
- position - position
@ -97,47 +125,3 @@ unitree_guide_controller:
- linear_acceleration.x - linear_acceleration.x
- linear_acceleration.y - linear_acceleration.y
- linear_acceleration.z - linear_acceleration.z
rl_quadruped_controller:
ros__parameters:
update_rate: 200 # Hz
config_folder: "/home/tlab-uav/ros2_ws/install/a1_description/share/a1_description/config/issacgym"
command_prefix: "leg_pd_controller"
joints:
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
imu_name: "imu_sensor"
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

View File

@ -165,6 +165,8 @@ ocs2_quadruped_controller:
rl_quadruped_controller: rl_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 200 # Hz update_rate: 200 # Hz
robot_pkg: "lite3_description"
model_folder: "legged_gym"
joints: joints:
- FL_HipX - FL_HipX
- FL_HipY - FL_HipY

View File

@ -101,7 +101,7 @@ unitree_guide_controller:
rl_quadruped_controller: rl_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 200 # Hz update_rate: 200 # Hz
config_folder: "/home/tlab-uav/ros2_ws/install/a1_description/share/a1_description/config/issacgym" config_folder: "/home/tlab-uav/ros2_ws/install/a1_description/share/a1_description/config/legged_gym"
command_prefix: "leg_pd_controller" command_prefix: "leg_pd_controller"
joints: joints:
- FL_hip_joint - FL_hip_joint

View File

@ -63,5 +63,5 @@ ros2 launch a1_description visualize.launch.py
* RL Quadruped Controller * RL Quadruped Controller
```bash ```bash
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash
ros2 launch a1_description gazebo_rl_control.launch.py ros2 launch rl_quadruped_controller gazebo.launch.py pkg_description:=a1_description height:=0.43
``` ```

View File

@ -26,7 +26,6 @@ imu_sensor_broadcaster:
leg_pd_controller: leg_pd_controller:
ros__parameters: ros__parameters:
update_rate: 500 # Hz
joints: joints:
- FR_hip_joint - FR_hip_joint
- FR_thigh_joint - FR_thigh_joint
@ -51,7 +50,7 @@ leg_pd_controller:
unitree_guide_controller: unitree_guide_controller:
ros__parameters: ros__parameters:
command_prefix: "leg_pd_controller" command_prefix: "leg_pd_controller"
update_rate: 500 # Hz update_rate: 200 # Hz
joints: joints:
- FR_hip_joint - FR_hip_joint
- FR_thigh_joint - FR_thigh_joint
@ -102,7 +101,8 @@ unitree_guide_controller:
rl_quadruped_controller: rl_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 200 # Hz update_rate: 200 # Hz
config_folder: "/home/tlab-uav/ros2_ws/install/a1_description/share/a1_description/config/issacgym" robot_pkg: "a1_description"
model_folder: "legged_gym"
command_prefix: "leg_pd_controller" command_prefix: "leg_pd_controller"
joints: joints:
- FL_hip_joint - FL_hip_joint

View File

@ -19,18 +19,10 @@ rl_kp: [20.1, 20.1, 20.1,
20.1, 20.1, 20.1, 20.1, 20.1, 20.1,
20.1, 20.1, 20.1, 20.1, 20.1, 20.1,
20.1, 20.1, 20.1] 20.1, 20.1, 20.1]
rl_kd: [0.54, 0.54, 0.54, rl_kd: [0.754, 0.754, 0.754,
0.54, 0.54, 0.54, 0.754, 0.754, 0.754,
0.54, 0.54, 0.54, 0.754, 0.754, 0.754,
0.54, 0.54, 0.54] 0.754, 0.754, 0.754]
fixed_kp: [80, 80, 80,
80, 80, 80,
80, 80, 80,
80, 80, 80]
fixed_kd: [3, 3, 3,
3, 3, 3,
3, 3, 3,
3, 3, 3]
hip_scale_reduction: 0.5 hip_scale_reduction: 0.5
hip_scale_reduction_indices: [0, 3, 6, 9] hip_scale_reduction_indices: [0, 3, 6, 9]
num_of_dofs: 12 num_of_dofs: 12
@ -44,7 +36,3 @@ 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]
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]

View File

@ -136,6 +136,8 @@ ocs2_quadruped_controller:
rl_quadruped_controller: rl_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 200 # Hz update_rate: 200 # Hz
robot_pkg: "a1_description"
model_folder: "legged_gym"
joints: joints:
- FL_hip_joint - FL_hip_joint
- FL_thigh_joint - FL_thigh_joint
@ -150,6 +152,20 @@ rl_quadruped_controller:
- RR_thigh_joint - RR_thigh_joint
- RR_calf_joint - RR_calf_joint
stand_pos:
- 0.1000
- 0.8000
- -1.5000
- -0.1000
- 0.8000
- -1.5000
- 0.1000
- 0.8000
- -1.5000
- -0.1000
- 0.8000
- -1.5000
command_interfaces: command_interfaces:
- effort - effort
- position - position
@ -168,6 +184,13 @@ rl_quadruped_controller:
- RL_foot - RL_foot
- RR_foot - RR_foot
foot_force_name: "foot_force"
foot_force_interfaces:
- FL
- RL
- FR
- RR
imu_name: "imu_sensor" imu_name: "imu_sensor"
base_name: "base" base_name: "base"

View File

@ -1,103 +0,0 @@
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
package_description = "a1_description"
def process_xacro():
pkg_path = os.path.join(get_package_share_directory(package_description))
xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
robot_description_config = xacro.process_file(xacro_file)
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro()
robot_controllers = PathJoinSubstitution(
[
FindPackageShare(package_description),
"config",
"robot_control.yaml",
]
)
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',
'issacgym'),
}],
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 LaunchDescription([
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
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],
)
),
])

View File

@ -34,8 +34,8 @@
<xacro:property name="hip_offset" value="0.065"/> <xacro:property name="hip_offset" value="0.065"/>
<!-- joint limits --> <!-- joint limits -->
<xacro:property name="damping" value="0"/> <xacro:property name="damping" value="0.1"/>
<xacro:property name="friction" value="0"/> <xacro:property name="friction" value="0.1"/>
<xacro:property name="hip_max" value="46"/> <xacro:property name="hip_max" value="46"/>
<xacro:property name="hip_min" value="-46"/> <xacro:property name="hip_min" value="-46"/>
<xacro:property name="hip_velocity_max" value="21"/> <xacro:property name="hip_velocity_max" value="21"/>

View File

@ -106,7 +106,7 @@
<gazebo> <gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control"> <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<parameters>$(find go2_description)/config/gazebo.yaml</parameters> <parameters>$(find a1_description)/config/gazebo.yaml</parameters>
</plugin> </plugin>
</gazebo> </gazebo>

View File

@ -13,10 +13,10 @@
<xacro:if value="$(arg GAZEBO)"> <xacro:if value="$(arg GAZEBO)">
<xacro:if value="$(arg CLASSIC)"> <xacro:if value="$(arg CLASSIC)">
<xacro:include filename="$(find go2_description)/xacro/gazebo_classic.xacro"/> <xacro:include filename="$(find a1_description)/xacro/gazebo_classic.xacro"/>
</xacro:if> </xacro:if>
<xacro:unless value="$(arg CLASSIC)"> <xacro:unless value="$(arg CLASSIC)">
<xacro:include filename="$(find go2_description)/xacro/gazebo.xacro"/> <xacro:include filename="$(find a1_description)/xacro/gazebo.xacro"/>
</xacro:unless> </xacro:unless>
</xacro:if> </xacro:if>
<xacro:unless value="$(arg GAZEBO)"> <xacro:unless value="$(arg GAZEBO)">

View File

@ -51,6 +51,8 @@ unitree_guide_controller:
ros__parameters: ros__parameters:
command_prefix: "leg_pd_controller" command_prefix: "leg_pd_controller"
update_rate: 500 # Hz update_rate: 500 # Hz
stand_kp: 100.0
stand_kd: 8.0
joints: joints:
- FR_hip_joint - FR_hip_joint
- FR_thigh_joint - FR_thigh_joint

View File

@ -39,12 +39,12 @@ ros2 launch go2_description visualize.launch.py
* OCS2 Quadruped Controller * OCS2 Quadruped Controller
```bash ```bash
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash
ros2 launch go2_description ocs2_control.launch.py ros2 launch ocs2_quadruped_controller mujoco.launch.py
``` ```
* RL Quadruped Controller * RL Quadruped Controller
```bash ```bash
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash
ros2 launch go2_description rl_control.launch.py ros2 launch rl_quadruped_controller mujoco.launch.py
``` ```
### Gazebo Classic 11 (ROS2 Humble) ### Gazebo Classic 11 (ROS2 Humble)

View File

@ -102,7 +102,6 @@ unitree_guide_controller:
rl_quadruped_controller: rl_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 200 # Hz update_rate: 200 # Hz
config_folder: "/home/biao/ros2_ws/install/go2_description/share/go2_description/config/issacgym"
command_prefix: "leg_pd_controller" command_prefix: "leg_pd_controller"
joints: joints:
- FL_hip_joint - FL_hip_joint

View File

@ -23,14 +23,6 @@ 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, 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: 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
@ -47,7 +39,3 @@ 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]
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]

View File

@ -135,6 +135,8 @@ ocs2_quadruped_controller:
rl_quadruped_controller: rl_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 200 # Hz update_rate: 200 # Hz
robot_pkg: "go2_description"
model_folder: "legged_gym"
joints: joints:
- FL_hip_joint - FL_hip_joint
- FL_thigh_joint - FL_thigh_joint
@ -149,6 +151,20 @@ rl_quadruped_controller:
- RR_thigh_joint - RR_thigh_joint
- RR_calf_joint - RR_calf_joint
stand_pos:
- 0.1000
- 0.8000
- -1.5000
- -0.1000
- 0.8000
- -1.5000
- 0.1000
- 0.8000
- -1.5000
- -0.1000
- 0.8000
- -1.5000
command_interfaces: command_interfaces:
- effort - effort
- position - position
@ -167,6 +183,13 @@ rl_quadruped_controller:
- RL_foot - RL_foot
- RR_foot - RR_foot
foot_force_name: "foot_force"
foot_force_interfaces:
- FL
- RL
- FR
- RR
imu_name: "imu_sensor" imu_name: "imu_sensor"
base_name: "base" base_name: "base"

View File

@ -74,7 +74,7 @@ def generate_launch_description():
parameters=[ parameters=[
{ {
'config_folder': os.path.join(get_package_share_directory(package_description), 'config', 'config_folder': os.path.join(get_package_share_directory(package_description), 'config',
'issacgym'), 'legged_gym'),
}], }],
) )

View File

@ -1,103 +0,0 @@
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
package_description = "go2_description"
def process_xacro():
pkg_path = os.path.join(get_package_share_directory(package_description))
xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
robot_description_config = xacro.process_file(xacro_file)
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro()
robot_controllers = PathJoinSubstitution(
[
FindPackageShare(package_description),
"config",
"robot_control.yaml",
]
)
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',
'issacgym'),
}],
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 LaunchDescription([
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
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],
)
),
])