diff --git a/README.md b/README.md index 7044bdc..47ea090 100644 --- a/README.md +++ b/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 \ can be `a1` or `gr1t1` or `gr1t2`. +Where \ can be `a1` or `a1_isaaclab` or `gr1t1` or `gr1t2`. Control: * Press **\** to toggle simulation start/stop. diff --git a/README_CN.md b/README_CN.md index d4a7f78..25a3105 100644 --- a/README_CN.md +++ b/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 ``` -其中 \ 可以是 `a1` 或 `gr1t1` 或 `gr1t2`. +其中 \ 可以是 `a1` 或 `a1_isaaclab` 或 `gr1t1` 或 `gr1t2`. 控制： diff --git a/src/rl_sar/config.yaml b/src/rl_sar/config.yaml index c723aa8..5eaff85 100644 --- a/src/rl_sar/config.yaml +++ b/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"] \ No newline at end of file + "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/launch/gazebo_a1_isaaclab.launch b/src/rl_sar/launch/gazebo_a1_isaaclab.launch new file mode 100644 index 0000000..964b226 --- /dev/null +++ b/src/rl_sar/launch/gazebo_a1_isaaclab.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/rl_sdk/rl_sdk.cpp index 0a549e3..d171ec6 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.cpp +++ b/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 ReadVectorFromYaml(const YAML::Node& node) return values; } +template +std::vector ReadVectorFromYaml(const YAML::Node& node, const std::string& framework, const int& rows, const int& cols) +{ + std::vector values; + for(const auto& val : node) + { + values.push_back(val.as()); + } + + if(framework == "isaacsim") + { + std::vector 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(); + this->params.framework = config["framework"].as(); + int rows = config["rows"].as(); + int cols = config["cols"].as(); this->params.dt = config["dt"].as(); this->params.decimation = config["decimation"].as(); this->params.num_observations = config["num_observations"].as(); this->params.clip_obs = config["clip_obs"].as(); - this->params.clip_actions_upper = torch::tensor(ReadVectorFromYaml(config["clip_actions_upper"])).view({1, -1}); - this->params.clip_actions_lower = torch::tensor(ReadVectorFromYaml(config["clip_actions_lower"])).view({1, -1}); + 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}); this->params.action_scale = config["action_scale"].as(); this->params.hip_scale_reduction = config["hip_scale_reduction"].as(); this->params.hip_scale_reduction_indices = ReadVectorFromYaml(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(); // this->params.commands_scale = torch::tensor(ReadVectorFromYaml(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(config["rl_kp"])).view({1, -1}); - this->params.rl_kd = torch::tensor(ReadVectorFromYaml(config["rl_kd"])).view({1, -1}); - this->params.fixed_kp = torch::tensor(ReadVectorFromYaml(config["fixed_kp"])).view({1, -1}); - this->params.fixed_kd = torch::tensor(ReadVectorFromYaml(config["fixed_kd"])).view({1, -1}); - this->params.torque_limits = torch::tensor(ReadVectorFromYaml(config["torque_limits"])).view({1, -1}); - this->params.default_dof_pos = torch::tensor(ReadVectorFromYaml(config["default_dof_pos"])).view({1, -1}); - this->params.joint_controller_names = ReadVectorFromYaml(config["joint_controller_names"]); + this->params.rl_kp = torch::tensor(ReadVectorFromYaml(config["rl_kp"], this->params.framework, rows, cols)).view({1, -1}); + this->params.rl_kd = torch::tensor(ReadVectorFromYaml(config["rl_kd"], this->params.framework, rows, cols)).view({1, -1}); + this->params.fixed_kp = torch::tensor(ReadVectorFromYaml(config["fixed_kp"], this->params.framework, rows, cols)).view({1, -1}); + this->params.fixed_kd = torch::tensor(ReadVectorFromYaml(config["fixed_kd"], this->params.framework, rows, cols)).view({1, -1}); + this->params.torque_limits = torch::tensor(ReadVectorFromYaml(config["torque_limits"], this->params.framework, rows, cols)).view({1, -1}); + this->params.default_dof_pos = torch::tensor(ReadVectorFromYaml(config["default_dof_pos"], this->params.framework, rows, cols)).view({1, -1}); + this->params.joint_controller_names = ReadVectorFromYaml(config["joint_controller_names"], this->params.framework, rows, cols); } void RL::CSVInit(std::string robot_name) diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.hpp b/src/rl_sar/library/rl_sdk/rl_sdk.hpp index 37761a2..51ab6c8 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.hpp +++ b/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 *state, RobotCommand *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); diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.py b/src/rl_sar/library/rl_sdk/rl_sdk.py index 3d74931..6cda041 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.py +++ b/src/rl_sar/library/rl_sdk/rl_sdk.py @@ -64,6 +64,7 @@ def __init__(self): 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 @@ def ComputePosition(self, actions): 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 @@ def KeyboardInterface(self, key): 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 @@ def ReadYaml(self, robot_name): 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 @@ def ReadYaml(self, robot_name): 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_lower = torch.tensor(config["clip_actions_lower"]).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(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_kd = torch.tensor(config["rl_kd"]).view(1, -1) - self.params.fixed_kp = torch.tensor(config["fixed_kp"]).view(1, -1) - self.params.fixed_kd = torch.tensor(config["fixed_kd"]).view(1, -1) - self.params.torque_limits = torch.tensor(config["torque_limits"]).view(1, -1) - self.params.default_dof_pos = torch.tensor(config["default_dof_pos"]).view(1, -1) - self.params.joint_controller_names = config["joint_controller_names"] + 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(self.ReadVectorFromYaml(config["rl_kd"], self.params.framework, rows, cols)).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(self.ReadVectorFromYaml(config["fixed_kd"], self.params.framework, rows, cols)).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(self.ReadVectorFromYaml(config["default_dof_pos"], self.params.framework, rows, cols)).view(1, -1) + 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') diff --git a/src/rl_sar/models/a1_isaaclab/policy.pt b/src/rl_sar/models/a1_isaaclab/policy.pt new file mode 100644 index 0000000..8dfe1eb Binary files /dev/null and b/src/rl_sar/models/a1_isaaclab/policy.pt differ diff --git a/src/rl_sar/scripts/rl_sim.py b/src/rl_sar/scripts/rl_sim.py index 552f198..beb5231 100644 --- a/src/rl_sar/scripts/rl_sim.py +++ b/src/rl_sar/scripts/rl_sim.py @@ -99,10 +99,16 @@ def __del__(self): print(LOGGER.INFO + "RL_Sim exit") def GetState(self, state): - 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 + 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 @@ 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.ang_vel_scale, - self.QuatRotateInverse(self.obs.base_quat, self.obs.gravity_vec), + 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, diff --git a/src/rl_sar/src/rl_real_a1.cpp b/src/rl_sar/src/rl_real_a1.cpp index 7b1718f..6203ac4 100644 --- a/src/rl_sar/src/rl_real_a1.cpp +++ b/src/rl_sar/src/rl_real_a1.cpp @@ -84,10 +84,21 @@ void RL_Real::GetState(RobotState *state) this->control.control_state = STATE_POS_GETDOWN; } - 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 + 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,14 +172,15 @@ 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.ang_vel_scale, - this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec), - 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 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; } diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 417bcf2..f203ea4 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -99,10 +99,20 @@ RL_Sim::~RL_Sim() void RL_Sim::GetState(RobotState *state) { - 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; + 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.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,