From 4124f78e1cae2d850992dfa3aa7595cbef936ab2 Mon Sep 17 00:00:00 2001 From: fan-ziqi Date: Wed, 13 Nov 2024 10:56:13 +0800 Subject: [PATCH] fix: the angular velocity of the IMU topic is in the body coordinate system. Close #39 --- src/rl_sar/scripts/rl_sim.py | 2 +- src/rl_sar/src/rl_sim.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/rl_sar/scripts/rl_sim.py b/src/rl_sar/scripts/rl_sim.py index abb7f64..b4ed030 100755 --- a/src/rl_sar/scripts/rl_sim.py +++ b/src/rl_sar/scripts/rl_sim.py @@ -54,7 +54,7 @@ class RL_Sim(RL, Node): self.ReadYaml(self.robot_name) for i in range(len(self.params.observations)): if self.params.observations[i] == "ang_vel": - self.params.observations[i] = "ang_vel_world" + self.params.observations[i] = "ang_vel_body" # init rl torch.set_grad_enabled(False) diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 57cf071..44806d4 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -41,10 +41,9 @@ RL_Sim::RL_Sim() this->ReadYaml(this->robot_name); for (std::string &observation : this->params.observations) { - // In Gazebo, the coordinate system for angular velocity is in the world coordinate system. if (observation == "ang_vel") { - observation = "ang_vel_world"; + observation = "ang_vel_body"; } }