From 6b532b61a3f8447fc23f25659cb5768bf879e9d7 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 15 Apr 2019 11:43:43 +0200 Subject: [PATCH 01/11] New EnvironmentCallbacksSingleton This singleton collects the callbacks used for all the agents. It is the first step toward the support of multiple agents (each of them associated to one of these callbacks) acting on independent environments. --- ignition/CMakeLists.txt | 25 ++++++++++- .../gazebo/EnvironmentCallbacksSingleton.h | 40 +++++++++++++++++ .../src/EnvironmentCallbacksSingleton.cpp | 45 +++++++++++++++++++ 3 files changed, 109 insertions(+), 1 deletion(-) create mode 100644 ignition/include/gympp/gazebo/EnvironmentCallbacksSingleton.h create mode 100644 ignition/src/EnvironmentCallbacksSingleton.cpp diff --git a/ignition/CMakeLists.txt b/ignition/CMakeLists.txt index bdc032a83..e6132cf51 100644 --- a/ignition/CMakeLists.txt +++ b/ignition/CMakeLists.txt @@ -2,6 +2,25 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +# ============================= +# EnvironmentCallbacksSingleton +# ============================= + +add_library(EnvironmentCallbacksSingleton SHARED + include/gympp/gazebo/EnvironmentCallbacksSingleton.h + src/EnvironmentCallbacksSingleton.cpp) + +target_include_directories(EnvironmentCallbacksSingleton PUBLIC + $ + $) + +target_link_libraries(EnvironmentCallbacksSingleton PUBLIC + gympp + ignition-gazebo1::core) + +set_target_properties(EnvironmentCallbacksSingleton PROPERTIES + PUBLIC_HEADER include/gympp/gazebo/EnvironmentCallbacksSingleton.h) + # ==================== # IGNITION ENVIRONMENT # ==================== @@ -70,7 +89,11 @@ target_include_directories(RobotSingleton PUBLIC # =================== install( - TARGETS IgnitionEnvironment RobotSingleton EnvironmentCallbacks + TARGETS + IgnitionEnvironment + RobotSingleton + EnvironmentCallbacks + EnvironmentCallbacksSingleton EXPORT gympp LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} diff --git a/ignition/include/gympp/gazebo/EnvironmentCallbacksSingleton.h b/ignition/include/gympp/gazebo/EnvironmentCallbacksSingleton.h new file mode 100644 index 000000000..be818f074 --- /dev/null +++ b/ignition/include/gympp/gazebo/EnvironmentCallbacksSingleton.h @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef GYMPP_ROBOT_ENVIRONMENTCALLBACKSSINGLETON_H +#define GYMPP_ROBOT_ENVIRONMENTCALLBACKSSINGLETON_H + +#include + +#include +#include +#include + +#include "gympp/gazebo/EnvironmentCallbacks.h" +namespace gympp { + namespace gazebo { + class EnvironmentCallbacksSingleton; + } // namespace gazebo +} // namespace gympp + +class gympp::gazebo::EnvironmentCallbacksSingleton + : public ignition::common::SingletonT +{ +private: + class Impl; + std::unique_ptr> pImpl; + +public: + EnvironmentCallbacksSingleton(); + + gympp::gazebo::EnvironmentCallbacks* get(const std::string& label); + bool storeEnvironmentCallback(const std::string& label, + gympp::gazebo::EnvironmentCallbacks* cb); +}; + +#endif // GYMPP_ROBOT_ENVIRONMENTCALLBACKSSINGLETON_H diff --git a/ignition/src/EnvironmentCallbacksSingleton.cpp b/ignition/src/EnvironmentCallbacksSingleton.cpp new file mode 100644 index 000000000..b5ad2cb1f --- /dev/null +++ b/ignition/src/EnvironmentCallbacksSingleton.cpp @@ -0,0 +1,45 @@ +#include "gympp/gazebo/EnvironmentCallbacksSingleton.h" +#include "gympp/Log.h" + +#include + +using namespace gympp::gazebo; + +class EnvironmentCallbacksSingleton::Impl +{ +public: + std::unordered_map callbacks; +}; + +EnvironmentCallbacksSingleton::EnvironmentCallbacksSingleton() + : pImpl{new Impl(), [](Impl* impl) { delete impl; }} +{} + +EnvironmentCallbacks* EnvironmentCallbacksSingleton::get(const std::string& label) +{ + if (pImpl->callbacks.find(label) == pImpl->callbacks.end()) { + gymppError << "Failed to find environment callbacks labelled as '" << label << "'" + << std::endl; + return nullptr; + } + + assert(pImpl->callbacks.at(label)); + return pImpl->callbacks.at(label); +} + +bool EnvironmentCallbacksSingleton::storeEnvironmentCallback(const std::string& label, + EnvironmentCallbacks* cb) +{ + if (!cb || label.empty()) { + gymppError << "Trying to store invalid environment callbacks" << std::endl; + return false; + } + + if (pImpl->callbacks.find(label) != pImpl->callbacks.end()) { + gymppError << "Environment callbacks with label '" << label + << "' have been already registered" << std::endl; + } + + pImpl->callbacks.insert({label, cb}); + return true; +} From ecdbb4fa7a2200b45bb99203bbcc6d02b0ddb852 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 15 Apr 2019 11:50:58 +0200 Subject: [PATCH 02/11] IgnitionRobot is now a class and not a plugin Ignition plugins that need a gympp::Robot pointer will contain an IgnitionRobot object and they configure it with their sdf configuration. --- ignition/CMakeLists.txt | 44 +++++++++---------- ignition/include/gympp/gazebo/IgnitionRobot.h | 18 ++++---- ignition/src/IgnitionRobot.cpp | 40 +++++++---------- 3 files changed, 43 insertions(+), 59 deletions(-) diff --git a/ignition/CMakeLists.txt b/ignition/CMakeLists.txt index e6132cf51..f7a1f582b 100644 --- a/ignition/CMakeLists.txt +++ b/ignition/CMakeLists.txt @@ -84,25 +84,9 @@ target_include_directories(RobotSingleton PUBLIC $ $) -# =================== -# INSTALL THE TARGETS -# =================== - -install( - TARGETS - IgnitionEnvironment - RobotSingleton - EnvironmentCallbacks - EnvironmentCallbacksSingleton - EXPORT gympp - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gympp/gazebo) - -# ==================== -# IgnitionRobot PLUGIN -# ==================== +# ============== +# IGNITION ROBOT +# ============== add_library(IgnitionRobot SHARED include/gympp/gazebo/IgnitionRobot.h @@ -111,17 +95,29 @@ add_library(IgnitionRobot SHARED target_link_libraries(IgnitionRobot PUBLIC gympp - IgnitionEnvironment RobotSingleton ignition-gazebo1::core) +set_target_properties(IgnitionRobot PROPERTIES + PUBLIC_HEADER include/gympp/gazebo/IgnitionRobot.h) + target_include_directories(IgnitionRobot PUBLIC $ $) +# =================== +# INSTALL THE TARGETS +# =================== + install( - TARGETS IgnitionRobot + TARGETS + IgnitionEnvironment + RobotSingleton + EnvironmentCallbacks + EnvironmentCallbacksSingleton + IgnitionRobot EXPORT gympp - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}/gympp/plugins - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}/gympp/plugins - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}/gympp/plugins) + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gympp/gazebo) diff --git a/ignition/include/gympp/gazebo/IgnitionRobot.h b/ignition/include/gympp/gazebo/IgnitionRobot.h index 85bcca8a6..2238db9e2 100644 --- a/ignition/include/gympp/gazebo/IgnitionRobot.h +++ b/ignition/include/gympp/gazebo/IgnitionRobot.h @@ -11,8 +11,11 @@ #include "gympp/Robot.h" -#include +#include +#include +#include +#include #include namespace gympp { @@ -21,10 +24,7 @@ namespace gympp { } // namespace gazebo } // namespace gympp -class gympp::gazebo::IgnitionRobot final - : public gympp::Robot - , public ignition::gazebo::System - , public ignition::gazebo::ISystemConfigure +class gympp::gazebo::IgnitionRobot : public gympp::Robot { private: class Impl; @@ -34,11 +34,9 @@ class gympp::gazebo::IgnitionRobot final IgnitionRobot(); ~IgnitionRobot() override; - void Configure(const ignition::gazebo::Entity& entity, - const std::shared_ptr& sdf, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& eventMgr) override; - + bool configureECM(const ignition::gazebo::Entity& entity, + const std::shared_ptr& sdf, + ignition::gazebo::EntityComponentManager& ecm); bool valid() const override; // =========== diff --git a/ignition/src/IgnitionRobot.cpp b/ignition/src/IgnitionRobot.cpp index a29321e0b..bdf33b189 100644 --- a/ignition/src/IgnitionRobot.cpp +++ b/ignition/src/IgnitionRobot.cpp @@ -10,14 +10,10 @@ #include "gympp/Log.h" #include "gympp/gazebo/RobotSingleton.h" -#include -#include - #include #include #include #include -//#include #include #include #include @@ -132,16 +128,12 @@ ComponentTypeT& IgnitionRobot::Impl::getOrCreateComponent(const ignition::gazebo // ============== IgnitionRobot::IgnitionRobot() - : System() - , pImpl{new Impl(), [](Impl* impl) { delete impl; }} + : pImpl{new Impl(), [](Impl* impl) { delete impl; }} {} -IgnitionRobot::~IgnitionRobot() = default; - -void IgnitionRobot::Configure(const ignition::gazebo::Entity& entity, - const std::shared_ptr& sdf, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& /*eventMgr*/) +bool IgnitionRobot::configureECM(const ignition::gazebo::Entity& entity, + const std::shared_ptr& sdf, + ignition::gazebo::EntityComponentManager& ecm) { // Store the address of the entity-component manager pImpl->ecm = &ecm; @@ -160,7 +152,7 @@ void IgnitionRobot::Configure(const ignition::gazebo::Entity& entity, gymppError << "The model associated to sdf element '" << sdfElementString << "is not valid" << std::endl; - return; + return false; } gymppDebug << "Processing model '" << pImpl->model.Name(ecm) << "'" << std::endl; @@ -221,20 +213,27 @@ void IgnitionRobot::Configure(const ignition::gazebo::Entity& entity, if (!valid()) { gymppError << "The IgnitionRobot object for model '" << pImpl->model.Name(ecm) << "' is not valid" << std::endl; - return; + return false; } - // Store the pointer of the exposed interface into the singleton + // Register the robot in the singleton if (!RobotSingleton::get().storeRobot(this)) { gymppError << "Failed to store the robot in the RobotSingleton" << std::endl; - return; + return false; } + + return true; } +IgnitionRobot::~IgnitionRobot() = default; bool IgnitionRobot::valid() const { // TODO: find the proper logic to check if this object is valid + if (!pImpl->ecm) { + return false; + } + if (pImpl->joints.size() == 0) { return false; } @@ -445,12 +444,3 @@ bool IgnitionRobot::resetJoint(const gympp::Robot::JointName& jointName, return true; } - -// ============= -// OTHER METHODS -// ============= - -IGNITION_ADD_PLUGIN(gympp::gazebo::IgnitionRobot, - gympp::gazebo::IgnitionRobot::System, - gympp::gazebo::IgnitionRobot::ISystemConfigure, - gympp::Robot) From 5c7304d59a3f0a087006a41ca0c14d64b272b85c Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 15 Apr 2019 13:53:49 +0200 Subject: [PATCH 03/11] IgnitionEnvironment gathers the env callbacks from the singleton The callbacks are registered by the plugins during their Configure step. Temporarily, the declaration of the gympp plugin is done in the sdf. As soon as robot and environment will be split, IgnitionEnvironment will handle that again. --- ignition/CMakeLists.txt | 1 + .../gympp/gazebo/IgnitionEnvironment.h | 2 +- ignition/src/IgnitionEnvironment.cpp | 141 +++++------------- models/CartPole/CartPole.sdf | 3 + 4 files changed, 43 insertions(+), 104 deletions(-) diff --git a/ignition/CMakeLists.txt b/ignition/CMakeLists.txt index f7a1f582b..c22a94f39 100644 --- a/ignition/CMakeLists.txt +++ b/ignition/CMakeLists.txt @@ -33,6 +33,7 @@ target_link_libraries(IgnitionEnvironment PUBLIC gympp PRIVATE + EnvironmentCallbacksSingleton ignition-gazebo1::core tiny-process-library) diff --git a/ignition/include/gympp/gazebo/IgnitionEnvironment.h b/ignition/include/gympp/gazebo/IgnitionEnvironment.h index 3ffb0786c..2750c4f21 100644 --- a/ignition/include/gympp/gazebo/IgnitionEnvironment.h +++ b/ignition/include/gympp/gazebo/IgnitionEnvironment.h @@ -58,7 +58,7 @@ class gympp::gazebo::IgnitionEnvironment EnvironmentPtr env(); static void setVerbosity(int level = DEFAULT_VERBOSITY); bool setupGazeboWorld(const std::string& worldFile, const std::vector& modelNames); - bool setupIgnitionPlugin(const std::string& libName, const std::string& pluginName); + bool setupIgnitionPlugin(const std::string& libName, const std::string& className); }; #endif // GYMPP_GYMS_IGNITION diff --git a/ignition/src/IgnitionEnvironment.cpp b/ignition/src/IgnitionEnvironment.cpp index 8ee301efa..2d3f9adcf 100644 --- a/ignition/src/IgnitionEnvironment.cpp +++ b/ignition/src/IgnitionEnvironment.cpp @@ -10,6 +10,7 @@ #include "gympp/Log.h" #include "gympp/Random.h" #include "gympp/gazebo/EnvironmentCallbacks.h" +#include "gympp/gazebo/EnvironmentCallbacksSingleton.h" #include "process.hpp" #include @@ -29,27 +30,25 @@ using namespace gympp::gazebo; -struct PluginData -{ - std::string libName; - std::string pluginName; - - EnvironmentCallbacks* environmentCallbacks = nullptr; - ignition::gazebo::SystemPluginPtr systemPluginPtr; -}; - class IgnitionEnvironment::Impl { public: - uint64_t numOfIterations = 0; - - PluginData pluginData; std::unique_ptr ignitionGui; + uint64_t numOfIterations = 0; ignition::gazebo::ServerConfig serverConfig; std::shared_ptr server; std::shared_ptr getServer(); + gympp::gazebo::EnvironmentCallbacks* envCallbacks() + { + auto ecSingleton = EnvironmentCallbacksSingleton::Instance(); + auto* callbacks = ecSingleton->get(scopedModelName); + assert(callbacks); + return callbacks; + } + + std::string scopedModelName; std::vector modelsNamesInSdf; }; @@ -63,27 +62,12 @@ std::shared_ptr IgnitionEnvironment::Impl::getServer() return nullptr; } - // Check that the ignition plugin was configured and loaded - if (!pluginData.environmentCallbacks || !pluginData.systemPluginPtr) { - gymppError << "The ignition plugin has not been correctly loaded" << std::endl; - return nullptr; - } - // Create the server gymppDebug << "Creating the server" << std::endl << std::flush; serverConfig.SetUseLevels(false); server = std::make_unique(serverConfig); assert(server); - // Add the plugin system in the server - // TODO: Configure() is not called in this way - auto ok = server->AddSystem(pluginData.systemPluginPtr); - - if (!(ok && ok.value())) { - gymppError << "Failed to add the system in the gazebo server" << std::endl; - return {}; - } - // The GUI needs the server already up. Warming up the first iteration and pausing the // server. It will be unpaused at the step() call. gymppDebug << "Starting the server as paused" << std::endl; @@ -113,35 +97,12 @@ IgnitionEnvironment::IgnitionEnvironment(const ActionSpacePtr aSpace, pImpl->serverConfig.SetUpdateRate(updateRate); } +static size_t counter = 0; bool IgnitionEnvironment::setupIgnitionPlugin(const std::string& libName, - const std::string& pluginName) + const std::string& className) { - pImpl->pluginData.libName = libName; - pImpl->pluginData.pluginName = pluginName; - - auto& pluginData = pImpl->pluginData; - - ignition::gazebo::SystemLoader sl; - auto plugin = sl.LoadPlugin(pluginData.libName, pluginData.pluginName, nullptr); - - if (!plugin.has_value()) { - gymppError << "Failed to load plugin '" << pluginData.pluginName << "'" << std::endl; - gymppError << "Make sure that the IGN_GAZEBO_SYSTEM_PLUGIN_PATH environment variable " - << "contains the path to the plugin '" << pluginData.libName << "'" << std::endl; - return false; - } - - pluginData.systemPluginPtr = plugin.value(); - - // Get the environment behavior interface out of it - pluginData.environmentCallbacks = - pluginData.systemPluginPtr->template QueryInterface(); - - if (!pluginData.environmentCallbacks) { - gymppError << "Failed to cast the plugin '" << pluginName - << "'to get the environment behavior interface" << std::endl; - return false; - } + auto uniqueID = counter++; + pImpl->scopedModelName = pImpl->modelsNamesInSdf.front() + std::to_string(uniqueID); return true; } @@ -160,6 +121,7 @@ IgnitionEnvironment::~IgnitionEnvironment() std::optional IgnitionEnvironment::step(const Action& action) { + // Get the gazebo server auto server = pImpl->getServer(); if (!server) { gymppError << "Failed to get the ignition server" << std::endl; @@ -169,7 +131,13 @@ std::optional IgnitionEnvironment::step(const Action assert(pImpl->server); assert(action_space); assert(observation_space); - assert(pImpl->pluginData.environmentCallbacks); + + // Get the environment callbacks + auto* envCallbacks = pImpl->envCallbacks(); + if (!envCallbacks) { + gymppError << "Failed to get the environment callbacks from the plugin" << std::endl; + return {}; + } if (!this->action_space->contains(action)) { gymppError << "The input action does not belong to the action space" << std::endl; @@ -177,7 +145,7 @@ std::optional IgnitionEnvironment::step(const Action } // Set the action to the environment - if (!pImpl->pluginData.environmentCallbacks->setAction(action)) { + if (!envCallbacks->setAction(action)) { gymppError << "Failed to set the action" << std::endl; return {}; } @@ -200,8 +168,7 @@ std::optional IgnitionEnvironment::step(const Action } // Get the observation from the environment - std::optional observation = - pImpl->pluginData.environmentCallbacks->getObservation(); + std::optional observation = envCallbacks->getObservation(); if (!observation) { gymppError << "The gympp plugin didn't return the observation" << std::endl; @@ -215,7 +182,7 @@ std::optional IgnitionEnvironment::step(const Action } // Get the reward from the environment - std::optional reward = pImpl->pluginData.environmentCallbacks->computeReward(); + std::optional reward = envCallbacks->computeReward(); if (!reward) { gymppError << "The gympp plugin didn't return the reward" << std::endl; @@ -229,7 +196,7 @@ std::optional IgnitionEnvironment::step(const Action } return IgnitionEnvironment::State{ - pImpl->pluginData.environmentCallbacks->isDone(), {}, reward.value(), observation.value()}; + envCallbacks->isDone(), {}, reward.value(), observation.value()}; } std::vector IgnitionEnvironment::seed(size_t seed) @@ -283,44 +250,10 @@ bool IgnitionEnvironment::setupGazeboWorld(const std::string& worldFile, // LOAD A ROBOT PLUGIN FOR EACH SDF MODEL // ====================================== - // TODO: In the future we might want to parse here the sdf file and get automatically - // all the names of the contained models. Right now we could make it work only - // if models are not included externally using sdf elements. - // Using the passed vector of names waiting this support. - if (modelNames.empty()) { - gymppError << "The sdf world must contain at least one model" << std::endl; - return false; - } - - // Add an IgnitionRobot plugin for each model in the sdf file - for (const auto& modelName : modelNames) { - sdf::ElementPtr sdf(new sdf::Element); - sdf->SetName("plugin"); - sdf->AddAttribute("name", "string", "gympp::gazebo::IgnitionRobot", true); - sdf->AddAttribute("filename", "string", "IgnitionRobot", true); - - ignition::gazebo::ServerConfig::PluginInfo pluginInfo{ - modelName, "model", "IgnitionRobot", "gympp::gazebo::IgnitionRobot", sdf}; - pImpl->serverConfig.AddPlugin(pluginInfo); - } - - // // Get the models names included in the sdf file - // sdf::Root root; - // auto errors = root.Load(pImpl->serverConfig.SdfFile()); - - // if (!errors.empty()) { - // gymppError << "Failed to load sdf file '" << sdfFile << "." << std::endl; - // for (const auto& error : errors) { - // gymppError << error << std::endl; - // } - // return false; - // } - - // for (unsigned i = 0; i < root.ModelCount(); ++i) { - // std::string modelName = root.ModelByIndex(i)->Name(); - // gymppDebug << "Found model '" << modelName << "' in the sdf file" << std::endl; - // pImpl->modelsNamesInSdf.push_back(modelName); - // } + // Store the model names + // TODO: We should separate robot and environment. In this way we would have an sdf file for + // the robot that we can parse to get automatically the model names. + pImpl->modelsNamesInSdf = modelNames; return true; } @@ -332,7 +265,7 @@ gympp::EnvironmentPtr IgnitionEnvironment::env() std::optional IgnitionEnvironment::reset() { - gymppDebug << "Resetting the environment" << std::endl; + gymppMessage << "Resetting the environment" << std::endl; // The plugin must be loaded in order to call its reset() method if (!pImpl->getServer()) { @@ -340,18 +273,20 @@ std::optional IgnitionEnvironment::reset() return {}; } - if (!pImpl->pluginData.environmentCallbacks) { - gymppError << "The plugin has not been initialized" << std::endl; + // Get the environment callbacks + auto* envCallbacks = pImpl->envCallbacks(); + if (!envCallbacks) { + gymppError << "Failed to get the environment callbacks from the plugin" << std::endl; return {}; } - if (!pImpl->pluginData.environmentCallbacks->reset()) { + if (!envCallbacks->reset()) { gymppError << "Failed to reset plugin" << std::endl; return {}; } gymppDebug << "Retrieving the initial observation after reset" << std::endl; - return pImpl->pluginData.environmentCallbacks->getObservation(); + return envCallbacks->getObservation(); } bool IgnitionEnvironment::render(RenderMode mode) diff --git a/models/CartPole/CartPole.sdf b/models/CartPole/CartPole.sdf index e18ecce70..77be378b0 100644 --- a/models/CartPole/CartPole.sdf +++ b/models/CartPole/CartPole.sdf @@ -1,5 +1,8 @@ + + cartpole_xacro0 + 0 0 0.5 0 -0 0 From 393256afe2273bbb1e015b43e13b4a1baf13e549 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 15 Apr 2019 13:54:15 +0200 Subject: [PATCH 04/11] CartPole plugin now holds a IgnitionRobot object and registers callbacks --- plugins/CartPole/CMakeLists.txt | 8 ++- plugins/CartPole/CartPolePlugin.cpp | 101 +++++++++++++++++----------- plugins/CartPole/CartPolePlugin.h | 6 ++ 3 files changed, 74 insertions(+), 41 deletions(-) diff --git a/plugins/CartPole/CMakeLists.txt b/plugins/CartPole/CMakeLists.txt index 1b8b22ff6..9e0732bb0 100644 --- a/plugins/CartPole/CMakeLists.txt +++ b/plugins/CartPole/CMakeLists.txt @@ -13,12 +13,14 @@ add_library(CartPolePlugin SHARED target_link_libraries(CartPolePlugin PUBLIC EnvironmentCallbacks - RobotSingleton + ignition-gazebo1::core PRIVATE - ignition-gazebo1::core) + IgnitionRobot + EnvironmentCallbacksSingleton + RobotSingleton) target_include_directories(CartPolePlugin PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}) + $) if(MSVC) # Import math symbols from standard cmath diff --git a/plugins/CartPole/CartPolePlugin.cpp b/plugins/CartPole/CartPolePlugin.cpp index 9953d8256..a41c0f4d2 100644 --- a/plugins/CartPole/CartPolePlugin.cpp +++ b/plugins/CartPole/CartPolePlugin.cpp @@ -11,6 +11,8 @@ #include "gympp/Log.h" #include "gympp/Random.h" #include "gympp/Robot.h" +#include "gympp/gazebo/EnvironmentCallbacksSingleton.h" +#include "gympp/gazebo/IgnitionRobot.h" #include "gympp/gazebo/RobotSingleton.h" #include @@ -18,6 +20,7 @@ #include #include +using namespace gympp::gazebo; using namespace gympp::plugins; using ActionDataType = int; @@ -46,9 +49,6 @@ enum CartPoleAction class CartPole::Impl { -private: - gympp::RobotPtr robot = nullptr; - public: unsigned seed; bool firstRun = true; @@ -59,23 +59,13 @@ class CartPole::Impl ObservationSample observationBuffer; std::optional action; + std::shared_ptr robot = nullptr; + double getRandomThetaInRad() { std::uniform_real_distribution<> distr(-MaxTheta0Rad, MaxTheta0Rad); return distr(gympp::Random::engine()); } - - gympp::RobotPtr getRobot() - { - if (!robot) { - auto& robotSingleton = gympp::gazebo::RobotSingleton::get(); - robot = robotSingleton.getRobot("cartpole_xacro"); - } - - assert(robot); - assert(robot->valid()); - return robot; - } }; CartPole::CartPole() @@ -89,6 +79,51 @@ CartPole::CartPole() pImpl->observationBuffer.resize(4); } +void CartPole::Configure(const ignition::gazebo::Entity& entity, + const std::shared_ptr& sdf, + ignition::gazebo::EntityComponentManager& ecm, + ignition::gazebo::EventManager& eventMgr) +{ + // Get the sdf element that contains the scoped name of the model. + // It is used only for multiple environments running in seraparated threads. + std::string scopedModelName; + auto sdfClone = sdf->Clone(); + sdf::ElementPtr element = sdfClone->GetElement("scoped_name"); + + if (!element) { + gymppError << "Failed to find 'scoped_name' sdf plugin element" << std::endl; + return; + } + + // Get the scoped name of the model + scopedModelName = element->Get(); + + // Auto-register the environment callbacks + auto ecSingleton = EnvironmentCallbacksSingleton::Instance(); + bool registered = ecSingleton->storeEnvironmentCallback(scopedModelName, this); + assert(registered); + + // Create a gympp::IgnitionRobot object from the ecm + auto ignRobot = std::make_shared(); + if (!ignRobot->configureECM(entity, sdf, ecm)) { + gymppError << "Failed to configure the Robot interface" << std::endl; + return; + } + + if (!ignRobot->valid()) { + gymppError << "The Robot interface is not valid" << std::endl; + return; + } + + // Store a pointer to gympp::Robot + pImpl->robot = ignRobot; + + // TODO: Right now the gympp::Robot interface is only used inside this plugin. + // Since we want to expose the robot also to python (in order to read and + // modify the state) it can be registered in the RobotSingleton using the + // scoped name. +} + void CartPole::PreUpdate(const ignition::gazebo::UpdateInfo& info, ignition::gazebo::EntityComponentManager& /*manager*/) { @@ -96,15 +131,13 @@ void CartPole::PreUpdate(const ignition::gazebo::UpdateInfo& info, return; } - // Get the pointer to the Robot interface - gympp::RobotPtr robot = pImpl->getRobot(); - assert(robot); + assert(pImpl->robot); if (pImpl->firstRun) { pImpl->firstRun = false; // Initialize the PID - if (!robot->setJointPID("linear", {10000, 50, 200})) { + if (!pImpl->robot->setJointPID("linear", {10000, 50, 200})) { gymppError << "Failed to set the PID of joint 'linear'" << std::endl; return; } @@ -114,7 +147,7 @@ void CartPole::PreUpdate(const ignition::gazebo::UpdateInfo& info, } // Set the step size - if (!robot->setdt(info.dt)) { + if (!pImpl->robot->setdt(info.dt)) { gymppError << "Failed to set the step size" << std::endl; return; } @@ -143,7 +176,7 @@ void CartPole::PreUpdate(const ignition::gazebo::UpdateInfo& info, pImpl->action.reset(); } - if (!robot->setJointForce("linear", appliedForce)) { + if (!pImpl->robot->setJointForce("linear", appliedForce)) { gymppError << "Failed to set the force to joint 'linear'" << std::endl; return; } @@ -157,15 +190,13 @@ void CartPole::PostUpdate(const ignition::gazebo::UpdateInfo& info, return; } - // Get the pointer to the Robot interface - gympp::RobotPtr robot = pImpl->getRobot(); - assert(robot); + assert(pImpl->robot); - double cartJointPosition = robot->jointPosition("linear"); - double poleJointPosition = robot->jointPosition("pivot"); + double cartJointPosition = pImpl->robot->jointPosition("linear"); + double poleJointPosition = pImpl->robot->jointPosition("pivot"); - double cartJointVelocity = robot->jointVelocity("linear"); - double poleJointVelocity = robot->jointVelocity("pivot"); + double cartJointVelocity = pImpl->robot->jointVelocity("linear"); + double poleJointVelocity = pImpl->robot->jointVelocity("pivot"); { std::lock_guard lock(pImpl->mutex); @@ -191,14 +222,7 @@ bool CartPole::isDone() bool CartPole::reset() { - // Get the pointer to the Robot interface - gympp::RobotPtr robot = pImpl->getRobot(); - assert(robot); - - if (!robot) { - gymppError << "Failed to get pointer to the robot interface" << std::endl; - return false; - } + assert(pImpl->robot); // Reset the number of iterations pImpl->iterations = 0; @@ -209,13 +233,13 @@ bool CartPole::reset() auto theta0 = pImpl->getRandomThetaInRad(); // Set the random pole angle - if (!robot->resetJoint("pivot", theta0, v0)) { + if (!pImpl->robot->resetJoint("pivot", theta0, v0)) { gymppError << "Failed to reset the position of joint 'pivot'" << std::endl; return false; } // Reset the cart position - if (!robot->resetJoint("linear", x0, v0)) { + if (!pImpl->robot->resetJoint("linear", x0, v0)) { gymppError << "Failed to reset the position of joint 'linear'" << std::endl; return false; } @@ -278,6 +302,7 @@ std::optional CartPole::getObs IGNITION_ADD_PLUGIN(gympp::plugins::CartPole, gympp::plugins::CartPole::System, + gympp::plugins::CartPole::ISystemConfigure, gympp::plugins::CartPole::ISystemPreUpdate, gympp::plugins::CartPole::ISystemPostUpdate, gympp::gazebo::EnvironmentCallbacks) diff --git a/plugins/CartPole/CartPolePlugin.h b/plugins/CartPole/CartPolePlugin.h index cfda4a285..c76db16b4 100644 --- a/plugins/CartPole/CartPolePlugin.h +++ b/plugins/CartPole/CartPolePlugin.h @@ -24,6 +24,7 @@ namespace gympp { class gympp::plugins::CartPole final : public ignition::gazebo::System + , public ignition::gazebo::ISystemConfigure , public ignition::gazebo::ISystemPreUpdate , public ignition::gazebo::ISystemPostUpdate , public gympp::gazebo::EnvironmentCallbacks @@ -36,6 +37,11 @@ class gympp::plugins::CartPole final CartPole(); ~CartPole() override = default; + void Configure(const ignition::gazebo::Entity& entity, + const std::shared_ptr& sdf, + ignition::gazebo::EntityComponentManager& ecm, + ignition::gazebo::EventManager& eventMgr) override; + void PreUpdate(const ignition::gazebo::UpdateInfo& info, ignition::gazebo::EntityComponentManager& manager) override; From 79ef7632f5c2b4abbe04aa0fa72eb03829544519 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 15 Apr 2019 12:09:24 +0200 Subject: [PATCH 05/11] Fixed methods call order in GymFactory Now setupGazeboWorld has to ben called before setupIgnitionPlugin --- plugins/GymFactory/src/GymFactory.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/plugins/GymFactory/src/GymFactory.cpp b/plugins/GymFactory/src/GymFactory.cpp index 1a9840ec3..3627244c5 100644 --- a/plugins/GymFactory/src/GymFactory.cpp +++ b/plugins/GymFactory/src/GymFactory.cpp @@ -76,6 +76,11 @@ gympp::EnvironmentPtr gympp::GymFactory::make(const std::__cxx11::string& envNam observationSpace, /*updateRate=*/50, /*iterations=*/10); + // Setup the world + if (!ignGym->setupGazeboWorld(md.worldFileName, md.modelNames)) { + gymppError << "Failed to setup SDF file"; + return nullptr; + } // Setup the CartPolePlugin if (!ignGym->setupIgnitionPlugin(md.libraryName, md.className)) { @@ -83,12 +88,6 @@ gympp::EnvironmentPtr gympp::GymFactory::make(const std::__cxx11::string& envNam return nullptr; } - // Setup the world - if (!ignGym->setupGazeboWorld(md.worldFileName, md.modelNames)) { - gymppError << "Failed to setup SDF file"; - return nullptr; - } - return ignGym->env(); } From a477ccbc04a16aff91592b9e6aa9c4e6e7f2a422 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 16 Apr 2019 18:21:59 +0200 Subject: [PATCH 06/11] Get env callbacks from the singleton and split robot and world This commit: - Get the EnvironmentCallbacks pointer from the singleton. Plugins autoregister themselves during their Configure step. - Split robot and world. They are stored in two different files and not anymore merged by sdformat. This allows parsing the model sdf and automatically get the models. The plugin is then attached to the model element, and this permits to the plugin to correctly initialize the gympp::Robot object. - The plugin is loaded again from cpp instead of sdf. - The pose of the model will be handled in another PR. --- .../gympp/gazebo/IgnitionEnvironment.h | 8 +- ignition/src/IgnitionEnvironment.cpp | 195 +++++++++++++----- models/CartPole/CartPole.sdf | 3 - models/worlds/CartPole.world | 6 +- 4 files changed, 155 insertions(+), 57 deletions(-) diff --git a/ignition/include/gympp/gazebo/IgnitionEnvironment.h b/ignition/include/gympp/gazebo/IgnitionEnvironment.h index 2750c4f21..61c808296 100644 --- a/ignition/include/gympp/gazebo/IgnitionEnvironment.h +++ b/ignition/include/gympp/gazebo/IgnitionEnvironment.h @@ -42,6 +42,8 @@ class gympp::gazebo::IgnitionEnvironment using Environment::Reward; using Environment::State; + static size_t EnvironmentId; + IgnitionEnvironment() = delete; IgnitionEnvironment(const ActionSpacePtr aSpace, const ObservationSpacePtr oSpace, @@ -57,7 +59,11 @@ class gympp::gazebo::IgnitionEnvironment // Public APIs EnvironmentPtr env(); static void setVerbosity(int level = DEFAULT_VERBOSITY); - bool setupGazeboWorld(const std::string& worldFile, const std::vector& modelNames); + + bool setupGazeboModel(const std::string& modelFile, + std::array pose = {0, 0, 0, 0, 0, 0}); + bool setupGazeboWorld(const std::string& worldFile); + bool setupIgnitionPlugin(const std::string& libName, const std::string& className); }; diff --git a/ignition/src/IgnitionEnvironment.cpp b/ignition/src/IgnitionEnvironment.cpp index 2d3f9adcf..6223a3518 100644 --- a/ignition/src/IgnitionEnvironment.cpp +++ b/ignition/src/IgnitionEnvironment.cpp @@ -19,7 +19,9 @@ #include #include #include -//#include +#include +#include +#include #include #include @@ -30,37 +32,56 @@ using namespace gympp::gazebo; +size_t IgnitionEnvironment::EnvironmentId = 0; + class IgnitionEnvironment::Impl { +private: + gympp::gazebo::EnvironmentCallbacks* cb = nullptr; + public: + size_t id; + std::unique_ptr ignitionGui; uint64_t numOfIterations = 0; ignition::gazebo::ServerConfig serverConfig; std::shared_ptr server; + + gympp::gazebo::EnvironmentCallbacks* envCallbacks(); std::shared_ptr getServer(); - gympp::gazebo::EnvironmentCallbacks* envCallbacks() - { - auto ecSingleton = EnvironmentCallbacksSingleton::Instance(); - auto* callbacks = ecSingleton->get(scopedModelName); - assert(callbacks); - return callbacks; - } + sdf::Root sdf; + bool findAndLoadSdf(const std::string& sdfFileName, sdf::Root& sdfRoot); + + ignition::common::SystemPaths systemPaths; std::string scopedModelName; - std::vector modelsNamesInSdf; }; +gympp::gazebo::EnvironmentCallbacks* IgnitionEnvironment::Impl::envCallbacks() +{ + if (!cb) { + auto ecSingleton = EnvironmentCallbacksSingleton::Instance(); + cb = ecSingleton->get(scopedModelName); + assert(cb); + } + + return cb; +} + std::shared_ptr IgnitionEnvironment::Impl::getServer() { // Lazy initialization of the server if (!server) { - if (serverConfig.SdfFile().empty() && serverConfig.SdfString().empty()) { - gymppError << "The sdf file was not configured" << std::endl; - return nullptr; - } + assert(sdf.Element()); + assert(!sdf.Element()->ToString("").empty()); + serverConfig.SetSdfString(sdf.Element()->ToString("")); + + sdf::Root root; + auto errors = root.LoadSdfString(sdf.Element()->ToString("")); + assert(errors.empty()); // This should be already ok // Create the server gymppDebug << "Creating the server" << std::endl << std::flush; @@ -80,6 +101,38 @@ std::shared_ptr IgnitionEnvironment::Impl::getServer() return server; } +// TODO: there's a bug in the destructor of sdf::Physics that prevents returning std::optional +bool IgnitionEnvironment::Impl::findAndLoadSdf(const std::string& sdfFileName, sdf::Root& root) +{ + if (sdfFileName.empty()) { + gymppError << "The SDF file name of the gazebo model is empty" << std::endl; + return {}; + } + + // Find the file + // TODO: add install directory of our world and model files + std::string sdfFilePath = systemPaths.FindFile(sdfFileName); + + if (sdfFilePath.empty()) { + gymppError << "Failed to find '" << sdfFileName << "'. " + << "Check that it's contained in the paths defined in IGN_GAZEBO_RESOURCE_PATH." + << std::endl; + return {}; + } + + // Load the sdf + auto errors = root.Load(sdfFilePath); + + if (!errors.empty()) { + gymppError << "Failed to load sdf file '" << sdfFilePath << "." << std::endl; + for (const auto& error : errors) { + gymppError << error << std::endl; + } + return {}; + } + return true; +} + // =============== // IGNITION GAZEBO // =============== @@ -91,18 +144,34 @@ IgnitionEnvironment::IgnitionEnvironment(const ActionSpacePtr aSpace, : Environment(aSpace, oSpace) , pImpl{new IgnitionEnvironment::Impl, [](Impl* impl) { delete impl; }} { - setVerbosity(4); - // pImpl->sdfFile = sdfFile; + // Assign an unique id to the object + pImpl->id = EnvironmentId++; + pImpl->numOfIterations = iterations; pImpl->serverConfig.SetUpdateRate(updateRate); + + pImpl->systemPaths.SetFilePathEnv("IGN_GAZEBO_RESOURCE_PATH"); + pImpl->systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); + // TODO: add install world / models path + + // Set default verbosity which is dependent on the compilation flags + setVerbosity(); } -static size_t counter = 0; bool IgnitionEnvironment::setupIgnitionPlugin(const std::string& libName, const std::string& className) { - auto uniqueID = counter++; - pImpl->scopedModelName = pImpl->modelsNamesInSdf.front() + std::to_string(uniqueID); + assert(!pImpl->scopedModelName.empty()); + + sdf::ElementPtr sdf(new sdf::Element); + sdf->SetName("plugin"); + sdf->AddAttribute("name", "string", className, true); + sdf->AddAttribute("filename", "string", libName, true); + + ignition::gazebo::ServerConfig::PluginInfo pluginInfo{ + pImpl->scopedModelName, "model", libName, className, sdf}; + + pImpl->serverConfig.AddPlugin(pluginInfo); return true; } @@ -213,47 +282,77 @@ void IgnitionEnvironment::setVerbosity(int level) ignition::common::Console::SetVerbosity(level); } -bool IgnitionEnvironment::setupGazeboWorld(const std::string& worldFile, - const std::vector& modelNames) +bool IgnitionEnvironment::setupGazeboModel(const std::string& modelFile, + std::array /*pose*/) { - // ================= - // LOAD THE SDF FILE - // ================= - - if (worldFile.empty()) { - gymppError << "Passed SDF file argument is an empty string" << std::endl; + if (!pImpl->scopedModelName.empty()) { + gymppError << "The model has been already configured previously" << std::endl; return false; } - // Find the file - // TODO: add install directory of our world files - ignition::common::SystemPaths systemPaths; - systemPaths.SetFilePathEnv("IGN_GAZEBO_RESOURCE_PATH"); - systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); - std::string filePath = systemPaths.FindFile(worldFile); + if (pImpl->sdf.WorldCount() <= 0) { + gymppError << "The gazebo world was not properly configured" << std::endl; + return false; + } - if (filePath.empty()) { - gymppError << "Failed to find '" << worldFile << "'. " - << "Check that it's contained in the paths defined in IGN_GAZEBO_RESOURCE_PATH." - << std::endl; - gymppError << "If you use the element, make sure to add the parent folder of the " - << " in the SDF_PATH variable." << std::endl; + // Find and load the sdf file that contains the model + sdf::Root sdfRoot; + if (!pImpl->findAndLoadSdf(modelFile, sdfRoot)) { + gymppError << "Failed to find and load sdf file '" << modelFile << "'" << std::endl; return false; } - if (!pImpl->serverConfig.SetSdfFile(filePath)) { - gymppError << "Failed to set the SDF file " << worldFile << std::endl; + if (sdfRoot.ModelCount() == 0) { + gymppError << "Failed to find any model in '" << modelFile << "' sdf file" << std::endl; return false; } - // ====================================== - // LOAD A ROBOT PLUGIN FOR EACH SDF MODEL - // ====================================== + // Get the models names included in the sdf file. + // In order to allow multithreading, the name of the model contained in the sdf must be scoped. + // We use the id of the IgnitionEnvironment object as scope. + for (unsigned i = 0; i < sdfRoot.ModelCount(); ++i) { + // Get the model name + std::string modelName = sdfRoot.ModelByIndex(i)->Name(); + gymppDebug << "Found model '" << modelName << "' in the sdf file" << std::endl; + + // Create a scoped name + std::string scopedModelName = std::to_string(pImpl->id) + "::" + modelName; + gymppDebug << "Registering scoped '" << scopedModelName << "' model" << std::endl; + + // Copy the content of the model in another sdf element. + // This has to be done because it is not possible to change in-place the name of the model, + // so we create a new element and we copy all the children. + sdf::ElementPtr renamedModel(new sdf::Element); + renamedModel->SetName("model"); + renamedModel->AddAttribute("name", "string", scopedModelName, true); + + sdf::ElementPtr child = sdfRoot.ModelByIndex(i)->Element()->GetFirstElement(); + + while (child) { + renamedModel->InsertElement(child); + child = child->GetNextElement(); + } + + // TODO: Temporarily support only one model, and it must be the one to which the gympp + // plugin will be attached to. + assert(sdfRoot.ModelCount() == 1); + pImpl->scopedModelName = scopedModelName; + + // Attach the model to the sdf world + assert(pImpl->sdf.WorldCount() == 1); + pImpl->sdf.WorldByIndex(0)->Element()->InsertElement(renamedModel); + } + + return true; +} - // Store the model names - // TODO: We should separate robot and environment. In this way we would have an sdf file for - // the robot that we can parse to get automatically the model names. - pImpl->modelsNamesInSdf = modelNames; +bool IgnitionEnvironment::setupGazeboWorld(const std::string& worldFile) +{ + // Find and load the sdf file that contains the world + if (!pImpl->findAndLoadSdf(worldFile, pImpl->sdf)) { + gymppError << "Failed to find and load sdf file '" << worldFile << "'" << std::endl; + return false; + } return true; } @@ -265,7 +364,7 @@ gympp::EnvironmentPtr IgnitionEnvironment::env() std::optional IgnitionEnvironment::reset() { - gymppMessage << "Resetting the environment" << std::endl; + gymppDebug << "Resetting the environment" << std::endl; // The plugin must be loaded in order to call its reset() method if (!pImpl->getServer()) { diff --git a/models/CartPole/CartPole.sdf b/models/CartPole/CartPole.sdf index 77be378b0..e18ecce70 100644 --- a/models/CartPole/CartPole.sdf +++ b/models/CartPole/CartPole.sdf @@ -1,8 +1,5 @@ - - cartpole_xacro0 - 0 0 0.5 0 -0 0 diff --git a/models/worlds/CartPole.world b/models/worlds/CartPole.world index 46085567a..854c1eef6 100644 --- a/models/worlds/CartPole.world +++ b/models/worlds/CartPole.world @@ -46,6 +46,7 @@ /world/default/control /world/default/stats +