EmbodiedAI
/
rl_sar
mirror of https://github.com/fan-ziqi/rl_sar.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

devel: isaaclab

This commit is contained in:
fan-ziqi 2024-07-15 19:31:56 +08:00
parent 2abcf192ca
commit 898ebee28e
11 changed files with 338 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
README.md
View File

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

4
README_CN.md
View File

@ -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`.
控制：

140
src/rl_sar/config.yaml
View File

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

54
src/rl_sar/launch/gazebo_a1_isaaclab.launch Normal file
View File

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

69
src/rl_sar/library/rl_sdk/rl_sdk.cpp
View File

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

3
src/rl_sar/library/rl_sdk/rl_sdk.hpp
View File

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

BIN
src/rl_sar/models/a1_isaaclab/policy.pt Normal file
View File

Binary file not shown.

42
src/rl_sar/scripts/rl_sdk.py
View File

@ -62,6 +62,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
@ -160,10 +161,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
@ -322,6 +327,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:
@ -331,6 +346,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"]
@ -338,19 +356,19 @@ 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_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)

18
src/rl_sar/scripts/rl_sim.py
View File

@ -93,10 +93,16 @@ class RL_Sim(RL):
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
@ -188,8 +194,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.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,

36
src/rl_sar/src/rl_real_a1.cpp
View File

@ -84,10 +84,21 @@ void RL_Real::GetState(RobotState<double> *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;
}

22
src/rl_sar/src/rl_sim.cpp
View File

@ -99,10 +99,20 @@ RL_Sim::~RL_Sim()
void RL_Sim::GetState(RobotState<double> *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,