feat: isaaclab framework support

README.md

@@ -4,6 +4,8 @@
 
 Simulation verification and physical deployment of robot reinforcement learning algorithms, suitable for quadruped robots, wheeled robots, and humanoid robots. "sar" stands for "simulation and real"
 
+This framework supports legged_gym based on IaacGym and IsaacLab based on IsaacSim. Use `framework` to distinguish them when using them.
+
 [Click to discuss on Discord](https://discord.gg/vmVjkhVugU)
 
 ## Preparation
@@ -90,7 +92,7 @@ source devel/setup.bash
 (for python version) rosrun rl_sar rl_sim.py
 ```
 
-Where \<ROBOT\> can be `a1` or `gr1t1` or `gr1t2`.
+Where \<ROBOT\> can be `a1` or `a1_isaaclab` or `gr1t1` or `gr1t2`.
 
 Control:
 * Press **\<Enter\>** to toggle simulation start/stop.

README_CN.md

@@ -4,6 +4,8 @@
 
 机器人强化学习算法的仿真验证与实物部署，适配四足机器人、轮足机器人、人形机器人。"sar"代表"simulation and real"
 
+本框架支持基于IaacGym的legged_gym，也支持基于IsaacSim的IsaacLab，使用时用`framework`加以区分。
+
 [点击在Discord上讨论](https://discord.gg/MC9KguQHtt)
 
 ## 准备
@@ -90,7 +92,7 @@ source devel/setup.bash
 (for python version) rosrun rl_sar rl_sim.py
 ```
 
-其中 \<ROBOT\> 可以是 `a1` 或 `gr1t1` 或 `gr1t2`.
+其中 \<ROBOT\> 可以是 `a1` 或 `a1_isaaclab` 或 `gr1t1` 或 `gr1t2`.
 
 控制：
 

src/rl_sar/config.yaml

@@ -1,5 +1,8 @@
 a1:
   model_name: "model_0702.pt"
+  framework: "isaacgym"
+  rows: 4
+  cols: 3
   dt: 0.005
   decimation: 4
   num_observations: 45
@@ -52,6 +55,9 @@ a1:
 
 gr1t1:
   model_name: "model_4000_jit.pt"
+  framework: "isaacgym"
+  rows: 2
+  cols: 5
   dt: 0.001
   decimation: 20
   num_observations: 39
@@ -86,6 +92,9 @@ gr1t1:
 
 gr1t2:
   model_name: "model_4000_jit.pt"
+  framework: "isaacgym"
+  rows: 2
+  cols: 5
   dt: 0.001
   decimation: 20
   num_observations: 39
@@ -116,4 +125,133 @@ gr1t2:
   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"]
+                           "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"]

src/rl_sar/launch/gazebo_a1_isaaclab.launch

@@ -0,0 +1,54 @@
+<launch>
+    <arg name="wname" default="stairs"/>
+    <arg name="rname" default="a1"/>
+    <param name="robot_name" type="str" value="$(arg rname)_isaaclab"/>
+    <param name="use_history" type="bool" value="false"/>
+    <param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
+    <arg name="robot_path" value="(find $(arg rname)_description)"/>
+    <arg name="dollar" value="$"/>
+
+    <arg name="paused" default="true"/>
+    <arg name="use_sim_time" default="true"/>
+    <arg name="gui" default="true"/>
+    <arg name="headless" default="false"/>
+    <arg name="debug" default="false"/>
+    <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
+    <arg name="user_debug" default="false"/>
+
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(find rl_sar)/worlds/$(arg wname).world"/>
+        <arg name="debug" value="$(arg debug)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="paused" value="$(arg paused)"/>
+        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+        <arg name="headless" value="$(arg headless)"/>
+    </include>
+
+    <!-- Load the URDF into the ROS Parameter Server -->
+    <param name="robot_description"
+           command="$(find xacro)/xacro --inorder '$(arg dollar)$(arg robot_path)/xacro/robot.xacro' 
+           DEBUG:=$(arg user_debug)"/>
+
+    <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
+    <!-- Set trunk and joint positions at startup -->
+    <node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
+          args="-urdf -z 0.6 -model $(arg rname)_gazebo -param robot_description"/>
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+    <rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>
+
+    <!-- load the controllers -->
+    <node pkg="controller_manager" type="spawner" name="controller_spawner" respawn="false"
+          output="screen" ns="/$(arg rname)_gazebo" args="joint_state_controller
+          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 "/>
+
+    <!-- convert joint states to TF transforms for rviz, etc -->
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"
+          respawn="false" output="screen">
+        <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
+    </node>
+
+</launch>

