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,138 @@ gr1t1:
 
 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

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.ang_vel, this->params.framework) * this->params.ang_vel_scale,
-        this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec),
+        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_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"])).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_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"])).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"])).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"])).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"])).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"])).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.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 @@ public:
     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);

src/rl_sar/library/rl_sdk/rl_sdk.py

@@ -64,6 +64,7 @@ class Control:
 class ModelParams:
     def __init__(self):
         self.model_name = None
+        self.framework = None
         self.dt = None
         self.decimation = None
         self.num_observations = None
@@ -162,10 +163,14 @@ class RL:
         actions_scaled = actions * self.params.action_scale
         return actions_scaled + self.params.default_dof_pos
 
-    def QuatRotateInverse(self, q, v):
+    def QuatRotateInverse(self, q, v, framework):
+        if framework == "isaacsim":
+            q_w = q[:, 0]
+            q_vec = q[:, 1:4]
+        elif framework == "isaacgym":
+            q_w = q[:, 3]
+            q_vec = q[:, 0:3]
         shape = q.shape
-        q_w = q[:, -1]
-        q_vec = q[:, :3]
         a = v * (2.0 * q_w ** 2 - 1.0).unsqueeze(-1)
         b = torch.cross(q_vec, v, dim=-1) * q_w.unsqueeze(-1) * 2.0
         c = q_vec * torch.bmm(q_vec.view(shape[0], 1, 3), v.view(shape[0], 3, 1)).squeeze(-1) * 2.0
@@ -324,6 +329,16 @@ class RL:
         except AttributeError:
             pass
 
+    def ReadVectorFromYaml(self, values, framework, rows, cols):
+        if framework == "isaacsim":
+            transposed_values = [0] * cols * rows
+            for r in range(rows):
+                for c in range(cols):
+                    transposed_values[c * rows + r] = values[r * cols + c]
+            return transposed_values
+        elif framework == "isaacgym":
+            return values
+
     def ReadYaml(self, robot_name):
         try:
             with open(CONFIG_PATH, 'r') as f:
@@ -333,6 +348,9 @@ class RL:
             return
 
         self.params.model_name = config["model_name"]
+        self.params.framework = config["framework"]
+        rows = config["rows"]
+        cols = config["cols"]
         self.params.dt = config["dt"]
         self.params.decimation = config["decimation"]
         self.params.num_observations = config["num_observations"]
@@ -340,21 +358,21 @@ class RL:
         self.params.action_scale = config["action_scale"]
         self.params.hip_scale_reduction = config["hip_scale_reduction"]
         self.params.hip_scale_reduction_indices = config["hip_scale_reduction_indices"]
-        self.params.clip_actions_upper = torch.tensor(config["clip_actions_upper"]).view(1, -1)
+        self.params.clip_actions_upper = torch.tensor(self.ReadVectorFromYaml(config["clip_actions_upper"], self.params.framework, rows, cols)).view(1, -1)
-        self.params.clip_actions_lower = torch.tensor(config["clip_actions_lower"]).view(1, -1)
+        self.params.clip_actions_lower = torch.tensor(self.ReadVectorFromYaml(config["clip_actions_lower"], self.params.framework, rows, cols)).view(1, -1)
         self.params.num_of_dofs = config["num_of_dofs"]
         self.params.lin_vel_scale = config["lin_vel_scale"]
         self.params.ang_vel_scale = config["ang_vel_scale"]
         self.params.dof_pos_scale = config["dof_pos_scale"]
         self.params.dof_vel_scale = config["dof_vel_scale"]
         self.params.commands_scale = torch.tensor([self.params.lin_vel_scale, self.params.lin_vel_scale, self.params.ang_vel_scale])
