rl controller enhanced

controllers/rl_quadruped_controller/CMakeLists.txt

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

controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h

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

controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h

@@ -40,6 +40,7 @@ struct CtrlComponent {
 
     control_input_msgs::msg::Inputs control_inputs_;
     int frequency_{};
+    bool enable_estimator_ = false;
 
     std::shared_ptr<QuadrupedRobot> robot_model_;
     std::shared_ptr<Estimator> estimator_;

controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/RobotLeg.h

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

controllers/rl_quadruped_controller/launch/gazebo.launch.py

@@ -4,33 +4,37 @@ 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.launch_description_sources import PythonLaunchDescriptionSource
 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"
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 
-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))
+
     xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'})
-    return robot_description_config.toxml()
+    robot_description = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'}).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()
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz_ocs2',
+        output='screen',
+        arguments=["-d", rviz_config_file]
+    )
 
     gz_spawn_entity = Node(
         package='ros_gz_sim',
         executable='create',
         output='screen',
         arguments=['-topic', 'robot_description', '-name',
-                   'a1', '-allow_renaming', 'true', '-z', '0.4'],
+                   'robot', '-allow_renaming', 'true', '-z', init_height]
     )
 
     robot_state_publisher = Node(
@@ -71,22 +75,12 @@ def generate_launch_description():
     controller = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["rl_quadruped_controller", "--controller-manager", "/controller_manager"],
-        parameters=[
-            {
-                'config_folder': os.path.join(get_package_share_directory(package_description), 'config',
-                                              'issacgym'),
-            }],
+        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]
-        ),
+
+    return [
+        rviz,
         Node(
             package='ros_gz_bridge',
             executable='parameter_bridge',
@@ -105,7 +99,27 @@ def generate_launch_description():
         RegisterEventHandler(
             event_handler=OnProcessExit(
                 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),
     ])

controllers/rl_quadruped_controller/launch/mujoco.launch.py

@@ -12,7 +12,6 @@ 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')
@@ -53,11 +52,7 @@ def launch_setup(context, *args, **kwargs):
     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),
-                    }],
+        parameters=[robot_controllers],
         remappings=[
             ("~/robot_description", "/robot_description"),
         ],
@@ -111,14 +106,7 @@ def generate_launch_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),
     ])

controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp

@@ -24,10 +24,11 @@ void StateFixedDown::enter() {
     }
     ctrl_comp_.control_inputs_.command = 0;
     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_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);
+        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_);
+        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(kd_);
     }
 }
 

controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp

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

controllers/rl_quadruped_controller/src/FSM/StateRL.cpp

@@ -55,8 +55,9 @@ StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path,
                                                                params_.observations_history.size());
     }
 
