From 2a658f9aee9e3713e31abb11767a9e2fb64b2c87 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 11 Oct 2024 13:20:52 +0800 Subject: [PATCH 01/19] Adds support for Reset in test fixture This PR adds support for the Reset API to the test fixture. As `TestFixture` is one of the main ways one can get access to the ECM in python when trying to write some scripts for Deep Reinforcement Learning I realized that without `Reset` supported in the `TestFixture` API, end users would have a very hard time using our python APIs (which are actually quite nice). For reference I'm hacking a demo template here: https://github.com/arjo129/gz_deep_rl_experiments/tree/ionic Signed-off-by: Arjo Chakravarty --- include/gz/sim/TestFixture.hh | 6 ++++++ python/src/gz/sim/TestFixture.cc | 11 +++++++++++ src/TestFixture.cc | 29 ++++++++++++++++++++++++++++- 3 files changed, 45 insertions(+), 1 deletion(-) diff --git a/include/gz/sim/TestFixture.hh b/include/gz/sim/TestFixture.hh index 24fb2298a4..6041062cad 100644 --- a/include/gz/sim/TestFixture.hh +++ b/include/gz/sim/TestFixture.hh @@ -96,6 +96,12 @@ class GZ_SIM_VISIBLE TestFixture public: TestFixture &OnPostUpdate(std::function _cb); + /// \brief Wrapper around a system's update callback + /// \param[in] _cb Function to be called every update + /// \return Reference to self. + public: TestFixture &OnReset(std::function _cb); + /// \brief Finalize all the functions and add fixture to server. /// Finalize must be called before running the server, otherwise none of the /// `On*` functions will be called. diff --git a/python/src/gz/sim/TestFixture.cc b/python/src/gz/sim/TestFixture.cc index 826558fbf4..1fa05d23c3 100644 --- a/python/src/gz/sim/TestFixture.cc +++ b/python/src/gz/sim/TestFixture.cc @@ -83,6 +83,17 @@ defineSimTestFixture(pybind11::object module) ), pybind11::return_value_policy::reference, "Wrapper around a system's post-update callback" + ) + .def( + "on_reset", WrapCallbacks( + [](TestFixture* self, std::function _cb) + { + self->OnReset(_cb); + } + ), + pybind11::return_value_policy::reference, + "Wrapper around a system's post-update callback" ); // TODO(ahcorde): This method is not compiling for the following reason: // The EventManager class has an unordered_map which holds a unique_ptr diff --git a/src/TestFixture.cc b/src/TestFixture.cc index 1d02a900ff..6c984b7996 100644 --- a/src/TestFixture.cc +++ b/src/TestFixture.cc @@ -29,7 +29,8 @@ class HelperSystem : public ISystemConfigure, public ISystemPreUpdate, public ISystemUpdate, - public ISystemPostUpdate + public ISystemPostUpdate, + public ISystemReset { // Documentation inherited public: void Configure( @@ -50,6 +51,10 @@ class HelperSystem : public: void PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) override; + // Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + /// \brief Function to call every time we configure a world public: std::function &_sdf, @@ -68,6 +73,10 @@ class HelperSystem : /// \brief Function to call every post-update public: std::function postUpdateCallback; + + /// \brief Reset callback + public: std::function resetCallback; }; ///////////////////////////////////////////////// @@ -105,6 +114,14 @@ void HelperSystem::PostUpdate(const UpdateInfo &_info, this->postUpdateCallback(_info, _ecm); } +///////////////////////////////////////////////// +void HelperSystem::Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + if (this->resetCallback) + this->resetCallback(_info, _ecm); +} + ////////////////////////////////////////////////// class gz::sim::TestFixture::Implementation { @@ -200,6 +217,16 @@ TestFixture &TestFixture::OnPostUpdate(std::function _cb) +{ + if (nullptr != this->dataPtr->helperSystem) + this->dataPtr->helperSystem->resetCallback = std::move(_cb); + return *this; +} + + ////////////////////////////////////////////////// std::shared_ptr TestFixture::Server() const { From 053152f66161de69fa8d9cb243a4020992b3ebec Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 11 Oct 2024 14:38:18 +0800 Subject: [PATCH 02/19] Add support for simulation reset via a publicly callable API This allows us to reset simulations without having to call into gz-transport making the code more readable from an external API. Depends on #2647 Signed-off-by: Arjo Chakravarty --- include/gz/sim/Server.hh | 3 +++ python/src/gz/sim/Server.cc | 4 +++- src/Server.cc | 11 +++++++++++ src/SimulationRunner.cc | 15 ++++++++++++++- src/SimulationRunner.hh | 3 +++ src/TestFixture_TEST.cc | 32 +++++++++++++++++++++++++++++--- 6 files changed, 63 insertions(+), 5 deletions(-) diff --git a/include/gz/sim/Server.hh b/include/gz/sim/Server.hh index 28b5b3feb1..7bc006c8a7 100644 --- a/include/gz/sim/Server.hh +++ b/include/gz/sim/Server.hh @@ -338,6 +338,9 @@ namespace gz /// \brief Stop the server. This will stop all running simulations. public: void Stop(); + /// \brief Reset All runners in this simulation + public: void ResetAll(); + /// \brief Private data private: std::unique_ptr dataPtr; }; diff --git a/python/src/gz/sim/Server.cc b/python/src/gz/sim/Server.cc index c148a08ff2..7ece7ec090 100644 --- a/python/src/gz/sim/Server.cc +++ b/python/src/gz/sim/Server.cc @@ -46,7 +46,9 @@ void defineSimServer(pybind11::object module) .def( "is_running", pybind11::overload_cast<>(&gz::sim::Server::Running, pybind11::const_), - "Get whether the server is running."); + "Get whether the server is running.") + .def("reset_all", &gz::sim::Server::ResetAll, + "Resets all simulation runners under this server."); } } // namespace python } // namespace sim diff --git a/src/Server.cc b/src/Server.cc index 712f273467..602eb1e569 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -485,6 +485,17 @@ bool Server::RequestRemoveEntity(const Entity _entity, return false; } +////////////////////////////////////////////////// +void Server::ResetAll() +{ + for (auto worldId = 0u; + worldId < this->dataPtr->simRunners.size(); + worldId++) + { + this->dataPtr->simRunners[worldId]->Reset(true, false, false); + } +} + ////////////////////////////////////////////////// void Server::Stop() { diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index baa2712adc..d98f7c1d02 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -95,7 +95,6 @@ struct MaybeGilScopedRelease #endif } - ////////////////////////////////////////////////// SimulationRunner::SimulationRunner(const sdf::World &_world, const SystemLoaderPtr &_systemLoader, @@ -1661,3 +1660,17 @@ void SimulationRunner::CreateEntities(const sdf::World &_world) // Store the initial state of the ECM; this->initialEntityCompMgr.CopyFrom(this->entityCompMgr); } + +///////////////////////////////////////////////// +void SimulationRunner::Reset(const bool all, + const bool time, const bool model) +{ + WorldControl control; + std::lock_guard lock(this->msgBufferMutex); + control.rewind = all || time; + if (model) + { + gzwarn << "Model reset not supported" <worldControls.push_back(control); +} \ No newline at end of file diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 8fe03511e7..8b5ac8bf36 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -369,6 +369,9 @@ namespace gz /// \brief Set the next step to be blocking and paused. public: void SetNextStepAsBlockingPaused(const bool value); + /// \brief Reset the current simulation runner + public: void Reset(const bool all, const bool time, const bool model); + /// \brief Updates the physics parameters of the simulation based on the /// Physics component of the world, if any. public: void UpdatePhysicsParams(); diff --git a/src/TestFixture_TEST.cc b/src/TestFixture_TEST.cc index f810ae64c1..21a9b7a9bd 100644 --- a/src/TestFixture_TEST.cc +++ b/src/TestFixture_TEST.cc @@ -72,6 +72,8 @@ TEST_F(TestFixtureTest, Callbacks) unsigned int preUpdates{0u}; unsigned int updates{0u}; unsigned int postUpdates{0u}; + unsigned int resets{0u}; + testFixture. OnConfigure([&](const Entity &_entity, const std::shared_ptr &_sdf, @@ -85,20 +87,33 @@ TEST_F(TestFixtureTest, Callbacks) { this->Updates(_info, _ecm); preUpdates++; - EXPECT_EQ(preUpdates, _info.iterations); + if (resets == 0) + { + EXPECT_EQ(preUpdates, _info.iterations); + } }). OnUpdate([&](const UpdateInfo &_info, EntityComponentManager &_ecm) { this->Updates(_info, _ecm); updates++; - EXPECT_EQ(updates, _info.iterations); + if (resets == 0) + { + EXPECT_EQ(updates, _info.iterations); + } }). OnPostUpdate([&](const UpdateInfo &_info, const EntityComponentManager &_ecm) { this->Updates(_info, _ecm); postUpdates++; - EXPECT_EQ(postUpdates, _info.iterations); + if (resets == 0) + { + EXPECT_EQ(postUpdates, _info.iterations); + } + }). + OnReset([&](const UpdateInfo &, EntityComponentManager &) + { + resets++; }). Finalize(); @@ -109,8 +124,19 @@ TEST_F(TestFixtureTest, Callbacks) EXPECT_EQ(expectedIterations, preUpdates); EXPECT_EQ(expectedIterations, updates); EXPECT_EQ(expectedIterations, postUpdates); + EXPECT_EQ(0u, resets); + + testFixture.Server()->ResetAll(); + + testFixture.Server()->Run(true, expectedIterations, false); + EXPECT_EQ(1u, configures); + EXPECT_EQ(expectedIterations * 2 - 1, preUpdates); + EXPECT_EQ(expectedIterations * 2 - 1, updates); + EXPECT_EQ(expectedIterations * 2 - 1, postUpdates); + EXPECT_EQ(1u, resets); } + ///////////////////////////////////////////////// TEST_F(TestFixtureTest, LoadConfig) { From ff468b79c63314ff5e0e1245c8b42957fbb6230a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 11 Oct 2024 14:46:24 +0800 Subject: [PATCH 03/19] Style Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index d98f7c1d02..9fbf41bac3 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -1673,4 +1673,4 @@ void SimulationRunner::Reset(const bool all, gzwarn << "Model reset not supported" <worldControls.push_back(control); -} \ No newline at end of file +} From e0ee0dd51e284a45a7638738c23f526245bc69da Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 11 Oct 2024 15:00:36 +0800 Subject: [PATCH 04/19] Style Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 9fbf41bac3..5656ee6786 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -1662,13 +1662,13 @@ void SimulationRunner::CreateEntities(const sdf::World &_world) } ///////////////////////////////////////////////// -void SimulationRunner::Reset(const bool all, - const bool time, const bool model) +void SimulationRunner::Reset(const bool _all, + const bool _time, const bool _model) { WorldControl control; std::lock_guard lock(this->msgBufferMutex); - control.rewind = all || time; - if (model) + control.rewind = _all || _time; + if (_model) { gzwarn << "Model reset not supported" < Date: Fri, 11 Oct 2024 15:01:26 +0800 Subject: [PATCH 05/19] Style Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 5656ee6786..96ac61d7d4 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -95,6 +95,7 @@ struct MaybeGilScopedRelease #endif } + ////////////////////////////////////////////////// SimulationRunner::SimulationRunner(const sdf::World &_world, const SystemLoaderPtr &_systemLoader, From 76adc263390d6ef49158cd82ab1a1424b061a809 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 11 Oct 2024 17:11:39 +0800 Subject: [PATCH 06/19] Typo Signed-off-by: Arjo Chakravarty --- include/gz/sim/Server.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/gz/sim/Server.hh b/include/gz/sim/Server.hh index 7bc006c8a7..ae49bd59f6 100644 --- a/include/gz/sim/Server.hh +++ b/include/gz/sim/Server.hh @@ -338,7 +338,7 @@ namespace gz /// \brief Stop the server. This will stop all running simulations. public: void Stop(); - /// \brief Reset All runners in this simulation + /// \brief Reset all runners in this simulation public: void ResetAll(); /// \brief Private data From 83b30c2bfe8031977b536ef38851642e243f9bc6 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 7 Nov 2024 14:56:51 +0800 Subject: [PATCH 07/19] Adds an initial StableBaselines3 RL environment as an example A lot of things are not working. Particularly when `ResetAll` is called, the EnableVelocityChecks does not trigger the phyics system to populate the velocity components. This is a blocker for the current example. Signed-off-by: Arjo Chakravarty --- .../simple_cart_pole/README.md | 37 ++ .../simple_cart_pole/cart_pole.sdf | 368 ++++++++++++++++++ .../simple_cart_pole/cart_pole_env.py | 120 ++++++ 3 files changed, 525 insertions(+) create mode 100644 examples/scripts/reinforcement_learning/simple_cart_pole/README.md create mode 100644 examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole.sdf create mode 100644 examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py diff --git a/examples/scripts/reinforcement_learning/simple_cart_pole/README.md b/examples/scripts/reinforcement_learning/simple_cart_pole/README.md new file mode 100644 index 0000000000..3bb276b1d4 --- /dev/null +++ b/examples/scripts/reinforcement_learning/simple_cart_pole/README.md @@ -0,0 +1,37 @@ +# Example for Reinforcement Learning (RL) With Gazebo + +This demo world shows you an example of how you can use SDFormat, Ray-RLLIB and Gazebo to perform RL with python. +We start with a very simple cart-pole world. This world is defined in our sdf file `cart_pole.sdf`. It is analogous to +the + +## Create a VENV + +First create a virtual environment using python, +``` +python3 -m venv venv +``` +Lets activate it and install rayrllib and pytorch. +``` +. venv/bin/activate +``` + +Lets install our dependencies +``` +pip install "ray[rllib]" torch +``` + +In the same terminal you should add your gazebo python install directory to the `PYTHONPATH` +If you built gazebo from source in the current working directory this would be: +``` +export PYTHONPATH=$PYTHONPATH:install/lib/python +``` + +You will also need to set PROTOCOL_BUFFERS_PYTHON_IMPLEMENTATION to python due to version +mis-matches. +``` +export PROTOCOL_BUFFERS_PYTHON_IMPLEMENTATION=python +``` + +## Exploring the environment + +You can see the environment by using `gz sim cart_pole.sdf`. \ No newline at end of file diff --git a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole.sdf b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole.sdf new file mode 100644 index 0000000000..6c1a1cd0de --- /dev/null +++ b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole.sdf @@ -0,0 +1,368 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 1.5 0 -0 0 + + 0.1 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 0.2 0.2 1.5 + + + + 0.5 1.0 0.5 1 + 0.5 1.0 0.5 1 + 0.0 1.0 0.0 1 + + + + + + 0.2 0.2 1.5 + + + + + + + -0.151427 0 2.2 0 -0 0 + + 10.0 + + 1.26164 + 0 + 0 + 4.16519 + 0 + 4.81014 + + + + + + 0.3 0.3 0.3 + + + + 0.5 1.0 0.5 1 + 0.5 1.0 0.5 1 + 0.0 1.0 0.0 1 + + + + + + 0.3 0.3 0.3 + + + + + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 1.0 0.5 1 + 0.5 1.0 0.5 1 + 0.0 1.0 0.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + 1 + 1 + 0.1 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + 1 + 1 + 0.1 + + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + 0 0 -0.75 0 0 0 + chassis + pole + + 0 1 0 + + -1.79769e+308 + 1.79769e+308 + + + + + + pole + pole_mass + + + + + + diff --git a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py new file mode 100644 index 0000000000..b45fa9599c --- /dev/null +++ b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py @@ -0,0 +1,120 @@ + +import os +import gymnasium as gym +import numpy as np + +from gz.common6 import set_verbosity +from gz.sim9 import TestFixture, World, world_entity, Model, Link +from gz.math8 import Vector3d +from gz.transport14 import Node +from gz.msgs11.world_control_pb2 import WorldControl +from gz.msgs11.world_reset_pb2 import WorldReset +from gz.msgs11.boolean_pb2 import Boolean + +from stable_baselines3 import A2C + +file_path = os.path.dirname(os.path.realpath(__file__)) + +class GzRewardScorer: + def __init__(self): + self.fixture = TestFixture(os.path.join(file_path, 'cart_pole.sdf')) + self.fixture.on_pre_update(self.on_pre_update) + self.fixture.on_post_update(self.on_post_update) + #self.fixture.on_configure(self.on_configure) + self.command = None + self.first_time = True # Hack cause configure does not work well + self.fixture.finalize() + self.server = self.fixture.server() + self.terminated = False + + def on_pre_update(self, info, ecm): + if self.first_time: + print("Enabling checks") + world = World(world_entity(ecm)) + self.model = Model(world.model_by_name(ecm, "vehicle_green")) + self.pole_entity = self.model.link_by_name(ecm, "pole") + self.chassis_entity = self.model.link_by_name(ecm, "chassis") + self.pole = Link(self.pole_entity) + self.pole.enable_velocity_checks(ecm) + self.chassis = Link(self.chassis_entity) + self.chassis.enable_velocity_checks(ecm) + self.first_time = False + if self.command == 1: + self.chassis.add_world_force(Vector3d(0, 100, 0)) + elif self.command == 0: + self.chassis.add_world_force(Vector3d(0, -100, 0)) + + def on_post_update(self, info, ecm): + pole_pose = self.pole.world_pose(ecm).rot().euler().y() + if self.pole.world_angular_velocity(ecm) is not None: + pole_angular_vel = self.pole.world_angular_velocity(ecm).y() + else: + pole_angular_vel = 0 + print("Warning failed to get angular velocity") + cart_pose = self.chassis.world_pose(ecm).pos().x() + cart_vel = self.chassis.world_linear_velocity(ecm) + + if cart_vel is not None: + cart_vel = cart_vel.x() + else: + cart_vel = 0 + print("Warning failed to get cart velocity") + + #print("pole", pole_pose) + #print("cart", cart_pose) + #print("Pole angvel ", pole_angular_vel) + self.state = np.array([cart_pose, cart_vel, pole_pose, pole_angular_vel], dtype=np.float32) + if not self.terminated: + self.terminated = pole_pose > 0.24 or pole_pose < -0.24 or cart_pose > 4.8 or cart_pose < -4.8 + + if self.terminated: + self.reward = 0.0 + else: + self.reward = 1.0 + + def step(self, action, paused=False): + self.action = action + self.server.run(True, 1, paused) + obs = self.state + reward = self.reward + return obs, reward, self.terminated, False, {} + + def reset(self): + print("Resetting") + self.server.reset_all() + self.first_time = True + self.command = None + self.terminated = False + obs, reward_, term_, tunc_, other_= self.step(None, paused=False) + return obs, {} + + + +class CustomCartPole(gym.Env): + def __init__(self, env_config): + self.env = GzRewardScorer() + #self.server = + self.action_space = gym.spaces.Discrete(2)#self.env.action_space + self.observation_space = gym.spaces.Box( + np.array([-10, float("-inf"), -0.418, -3.4028235e+38]), + np.array([10, float("inf"), 0.418, 3.4028235e+38]), + (4,), np.float32) + + def reset(self, seed=123): + return self.env.reset() + + def step(self, action): + obs, reward, done, truncated, info = self.env.step(action) + return obs, reward, done, truncated, info + + +env = CustomCartPole({}) +model = A2C("MlpPolicy", env, verbose=1) +model.learn(total_timesteps=10_000) + +vec_env = model.get_env() +obs = vec_env.reset() +for i in range(5000): + action, _state = model.predict(obs, deterministic=True) + obs, reward, done, info = vec_env.step(action) + # Nice to have spawn a gz sim client \ No newline at end of file From c3eea00d43be4df88eb9c5f20e13ced7fb159b8c Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 11 Nov 2024 13:12:25 +0800 Subject: [PATCH 08/19] Fixed readme instructions Signed-off-by: Arjo Chakravarty --- .../simple_cart_pole/README.md | 2 +- .../simple_cart_pole/cart_pole_env.py | 27 +++++++------------ 2 files changed, 10 insertions(+), 19 deletions(-) diff --git a/examples/scripts/reinforcement_learning/simple_cart_pole/README.md b/examples/scripts/reinforcement_learning/simple_cart_pole/README.md index 3bb276b1d4..2e8f396809 100644 --- a/examples/scripts/reinforcement_learning/simple_cart_pole/README.md +++ b/examples/scripts/reinforcement_learning/simple_cart_pole/README.md @@ -17,7 +17,7 @@ Lets activate it and install rayrllib and pytorch. Lets install our dependencies ``` -pip install "ray[rllib]" torch +pip install stable-baselines3[extra] ``` In the same terminal you should add your gazebo python install directory to the `PYTHONPATH` diff --git a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py index b45fa9599c..d032ae5079 100644 --- a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py +++ b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py @@ -20,25 +20,21 @@ def __init__(self): self.fixture = TestFixture(os.path.join(file_path, 'cart_pole.sdf')) self.fixture.on_pre_update(self.on_pre_update) self.fixture.on_post_update(self.on_post_update) - #self.fixture.on_configure(self.on_configure) self.command = None - self.first_time = True # Hack cause configure does not work well self.fixture.finalize() self.server = self.fixture.server() self.terminated = False def on_pre_update(self, info, ecm): - if self.first_time: - print("Enabling checks") - world = World(world_entity(ecm)) - self.model = Model(world.model_by_name(ecm, "vehicle_green")) - self.pole_entity = self.model.link_by_name(ecm, "pole") - self.chassis_entity = self.model.link_by_name(ecm, "chassis") - self.pole = Link(self.pole_entity) - self.pole.enable_velocity_checks(ecm) - self.chassis = Link(self.chassis_entity) - self.chassis.enable_velocity_checks(ecm) - self.first_time = False + + world = World(world_entity(ecm)) + self.model = Model(world.model_by_name(ecm, "vehicle_green")) + self.pole_entity = self.model.link_by_name(ecm, "pole") + self.chassis_entity = self.model.link_by_name(ecm, "chassis") + self.pole = Link(self.pole_entity) + self.pole.enable_velocity_checks(ecm) + self.chassis = Link(self.chassis_entity) + self.chassis.enable_velocity_checks(ecm) if self.command == 1: self.chassis.add_world_force(Vector3d(0, 100, 0)) elif self.command == 0: @@ -60,9 +56,6 @@ def on_post_update(self, info, ecm): cart_vel = 0 print("Warning failed to get cart velocity") - #print("pole", pole_pose) - #print("cart", cart_pose) - #print("Pole angvel ", pole_angular_vel) self.state = np.array([cart_pose, cart_vel, pole_pose, pole_angular_vel], dtype=np.float32) if not self.terminated: self.terminated = pole_pose > 0.24 or pole_pose < -0.24 or cart_pose > 4.8 or cart_pose < -4.8 @@ -80,9 +73,7 @@ def step(self, action, paused=False): return obs, reward, self.terminated, False, {} def reset(self): - print("Resetting") self.server.reset_all() - self.first_time = True self.command = None self.terminated = False obs, reward_, term_, tunc_, other_= self.step(None, paused=False) From 9a2b742f5d28e074570111083cf9fae4cd0b7805 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 11 Nov 2024 13:15:25 +0800 Subject: [PATCH 09/19] Style Signed-off-by: Arjo Chakravarty --- .../reinforcement_learning/simple_cart_pole/cart_pole.sdf | 1 + .../reinforcement_learning/simple_cart_pole/cart_pole_env.py | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole.sdf b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole.sdf index 6c1a1cd0de..97d8c0b844 100644 --- a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole.sdf +++ b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole.sdf @@ -366,3 +366,4 @@ + diff --git a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py index d032ae5079..f94792d314 100644 --- a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py +++ b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py @@ -108,4 +108,4 @@ def step(self, action): for i in range(5000): action, _state = model.predict(obs, deterministic=True) obs, reward, done, info = vec_env.step(action) - # Nice to have spawn a gz sim client \ No newline at end of file + # Nice to have spawn a gz sim client From da33f11442874b450474bd82d772931423f54284 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 11 Nov 2024 17:30:38 +0800 Subject: [PATCH 10/19] Got the gui working. Time for gradient descent by grad student Signed-off-by: Arjo Chakravarty --- .../simple_cart_pole/README.md | 7 +++ .../simple_cart_pole/cart_pole_env.py | 23 ++++---- python/CMakeLists.txt | 2 + python/src/gz/sim/Gui.cc | 52 +++++++++++++++++++ python/src/gz/sim/Gui.hh | 39 ++++++++++++++ python/src/gz/sim/_gz_sim_pybind11.cc | 2 + 6 files changed, 115 insertions(+), 10 deletions(-) create mode 100644 python/src/gz/sim/Gui.cc create mode 100644 python/src/gz/sim/Gui.hh diff --git a/examples/scripts/reinforcement_learning/simple_cart_pole/README.md b/examples/scripts/reinforcement_learning/simple_cart_pole/README.md index 2e8f396809..53b2dae9e1 100644 --- a/examples/scripts/reinforcement_learning/simple_cart_pole/README.md +++ b/examples/scripts/reinforcement_learning/simple_cart_pole/README.md @@ -19,6 +19,12 @@ Lets install our dependencies ``` pip install stable-baselines3[extra] ``` +For visuallization to work you will also need to: +``` +pip uninstall opencv-python +pip install opencv-python-headless +``` +This is because `opencv-python` brings in Qt5 by default. In the same terminal you should add your gazebo python install directory to the `PYTHONPATH` If you built gazebo from source in the current working directory this would be: @@ -32,6 +38,7 @@ mis-matches. export PROTOCOL_BUFFERS_PYTHON_IMPLEMENTATION=python ``` + ## Exploring the environment You can see the environment by using `gz sim cart_pole.sdf`. \ No newline at end of file diff --git a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py index f94792d314..31d336b817 100644 --- a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py +++ b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py @@ -4,14 +4,15 @@ import numpy as np from gz.common6 import set_verbosity -from gz.sim9 import TestFixture, World, world_entity, Model, Link +from gz.sim9 import TestFixture, World, world_entity, Model, Link, run_gui from gz.math8 import Vector3d from gz.transport14 import Node from gz.msgs11.world_control_pb2 import WorldControl from gz.msgs11.world_reset_pb2 import WorldReset from gz.msgs11.boolean_pb2 import Boolean -from stable_baselines3 import A2C +from stable_baselines3 import PPO +import time file_path = os.path.dirname(os.path.realpath(__file__)) @@ -36,9 +37,9 @@ def on_pre_update(self, info, ecm): self.chassis = Link(self.chassis_entity) self.chassis.enable_velocity_checks(ecm) if self.command == 1: - self.chassis.add_world_force(Vector3d(0, 100, 0)) + self.chassis.add_world_force(ecm, Vector3d(2000, 0, 0)) elif self.command == 0: - self.chassis.add_world_force(Vector3d(0, -100, 0)) + self.chassis.add_world_force(ecm, Vector3d(-2000, 0, 0)) def on_post_update(self, info, ecm): pole_pose = self.pole.world_pose(ecm).rot().euler().y() @@ -58,7 +59,7 @@ def on_post_update(self, info, ecm): self.state = np.array([cart_pose, cart_vel, pole_pose, pole_angular_vel], dtype=np.float32) if not self.terminated: - self.terminated = pole_pose > 0.24 or pole_pose < -0.24 or cart_pose > 4.8 or cart_pose < -4.8 + self.terminated = pole_pose > 0.48 or pole_pose < -0.48 or cart_pose > 4.8 or cart_pose < -4.8 if self.terminated: self.reward = 0.0 @@ -66,8 +67,8 @@ def on_post_update(self, info, ecm): self.reward = 1.0 def step(self, action, paused=False): - self.action = action - self.server.run(True, 1, paused) + self.command = action + self.server.run(True, 5, paused) obs = self.state reward = self.reward return obs, reward, self.terminated, False, {} @@ -100,12 +101,14 @@ def step(self, action): env = CustomCartPole({}) -model = A2C("MlpPolicy", env, verbose=1) -model.learn(total_timesteps=10_000) +model = PPO("MlpPolicy", env, verbose=1) +model.learn(total_timesteps=25_000) vec_env = model.get_env() obs = vec_env.reset() -for i in range(5000): +run_gui() +time.sleep(10) +for i in range(50000): action, _state = model.predict(obs, deterministic=True) obs, reward, done, info = vec_env.step(action) # Nice to have spawn a gz sim client diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 98bbe66650..d5ed701042 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -42,10 +42,12 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE src/gz/sim/UpdateInfo.cc src/gz/sim/Util.cc src/gz/sim/World.cc + src/gz/sim/Gui.cc ) target_link_libraries(${BINDINGS_MODULE_NAME} PRIVATE ${PROJECT_LIBRARY_TARGET_NAME} + ${PROJECT_LIBRARY_TARGET_NAME}-gui gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} ) diff --git a/python/src/gz/sim/Gui.cc b/python/src/gz/sim/Gui.cc new file mode 100644 index 0000000000..c0a04abd3c --- /dev/null +++ b/python/src/gz/sim/Gui.cc @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include + +#include +#include + +#include "Server.hh" + +namespace gz +{ +namespace sim +{ +namespace python +{ +void defineGuiClient(pybind11::module &_module) +{ + + _module.def("run_gui", [](){ + auto pid = fork(); + if (pid == -1) + { + gzerr << "Failed to instantiate new process"; + return; + } + if (pid != 0) + { + return; + } + int zero = 0; + gz::sim::gui::runGui(zero, nullptr, nullptr); + }, + "Run the gui"); +} +} // namespace python +} // namespace sim +} // namespace gz diff --git a/python/src/gz/sim/Gui.hh b/python/src/gz/sim/Gui.hh new file mode 100644 index 0000000000..1856f7b18f --- /dev/null +++ b/python/src/gz/sim/Gui.hh @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef GZ_SIM_PYTHON___HH_ +#define GZ_SIM_PYTHON__SERVER_HH_ + +#include + +namespace gz +{ +namespace sim +{ +namespace python +{ +/// Define a pybind11 wrapper for a gz::sim::Server +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +defineGuiClient(pybind11::module &_module); +} // namespace python +} // namespace sim +} // namespace gz + +#endif // GZ_SIM_PYTHON__SERVER_HH_ \ No newline at end of file diff --git a/python/src/gz/sim/_gz_sim_pybind11.cc b/python/src/gz/sim/_gz_sim_pybind11.cc index acf9373dc5..9645cd66bf 100644 --- a/python/src/gz/sim/_gz_sim_pybind11.cc +++ b/python/src/gz/sim/_gz_sim_pybind11.cc @@ -32,6 +32,7 @@ #include "UpdateInfo.hh" #include "Util.hh" #include "World.hh" +#include "Gui.hh" PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { m.doc() = "Gazebo Sim Python Library."; @@ -50,4 +51,5 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { gz::sim::python::defineSimUpdateInfo(m); gz::sim::python::defineSimWorld(m); gz::sim::python::defineSimUtil(m); + gz::sim::python::defineGuiClient(m); } From 3e83828a5b2d87576acef4337fd2c6f06ddcd481 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 8 Jan 2025 19:14:34 +0800 Subject: [PATCH 11/19] Address feedback Signed-off-by: Arjo Chakravarty --- python/src/gz/sim/TestFixture.cc | 2 +- src/SimulationRunner.hh | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/python/src/gz/sim/TestFixture.cc b/python/src/gz/sim/TestFixture.cc index 1fa05d23c3..dc2589cd51 100644 --- a/python/src/gz/sim/TestFixture.cc +++ b/python/src/gz/sim/TestFixture.cc @@ -93,7 +93,7 @@ defineSimTestFixture(pybind11::object module) } ), pybind11::return_value_policy::reference, - "Wrapper around a system's post-update callback" + "Wrapper around a system's reset callback" ); // TODO(ahcorde): This method is not compiling for the following reason: // The EventManager class has an unordered_map which holds a unique_ptr diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 8b5ac8bf36..03ac9e47f8 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -370,6 +370,9 @@ namespace gz public: void SetNextStepAsBlockingPaused(const bool value); /// \brief Reset the current simulation runner + /// \param[in] all - Reset all parameters + /// \param[in] time - Reset the time + /// \param[in] model - Reset the model only [currently unsupported] public: void Reset(const bool all, const bool time, const bool model); /// \brief Updates the physics parameters of the simulation based on the From 15fa86727f197bd3591524453a19b01a47732b72 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 8 Jan 2025 20:01:28 +0800 Subject: [PATCH 12/19] Add support for individual resets Signed-off-by: Arjo Chakravarty --- include/gz/sim/Server.hh | 4 ++++ python/src/gz/sim/Server.cc | 4 +++- src/Server.cc | 11 +++++++++++ 3 files changed, 18 insertions(+), 1 deletion(-) diff --git a/include/gz/sim/Server.hh b/include/gz/sim/Server.hh index ae49bd59f6..9867ca68d7 100644 --- a/include/gz/sim/Server.hh +++ b/include/gz/sim/Server.hh @@ -340,6 +340,10 @@ namespace gz /// \brief Reset all runners in this simulation public: void ResetAll(); + /// \brief Reset a specific runner in this server + /// \param[in] runnerId - The runner which you want to reset + /// \ return False if the runner does not exist, true otherwise. + public: bool Reset(const std::size_t _runnerId); /// \brief Private data private: std::unique_ptr dataPtr; diff --git a/python/src/gz/sim/Server.cc b/python/src/gz/sim/Server.cc index 7ece7ec090..b5688ece9b 100644 --- a/python/src/gz/sim/Server.cc +++ b/python/src/gz/sim/Server.cc @@ -48,7 +48,9 @@ void defineSimServer(pybind11::object module) pybind11::overload_cast<>(&gz::sim::Server::Running, pybind11::const_), "Get whether the server is running.") .def("reset_all", &gz::sim::Server::ResetAll, - "Resets all simulation runners under this server."); + "Resets all simulation runners under this server.") + .def("reset", &gz::sim::Server::Reset, + "Resets a specific simulation runner under this server."); } } // namespace python } // namespace sim diff --git a/src/Server.cc b/src/Server.cc index 602eb1e569..25497fa960 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -496,6 +496,17 @@ void Server::ResetAll() } } +////////////////////////////////////////////////// +bool Server::Reset(const std::size_t _runnerId) +{ + if (_runnerId >= this->dataPtr->simRunners.size()) + { + return false; + } + this->dataPtr->simRunners[_runnerId]->Reset(true, false, false); + return true; +} + ////////////////////////////////////////////////// void Server::Stop() { From 16d4407e9f4e5d8cef02249ce394c3a0774c41c9 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 7 Feb 2025 09:52:40 +0800 Subject: [PATCH 13/19] Style Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.hh | 8 ++++---- src/TestFixture_TEST.cc | 1 - 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 03ac9e47f8..3c4e0ec1c7 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -370,10 +370,10 @@ namespace gz public: void SetNextStepAsBlockingPaused(const bool value); /// \brief Reset the current simulation runner - /// \param[in] all - Reset all parameters - /// \param[in] time - Reset the time - /// \param[in] model - Reset the model only [currently unsupported] - public: void Reset(const bool all, const bool time, const bool model); + /// \param[in] _all - Reset all parameters + /// \param[in] _time - Reset the time + /// \param[in] _model - Reset the model only [currently unsupported] + public: void Reset(const bool _all, const bool _time, const bool _model); /// \brief Updates the physics parameters of the simulation based on the /// Physics component of the world, if any. diff --git a/src/TestFixture_TEST.cc b/src/TestFixture_TEST.cc index 21a9b7a9bd..6dbba2a34b 100644 --- a/src/TestFixture_TEST.cc +++ b/src/TestFixture_TEST.cc @@ -136,7 +136,6 @@ TEST_F(TestFixtureTest, Callbacks) EXPECT_EQ(1u, resets); } - ///////////////////////////////////////////////// TEST_F(TestFixtureTest, LoadConfig) { From 6c8131cacc2daf3e4809bd54137f9a1e58826925 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Fri, 7 Feb 2025 17:11:17 +0800 Subject: [PATCH 14/19] Bind installation directories and install Signed-off-by: Ubuntu --- python/src/gz/sim/Gui.cc | 52 -------------- python/src/gz/sim/InstallationDirectories.cc | 68 +++++++++++++++++++ .../{Gui.hh => InstallationDirectories.hh} | 10 +-- python/src/gz/sim/_gz_sim_pybind11.cc | 4 +- src/CMakeLists.txt | 1 + 5 files changed, 76 insertions(+), 59 deletions(-) delete mode 100644 python/src/gz/sim/Gui.cc create mode 100644 python/src/gz/sim/InstallationDirectories.cc rename python/src/gz/sim/{Gui.hh => InstallationDirectories.hh} (75%) diff --git a/python/src/gz/sim/Gui.cc b/python/src/gz/sim/Gui.cc deleted file mode 100644 index c0a04abd3c..0000000000 --- a/python/src/gz/sim/Gui.cc +++ /dev/null @@ -1,52 +0,0 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include - -#include -#include - -#include "Server.hh" - -namespace gz -{ -namespace sim -{ -namespace python -{ -void defineGuiClient(pybind11::module &_module) -{ - - _module.def("run_gui", [](){ - auto pid = fork(); - if (pid == -1) - { - gzerr << "Failed to instantiate new process"; - return; - } - if (pid != 0) - { - return; - } - int zero = 0; - gz::sim::gui::runGui(zero, nullptr, nullptr); - }, - "Run the gui"); -} -} // namespace python -} // namespace sim -} // namespace gz diff --git a/python/src/gz/sim/InstallationDirectories.cc b/python/src/gz/sim/InstallationDirectories.cc new file mode 100644 index 0000000000..e73b89214d --- /dev/null +++ b/python/src/gz/sim/InstallationDirectories.cc @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2025 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include + +#include + +#include "InstallationDirectories.hh" + +namespace gz +{ +namespace sim +{ +namespace python +{ +void defineSimInstallationDirectories(pybind11::module &_module) +{ + + _module.def("get_install_prefix", [](){ + return getInstallPrefix(); + }, + "getInstallPrefix return the install prefix of the library") + .def("get_gui_config_path", [](){ + return getGUIConfigPath(); + }, + "getGUIConfigPath return the GUI config path") + .def("get_system_config_path", [](){ + return getSystemConfigPath(); + }, + "getSystemConfigPath return the system config path") + .def("get_server_config_path", [](){ + return getServerConfigPath(); + }, + "getServerConfigPath return the server config path") + .def("get_plugin_install_dir", [](){ + return getPluginInstallDir(); + }, + "getPluginInstallDir return the plugin install dir") + .def("get_gui_plugin_install_dir", [](){ + return getGUIPluginInstallDir(); + }, + "getGUIPluginInstallDir return the GUI plugin install dir") + .def("get_world_install_dir", [](){ + return getWorldInstallDir(); + }, + "getWorldInstallDir return the world install dir") + .def("get_media_install_dir", [](){ + return getMediaInstallDir(); + }, + "getMediaInstallDir return the media install dir"); +} +} // namespace python +} // namespace sim +} // namespace gz diff --git a/python/src/gz/sim/Gui.hh b/python/src/gz/sim/InstallationDirectories.hh similarity index 75% rename from python/src/gz/sim/Gui.hh rename to python/src/gz/sim/InstallationDirectories.hh index 1856f7b18f..c942419e93 100644 --- a/python/src/gz/sim/Gui.hh +++ b/python/src/gz/sim/InstallationDirectories.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2025 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -15,8 +15,8 @@ */ -#ifndef GZ_SIM_PYTHON___HH_ -#define GZ_SIM_PYTHON__SERVER_HH_ +#ifndef GZ_SIM_PYTHON__INSTALLATION_DIRECTORIES_HH_ +#define GZ_SIM_PYTHON__INSTALLATION_DIRECTORIES_HH_ #include @@ -31,9 +31,9 @@ namespace python * \param[in] module a pybind11 module to add the definition to */ void -defineGuiClient(pybind11::module &_module); +defineSimInstallationDirectories(pybind11::module &_module); } // namespace python } // namespace sim } // namespace gz -#endif // GZ_SIM_PYTHON__SERVER_HH_ \ No newline at end of file +#endif // GZ_SIM_PYTHON__INSTALLATION_DIRECTORIES_HH_ \ No newline at end of file diff --git a/python/src/gz/sim/_gz_sim_pybind11.cc b/python/src/gz/sim/_gz_sim_pybind11.cc index 9645cd66bf..fea576a161 100644 --- a/python/src/gz/sim/_gz_sim_pybind11.cc +++ b/python/src/gz/sim/_gz_sim_pybind11.cc @@ -21,6 +21,7 @@ #include "Actor.hh" #include "EntityComponentManager.hh" #include "EventManager.hh" +#include "InstallationDirectories.hh" #include "Joint.hh" #include "Light.hh" #include "Link.hh" @@ -32,7 +33,6 @@ #include "UpdateInfo.hh" #include "Util.hh" #include "World.hh" -#include "Gui.hh" PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { m.doc() = "Gazebo Sim Python Library."; @@ -40,6 +40,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { gz::sim::python::defineSimActor(m); gz::sim::python::defineSimEntityComponentManager(m); gz::sim::python::defineSimEventManager(m); + gz::sim::python::defineSimInstallationDirectories(m); gz::sim::python::defineSimJoint(m); gz::sim::python::defineSimLight(m); gz::sim::python::defineSimLink(m); @@ -51,5 +52,4 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { gz::sim::python::defineSimUpdateInfo(m); gz::sim::python::defineSimWorld(m); gz::sim::python::defineSimUtil(m); - gz::sim::python::defineGuiClient(m); } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index d4a5589b19..346ae3b768 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -189,6 +189,7 @@ target_link_libraries(${gz_lib_target} # Executable target that runs the GUI without ruby for debugging purposes. add_executable(runGui cmd/runGui_main.cc) target_link_libraries(runGui PRIVATE ${gz_lib_target}) +install(TARGETS runGui DESTINATION ${CMAKE_INSTALL_PREFIX}/libexec) # Create the library target gz_create_core_library(SOURCES ${sources} CXX_STANDARD 17) From 5117071ed6ee83a7476a81d1417ad88b2e27e8b3 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Fri, 7 Feb 2025 17:12:35 +0800 Subject: [PATCH 15/19] Missed the file in the last commit Signed-off-by: Ubuntu --- python/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index d5ed701042..cc6176ede8 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -42,7 +42,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE src/gz/sim/UpdateInfo.cc src/gz/sim/Util.cc src/gz/sim/World.cc - src/gz/sim/Gui.cc + src/gz/sim/InstallationDirectories.cc ) target_link_libraries(${BINDINGS_MODULE_NAME} PRIVATE From 46e3ca85a1e8e16fe7095cd02ec0a67f7b585dbe Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Fri, 7 Feb 2025 17:27:58 +0800 Subject: [PATCH 16/19] style Signed-off-by: Ubuntu --- .../reinforcement_learning/simple_cart_pole/cart_pole.sdf | 1 - python/src/gz/sim/InstallationDirectories.hh | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole.sdf b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole.sdf index 97d8c0b844..6c1a1cd0de 100644 --- a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole.sdf +++ b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole.sdf @@ -366,4 +366,3 @@ - diff --git a/python/src/gz/sim/InstallationDirectories.hh b/python/src/gz/sim/InstallationDirectories.hh index c942419e93..621f898636 100644 --- a/python/src/gz/sim/InstallationDirectories.hh +++ b/python/src/gz/sim/InstallationDirectories.hh @@ -36,4 +36,4 @@ defineSimInstallationDirectories(pybind11::module &_module); } // namespace sim } // namespace gz -#endif // GZ_SIM_PYTHON__INSTALLATION_DIRECTORIES_HH_ \ No newline at end of file +#endif // GZ_SIM_PYTHON__INSTALLATION_DIRECTORIES_HH_ From 6799df2cf9372816c053802abd27225ef8fe4b8a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 10 Feb 2025 13:44:06 +0800 Subject: [PATCH 17/19] Style Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.hh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 3c4e0ec1c7..4b58267447 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -370,9 +370,9 @@ namespace gz public: void SetNextStepAsBlockingPaused(const bool value); /// \brief Reset the current simulation runner - /// \param[in] _all - Reset all parameters - /// \param[in] _time - Reset the time - /// \param[in] _model - Reset the model only [currently unsupported] + /// \param[in] _all Reset all parameters + /// \param[in] _time Reset the time + /// \param[in] _model Reset the model only [currently unsupported] public: void Reset(const bool _all, const bool _time, const bool _model); /// \brief Updates the physics parameters of the simulation based on the From 856f31654bb9848397195151a67bbd8ef4a8da94 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 10 Feb 2025 14:52:16 +0800 Subject: [PATCH 18/19] Style Signed-off-by: Arjo Chakravarty --- .../scripts/reinforcement_learning/simple_cart_pole/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/scripts/reinforcement_learning/simple_cart_pole/README.md b/examples/scripts/reinforcement_learning/simple_cart_pole/README.md index 53b2dae9e1..e7ea450c57 100644 --- a/examples/scripts/reinforcement_learning/simple_cart_pole/README.md +++ b/examples/scripts/reinforcement_learning/simple_cart_pole/README.md @@ -41,4 +41,4 @@ export PROTOCOL_BUFFERS_PYTHON_IMPLEMENTATION=python ## Exploring the environment -You can see the environment by using `gz sim cart_pole.sdf`. \ No newline at end of file +You can see the environment by using `gz sim cart_pole.sdf`. From f8ddbf6ac540040ef8fde7068c39881252f23cb6 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 5 Mar 2025 03:14:42 +0000 Subject: [PATCH 19/19] Enable GUI rollout Signed-off-by: Arjo Chakravarty --- .../simple_cart_pole/README.md | 4 ++-- .../simple_cart_pole/cart_pole_env.py | 12 ++++++++++-- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/examples/scripts/reinforcement_learning/simple_cart_pole/README.md b/examples/scripts/reinforcement_learning/simple_cart_pole/README.md index e7ea450c57..a629368fa8 100644 --- a/examples/scripts/reinforcement_learning/simple_cart_pole/README.md +++ b/examples/scripts/reinforcement_learning/simple_cart_pole/README.md @@ -1,6 +1,6 @@ # Example for Reinforcement Learning (RL) With Gazebo -This demo world shows you an example of how you can use SDFormat, Ray-RLLIB and Gazebo to perform RL with python. +This demo world shows you an example of how you can use SDFormat, Stable Baselines 3 and Gazebo to perform RL with python. We start with a very simple cart-pole world. This world is defined in our sdf file `cart_pole.sdf`. It is analogous to the @@ -10,7 +10,7 @@ First create a virtual environment using python, ``` python3 -m venv venv ``` -Lets activate it and install rayrllib and pytorch. +Lets activate it and install stablebaselines3 and pytorch. ``` . venv/bin/activate ``` diff --git a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py index 31d336b817..60c0b8a8ea 100644 --- a/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py +++ b/examples/scripts/reinforcement_learning/simple_cart_pole/cart_pole_env.py @@ -4,7 +4,7 @@ import numpy as np from gz.common6 import set_verbosity -from gz.sim9 import TestFixture, World, world_entity, Model, Link, run_gui +from gz.sim10 import TestFixture, World, world_entity, Model, Link, get_install_prefix from gz.math8 import Vector3d from gz.transport14 import Node from gz.msgs11.world_control_pb2 import WorldControl @@ -13,9 +13,17 @@ from stable_baselines3 import PPO import time +import subprocess file_path = os.path.dirname(os.path.realpath(__file__)) +def run_gui(): + if os.name == 'nt': + base = os.path.join(get_install_prefix(), "libexec", "runGui.exe") + else: + base = os.path.join(get_install_prefix(), "libexec", "runGui") + subprocess.Popen(base) + class GzRewardScorer: def __init__(self): self.fixture = TestFixture(os.path.join(file_path, 'cart_pole.sdf')) @@ -99,13 +107,13 @@ def step(self, action): obs, reward, done, truncated, info = self.env.step(action) return obs, reward, done, truncated, info - env = CustomCartPole({}) model = PPO("MlpPolicy", env, verbose=1) model.learn(total_timesteps=25_000) vec_env = model.get_env() obs = vec_env.reset() + run_gui() time.sleep(10) for i in range(50000):