From 2085140209c3fb29fbf9f7e5eeec0755b393e296 Mon Sep 17 00:00:00 2001 From: fan-ziqi Date: Thu, 15 Aug 2024 23:14:48 +0800 Subject: [PATCH 1/2] feat: [Breaking change] split config.yaml into separate model folders --- README.md | 19 +- README_CN.md | 19 +- src/rl_sar/config.yaml | 257 ------------------ src/rl_sar/include/rl_sim.hpp | 2 +- ...aclab.launch => gazebo_a1_isaacgym.launch} | 4 +- ...bo_a1.launch => gazebo_a1_isaacsim.launch} | 4 +- ...t1.launch => gazebo_gr1t1_isaacgym.launch} | 4 +- ...t2.launch => gazebo_gr1t2_isaacgym.launch} | 4 +- src/rl_sar/library/rl_sdk/rl_sdk.cpp | 11 +- src/rl_sar/library/rl_sdk/rl_sdk.hpp | 2 +- src/rl_sar/models/a1_isaacgym/config.yaml | 55 ++++ .../models/{a1 => a1_isaacgym}/model_0501.pt | Bin .../models/{a1 => a1_isaacgym}/model_0522.pt | Bin .../models/{a1 => a1_isaacgym}/model_0526.pt | Bin .../models/{a1 => a1_isaacgym}/model_0702.pt | Bin src/rl_sar/models/a1_isaacsim/config.yaml | 55 ++++ .../{a1_isaaclab => a1_isaacsim}/policy.pt | Bin src/rl_sar/models/gr1t1_isaacgym/config.yaml | 37 +++ .../model_4000_jit.pt | Bin src/rl_sar/models/gr1t1_isaacsim/config.yaml | 37 +++ src/rl_sar/models/gr1t2_isaacgym/config.yaml | 37 +++ .../model_4000_jit.pt | Bin src/rl_sar/models/gr1t2_isaacsim/config.yaml | 37 +++ src/rl_sar/scripts/rl_sdk.py | 11 +- src/rl_sar/scripts/rl_sim.py | 9 +- src/rl_sar/src/rl_real_a1.cpp | 2 +- src/rl_sar/src/rl_sim.cpp | 9 +- 27 files changed, 314 insertions(+), 301 deletions(-) delete mode 100644 src/rl_sar/config.yaml rename src/rl_sar/launch/{gazebo_a1_isaaclab.launch => gazebo_a1_isaacgym.launch} (96%) rename src/rl_sar/launch/{gazebo_a1.launch => gazebo_a1_isaacsim.launch} (94%) rename src/rl_sar/launch/{gazebo_gr1t1.launch => gazebo_gr1t1_isaacgym.launch} (94%) rename src/rl_sar/launch/{gazebo_gr1t2.launch => gazebo_gr1t2_isaacgym.launch} (94%) create mode 100644 src/rl_sar/models/a1_isaacgym/config.yaml rename src/rl_sar/models/{a1 => a1_isaacgym}/model_0501.pt (100%) rename src/rl_sar/models/{a1 => a1_isaacgym}/model_0522.pt (100%) rename src/rl_sar/models/{a1 => a1_isaacgym}/model_0526.pt (100%) rename src/rl_sar/models/{a1 => a1_isaacgym}/model_0702.pt (100%) create mode 100644 src/rl_sar/models/a1_isaacsim/config.yaml rename src/rl_sar/models/{a1_isaaclab => a1_isaacsim}/policy.pt (100%) create mode 100644 src/rl_sar/models/gr1t1_isaacgym/config.yaml rename src/rl_sar/models/{gr1t1 => gr1t1_isaacgym}/model_4000_jit.pt (100%) create mode 100644 src/rl_sar/models/gr1t1_isaacsim/config.yaml create mode 100644 src/rl_sar/models/gr1t2_isaacgym/config.yaml rename src/rl_sar/models/{gr1t2 => gr1t2_isaacgym}/model_4000_jit.pt (100%) create mode 100644 src/rl_sar/models/gr1t2_isaacsim/config.yaml diff --git a/README.md b/README.md index b4c67ab..45c4da2 100644 --- a/README.md +++ b/README.md @@ -71,6 +71,8 @@ cd .. catkin build ``` +If catkin build report errors: `Unable to find either executable 'empy' or Python module 'em'`, run `catkin config -DPYTHON_EXECUTABLE=/usr/bin/python3` before `catkin build` + ## Running Before running, copy the trained pt model file to `rl_sar/src/rl_sar/models/YOUR_ROBOT_NAME`, and configure the parameters in `config.yaml`. @@ -100,9 +102,9 @@ Control: * Press **\** to sets all control commands to zero. * If robot falls down, press **R** to reset Gazebo environment. -### Physical Robots +### Real Robots -#### Unitree A1 +**Example: Unitree A1** Unitree A1 can be connected using both wireless and wired methods: @@ -135,14 +137,13 @@ Or press **0** on the keyboard to switch the robot to the default standing posit ## Add Your Robot -In the following, let ROBOT represent the name of your robot. +In the following text, `` represents the name of the robot -1. Create a model package named ROBOT_description in the robots folder. Place the URDF model in the urdf path within the folder and name it ROBOT.urdf. Create a namespace named ROBOT_gazebo in the config folder within the model file for joint configuration. -2. Place the model file in models/ROBOT. -3. Add a new field in rl_sar/config.yaml named ROBOT and adjust the parameters, such as changing the model_name to the model file name from the previous step. -4. Add a new launch file in the rl_sar/launch folder. Refer to other launch files for guidance on modification. -5. Change ROBOT_NAME to ROBOT in rl_xxx.cpp. -6. Compile and run. +1. Create a model package named `_description` in the `rl_sar/src/robots` directory. Place the robot's URDF file in the `rl_sar/src/robots/_description/urdf` directory and name it `.urdf`. Additionally, create a joint configuration file with the namespace `_gazebo` in the `rl_sar/src/robots/_description/config` directory. +2. Place the trained RL model files in the `rl_sar/src/rl_sar/models/` directory. +3. In the `rl_sar/src/rl_sar/models/` directory, create a `config.yaml` file, and modify its parameters based on the `rl_sar/src/rl_sar/models/a1_isaacgym/config.yaml` file. +4. If you need to run simulations, modify the launch files as needed by referring to those in the `rl_sar/src/rl_sar/launch` directory. +5. If you need to run on the physical robot, modify the file `rl_sar/src/rl_sar/src/rl_real_a1.cpp` as needed. ## Reference diff --git a/README_CN.md b/README_CN.md index 3796415..4a7f1e6 100644 --- a/README_CN.md +++ b/README_CN.md @@ -71,6 +71,8 @@ cd .. catkin build ``` +如果 catkin build 报错: `Unable to find either executable 'empy' or Python module 'em'`, 在`catkin build` 之前执行 `catkin config -DPYTHON_EXECUTABLE=/usr/bin/python3` + ## 运行 运行前请将训练好的pt模型文件拷贝到`rl_sar/src/rl_sar/models/YOUR_ROBOT_NAME`中，并配置`config.yaml`中的参数。 @@ -101,9 +103,9 @@ source devel/setup.bash * 按 **\** 将所有控制指令设置为零。 * 如果机器人摔倒，按 **R** 重置Gazebo环境。 -### 实物 +### 真实机器人 -#### Unitree A1 +**示例：Unitree A1** 与Unitree A1连接可以使用无线与有线两种方式 @@ -136,14 +138,13 @@ rosrun rl_sar rl_real_a1 ## 添加你的机器人 -下文中将ROBOT代表机器人名称 +下文中将``代表机器人名称 -1. 在robots文件夹中创建名为ROBOT_description的模型包，将模型的urdf放到文件夹中的urdf路径下并命名为ROBOT.urdf，在模型文件中的config文件夹中创建命名空间为ROBOT_gazebo的关节配置文件 -2. 将模型文件放到models/ROBOT中 -3. 在rl_sar/config.yaml中添加一个新的字段，命名为ROBOT，更改其中参数，如将model_name改为上一步的模型文件名 -4. 在rl_sar/launch文件夹中添加一个新的launch文件，请参考其他launch文件自行修改 -5. 修改rl_xxx.cpp中的ROBOT_NAME为ROBOT -6. 编译运行 +1. 在`rl_sar/src/robots`路径下创建名为`_description`的模型包，将模型的urdf放到`rl_sar/src/robots/_description/urdf`路径下并命名为`.urdf`，并在`rl_sar/src/robots/_description/config`路径下创建命名空间为`_gazebo`的关节配置文件 +2. 将训练好的RL模型文件放到`rl_sar/src/rl_sar/models/`路径下 +3. 在`rl_sar/src/rl_sar/models/`中新建config.yaml文件，参考`rl_sar/src/rl_sar/models/a1_isaacgym/config.yaml`文件修改其中参数 +4. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改 +5. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改 ## 参考 diff --git a/src/rl_sar/config.yaml b/src/rl_sar/config.yaml deleted file mode 100644 index 5eaff85..0000000 --- a/src/rl_sar/config.yaml +++ /dev/null @@ -1,257 +0,0 @@ -a1: - model_name: "model_0702.pt" - framework: "isaacgym" - rows: 4 - cols: 3 - dt: 0.005 - decimation: 4 - num_observations: 45 - 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: [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_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, 1.0] - 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] - joint_controller_names: ["FL_hip_controller", "FL_thigh_controller", "FL_calf_controller", - "FR_hip_controller", "FR_thigh_controller", "FR_calf_controller", - "RL_hip_controller", "RL_thigh_controller", "RL_calf_controller", - "RR_hip_controller", "RR_thigh_controller", "RR_calf_controller"] - -gr1t1: - model_name: "model_4000_jit.pt" - framework: "isaacgym" - rows: 2 - cols: 5 - dt: 0.001 - decimation: 20 - num_observations: 39 - clip_obs: 100.0 - clip_actions_lower: [-0.4391, -1.0491, -2.0991, -0.4391, -1.3991, - -1.1391, -1.0491, -2.0991, -0.4391, -1.3991] - clip_actions_upper: [1.1391, 1.0491, 1.0491, 2.2691, 0.8691, - 0.4391, 1.0491, 1.0491, 2.2691, 0.8691] - rl_kp: [57.0, 43.0, 114.0, 114.0, 15.3, - 57.0, 43.0, 114.0, 114.0, 15.3] - rl_kd: [5.7, 4.3, 11.4, 11.4, 1.5, - 5.7, 4.3, 11.4, 11.4, 1.5] - fixed_kp: [57.0, 43.0, 114.0, 114.0, 15.3, - 57.0, 43.0, 114.0, 114.0, 15.3] - fixed_kd: [5.7, 4.3, 11.4, 11.4, 1.5, - 5.7, 4.3, 11.4, 11.4, 1.5] - hip_scale_reduction: 1.0 - hip_scale_reduction_indices: [] - num_of_dofs: 10 - action_scale: 1.0 - lin_vel_scale: 1.0 - ang_vel_scale: 1.0 - dof_pos_scale: 1.0 - dof_vel_scale: 1.0 - commands_scale: [1.0, 1.0, 1.0] - torque_limits: [60.0, 45.0, 130.0, 130.0, 16.0, - 60.0, 45.0, 130.0, 130.0, 16.0] - default_dof_pos: [0.0, 0.0, -0.2618, 0.5236, -0.2618, - 0.0, 0.0, -0.2618, 0.5236, -0.2618] - joint_controller_names: ["l_hip_roll_controller", "l_hip_yaw_controller", "l_hip_pitch_controller", "l_knee_pitch_controller", "l_ankle_pitch_controller", - "r_hip_roll_controller", "r_hip_yaw_controller", "r_hip_pitch_controller", "r_knee_pitch_controller", "r_ankle_pitch_controller"] - -gr1t2: - model_name: "model_4000_jit.pt" - framework: "isaacgym" - rows: 2 - cols: 5 - dt: 0.001 - decimation: 20 - num_observations: 39 - clip_obs: 100.0 - clip_actions_lower: [-0.4391, -1.0491, -2.0991, -0.4391, -1.3991, - -1.1391, -1.0491, -2.0991, -0.4391, -1.3991] - clip_actions_upper: [1.1391, 1.0491, 1.0491, 2.2691, 0.8691, - 0.4391, 1.0491, 1.0491, 2.2691, 0.8691] - rl_kp: [57.0, 43.0, 114.0, 114.0, 15.3, - 57.0, 43.0, 114.0, 114.0, 15.3] - rl_kd: [5.7, 4.3, 11.4, 11.4, 1.5, - 5.7, 4.3, 11.4, 11.4, 1.5] - fixed_kp: [57.0, 43.0, 114.0, 114.0, 15.3, - 57.0, 43.0, 114.0, 114.0, 15.3] - fixed_kd: [5.7, 4.3, 11.4, 11.4, 1.5, - 5.7, 4.3, 11.4, 11.4, 1.5] - hip_scale_reduction: 1.0 - hip_scale_reduction_indices: [] - num_of_dofs: 10 - action_scale: 1.0 - lin_vel_scale: 1.0 - ang_vel_scale: 1.0 - dof_pos_scale: 1.0 - dof_vel_scale: 1.0 - commands_scale: [1.0, 1.0, 1.0] - torque_limits: [60.0, 45.0, 130.0, 130.0, 16.0, - 60.0, 45.0, 130.0, 130.0, 16.0] - default_dof_pos: [0.0, 0.0, -0.2618, 0.5236, -0.2618, - 0.0, 0.0, -0.2618, 0.5236, -0.2618] - joint_controller_names: ["l_hip_roll_controller", "l_hip_yaw_controller", "l_hip_pitch_controller", "l_knee_pitch_controller", "l_ankle_pitch_controller", - "r_hip_roll_controller", "r_hip_yaw_controller", "r_hip_pitch_controller", "r_knee_pitch_controller", "r_ankle_pitch_controller"] - -a1_isaaclab: - model_name: "policy.pt" - framework: "isaacsim" - rows: 4 - cols: 3 - dt: 0.005 - decimation: 4 - num_observations: 45 - 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: [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: 1.0 - hip_scale_reduction_indices: [] - 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: [1.0, 1.0, 1.0] - 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] - joint_controller_names: ["FL_hip_controller", "FL_thigh_controller", "FL_calf_controller", - "FR_hip_controller", "FR_thigh_controller", "FR_calf_controller", - "RL_hip_controller", "RL_thigh_controller", "RL_calf_controller", - "RR_hip_controller", "RR_thigh_controller", "RR_calf_controller"] - -gr1t1_isaaclab: - model_name: "policy.pt" - framework: "isaacsim" - rows: 2 - cols: 5 - dt: 0.001 - decimation: 20 - num_observations: 39 - clip_obs: 100.0 - clip_actions_lower: [-0.4391, -1.0491, -2.0991, -0.4391, -1.3991, - -1.1391, -1.0491, -2.0991, -0.4391, -1.3991] - clip_actions_upper: [1.1391, 1.0491, 1.0491, 2.2691, 0.8691, - 0.4391, 1.0491, 1.0491, 2.2691, 0.8691] - rl_kp: [57.0, 43.0, 114.0, 114.0, 15.3, - 57.0, 43.0, 114.0, 114.0, 15.3] - rl_kd: [5.7, 4.3, 11.4, 11.4, 1.5, - 5.7, 4.3, 11.4, 11.4, 1.5] - fixed_kp: [57.0, 43.0, 114.0, 114.0, 15.3, - 57.0, 43.0, 114.0, 114.0, 15.3] - fixed_kd: [5.7, 4.3, 11.4, 11.4, 1.5, - 5.7, 4.3, 11.4, 11.4, 1.5] - hip_scale_reduction: 1.0 - hip_scale_reduction_indices: [] - num_of_dofs: 10 - action_scale: 1.0 - lin_vel_scale: 1.0 - ang_vel_scale: 1.0 - dof_pos_scale: 1.0 - dof_vel_scale: 1.0 - commands_scale: [1.0, 1.0, 1.0] - torque_limits: [60.0, 45.0, 130.0, 130.0, 16.0, - 60.0, 45.0, 130.0, 130.0, 16.0] - default_dof_pos: [0.0, 0.0, -0.2618, 0.5236, -0.2618, - 0.0, 0.0, -0.2618, 0.5236, -0.2618] - joint_controller_names: ["l_hip_roll_controller", "l_hip_yaw_controller", "l_hip_pitch_controller", "l_knee_pitch_controller", "l_ankle_pitch_controller", - "r_hip_roll_controller", "r_hip_yaw_controller", "r_hip_pitch_controller", "r_knee_pitch_controller", "r_ankle_pitch_controller"] - -gr1t2_isaaclab: - model_name: "policy.pt" - framework: "isaacsim" - rows: 2 - cols: 5 - dt: 0.001 - decimation: 20 - num_observations: 39 - clip_obs: 100.0 - clip_actions_lower: [-0.4391, -1.0491, -2.0991, -0.4391, -1.3991, - -1.1391, -1.0491, -2.0991, -0.4391, -1.3991] - clip_actions_upper: [1.1391, 1.0491, 1.0491, 2.2691, 0.8691, - 0.4391, 1.0491, 1.0491, 2.2691, 0.8691] - rl_kp: [57.0, 43.0, 114.0, 114.0, 15.3, - 57.0, 43.0, 114.0, 114.0, 15.3] - rl_kd: [5.7, 4.3, 11.4, 11.4, 1.5, - 5.7, 4.3, 11.4, 11.4, 1.5] - fixed_kp: [57.0, 43.0, 114.0, 114.0, 15.3, - 57.0, 43.0, 114.0, 114.0, 15.3] - fixed_kd: [5.7, 4.3, 11.4, 11.4, 1.5, - 5.7, 4.3, 11.4, 11.4, 1.5] - hip_scale_reduction: 1.0 - hip_scale_reduction_indices: [] - num_of_dofs: 10 - action_scale: 1.0 - lin_vel_scale: 1.0 - ang_vel_scale: 1.0 - dof_pos_scale: 1.0 - dof_vel_scale: 1.0 - commands_scale: [1.0, 1.0, 1.0] - torque_limits: [60.0, 45.0, 130.0, 130.0, 16.0, - 60.0, 45.0, 130.0, 130.0, 16.0] - default_dof_pos: [0.0, 0.0, -0.2618, 0.5236, -0.2618, - 0.0, 0.0, -0.2618, 0.5236, -0.2618] - joint_controller_names: ["l_hip_roll_controller", "l_hip_yaw_controller", "l_hip_pitch_controller", "l_knee_pitch_controller", "l_ankle_pitch_controller", - "r_hip_roll_controller", "r_hip_yaw_controller", "r_hip_pitch_controller", "r_knee_pitch_controller", "r_ankle_pitch_controller"] diff --git a/src/rl_sar/include/rl_sim.hpp b/src/rl_sar/include/rl_sim.hpp index b6512a9..2477b88 100644 --- a/src/rl_sar/include/rl_sim.hpp +++ b/src/rl_sar/include/rl_sim.hpp @@ -31,7 +31,6 @@ private: void RobotControl(); // history buffer - bool use_history = true; ObservationBuffer history_obs_buf; torch::Tensor history_obs; @@ -65,6 +64,7 @@ private: void CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg); // others + std::string gazebo_model_name; int motiontime = 0; std::map sorted_to_original_index; std::vector mapped_joint_positions; diff --git a/src/rl_sar/launch/gazebo_a1_isaaclab.launch b/src/rl_sar/launch/gazebo_a1_isaacgym.launch similarity index 96% rename from src/rl_sar/launch/gazebo_a1_isaaclab.launch rename to src/rl_sar/launch/gazebo_a1_isaacgym.launch index 964b226..d6e511b 100644 --- a/src/rl_sar/launch/gazebo_a1_isaaclab.launch +++ b/src/rl_sar/launch/gazebo_a1_isaacgym.launch @@ -1,9 +1,9 @@ - - + + diff --git a/src/rl_sar/launch/gazebo_a1.launch b/src/rl_sar/launch/gazebo_a1_isaacsim.launch similarity index 94% rename from src/rl_sar/launch/gazebo_a1.launch rename to src/rl_sar/launch/gazebo_a1_isaacsim.launch index a660845..245ff5f 100644 --- a/src/rl_sar/launch/gazebo_a1.launch +++ b/src/rl_sar/launch/gazebo_a1_isaacsim.launch @@ -1,9 +1,9 @@ - - + + diff --git a/src/rl_sar/launch/gazebo_gr1t1.launch b/src/rl_sar/launch/gazebo_gr1t1_isaacgym.launch similarity index 94% rename from src/rl_sar/launch/gazebo_gr1t1.launch rename to src/rl_sar/launch/gazebo_gr1t1_isaacgym.launch index 1ad42d7..e92169c 100644 --- a/src/rl_sar/launch/gazebo_gr1t1.launch +++ b/src/rl_sar/launch/gazebo_gr1t1_isaacgym.launch @@ -1,9 +1,9 @@ - - + + diff --git a/src/rl_sar/launch/gazebo_gr1t2.launch b/src/rl_sar/launch/gazebo_gr1t2_isaacgym.launch similarity index 94% rename from src/rl_sar/launch/gazebo_gr1t2.launch rename to src/rl_sar/launch/gazebo_gr1t2_isaacgym.launch index 76f0d0e..92cea76 100644 --- a/src/rl_sar/launch/gazebo_gr1t2.launch +++ b/src/rl_sar/launch/gazebo_gr1t2_isaacgym.launch @@ -1,9 +1,9 @@ - - + + diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/rl_sdk/rl_sdk.cpp index d171ec6..30ad610 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.cpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.cpp @@ -340,18 +340,24 @@ std::vector ReadVectorFromYaml(const YAML::Node& node, const std::string& fra { return values; } + else + { + throw std::invalid_argument("Unsupported framework: " + framework); + } } void RL::ReadYaml(std::string robot_name) { + // The config file is located at "rl_sar/src/rl_sar/models//config.yaml" + std::string config_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_name + "/config.yaml"; YAML::Node config; try { - config = YAML::LoadFile(CONFIG_PATH)[robot_name]; + config = YAML::LoadFile(config_path)[robot_name]; } catch(YAML::BadFile &e) { - std::cout << LOGGER::ERROR << "The file '" << CONFIG_PATH << "' does not exist" << std::endl; + std::cout << LOGGER::ERROR << "The file '" << config_path << "' does not exist" << std::endl; return; } @@ -359,6 +365,7 @@ void RL::ReadYaml(std::string robot_name) this->params.framework = config["framework"].as(); int rows = config["rows"].as(); int cols = config["cols"].as(); + this->params.use_history = config["use_history"].as(); this->params.dt = config["dt"].as(); this->params.decimation = config["decimation"].as(); this->params.num_observations = config["num_observations"].as(); diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.hpp b/src/rl_sar/library/rl_sdk/rl_sdk.hpp index 51ab6c8..e861eb2 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.hpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.hpp @@ -7,7 +7,6 @@ #include #include -#define CONFIG_PATH CMAKE_CURRENT_SOURCE_DIR "/config.yaml" namespace LOGGER { const char* const INFO = "\033[0;37m[INFO]\033[0m "; @@ -71,6 +70,7 @@ struct ModelParams { std::string model_name; std::string framework; + bool use_history; double dt; int decimation; int num_observations; diff --git a/src/rl_sar/models/a1_isaacgym/config.yaml b/src/rl_sar/models/a1_isaacgym/config.yaml new file mode 100644 index 0000000..0f11b42 --- /dev/null +++ b/src/rl_sar/models/a1_isaacgym/config.yaml @@ -0,0 +1,55 @@ +a1_isaacgym: + model_name: "model_0702.pt" + framework: "isaacgym" + rows: 4 + cols: 3 + use_history: True + dt: 0.005 + decimation: 4 + num_observations: 45 + 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: [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_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, 1.0] + 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] + joint_controller_names: ["FL_hip_controller", "FL_thigh_controller", "FL_calf_controller", + "FR_hip_controller", "FR_thigh_controller", "FR_calf_controller", + "RL_hip_controller", "RL_thigh_controller", "RL_calf_controller", + "RR_hip_controller", "RR_thigh_controller", "RR_calf_controller"] diff --git a/src/rl_sar/models/a1/model_0501.pt b/src/rl_sar/models/a1_isaacgym/model_0501.pt similarity index 100% rename from src/rl_sar/models/a1/model_0501.pt rename to src/rl_sar/models/a1_isaacgym/model_0501.pt diff --git a/src/rl_sar/models/a1/model_0522.pt b/src/rl_sar/models/a1_isaacgym/model_0522.pt similarity index 100% rename from src/rl_sar/models/a1/model_0522.pt rename to src/rl_sar/models/a1_isaacgym/model_0522.pt diff --git a/src/rl_sar/models/a1/model_0526.pt b/src/rl_sar/models/a1_isaacgym/model_0526.pt similarity index 100% rename from src/rl_sar/models/a1/model_0526.pt rename to src/rl_sar/models/a1_isaacgym/model_0526.pt diff --git a/src/rl_sar/models/a1/model_0702.pt b/src/rl_sar/models/a1_isaacgym/model_0702.pt similarity index 100% rename from src/rl_sar/models/a1/model_0702.pt rename to src/rl_sar/models/a1_isaacgym/model_0702.pt diff --git a/src/rl_sar/models/a1_isaacsim/config.yaml b/src/rl_sar/models/a1_isaacsim/config.yaml new file mode 100644 index 0000000..2c5f3d8 --- /dev/null +++ b/src/rl_sar/models/a1_isaacsim/config.yaml @@ -0,0 +1,55 @@ +a1_isaacsim: + model_name: "policy.pt" + framework: "isaacsim" + rows: 4 + cols: 3 + use_history: False + dt: 0.005 + decimation: 4 + num_observations: 45 + 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: [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: 1.0 + hip_scale_reduction_indices: [] + 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: [1.0, 1.0, 1.0] + 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] + joint_controller_names: ["FL_hip_controller", "FL_thigh_controller", "FL_calf_controller", + "FR_hip_controller", "FR_thigh_controller", "FR_calf_controller", + "RL_hip_controller", "RL_thigh_controller", "RL_calf_controller", + "RR_hip_controller", "RR_thigh_controller", "RR_calf_controller"] diff --git a/src/rl_sar/models/a1_isaaclab/policy.pt b/src/rl_sar/models/a1_isaacsim/policy.pt similarity index 100% rename from src/rl_sar/models/a1_isaaclab/policy.pt rename to src/rl_sar/models/a1_isaacsim/policy.pt diff --git a/src/rl_sar/models/gr1t1_isaacgym/config.yaml b/src/rl_sar/models/gr1t1_isaacgym/config.yaml new file mode 100644 index 0000000..7062f4d --- /dev/null +++ b/src/rl_sar/models/gr1t1_isaacgym/config.yaml @@ -0,0 +1,37 @@ +gr1t1_isaacgym: + model_name: "model_4000_jit.pt" + framework: "isaacgym" + rows: 2 + cols: 5 + use_history: False + dt: 0.001 + decimation: 20 + num_observations: 39 + clip_obs: 100.0 + clip_actions_lower: [-0.4391, -1.0491, -2.0991, -0.4391, -1.3991, + -1.1391, -1.0491, -2.0991, -0.4391, -1.3991] + clip_actions_upper: [1.1391, 1.0491, 1.0491, 2.2691, 0.8691, + 0.4391, 1.0491, 1.0491, 2.2691, 0.8691] + rl_kp: [57.0, 43.0, 114.0, 114.0, 15.3, + 57.0, 43.0, 114.0, 114.0, 15.3] + rl_kd: [5.7, 4.3, 11.4, 11.4, 1.5, + 5.7, 4.3, 11.4, 11.4, 1.5] + fixed_kp: [57.0, 43.0, 114.0, 114.0, 15.3, + 57.0, 43.0, 114.0, 114.0, 15.3] + fixed_kd: [5.7, 4.3, 11.4, 11.4, 1.5, + 5.7, 4.3, 11.4, 11.4, 1.5] + hip_scale_reduction: 1.0 + hip_scale_reduction_indices: [] + num_of_dofs: 10 + action_scale: 1.0 + lin_vel_scale: 1.0 + ang_vel_scale: 1.0 + dof_pos_scale: 1.0 + dof_vel_scale: 1.0 + commands_scale: [1.0, 1.0, 1.0] + torque_limits: [60.0, 45.0, 130.0, 130.0, 16.0, + 60.0, 45.0, 130.0, 130.0, 16.0] + default_dof_pos: [0.0, 0.0, -0.2618, 0.5236, -0.2618, + 0.0, 0.0, -0.2618, 0.5236, -0.2618] + joint_controller_names: ["l_hip_roll_controller", "l_hip_yaw_controller", "l_hip_pitch_controller", "l_knee_pitch_controller", "l_ankle_pitch_controller", + "r_hip_roll_controller", "r_hip_yaw_controller", "r_hip_pitch_controller", "r_knee_pitch_controller", "r_ankle_pitch_controller"] diff --git a/src/rl_sar/models/gr1t1/model_4000_jit.pt b/src/rl_sar/models/gr1t1_isaacgym/model_4000_jit.pt similarity index 100% rename from src/rl_sar/models/gr1t1/model_4000_jit.pt rename to src/rl_sar/models/gr1t1_isaacgym/model_4000_jit.pt diff --git a/src/rl_sar/models/gr1t1_isaacsim/config.yaml b/src/rl_sar/models/gr1t1_isaacsim/config.yaml new file mode 100644 index 0000000..8dc3319 --- /dev/null +++ b/src/rl_sar/models/gr1t1_isaacsim/config.yaml @@ -0,0 +1,37 @@ +gr1t1_isaacsim: + model_name: "policy.pt" + framework: "isaacsim" + rows: 2 + cols: 5 + use_history: False + dt: 0.001 + decimation: 20 + num_observations: 39 + clip_obs: 100.0 + clip_actions_lower: [-0.4391, -1.0491, -2.0991, -0.4391, -1.3991, + -1.1391, -1.0491, -2.0991, -0.4391, -1.3991] + clip_actions_upper: [1.1391, 1.0491, 1.0491, 2.2691, 0.8691, + 0.4391, 1.0491, 1.0491, 2.2691, 0.8691] + rl_kp: [57.0, 43.0, 114.0, 114.0, 15.3, + 57.0, 43.0, 114.0, 114.0, 15.3] + rl_kd: [5.7, 4.3, 11.4, 11.4, 1.5, + 5.7, 4.3, 11.4, 11.4, 1.5] + fixed_kp: [57.0, 43.0, 114.0, 114.0, 15.3, + 57.0, 43.0, 114.0, 114.0, 15.3] + fixed_kd: [5.7, 4.3, 11.4, 11.4, 1.5, + 5.7, 4.3, 11.4, 11.4, 1.5] + hip_scale_reduction: 1.0 + hip_scale_reduction_indices: [] + num_of_dofs: 10 + action_scale: 1.0 + lin_vel_scale: 1.0 + ang_vel_scale: 1.0 + dof_pos_scale: 1.0 + dof_vel_scale: 1.0 + commands_scale: [1.0, 1.0, 1.0] + torque_limits: [60.0, 45.0, 130.0, 130.0, 16.0, + 60.0, 45.0, 130.0, 130.0, 16.0] + default_dof_pos: [0.0, 0.0, -0.2618, 0.5236, -0.2618, + 0.0, 0.0, -0.2618, 0.5236, -0.2618] + joint_controller_names: ["l_hip_roll_controller", "l_hip_yaw_controller", "l_hip_pitch_controller", "l_knee_pitch_controller", "l_ankle_pitch_controller", + "r_hip_roll_controller", "r_hip_yaw_controller", "r_hip_pitch_controller", "r_knee_pitch_controller", "r_ankle_pitch_controller"] diff --git a/src/rl_sar/models/gr1t2_isaacgym/config.yaml b/src/rl_sar/models/gr1t2_isaacgym/config.yaml new file mode 100644 index 0000000..87bbd25 --- /dev/null +++ b/src/rl_sar/models/gr1t2_isaacgym/config.yaml @@ -0,0 +1,37 @@ +gr1t2_isaacgym: + model_name: "model_4000_jit.pt" + framework: "isaacgym" + rows: 2 + cols: 5 + use_history: False + dt: 0.001 + decimation: 20 + num_observations: 39 + clip_obs: 100.0 + clip_actions_lower: [-0.4391, -1.0491, -2.0991, -0.4391, -1.3991, + -1.1391, -1.0491, -2.0991, -0.4391, -1.3991] + clip_actions_upper: [1.1391, 1.0491, 1.0491, 2.2691, 0.8691, + 0.4391, 1.0491, 1.0491, 2.2691, 0.8691] + rl_kp: [57.0, 43.0, 114.0, 114.0, 15.3, + 57.0, 43.0, 114.0, 114.0, 15.3] + rl_kd: [5.7, 4.3, 11.4, 11.4, 1.5, + 5.7, 4.3, 11.4, 11.4, 1.5] + fixed_kp: [57.0, 43.0, 114.0, 114.0, 15.3, + 57.0, 43.0, 114.0, 114.0, 15.3] + fixed_kd: [5.7, 4.3, 11.4, 11.4, 1.5, + 5.7, 4.3, 11.4, 11.4, 1.5] + hip_scale_reduction: 1.0 + hip_scale_reduction_indices: [] + num_of_dofs: 10 + action_scale: 1.0 + lin_vel_scale: 1.0 + ang_vel_scale: 1.0 + dof_pos_scale: 1.0 + dof_vel_scale: 1.0 + commands_scale: [1.0, 1.0, 1.0] + torque_limits: [60.0, 45.0, 130.0, 130.0, 16.0, + 60.0, 45.0, 130.0, 130.0, 16.0] + default_dof_pos: [0.0, 0.0, -0.2618, 0.5236, -0.2618, + 0.0, 0.0, -0.2618, 0.5236, -0.2618] + joint_controller_names: ["l_hip_roll_controller", "l_hip_yaw_controller", "l_hip_pitch_controller", "l_knee_pitch_controller", "l_ankle_pitch_controller", + "r_hip_roll_controller", "r_hip_yaw_controller", "r_hip_pitch_controller", "r_knee_pitch_controller", "r_ankle_pitch_controller"] diff --git a/src/rl_sar/models/gr1t2/model_4000_jit.pt b/src/rl_sar/models/gr1t2_isaacgym/model_4000_jit.pt similarity index 100% rename from src/rl_sar/models/gr1t2/model_4000_jit.pt rename to src/rl_sar/models/gr1t2_isaacgym/model_4000_jit.pt diff --git a/src/rl_sar/models/gr1t2_isaacsim/config.yaml b/src/rl_sar/models/gr1t2_isaacsim/config.yaml new file mode 100644 index 0000000..d09a118 --- /dev/null +++ b/src/rl_sar/models/gr1t2_isaacsim/config.yaml @@ -0,0 +1,37 @@ +gr1t2_isaacsim: + model_name: "policy.pt" + framework: "isaacsim" + rows: 2 + cols: 5 + use_history: False + dt: 0.001 + decimation: 20 + num_observations: 39 + clip_obs: 100.0 + clip_actions_lower: [-0.4391, -1.0491, -2.0991, -0.4391, -1.3991, + -1.1391, -1.0491, -2.0991, -0.4391, -1.3991] + clip_actions_upper: [1.1391, 1.0491, 1.0491, 2.2691, 0.8691, + 0.4391, 1.0491, 1.0491, 2.2691, 0.8691] + rl_kp: [57.0, 43.0, 114.0, 114.0, 15.3, + 57.0, 43.0, 114.0, 114.0, 15.3] + rl_kd: [5.7, 4.3, 11.4, 11.4, 1.5, + 5.7, 4.3, 11.4, 11.4, 1.5] + fixed_kp: [57.0, 43.0, 114.0, 114.0, 15.3, + 57.0, 43.0, 114.0, 114.0, 15.3] + fixed_kd: [5.7, 4.3, 11.4, 11.4, 1.5, + 5.7, 4.3, 11.4, 11.4, 1.5] + hip_scale_reduction: 1.0 + hip_scale_reduction_indices: [] + num_of_dofs: 10 + action_scale: 1.0 + lin_vel_scale: 1.0 + ang_vel_scale: 1.0 + dof_pos_scale: 1.0 + dof_vel_scale: 1.0 + commands_scale: [1.0, 1.0, 1.0] + torque_limits: [60.0, 45.0, 130.0, 130.0, 16.0, + 60.0, 45.0, 130.0, 130.0, 16.0] + default_dof_pos: [0.0, 0.0, -0.2618, 0.5236, -0.2618, + 0.0, 0.0, -0.2618, 0.5236, -0.2618] + joint_controller_names: ["l_hip_roll_controller", "l_hip_yaw_controller", "l_hip_pitch_controller", "l_knee_pitch_controller", "l_ankle_pitch_controller", + "r_hip_roll_controller", "r_hip_yaw_controller", "r_hip_pitch_controller", "r_knee_pitch_controller", "r_ankle_pitch_controller"] diff --git a/src/rl_sar/scripts/rl_sdk.py b/src/rl_sar/scripts/rl_sdk.py index 8fe1174..5cdbc4b 100644 --- a/src/rl_sar/scripts/rl_sdk.py +++ b/src/rl_sar/scripts/rl_sdk.py @@ -6,7 +6,6 @@ from pynput import keyboard from enum import Enum, auto BASE_PATH = os.path.join(os.path.dirname(__file__), "../") -CONFIG_PATH = os.path.join(BASE_PATH, "config.yaml") class LOGGER: INFO = "\033[0;37m[INFO]\033[0m " @@ -65,6 +64,7 @@ class ModelParams: def __init__(self): self.model_name = None self.framework = None + self.use_history = None self.dt = None self.decimation = None self.num_observations = None @@ -338,19 +338,24 @@ class RL: return transposed_values elif framework == "isaacgym": return values + else: + raise ValueError(f"Unsupported framework: {framework}") def ReadYaml(self, robot_name): + # The config file is located at "rl_sar/src/rl_sar/models//config.yaml" + config_path = os.path.join(BASE_PATH, "models", robot_name, "config.yaml") try: - with open(CONFIG_PATH, 'r') as f: + with open(config_path, 'r') as f: config = yaml.safe_load(f)[robot_name] except FileNotFoundError as e: - print(LOGGER.ERROR + "The file '{CONFIG_PATH}' does not exist") + print(LOGGER.ERROR + f"The file '{config_path}' does not exist") return self.params.model_name = config["model_name"] self.params.framework = config["framework"] rows = config["rows"] cols = config["cols"] + self.params.use_history = config["use_history"] self.params.dt = config["dt"] self.params.decimation = config["decimation"] self.params.num_observations = config["num_observations"] diff --git a/src/rl_sar/scripts/rl_sim.py b/src/rl_sar/scripts/rl_sim.py index d1482a2..b7fd821 100644 --- a/src/rl_sar/scripts/rl_sim.py +++ b/src/rl_sar/scripts/rl_sim.py @@ -36,8 +36,7 @@ class RL_Sim(RL): self.ReadYaml(self.robot_name) # history - self.use_history = rospy.get_param("use_history", "") - if self.use_history: + if self.params.use_history: self.history_obs_buf = ObservationBuffer(1, self.params.num_observations, 6) # Due to the fact that the robot_state_publisher sorts the joint names alphabetically, @@ -75,6 +74,7 @@ class RL_Sim(RL): self.joint_state_subscriber = rospy.Subscriber(joint_states_topic, JointState, self.JointStatesCallback, queue_size=10) # service + self.gazebo_model_name = rospy.get_param("gazebo_model_name", "") self.gazebo_set_model_state_client = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState) self.gazebo_pause_physics_client = rospy.ServiceProxy("/gazebo/pause_physics", Empty) self.gazebo_unpause_physics_client = rospy.ServiceProxy("/gazebo/unpause_physics", Empty) @@ -135,8 +135,7 @@ class RL_Sim(RL): def RobotControl(self): if self.control.control_state == STATE.STATE_RESET_SIMULATION: set_model_state = SetModelStateRequest().model_state - gazebo_model_name = f"{self.robot_name}_gazebo" - set_model_state.model_name = gazebo_model_name + set_model_state.model_name = self.gazebo_model_name set_model_state.pose.position.z = 1.0 set_model_state.reference_frame = "world" self.gazebo_set_model_state_client(set_model_state) @@ -217,7 +216,7 @@ class RL_Sim(RL): def Forward(self): torch.set_grad_enabled(False) clamped_obs = self.ComputeObservation() - if self.use_history: + if self.params.use_history: self.history_obs_buf.insert(clamped_obs) history_obs = self.history_obs_buf.get_obs_vec(np.arange(6)) actions = self.model.forward(history_obs) diff --git a/src/rl_sar/src/rl_real_a1.cpp b/src/rl_sar/src/rl_real_a1.cpp index 6203ac4..479ef4e 100644 --- a/src/rl_sar/src/rl_real_a1.cpp +++ b/src/rl_sar/src/rl_real_a1.cpp @@ -8,7 +8,7 @@ RL_Real rl_sar; RL_Real::RL_Real() : unitree_safe(UNITREE_LEGGED_SDK::LeggedType::A1), unitree_udp(UNITREE_LEGGED_SDK::LOWLEVEL) { // read params from yaml - this->robot_name = "a1"; + this->robot_name = "a1_isaacgym"; this->ReadYaml(this->robot_name); // history diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index f203ea4..2bfee62 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -12,8 +12,7 @@ RL_Sim::RL_Sim() this->ReadYaml(this->robot_name); // history - nh.param("use_history", this->use_history, ""); - if(this->use_history) + if(this->params.use_history) { this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6); } @@ -56,6 +55,7 @@ RL_Sim::RL_Sim() this->joint_state_subscriber = nh.subscribe(this->ros_namespace + "joint_states", 10, &RL_Sim::JointStatesCallback, this); // service + nh.param("gazebo_model_name", this->gazebo_model_name, ""); this->gazebo_set_model_state_client = nh.serviceClient("/gazebo/set_model_state"); this->gazebo_pause_physics_client = nh.serviceClient("/gazebo/pause_physics"); this->gazebo_unpause_physics_client = nh.serviceClient("/gazebo/unpause_physics"); @@ -150,8 +150,7 @@ void RL_Sim::RobotControl() if(this->control.control_state == STATE_RESET_SIMULATION) { gazebo_msgs::SetModelState set_model_state; - std::string gazebo_model_name = this->robot_name + "_gazebo"; - set_model_state.request.model_state.model_name = gazebo_model_name; + set_model_state.request.model_state.model_name = this->gazebo_model_name; set_model_state.request.model_state.pose.position.z = 1.0; set_model_state.request.model_state.reference_frame = "world"; this->gazebo_set_model_state_client.call(set_model_state); @@ -265,7 +264,7 @@ torch::Tensor RL_Sim::Forward() torch::autograd::GradMode::set_enabled(false); torch::Tensor clamped_obs = this->ComputeObservation(); torch::Tensor actions; - if(this->use_history) + if(this->params.use_history) { this->history_obs_buf.insert(clamped_obs); this->history_obs = this->history_obs_buf.get_obs_vec({0, 1, 2, 3, 4, 5}); From 0472882f8a67ef4355e49d38b129f8f60d8398da Mon Sep 17 00:00:00 2001 From: fan-ziqi Date: Fri, 16 Aug 2024 11:44:00 +0800 Subject: [PATCH 2/2] feat: add obs list in config.yaml and move ComputeObservation to rl_sdk --- README.md | 14 ++--- README_CN.md | 14 ++--- src/rl_sar/include/rl_real_a1.hpp | 1 - src/rl_sar/include/rl_sim.hpp | 1 - src/rl_sar/library/rl_sdk/rl_sdk.cpp | 59 ++++++++++++++------ src/rl_sar/library/rl_sdk/rl_sdk.hpp | 3 +- src/rl_sar/models/a1_isaacgym/config.yaml | 1 + src/rl_sar/models/a1_isaacsim/config.yaml | 1 + src/rl_sar/models/gr1t1_isaacgym/config.yaml | 1 + src/rl_sar/models/gr1t1_isaacsim/config.yaml | 1 + src/rl_sar/models/gr1t2_isaacgym/config.yaml | 1 + src/rl_sar/models/gr1t2_isaacsim/config.yaml | 1 + src/rl_sar/scripts/rl_sdk.py | 24 ++++++++ src/rl_sar/scripts/rl_sim.py | 16 +----- src/rl_sar/src/rl_real_a1.cpp | 15 ----- src/rl_sar/src/rl_sim.cpp | 18 +----- 16 files changed, 85 insertions(+), 86 deletions(-) diff --git a/README.md b/README.md index 45c4da2..7c7bb97 100644 --- a/README.md +++ b/README.md @@ -57,14 +57,7 @@ sudo ldconfig ## Compilation -Customize the following two functions in your code to adapt to different models: - -```cpp -torch::Tensor forward() override; -torch::Tensor compute_observation() override; -``` - -Then compile in the root directory +Compile in the root directory of the project ```bash cd .. @@ -142,8 +135,9 @@ In the following text, `` represents the name of the robot 1. Create a model package named `_description` in the `rl_sar/src/robots` directory. Place the robot's URDF file in the `rl_sar/src/robots/_description/urdf` directory and name it `.urdf`. Additionally, create a joint configuration file with the namespace `_gazebo` in the `rl_sar/src/robots/_description/config` directory. 2. Place the trained RL model files in the `rl_sar/src/rl_sar/models/` directory. 3. In the `rl_sar/src/rl_sar/models/` directory, create a `config.yaml` file, and modify its parameters based on the `rl_sar/src/rl_sar/models/a1_isaacgym/config.yaml` file. -4. If you need to run simulations, modify the launch files as needed by referring to those in the `rl_sar/src/rl_sar/launch` directory. -5. If you need to run on the physical robot, modify the file `rl_sar/src/rl_sar/src/rl_real_a1.cpp` as needed. +4. Modify the `forward()` function in the code as needed to adapt to different models. +5. If you need to run simulations, modify the launch files as needed by referring to those in the `rl_sar/src/rl_sar/launch` directory. +6. If you need to run on the physical robot, modify the file `rl_sar/src/rl_sar/src/rl_real_a1.cpp` as needed. ## Reference diff --git a/README_CN.md b/README_CN.md index 4a7f1e6..5d92c70 100644 --- a/README_CN.md +++ b/README_CN.md @@ -57,14 +57,7 @@ sudo ldconfig ## 编译 -自定义代码中的以下两个函数，以适配不同的模型： - -```cpp -torch::Tensor forward() override; -torch::Tensor compute_observation() override; -``` - -然后到根目录编译 +在项目根目录编译 ```bash cd .. @@ -143,8 +136,9 @@ rosrun rl_sar rl_real_a1 1. 在`rl_sar/src/robots`路径下创建名为`_description`的模型包，将模型的urdf放到`rl_sar/src/robots/_description/urdf`路径下并命名为`.urdf`，并在`rl_sar/src/robots/_description/config`路径下创建命名空间为`_gazebo`的关节配置文件 2. 将训练好的RL模型文件放到`rl_sar/src/rl_sar/models/`路径下 3. 在`rl_sar/src/rl_sar/models/`中新建config.yaml文件，参考`rl_sar/src/rl_sar/models/a1_isaacgym/config.yaml`文件修改其中参数 -4. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改 -5. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改 +4. 按需修改代码中的`forward()`函数，以适配不同的模型 +5. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改 +6. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改 ## 参考 diff --git a/src/rl_sar/include/rl_real_a1.hpp b/src/rl_sar/include/rl_real_a1.hpp index c330b21..f60218e 100644 --- a/src/rl_sar/include/rl_real_a1.hpp +++ b/src/rl_sar/include/rl_real_a1.hpp @@ -19,7 +19,6 @@ public: private: // rl functions torch::Tensor Forward() override; - torch::Tensor ComputeObservation() override; void GetState(RobotState *state) override; void SetCommand(const RobotCommand *command) override; void RunModel(); diff --git a/src/rl_sar/include/rl_sim.hpp b/src/rl_sar/include/rl_sim.hpp index 2477b88..da0c6f3 100644 --- a/src/rl_sar/include/rl_sim.hpp +++ b/src/rl_sar/include/rl_sim.hpp @@ -24,7 +24,6 @@ public: private: // rl functions torch::Tensor Forward() override; - torch::Tensor ComputeObservation() override; void GetState(RobotState *state) override; void SetCommand(const RobotCommand *command) override; void RunModel(); diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/rl_sdk/rl_sdk.cpp index 30ad610..78a9bd8 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.cpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.cpp @@ -1,21 +1,5 @@ #include "rl_sdk.hpp" -/* You may need to override this ComputeObservation() function -torch::Tensor RL_XXX::ComputeObservation() -{ - torch::Tensor obs = torch::cat({ - this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel, this->params.framework) * this->params.ang_vel_scale, - this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec, this->params.framework), - this->obs.commands * this->params.commands_scale, - (this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale, - this->obs.dof_vel * this->params.dof_vel_scale, - this->obs.actions - },1); - torch::Tensor clamped_obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs); - return clamped_obs; -} -*/ - /* You may need to override this Forward() function torch::Tensor RL_XXX::Forward() { @@ -27,6 +11,48 @@ torch::Tensor RL_XXX::Forward() } */ +torch::Tensor RL::ComputeObservation() +{ + std::vector obs_list; + + for(const std::string& observation : this->params.observations) + { + if(observation == "lin_vel") + { + obs_list.push_back(this->obs.lin_vel * this->params.lin_vel_scale); + } + else if(observation == "ang_vel") + { + // obs_list.push_back(this->obs.ang_vel * this->params.ang_vel_scale); // TODO is QuatRotateInverse necessery? + obs_list.push_back(this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel, this->params.framework) * this->params.ang_vel_scale); + } + else if(observation == "gravity_vec") + { + obs_list.push_back(this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec, this->params.framework)); + } + else if(observation == "commands") + { + obs_list.push_back(this->obs.commands * this->params.commands_scale); + } + else if(observation == "dof_pos") + { + obs_list.push_back((this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale); + } + else if(observation == "dof_vel") + { + obs_list.push_back(this->obs.dof_vel * this->params.dof_vel_scale); + } + else if(observation == "actions") + { + obs_list.push_back(this->obs.actions); + } + } + + torch::Tensor obs = torch::cat(obs_list, 1); + torch::Tensor clamped_obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs); + return clamped_obs; +} + void RL::InitObservations() { this->obs.lin_vel = torch::tensor({{0.0, 0.0, 0.0}}); @@ -369,6 +395,7 @@ void RL::ReadYaml(std::string robot_name) this->params.dt = config["dt"].as(); this->params.decimation = config["decimation"].as(); this->params.num_observations = config["num_observations"].as(); + this->params.observations = ReadVectorFromYaml(config["observations"]); this->params.clip_obs = config["clip_obs"].as(); this->params.clip_actions_upper = torch::tensor(ReadVectorFromYaml(config["clip_actions_upper"], this->params.framework, rows, cols)).view({1, -1}); this->params.clip_actions_lower = torch::tensor(ReadVectorFromYaml(config["clip_actions_lower"], this->params.framework, rows, cols)).view({1, -1}); diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.hpp b/src/rl_sar/library/rl_sdk/rl_sdk.hpp index e861eb2..703770b 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.hpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.hpp @@ -74,6 +74,7 @@ struct ModelParams double dt; int decimation; int num_observations; + std::vector observations; double damping; double stiffness; double action_scale; @@ -128,7 +129,7 @@ public: // rl functions virtual torch::Tensor Forward() = 0; - virtual torch::Tensor ComputeObservation() = 0; + torch::Tensor ComputeObservation(); virtual void GetState(RobotState *state) = 0; virtual void SetCommand(const RobotCommand *command) = 0; void StateController(const RobotState *state, RobotCommand *command); diff --git a/src/rl_sar/models/a1_isaacgym/config.yaml b/src/rl_sar/models/a1_isaacgym/config.yaml index 0f11b42..1462540 100644 --- a/src/rl_sar/models/a1_isaacgym/config.yaml +++ b/src/rl_sar/models/a1_isaacgym/config.yaml @@ -7,6 +7,7 @@ a1_isaacgym: dt: 0.005 decimation: 4 num_observations: 45 + observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] clip_obs: 100.0 clip_actions_lower: [-100, -100, -100, -100, -100, -100, diff --git a/src/rl_sar/models/a1_isaacsim/config.yaml b/src/rl_sar/models/a1_isaacsim/config.yaml index 2c5f3d8..845bbdd 100644 --- a/src/rl_sar/models/a1_isaacsim/config.yaml +++ b/src/rl_sar/models/a1_isaacsim/config.yaml @@ -7,6 +7,7 @@ a1_isaacsim: dt: 0.005 decimation: 4 num_observations: 45 + observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] clip_obs: 100.0 clip_actions_lower: [-100, -100, -100, -100, -100, -100, diff --git a/src/rl_sar/models/gr1t1_isaacgym/config.yaml b/src/rl_sar/models/gr1t1_isaacgym/config.yaml index 7062f4d..c6a7a65 100644 --- a/src/rl_sar/models/gr1t1_isaacgym/config.yaml +++ b/src/rl_sar/models/gr1t1_isaacgym/config.yaml @@ -7,6 +7,7 @@ gr1t1_isaacgym: dt: 0.001 decimation: 20 num_observations: 39 + observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] clip_obs: 100.0 clip_actions_lower: [-0.4391, -1.0491, -2.0991, -0.4391, -1.3991, -1.1391, -1.0491, -2.0991, -0.4391, -1.3991] diff --git a/src/rl_sar/models/gr1t1_isaacsim/config.yaml b/src/rl_sar/models/gr1t1_isaacsim/config.yaml index 8dc3319..f8b54a5 100644 --- a/src/rl_sar/models/gr1t1_isaacsim/config.yaml +++ b/src/rl_sar/models/gr1t1_isaacsim/config.yaml @@ -7,6 +7,7 @@ gr1t1_isaacsim: dt: 0.001 decimation: 20 num_observations: 39 + observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] clip_obs: 100.0 clip_actions_lower: [-0.4391, -1.0491, -2.0991, -0.4391, -1.3991, -1.1391, -1.0491, -2.0991, -0.4391, -1.3991] diff --git a/src/rl_sar/models/gr1t2_isaacgym/config.yaml b/src/rl_sar/models/gr1t2_isaacgym/config.yaml index 87bbd25..9b25700 100644 --- a/src/rl_sar/models/gr1t2_isaacgym/config.yaml +++ b/src/rl_sar/models/gr1t2_isaacgym/config.yaml @@ -7,6 +7,7 @@ gr1t2_isaacgym: dt: 0.001 decimation: 20 num_observations: 39 + observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] clip_obs: 100.0 clip_actions_lower: [-0.4391, -1.0491, -2.0991, -0.4391, -1.3991, -1.1391, -1.0491, -2.0991, -0.4391, -1.3991] diff --git a/src/rl_sar/models/gr1t2_isaacsim/config.yaml b/src/rl_sar/models/gr1t2_isaacsim/config.yaml index d09a118..c8abfab 100644 --- a/src/rl_sar/models/gr1t2_isaacsim/config.yaml +++ b/src/rl_sar/models/gr1t2_isaacsim/config.yaml @@ -7,6 +7,7 @@ gr1t2_isaacsim: dt: 0.001 decimation: 20 num_observations: 39 + observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] clip_obs: 100.0 clip_actions_lower: [-0.4391, -1.0491, -2.0991, -0.4391, -1.3991, -1.1391, -1.0491, -2.0991, -0.4391, -1.3991] diff --git a/src/rl_sar/scripts/rl_sdk.py b/src/rl_sar/scripts/rl_sdk.py index 5cdbc4b..69556b5 100644 --- a/src/rl_sar/scripts/rl_sdk.py +++ b/src/rl_sar/scripts/rl_sdk.py @@ -68,6 +68,7 @@ class ModelParams: self.dt = None self.decimation = None self.num_observations = None + self.observations = None self.damping = None self.stiffness = None self.action_scale = None @@ -134,6 +135,28 @@ class RL: self.output_torques = torch.zeros(1, 32) self.output_dof_pos = torch.zeros(1, 32) + def ComputeObservation(self): + obs_list = [] + for observation in self.params.observations: + if observation == "lin_vel": + obs_list.append(self.obs.lin_vel * self.params.lin_vel_scale) + elif observation == "ang_vel": + # obs_list.append(self.obs.ang_vel * self.params.ang_vel_scale) # TODO is QuatRotateInverse necessery? + obs_list.append(self.QuatRotateInverse(self.obs.base_quat, self.obs.ang_vel, self.params.framework) * self.params.ang_vel_scale) + elif observation == "gravity_vec": + obs_list.append(self.QuatRotateInverse(self.obs.base_quat, self.obs.gravity_vec, self.params.framework)) + elif observation == "commands": + obs_list.append(self.obs.commands * self.params.commands_scale) + elif observation == "dof_pos": + obs_list.append((self.obs.dof_pos - self.params.default_dof_pos) * self.params.dof_pos_scale) + elif observation == "dof_vel": + obs_list.append(self.obs.dof_vel * self.params.dof_vel_scale) + elif observation == "actions": + obs_list.append(self.obs.actions) + obs = torch.cat(obs_list, dim=-1) + clamped_obs = torch.clamp(obs, -self.params.clip_obs, self.params.clip_obs) + return clamped_obs + def InitObservations(self): self.obs.lin_vel = torch.zeros(1, 3, dtype=torch.float) self.obs.ang_vel = torch.zeros(1, 3, dtype=torch.float) @@ -359,6 +382,7 @@ class RL: self.params.dt = config["dt"] self.params.decimation = config["decimation"] self.params.num_observations = config["num_observations"] + self.params.observations = config["observations"] self.params.clip_obs = config["clip_obs"] self.params.action_scale = config["action_scale"] self.params.hip_scale_reduction = config["hip_scale_reduction"] diff --git a/src/rl_sar/scripts/rl_sim.py b/src/rl_sar/scripts/rl_sim.py index b7fd821..f9fde79 100644 --- a/src/rl_sar/scripts/rl_sim.py +++ b/src/rl_sar/scripts/rl_sim.py @@ -173,7 +173,7 @@ class RL_Sim(RL): def RunModel(self): if self.running_state == STATE.STATE_RL_RUNNING and self.simulation_running: - # self.obs.lin_vel = torch.tensor([[self.vel.linear.x, self.vel.linear.y, self.vel.linear.z]]) + self.obs.lin_vel = torch.tensor([[self.vel.linear.x, self.vel.linear.y, self.vel.linear.z]]) self.obs.ang_vel = torch.tensor(self.robot_state.imu.gyroscope).unsqueeze(0) # self.obs.commands = torch.tensor([[self.cmd_vel.linear.x, self.cmd_vel.linear.y, self.cmd_vel.angular.z]]) self.obs.commands = torch.tensor([[self.control.x, self.control.y, self.control.yaw]]) @@ -199,20 +199,6 @@ class RL_Sim(RL): tau_est = torch.tensor(self.mapped_joint_efforts).unsqueeze(0) self.CSVLogger(self.output_torques, tau_est, self.obs.dof_pos, self.output_dof_pos, self.obs.dof_vel) - def ComputeObservation(self): - obs = torch.cat([ - # self.obs.lin_vel * self.params.lin_vel_scale, - # self.obs.ang_vel * self.params.ang_vel_scale, # TODO is QuatRotateInverse necessery? - self.QuatRotateInverse(self.obs.base_quat, self.obs.ang_vel, self.params.framework) * self.params.ang_vel_scale, - self.QuatRotateInverse(self.obs.base_quat, self.obs.gravity_vec, self.params.framework), - self.obs.commands * self.params.commands_scale, - (self.obs.dof_pos - self.params.default_dof_pos) * self.params.dof_pos_scale, - self.obs.dof_vel * self.params.dof_vel_scale, - self.obs.actions - ], dim = -1) - clamped_obs = torch.clamp(obs, -self.params.clip_obs, self.params.clip_obs) - return clamped_obs - def Forward(self): torch.set_grad_enabled(False) clamped_obs = self.ComputeObservation() diff --git a/src/rl_sar/src/rl_real_a1.cpp b/src/rl_sar/src/rl_real_a1.cpp index 479ef4e..4a9f98f 100644 --- a/src/rl_sar/src/rl_real_a1.cpp +++ b/src/rl_sar/src/rl_real_a1.cpp @@ -170,21 +170,6 @@ void RL_Real::RunModel() } } -torch::Tensor RL_Real::ComputeObservation() -{ - torch::Tensor obs = torch::cat({ - // this->QuatRotateInverse(this->obs.base_quat, this->obs.lin_vel) * this->params.lin_vel_scale, - this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel, this->params.framework) * this->params.ang_vel_scale, - this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec, this->params.framework), - this->obs.commands * this->params.commands_scale, - (this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale, - this->obs.dof_vel * this->params.dof_vel_scale, - this->obs.actions - },1); - torch::Tensor clamped_obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs); - return clamped_obs; -} - torch::Tensor RL_Real::Forward() { torch::autograd::GradMode::set_enabled(false); diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 2bfee62..fd3ea09 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -212,7 +212,7 @@ void RL_Sim::RunModel() { if(this->running_state == STATE_RL_RUNNING && simulation_running) { - // this->obs.lin_vel = torch::tensor({{this->vel.linear.x, this->vel.linear.y, this->vel.linear.z}}); + this->obs.lin_vel = torch::tensor({{this->vel.linear.x, this->vel.linear.y, this->vel.linear.z}}); this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0); // this->obs.commands = torch::tensor({{this->cmd_vel.linear.x, this->cmd_vel.linear.y, this->cmd_vel.angular.z}}); this->obs.commands = torch::tensor({{this->control.x, this->control.y, this->control.yaw}}); @@ -243,22 +243,6 @@ void RL_Sim::RunModel() } } -torch::Tensor RL_Sim::ComputeObservation() -{ - torch::Tensor obs = torch::cat({ - // this->obs.lin_vel * this->params.lin_vel_scale, - // this->obs.ang_vel * this->params.ang_vel_scale, // TODO is QuatRotateInverse necessery? - this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel, this->params.framework) * this->params.ang_vel_scale, - this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec, this->params.framework), - this->obs.commands * this->params.commands_scale, - (this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale, - this->obs.dof_vel * this->params.dof_vel_scale, - this->obs.actions - },1); - torch::Tensor clamped_obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs); - return clamped_obs; -} - torch::Tensor RL_Sim::Forward() { torch::autograd::GradMode::set_enabled(false);