+    std::cout << "Model loading: " << config_path + "/" + params_.model_name << std::endl;
     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()) {
@@ -149,7 +150,7 @@ torch::Tensor StateRL::computeObservation() {
 
     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);
     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({
         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(
         ReadVectorFromYaml<double>(config["torque_limits"], params_.framework, rows, cols)).view({1, -1});
 
@@ -288,8 +285,10 @@ void StateRL::getState() {
 }
 
 void StateRL::runModel() {
-    obs_.lin_vel = torch::from_blob(ctrl_comp_.estimator_->getVelocity().data(), {3}, torch::kDouble).clone().
-            to(torch::kFloat).unsqueeze(0);
+    if (ctrl_comp_.enable_estimator_) {
+        obs_.lin_vel = torch::from_blob(ctrl_comp_.estimator_->getVelocity().data(), {3}, torch::kDouble).clone().
+        to(torch::kFloat).unsqueeze(0);
+    }
     obs_.ang_vel = torch::tensor(robot_state_.imu.gyroscope).unsqueeze(0);
     obs_.commands = torch::tensor({{control_.x, control_.y, control_.yaw}});
     obs_.base_quat = torch::tensor(robot_state_.imu.quaternion).unsqueeze(0);

controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp

@@ -3,6 +3,7 @@
 //
 
 #include "RlQuadrupedController.h"
+#include <ament_index_cpp/get_package_share_directory.hpp>
 
 namespace rl_quadruped_controller {
     using config_type = controller_interface::interface_configuration_type;
@@ -47,12 +48,14 @@ namespace rl_quadruped_controller {
 
     controller_interface::return_type LeggedGymController::
     update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
-        if (ctrl_comp_.robot_model_ == nullptr) {
-            return controller_interface::return_type::OK;
-        }
+        if (ctrl_comp_.enable_estimator_) {
+            if (ctrl_comp_.robot_model_ == nullptr) {
+                return controller_interface::return_type::OK;
+            }
 
-        ctrl_comp_.robot_model_->update();
-        ctrl_comp_.estimator_->update();
+            ctrl_comp_.robot_model_->update();
+            ctrl_comp_.estimator_->update();
+        }
 
         if (mode_ == FSMMode::NORMAL) {
             current_state_->run();
@@ -96,7 +99,8 @@ namespace rl_quadruped_controller {
                     auto_declare<std::vector<std::string> >("foot_force_interfaces", foot_force_interface_types_);
 
             // 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
             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_);
             RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", ctrl_comp_.frequency_);
 
-            ctrl_comp_.estimator_ = std::make_shared<Estimator>(ctrl_comp_);
+            if (foot_force_interface_types_.size() == 4) {
+                ctrl_comp_.enable_estimator_ = true;
+                ctrl_comp_.estimator_ = std::make_shared<Estimator>(ctrl_comp_);
+            }
         } catch (const std::exception &e) {
             fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
             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", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
             [this](const std_msgs::msg::String::SharedPtr msg) {
-                ctrl_comp_.robot_model_ = std::make_shared<QuadrupedRobot>(
-                    ctrl_comp_, msg->data, feet_names_, base_name_);
+                if (ctrl_comp_.enable_estimator_) {
+                    ctrl_comp_.robot_model_ = std::make_shared<QuadrupedRobot>(
+                        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_.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_);
+
+
+        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
         current_state_ = state_list_.passive;

controllers/rl_quadruped_controller/src/RlQuadrupedController.h

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

controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp

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

controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp

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

descriptions/deep_robotics/lite3_description/README.md

@@ -51,9 +51,4 @@ ros2 launch lite3_description visualize.launch.py
   ```bash
   source ~/ros2_ws/install/setup.bash
   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
   ```

descriptions/deep_robotics/lite3_description/config/gazebo.yaml

@@ -65,6 +65,34 @@ unitree_guide_controller:
       - HL_HipY
       - 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:
       - effort
       - position
@@ -86,50 +114,6 @@ unitree_guide_controller:
     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
-
-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

descriptions/deep_robotics/lite3_description/config/robot_control.yaml

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

descriptions/deep_robotics/x30_description/config/gazebo.yaml

@@ -101,7 +101,7 @@ unitree_guide_controller:
 rl_quadruped_controller:
   ros__parameters:
     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"
     joints:
       - FL_hip_joint

descriptions/unitree/a1_description/README.md

@@ -63,5 +63,5 @@ ros2 launch a1_description visualize.launch.py
 * RL Quadruped Controller
   ```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
   ```

descriptions/unitree/a1_description/config/gazebo.yaml

@@ -26,7 +26,6 @@ imu_sensor_broadcaster:
 
 leg_pd_controller:
   ros__parameters:
-    update_rate: 500  # Hz
     joints:
       - FR_hip_joint
       - FR_thigh_joint
@@ -51,7 +50,7 @@ leg_pd_controller:
 unitree_guide_controller:
   ros__parameters:
     command_prefix: "leg_pd_controller"
-    update_rate: 500  # Hz
+    update_rate: 200  # Hz
     joints:
       - FR_hip_joint
       - FR_thigh_joint
@@ -102,7 +101,8 @@ unitree_guide_controller:
 rl_quadruped_controller:
   ros__parameters:
     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"
     joints:
       - FL_hip_joint

descriptions/unitree/a1_description/config/legged_gym/config.yaml

@@ -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]
-rl_kd: [0.54, 0.54, 0.54,
-        0.54, 0.54, 0.54,
-        0.54, 0.54, 0.54,
-        0.54, 0.54, 0.54]
-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]
+rl_kd: [0.754, 0.754, 0.754,
+        0.754, 0.754, 0.754,
+        0.754, 0.754, 0.754,
+        0.754, 0.754, 0.754]
 hip_scale_reduction: 0.5
 hip_scale_reduction_indices: [0, 3, 6, 9]
 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]
-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]

descriptions/unitree/a1_description/config/robot_control.yaml

@@ -136,6 +136,8 @@ ocs2_quadruped_controller:
 rl_quadruped_controller:
   ros__parameters:
     update_rate: 200  # Hz
+    robot_pkg: "a1_description"
+    model_folder: "legged_gym"
     joints:
       - FL_hip_joint
       - FL_thigh_joint
@@ -150,6 +152,20 @@ rl_quadruped_controller:
       - RR_thigh_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:
       - effort
       - position
@@ -168,6 +184,13 @@ rl_quadruped_controller:
       - RL_foot
       - RR_foot
 
+    foot_force_name: "foot_force"
+    foot_force_interfaces:
+      - FL
+      - RL
+      - FR
+      - RR
+
     imu_name: "imu_sensor"
     base_name: "base"
 

descriptions/unitree/a1_description/launch/rl_control.launch.py

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

descriptions/unitree/a1_description/xacro/const.xacro

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

descriptions/unitree/a1_description/xacro/gazebo_classic.xacro

@@ -106,7 +106,7 @@
 
   <gazebo>
     <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>
   </gazebo>
 

descriptions/unitree/a1_description/xacro/robot.xacro

@@ -13,10 +13,10 @@
 
     <xacro:if value="$(arg GAZEBO)">
         <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: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:if>
     <xacro:unless value="$(arg GAZEBO)">

descriptions/unitree/aliengo_description/config/gazebo.yaml

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

descriptions/unitree/go2_description/README.md

@@ -39,12 +39,12 @@ ros2 launch go2_description visualize.launch.py
 * OCS2 Quadruped Controller
   ```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
   ```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)

descriptions/unitree/go2_description/config/gazebo.yaml

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

descriptions/unitree/go2_description/config/legged_gym/config.yaml

@@ -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]
-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
@@ -46,8 +38,4 @@ 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]
+                33.5, 33.5, 33.5]

descriptions/unitree/go2_description/config/robot_control.yaml

@@ -135,6 +135,8 @@ ocs2_quadruped_controller:
 rl_quadruped_controller:
   ros__parameters:
     update_rate: 200  # Hz
+    robot_pkg: "go2_description"
+    model_folder: "legged_gym"
     joints:
       - FL_hip_joint
       - FL_thigh_joint
@@ -149,6 +151,20 @@ rl_quadruped_controller:
       - RR_thigh_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:
       - effort
       - position
@@ -167,6 +183,13 @@ rl_quadruped_controller:
       - RL_foot
       - RR_foot
 
+    foot_force_name: "foot_force"
+    foot_force_interfaces:
+      - FL
+      - RL
+      - FR
+      - RR
+
     imu_name: "imu_sensor"
     base_name: "base"
 

descriptions/unitree/go2_description/launch/gazebo_rl_control.launch.py

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

descriptions/unitree/go2_description/launch/rl_control.launch.py

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