src/rl_sar/library/rl_sdk/rl_sdk.cpp

@@ -4,8 +4,8 @@
 torch::Tensor RL_XXX::ComputeObservation()
 {
     torch::Tensor obs = torch::cat({
-        this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel) * this->params.ang_vel_scale,
-        this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec),
+        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,
@@ -66,11 +66,22 @@ torch::Tensor RL::ComputePosition(torch::Tensor actions)
     return actions_scaled + this->params.default_dof_pos;
 }
 
-torch::Tensor RL::QuatRotateInverse(torch::Tensor q, torch::Tensor v)
+torch::Tensor RL::QuatRotateInverse(torch::Tensor q, torch::Tensor v, const std::string& framework)
 {
+    torch::Tensor q_w;
+    torch::Tensor q_vec;
+    if(framework == "isaacsim")
+    {
+        q_w = q.index({torch::indexing::Slice(), 0});
+        q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(1, 4)});
+    }
+    else if(framework == "isaacgym")
+    {
+        q_w = q.index({torch::indexing::Slice(), 3});
+        q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(0, 3)});
+    }
     c10::IntArrayRef shape = q.sizes();
-    torch::Tensor q_w = q.index({torch::indexing::Slice(), -1});
-    torch::Tensor q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(0, 3)});
+
     torch::Tensor a = v * (2.0 * torch::pow(q_w, 2) - 1.0).unsqueeze(-1);
     torch::Tensor b = torch::cross(q_vec, v, -1) * q_w.unsqueeze(-1) * 2.0;
     torch::Tensor c = q_vec * torch::bmm(q_vec.view({shape[0], 1, 3}), v.view({shape[0], 3, 1})).squeeze(-1) * 2.0;
@@ -304,6 +315,33 @@ std::vector<T> ReadVectorFromYaml(const YAML::Node& node)
     return values;
 }
 
+template<typename T>
+std::vector<T> ReadVectorFromYaml(const YAML::Node& node, const std::string& framework, const int& rows, const int& cols)
+{
+    std::vector<T> values;
+    for(const auto& val : node)
+    {
+        values.push_back(val.as<T>());
+    }
+
+    if(framework == "isaacsim")
+    {
+        std::vector<T> transposed_values(cols * rows);
+        for(int r = 0; r < rows; ++r)
+        {
+            for(int c = 0; c < cols; ++c)
+            {
+                transposed_values[c * rows + r] = values[r * cols + c];
+            }
+        }
+        return transposed_values;
+    }
+    else if(framework == "isaacgym")
+    {
+        return values;
+    }
+}
+
 void RL::ReadYaml(std::string robot_name)
 {
     YAML::Node config;
@@ -318,12 +356,15 @@ void RL::ReadYaml(std::string robot_name)
     }
 
     this->params.model_name = config["model_name"].as<std::string>();
+    this->params.framework = config["framework"].as<std::string>();
+    int rows = config["rows"].as<int>();
+    int cols = config["cols"].as<int>();
     this->params.dt = config["dt"].as<double>();
     this->params.decimation = config["decimation"].as<int>();
     this->params.num_observations = config["num_observations"].as<int>();
     this->params.clip_obs = config["clip_obs"].as<double>();
