add default pos for rl controller

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)
 

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<double> &target_pos,
+                            double kp,
+                            double kd
+    );
 
     void enter() override;
 
@@ -19,15 +23,12 @@ public:
 
     FSMStateName checkChange() override;
 private:
-    double target_pos_[12] = {
+    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 start_pos_[12] = {};
     rclcpp::Time start_time_;
 
+    double kp_, kd_;
+
     double duration_ = 600; // steps
     double percent_ = 0; //%
     double phase = 0.0;

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<double> &target_pos,
+                             double kp,
+                             double kd);
 
     void enter() override;
 
@@ -20,16 +23,12 @@ public:
     FSMStateName checkChange() override;
 
 private:
-    double target_pos_[12] = {
+    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 start_pos_[12] = {};
     rclcpp::Time start_time_;
 
+    double kp_, kd_;
+
     double duration_ = 600; // steps
     double percent_ = 0; //%
     double phase = 0.0;

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<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_;

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

controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp

@@ -6,9 +6,16 @@
 
 #include <cmath>
 
-StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp): FSMState(
+StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp,
-    FSMStateName::FIXEDDOWN, "fixed down", 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);
     }
 }
 

controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp

@@ -6,15 +6,27 @@
 
 #include <cmath>
 
-StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp): FSMState(
+StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp, const std::vector<double> &target_pos,
-    FSMStateName::FIXEDSTAND, "fixed stand", ctrlComp) {
+                                 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);
     }
 }
 

controllers/rl_quadruped_controller/src/FSM/StateRL.cpp

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

controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp

@@ -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_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_, down_pos_, stand_kp_, stand_kd_);
-        state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_);
+        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_);
+        state_list_.rl = std::make_shared<StateRL>(ctrl_comp_, rl_config_folder_, stand_pos_);
 
         // Initialize FSM
         current_state_ = state_list_.passive;

controllers/rl_quadruped_controller/src/RlQuadrupedController.h

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

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

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]

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

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)