-        self.params.rl_kp = torch.tensor(config["rl_kp"]).view(1, -1)
+        self.params.rl_kp = torch.tensor(self.ReadVectorFromYaml(config["rl_kp"], self.params.framework, rows, cols)).view(1, -1)
-        self.params.rl_kd = torch.tensor(config["rl_kd"]).view(1, -1)
+        self.params.rl_kd = torch.tensor(self.ReadVectorFromYaml(config["rl_kd"], self.params.framework, rows, cols)).view(1, -1)
-        self.params.fixed_kp = torch.tensor(config["fixed_kp"]).view(1, -1)
+        self.params.fixed_kp = torch.tensor(self.ReadVectorFromYaml(config["fixed_kp"], self.params.framework, rows, cols)).view(1, -1)
-        self.params.fixed_kd = torch.tensor(config["fixed_kd"]).view(1, -1)
+        self.params.fixed_kd = torch.tensor(self.ReadVectorFromYaml(config["fixed_kd"], self.params.framework, rows, cols)).view(1, -1)
-        self.params.torque_limits = torch.tensor(config["torque_limits"]).view(1, -1)
+        self.params.torque_limits = torch.tensor(self.ReadVectorFromYaml(config["torque_limits"], self.params.framework, rows, cols)).view(1, -1)
-        self.params.default_dof_pos = torch.tensor(config["default_dof_pos"]).view(1, -1)
+        self.params.default_dof_pos = torch.tensor(self.ReadVectorFromYaml(config["default_dof_pos"], self.params.framework, rows, cols)).view(1, -1)
-        self.params.joint_controller_names = config["joint_controller_names"]
+        self.params.joint_controller_names = self.ReadVectorFromYaml(config["joint_controller_names"], self.params.framework, rows, cols)
 
     def CSVInit(self, robot_name):
         self.csv_filename = os.path.join(BASE_PATH, "models", robot_name, 'motor')

src/rl_sar/scripts/rl_sim.py

@@ -99,10 +99,16 @@ class RL_Sim(RL):
         print(LOGGER.INFO + "RL_Sim exit")
 
     def GetState(self, state):
+        if self.params.framework == "isaacgym":
             state.imu.quaternion[3] = self.pose.orientation.w
             state.imu.quaternion[0] = self.pose.orientation.x
             state.imu.quaternion[1] = self.pose.orientation.y
             state.imu.quaternion[2] = self.pose.orientation.z
+        elif self.params.framework == "isaacsim":
+            state.imu.quaternion[0] = self.pose.orientation.w
+            state.imu.quaternion[1] = self.pose.orientation.x
+            state.imu.quaternion[2] = self.pose.orientation.y
+            state.imu.quaternion[3] = self.pose.orientation.z
 
         state.imu.gyroscope[0] = self.vel.angular.x
         state.imu.gyroscope[1] = self.vel.angular.y
@@ -198,8 +204,8 @@ class RL_Sim(RL):
         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.ang_vel_scale,
+            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.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,

src/rl_sar/src/rl_real_a1.cpp

@@ -84,10 +84,21 @@ void RL_Real::GetState(RobotState<double> *state)
         this->control.control_state = STATE_POS_GETDOWN;
     }
 
+    if(this->params.framework == "isaacgym")
+    {
         state->imu.quaternion[3] = this->unitree_low_state.imu.quaternion[0]; // w
         state->imu.quaternion[0] = this->unitree_low_state.imu.quaternion[1]; // x
         state->imu.quaternion[1] = this->unitree_low_state.imu.quaternion[2]; // y
         state->imu.quaternion[2] = this->unitree_low_state.imu.quaternion[3]; // z
+    }
+    else if(this->params.framework == "isaacsim")
+    {
+        state->imu.quaternion[0] = this->unitree_low_state.imu.quaternion[0]; // w
+        state->imu.quaternion[1] = this->unitree_low_state.imu.quaternion[1]; // x
+        state->imu.quaternion[2] = this->unitree_low_state.imu.quaternion[2]; // y
+        state->imu.quaternion[3] = this->unitree_low_state.imu.quaternion[3]; // z
+    }
+
     for(int i = 0; i < 3; ++i)
     {
         state->imu.gyroscope[i] = this->unitree_low_state.imu.gyroscope[i];
@@ -161,9 +172,10 @@ 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,
+    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.lin_vel) * this->params.lin_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,

src/rl_sar/src/rl_sim.cpp

@@ -98,11 +98,21 @@ RL_Sim::~RL_Sim()
 }
 
 void RL_Sim::GetState(RobotState<double> *state)
+{
+    if(this->params.framework == "isaacgym")
     {
         state->imu.quaternion[3] = this->pose.orientation.w;
         state->imu.quaternion[0] = this->pose.orientation.x;
         state->imu.quaternion[1] = this->pose.orientation.y;
         state->imu.quaternion[2] = this->pose.orientation.z;
+    }
+    else if(this->params.framework == "isaacsim")
+    {
+        state->imu.quaternion[0] = this->pose.orientation.w;
+        state->imu.quaternion[1] = this->pose.orientation.x;
+        state->imu.quaternion[2] = this->pose.orientation.y;
+        state->imu.quaternion[3] = this->pose.orientation.z;
+    }
 
     state->imu.gyroscope[0] = this->vel.angular.x;
     state->imu.gyroscope[1] = this->vel.angular.y;
@@ -239,8 +249,8 @@ 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.ang_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->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,