-    this->params.clip_actions_upper = torch::tensor(ReadVectorFromYaml<double>(config["clip_actions_upper"])).view({1, -1});
-    this->params.clip_actions_lower = torch::tensor(ReadVectorFromYaml<double>(config["clip_actions_lower"])).view({1, -1});
+    this->params.clip_actions_upper = torch::tensor(ReadVectorFromYaml<double>(config["clip_actions_upper"], this->params.framework, rows, cols)).view({1, -1});
+    this->params.clip_actions_lower = torch::tensor(ReadVectorFromYaml<double>(config["clip_actions_lower"], this->params.framework, rows, cols)).view({1, -1});
     this->params.action_scale = config["action_scale"].as<double>();
     this->params.hip_scale_reduction = config["hip_scale_reduction"].as<double>();
     this->params.hip_scale_reduction_indices = ReadVectorFromYaml<int>(config["hip_scale_reduction_indices"]);
@@ -334,13 +375,13 @@ void RL::ReadYaml(std::string robot_name)
     this->params.dof_vel_scale = config["dof_vel_scale"].as<double>();
     // this->params.commands_scale = torch::tensor(ReadVectorFromYaml<double>(config["commands_scale"])).view({1, -1});
     this->params.commands_scale = torch::tensor({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale});
-    this->params.rl_kp = torch::tensor(ReadVectorFromYaml<double>(config["rl_kp"])).view({1, -1});
-    this->params.rl_kd = torch::tensor(ReadVectorFromYaml<double>(config["rl_kd"])).view({1, -1});
-    this->params.fixed_kp = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kp"])).view({1, -1});
-    this->params.fixed_kd = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kd"])).view({1, -1});
-    this->params.torque_limits = torch::tensor(ReadVectorFromYaml<double>(config["torque_limits"])).view({1, -1});
-    this->params.default_dof_pos = torch::tensor(ReadVectorFromYaml<double>(config["default_dof_pos"])).view({1, -1});
-    this->params.joint_controller_names = ReadVectorFromYaml<std::string>(config["joint_controller_names"]);
+    this->params.rl_kp = torch::tensor(ReadVectorFromYaml<double>(config["rl_kp"], this->params.framework, rows, cols)).view({1, -1});
+    this->params.rl_kd = torch::tensor(ReadVectorFromYaml<double>(config["rl_kd"], this->params.framework, rows, cols)).view({1, -1});
+    this->params.fixed_kp = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kp"], this->params.framework, rows, cols)).view({1, -1});
+    this->params.fixed_kd = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kd"], this->params.framework, rows, cols)).view({1, -1});
+    this->params.torque_limits = torch::tensor(ReadVectorFromYaml<double>(config["torque_limits"], this->params.framework, rows, cols)).view({1, -1});
+    this->params.default_dof_pos = torch::tensor(ReadVectorFromYaml<double>(config["default_dof_pos"], this->params.framework, rows, cols)).view({1, -1});
+    this->params.joint_controller_names = ReadVectorFromYaml<std::string>(config["joint_controller_names"], this->params.framework, rows, cols);
 }
 
 void RL::CSVInit(std::string robot_name)

src/rl_sar/library/rl_sdk/rl_sdk.hpp

@@ -70,6 +70,7 @@ struct Control
 struct ModelParams
 {
     std::string model_name;
+    std::string framework;
     double dt;
     int decimation;
     int num_observations;
@@ -133,7 +134,7 @@ class RL
     void StateController(const RobotState<double> *state, RobotCommand<double> *command);
     torch::Tensor ComputeTorques(torch::Tensor actions);
     torch::Tensor ComputePosition(torch::Tensor actions);
-    torch::Tensor QuatRotateInverse(torch::Tensor q, torch::Tensor v);
+    torch::Tensor QuatRotateInverse(torch::Tensor q, torch::Tensor v, const std::string& framework);
 
     // yaml params
     void ReadYaml(std::string robot_name);