diff --git a/controllers/rl_quadruped_controller/CMakeLists.txt b/controllers/rl_quadruped_controller/CMakeLists.txt index 3466652..c5955fd 100644 --- a/controllers/rl_quadruped_controller/CMakeLists.txt +++ b/controllers/rl_quadruped_controller/CMakeLists.txt @@ -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) diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedDown.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedDown.h index f46b42f..012fee5 100644 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedDown.h +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedDown.h @@ -9,7 +9,11 @@ class StateFixedDown final : public FSMState { public: - explicit StateFixedDown(CtrlComponent &ctrlComp); + explicit StateFixedDown(CtrlComponent &ctrlComp, + const std::vector &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; diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedStand.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedStand.h index 473cbd5..b9f4cd8 100644 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedStand.h +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedStand.h @@ -9,7 +9,10 @@ class StateFixedStand final : public FSMState { public: - explicit StateFixedStand(CtrlComponent &ctrlComp); + explicit StateFixedStand(CtrlComponent &ctrlComp, + const std::vector &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; diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h index 98cd52b..0513acb 100644 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h @@ -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 &target_pos); void enter() override; @@ -137,6 +137,7 @@ private: ModelParams params_; Observations obs_; Control control_; + double init_pos_[12] = {}; RobotState robot_state_; RobotCommand robot_command_; diff --git a/controllers/rl_quadruped_controller/launch/mujoco.launch.py b/controllers/rl_quadruped_controller/launch/mujoco.launch.py new file mode 100644 index 0000000..866c092 --- /dev/null +++ b/controllers/rl_quadruped_controller/launch/mujoco.launch.py @@ -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), + ]) diff --git a/controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp b/controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp index a7637dd..75f64f1 100644 --- a/controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp +++ b/controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp @@ -6,9 +6,16 @@ #include -StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp): FSMState( - FSMStateName::FIXEDDOWN, "fixed down", ctrlComp) { +StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp, + const std::vector &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); } } diff --git a/controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp b/controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp index 102c0c1..866569e 100644 --- a/controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp +++ b/controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp @@ -6,15 +6,27 @@ #include -StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp): FSMState( - FSMStateName::FIXEDSTAND, "fixed stand", ctrlComp) { +StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp, const std::vector &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); } } diff --git a/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp index 3294b1b..1986a40 100644 --- a/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp +++ b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp @@ -39,8 +39,13 @@ std::vector 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 &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(config["torque_limits"], params_.framework, rows, cols)).view({1, -1}); - params_.default_dof_pos = torch::tensor( - ReadVectorFromYaml(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(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) { diff --git a/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp b/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp index 4702dbc..853d5f8 100644 --- a/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp +++ b/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp @@ -98,6 +98,12 @@ namespace rl_quadruped_controller { // rl config folder rl_config_folder_ = auto_declare("config_folder", rl_config_folder_); + // pose parameters + down_pos_ = auto_declare >("down_pos", down_pos_); + stand_pos_ = auto_declare >("stand_pos", stand_pos_); + stand_kp_ = auto_declare("stand_kp", stand_kp_); + stand_kd_ = auto_declare("stand_kd", stand_kd_); + get_node()->get_parameter("update_rate", ctrl_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(ctrl_comp_); - state_list_.fixedDown = std::make_shared(ctrl_comp_); - state_list_.fixedStand = std::make_shared(ctrl_comp_); - state_list_.rl = std::make_shared(ctrl_comp_, rl_config_folder_); + state_list_.fixedDown = std::make_shared(ctrl_comp_, down_pos_, stand_kp_, stand_kd_); + state_list_.fixedStand = std::make_shared(ctrl_comp_, stand_pos_, stand_kp_, stand_kd_); + state_list_.rl = std::make_shared(ctrl_comp_, rl_config_folder_, stand_pos_); // Initialize FSM current_state_ = state_list_.passive; diff --git a/controllers/rl_quadruped_controller/src/RlQuadrupedController.h b/controllers/rl_quadruped_controller/src/RlQuadrupedController.h index c0c9c18..f19152b 100644 --- a/controllers/rl_quadruped_controller/src/RlQuadrupedController.h +++ b/controllers/rl_quadruped_controller/src/RlQuadrupedController.h @@ -83,6 +83,24 @@ namespace rl_quadruped_controller { std::string foot_force_name_ = "foot_force"; std::vector foot_force_interface_types_ = {"FL", "FR", "RL", "RR"}; + // FR FL RR RL + std::vector 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 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< diff --git a/descriptions/deep_robotics/lite3_description/README.md b/descriptions/deep_robotics/lite3_description/README.md index 46392b3..09fb517 100644 --- a/descriptions/deep_robotics/lite3_description/README.md +++ b/descriptions/deep_robotics/lite3_description/README.md @@ -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 diff --git a/descriptions/deep_robotics/lite3_description/config/legged_gym/config.yaml b/descriptions/deep_robotics/lite3_description/config/legged_gym/config.yaml new file mode 100644 index 0000000..7a4ae22 --- /dev/null +++ b/descriptions/deep_robotics/lite3_description/config/legged_gym/config.yaml @@ -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] \ No newline at end of file diff --git a/descriptions/deep_robotics/lite3_description/config/legged_gym/policy.pt b/descriptions/deep_robotics/lite3_description/config/legged_gym/policy.pt new file mode 100644 index 0000000..d28d697 Binary files /dev/null and b/descriptions/deep_robotics/lite3_description/config/legged_gym/policy.pt differ diff --git a/descriptions/deep_robotics/lite3_description/config/robot_control.yaml b/descriptions/deep_robotics/lite3_description/config/robot_control.yaml index 0c850d4..abb537a 100644 --- a/descriptions/deep_robotics/lite3_description/config/robot_control.yaml +++ b/descriptions/deep_robotics/lite3_description/config/robot_control.yaml @@ -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 \ No newline at end of file diff --git a/descriptions/unitree/a1_description/README.md b/descriptions/unitree/a1_description/README.md index 4bd6126..0cce7fd 100644 --- a/descriptions/unitree/a1_description/README.md +++ b/descriptions/unitree/a1_description/README.md @@ -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)