diff --git a/CMakeLists.txt b/CMakeLists.txt index 2d5873141..d4bb13db8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -120,11 +120,11 @@ add_subdirectory(cpp/gympp/base) add_subdirectory(cpp/gympp/gazebo) add_subdirectory(cpp/gympp/plugins) add_subdirectory(cpp/scenario/gazebo) +add_subdirectory(cpp/scenario/plugins) # Add the targets depending on Ignition Robotics if(${GYMIGNITION_USE_IGNITION}) add_subdirectory(ignition) - add_subdirectory(plugins) add_subdirectory(gym_ignition_data) add_subdirectory(examples/cpp) diff --git a/bindings/CMakeLists.txt b/bindings/CMakeLists.txt index befdafae1..6f8de1ee8 100644 --- a/bindings/CMakeLists.txt +++ b/bindings/CMakeLists.txt @@ -40,6 +40,7 @@ target_link_libraries(${swig_name} PUBLIC RobotSingleton GymFactory GazeboEnvironment + ECMSingleton RobotSingleton GazeboWrapper ScenarioGazebo diff --git a/cpp/scenario/plugins/CMakeLists.txt b/cpp/scenario/plugins/CMakeLists.txt new file mode 100644 index 000000000..9ab2d3202 --- /dev/null +++ b/cpp/scenario/plugins/CMakeLists.txt @@ -0,0 +1,27 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) +# All rights reserved. +# +# This project is dual licensed under LGPL v2.1+ or Apache License. +# +# - - - - - - - - - - - - - - - - - - +# +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. +# +# - - - - - - - - - - - - - - - - - - +# +# 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. + +add_subdirectory(Physics) +add_subdirectory(ECMProvider) +add_subdirectory(RobotController) diff --git a/cpp/scenario/plugins/ECMProvider/CMakeLists.txt b/cpp/scenario/plugins/ECMProvider/CMakeLists.txt new file mode 100644 index 000000000..65f69d7f1 --- /dev/null +++ b/cpp/scenario/plugins/ECMProvider/CMakeLists.txt @@ -0,0 +1,84 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) +# All rights reserved. +# +# This project is dual licensed under LGPL v2.1+ or Apache License. +# +# - - - - - - - - - - - - - - - - - - +# +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. +# +# - - - - - - - - - - - - - - - - - - +# +# 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. + +# =========== +# ECMProvider +# =========== + +add_library(ECMProvider SHARED + include/scenario/plugins/gazebo/ECMProvider.h + ECMProvider.cpp) + +target_link_libraries(ECMProvider + PUBLIC + ignition-gazebo3::core + PRIVATE + ECMSingleton + ExtraComponents) + +target_include_directories(ECMProvider PRIVATE + $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>) + +# ============ +# ECMSingleton +# ============ + +add_library(ECMSingleton + include/scenario/plugins/gazebo/ECMSingleton.h + ECMSingleton.cpp) + +target_include_directories(ECMSingleton PUBLIC + $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> + $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>) + +target_link_libraries(ECMSingleton + PUBLIC + ignition-gazebo3::core + PRIVATE + ScenarioGazebo) + +if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI") + set_target_properties(ECMSingleton PROPERTIES + PUBLIC_HEADER include/scenario/plugins/gazebo/ECMSingleton.h) +endif() + +# =================== +# Install the targets +# =================== + +if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI") + install( + TARGETS ECMProvider + EXPORT scenario + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}/scenario/plugins + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}/scenario/plugins + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}/scenario/plugins) + install( + TARGETS ECMSingleton + EXPORT scenario + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/scenario/plugins/gazebo) +endif() diff --git a/cpp/scenario/plugins/ECMProvider/ECMProvider.cpp b/cpp/scenario/plugins/ECMProvider/ECMProvider.cpp new file mode 100644 index 000000000..17a60fa01 --- /dev/null +++ b/cpp/scenario/plugins/ECMProvider/ECMProvider.cpp @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apache License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * 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 "scenario/plugins/gazebo/ECMProvider.h" +#include "scenario/gazebo/Log.h" +#include "scenario/plugins/gazebo/ECMSingleton.h" + +#include <ignition/gazebo/Entity.hh> +#include <ignition/plugin/Register.hh> + +using namespace scenario::plugins::gazebo; + +class ECMProvider::Impl +{ +public: +}; + +ECMProvider::ECMProvider() + : System() + , pImpl{std::make_unique<Impl>()} +{} + +ECMProvider::~ECMProvider() +{ + gymppDebug << "Destroying the ECMProvider" << std::endl; +}; + +void ECMProvider::Configure(const ignition::gazebo::Entity& entity, + const std::shared_ptr<const sdf::Element>& /*sdf*/, + ignition::gazebo::EntityComponentManager& ecm, + ignition::gazebo::EventManager& eventMgr) +{ + if (ECMSingleton::Instance().valid()) { + gymppWarning << "The ECM singleton has been already configured" + << std::endl; + return; + } + + gymppDebug << "Storing ECM resources in the singleton" << std::endl; + + if (!ECMSingleton::Instance().storePtrs(&ecm, &eventMgr)) { + gymppError << "Failed to store ECM in the singleton for entity [" + << entity << "]" << std::endl; + return; + } +} + +IGNITION_ADD_PLUGIN(scenario::plugins::gazebo::ECMProvider, + scenario::plugins::gazebo::ECMProvider::System, + scenario::plugins::gazebo::ECMProvider::ISystemConfigure) diff --git a/cpp/scenario/plugins/ECMProvider/ECMSingleton.cpp b/cpp/scenario/plugins/ECMProvider/ECMSingleton.cpp new file mode 100644 index 000000000..a07048b80 --- /dev/null +++ b/cpp/scenario/plugins/ECMProvider/ECMSingleton.cpp @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apache License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * 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 "scenario/plugins/gazebo/ECMSingleton.h" +#include "scenario/gazebo/Log.h" + +#include <atomic> +#include <ostream> +#include <string> + +using namespace scenario::plugins::gazebo; + +class ECMSingleton::Impl +{ +public: + std::atomic<ignition::gazebo::EventManager*> eventMgr = nullptr; + std::atomic<ignition::gazebo::EntityComponentManager*> ecm = nullptr; +}; + +ECMSingleton::ECMSingleton() + : pImpl{new Impl()} +{} + +ECMSingleton& ECMSingleton::Instance() +{ + static ECMSingleton instance; + return instance; +} + +bool ECMSingleton::valid() const +{ + return pImpl->ecm && pImpl->eventMgr; +} + +ignition::gazebo::EventManager* ECMSingleton::getEventManager() const +{ + if (!this->valid()) { + gymppError << "The pointers are not valid" << std::endl; + return nullptr; + } + + return pImpl->eventMgr; +} + +bool ECMSingleton::storePtrs(ignition::gazebo::EntityComponentManager* ecm, + ignition::gazebo::EventManager* eventMgr) +{ + if (!ecm || !eventMgr) { + gymppError << "The pointer to the ECM or EventManager is not valid" + << std::endl; + return false; + } + + pImpl->ecm = ecm; + pImpl->eventMgr = eventMgr; + + return true; +} + +ignition::gazebo::EntityComponentManager* ECMSingleton::getECM() const +{ + + if (!this->valid()) { + gymppError << "The pointers are not valid" << std::endl; + return nullptr; + } + + return pImpl->ecm; +} + +void ECMSingleton::clean() +{ + pImpl->ecm = nullptr; + pImpl->eventMgr = nullptr; +} diff --git a/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMProvider.h b/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMProvider.h new file mode 100644 index 000000000..0009112c3 --- /dev/null +++ b/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMProvider.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apache License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * 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 SCENARIO_PLUGINS_GAZEBO_ECMPROVIDER +#define SCENARIO_PLUGINS_GAZEBO_ECMPROVIDER + +#include <ignition/gazebo/Entity.hh> +#include <ignition/gazebo/EntityComponentManager.hh> +#include <ignition/gazebo/EventManager.hh> +#include <ignition/gazebo/System.hh> +#include <sdf/Element.hh> + +#include <memory> + +namespace scenario { + namespace plugins { + namespace gazebo { + class ECMProvider; + } // namespace gazebo + } // namespace plugins +} // namespace scenario + +class scenario::plugins::gazebo::ECMProvider final + : public ignition::gazebo::System + , public ignition::gazebo::ISystemConfigure +{ +public: + ECMProvider(); + ~ECMProvider() override; + + void Configure(const ignition::gazebo::Entity& entity, + const std::shared_ptr<const sdf::Element>& sdf, + ignition::gazebo::EntityComponentManager& ecm, + ignition::gazebo::EventManager& eventMgr) override; + +private: + class Impl; + std::unique_ptr<Impl> pImpl = nullptr; +}; + +#endif // SCENARIO_PLUGINS_GAZEBO_ECMPROVIDER diff --git a/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMSingleton.h b/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMSingleton.h new file mode 100644 index 000000000..3380719ff --- /dev/null +++ b/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMSingleton.h @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apache License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * 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 SCENARIO_PLUGINS_GAZEBO_ECMSINGLETON_H +#define SCENARIO_PLUGINS_GAZEBO_ECMSINGLETON_H + +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/EventManager.hh" + +#include <memory> + +namespace scenario { + namespace plugins { + namespace gazebo { + class ECMSingleton; + } // namespace gazebo + } // namespace plugins +} // namespace scenario + +class scenario::plugins::gazebo::ECMSingleton +{ +public: + ECMSingleton(); + ~ECMSingleton() = default; + + ECMSingleton(ECMSingleton&) = delete; + void operator=(const ECMSingleton&) = delete; + + static ECMSingleton& Instance(); + + void clean(); + bool valid() const; + + ignition::gazebo::EventManager* getEventManager() const; + ignition::gazebo::EntityComponentManager* getECM() const; + + bool storePtrs(ignition::gazebo::EntityComponentManager* ecm, + ignition::gazebo::EventManager* eventMgr); + +private: + class Impl; + std::unique_ptr<Impl> pImpl; +}; + +#endif // SCENARIO_PLUGINS_GAZEBO_ECMSINGLETON_H diff --git a/cpp/scenario/plugins/Physics/CMakeLists.txt b/cpp/scenario/plugins/Physics/CMakeLists.txt new file mode 100644 index 000000000..61c117443 --- /dev/null +++ b/cpp/scenario/plugins/Physics/CMakeLists.txt @@ -0,0 +1,52 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) +# All rights reserved. +# +# This project is dual licensed under LGPL v2.1+ or Apache License. +# +# - - - - - - - - - - - - - - - - - - +# +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. +# +# - - - - - - - - - - - - - - - - - - +# +# 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. + +find_package(ignition-physics2 COMPONENTS dartsim REQUIRED) + +add_library(PhysicsSystem SHARED + Physics.h + Physics.cpp) + +target_link_libraries(PhysicsSystem + PUBLIC + ignition-gazebo3::core + PRIVATE + ignition-physics2 + ExtraComponents + ScenarioGazebo) + +target_include_directories(PhysicsSystem PRIVATE + $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>) + +target_compile_definitions(PhysicsSystem PRIVATE + ""dartsim_plugin_LIB=\"$<TARGET_SONAME_FILE:ignition-physics2::ignition-physics2-dartsim-plugin>\""") + +if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI") + install( + TARGETS PhysicsSystem + EXPORT scenario + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}/scenario/plugins + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}/scenario/plugins + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}/scenario/plugins) +endif() diff --git a/plugins/Physics/Physics.cpp b/cpp/scenario/plugins/Physics/Physics.cpp similarity index 79% rename from plugins/Physics/Physics.cpp rename to cpp/scenario/plugins/Physics/Physics.cpp index f137c84ff..03799466c 100644 --- a/plugins/Physics/Physics.cpp +++ b/cpp/scenario/plugins/Physics/Physics.cpp @@ -1,14 +1,7 @@ /* - * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2020 Open Source Robotics Foundation * 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. - * - * ================================================== - * - * Copyright (C) 2018 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 @@ -20,13 +13,16 @@ * 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 "Physics.h" +#include "scenario/gazebo/components/ExternalWorldWrenchCmdWithDuration.h" +#include "scenario/gazebo/components/HistoryOfAppliedJointForces.h" #include "scenario/gazebo/components/JointPositionReset.h" #include "scenario/gazebo/components/JointVelocityReset.h" +#include "scenario/gazebo/components/SimulatedTime.h" #include "scenario/gazebo/components/WorldVelocityCmd.h" +#include "scenario/gazebo/helpers.h" #include <ignition/common/MeshManager.hh> #include <ignition/gazebo/EntityComponentManager.hh> @@ -61,7 +57,6 @@ #include <ignition/gazebo/components/PoseCmd.hh> #include <ignition/gazebo/components/Static.hh> #include <ignition/gazebo/components/ThreadPitch.hh> -#include <ignition/gazebo/components/Visual.hh> #include <ignition/gazebo/components/World.hh> #include <ignition/math/eigen3/Conversions.hh> #include <ignition/msgs/Utility.hh> @@ -89,7 +84,6 @@ #include <ignition/physics/sdf/ConstructJoint.hh> #include <ignition/physics/sdf/ConstructLink.hh> #include <ignition/physics/sdf/ConstructModel.hh> -#include <ignition/physics/sdf/ConstructVisual.hh> #include <ignition/physics/sdf/ConstructWorld.hh> #include <ignition/plugin/Loader.hh> #include <ignition/plugin/PluginPtr.hh> @@ -99,7 +93,6 @@ #include <sdf/Link.hh> #include <sdf/Mesh.hh> #include <sdf/Model.hh> -#include <sdf/Visual.hh> #include <sdf/World.hh> #include <deque> @@ -107,7 +100,7 @@ #include <unordered_map> using namespace ignition; -using namespace gympp::plugins; +using namespace scenario::plugins::gazebo; using namespace ignition::gazebo; using namespace ignition::gazebo::systems; using namespace ignition::gazebo::components; @@ -136,7 +129,6 @@ class Physics::Impl ignition::physics::sdf::ConstructSdfJoint, ignition::physics::sdf::ConstructSdfLink, ignition::physics::sdf::ConstructSdfModel, - ignition::physics::sdf::ConstructSdfVisual, ignition::physics::sdf::ConstructSdfWorld> {}; @@ -174,7 +166,7 @@ class Physics::Impl /// \brief Update physics from components /// \param[in] _ecm Constant reference to ECM. - void UpdatePhysics(EntityComponentManager& _ecm); + void UpdatePhysics(const ignition::gazebo::UpdateInfo& _info, EntityComponentManager& _ecm); /// \brief Step the simulationrfor each world /// \param[in] _dt Duration @@ -182,7 +174,7 @@ class Physics::Impl /// \brief Update components from physics simulation /// \param[in] _ecm Mutable reference to ECM. - void UpdateSim(EntityComponentManager& _ecm) const; + void UpdateSim(const ignition::gazebo::UpdateInfo& _info, EntityComponentManager& _ecm) const; /// \brief Update collision components from physics simulation /// \param[in] _ecm Mutable reference to ECM. @@ -245,6 +237,10 @@ class Physics::Impl && math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) && math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6); }}; + + /// \brief Boolean value that is true only the first call of Configure and + /// PreUpdate. + bool firstRun = true; }; Physics::Physics() @@ -284,15 +280,26 @@ void Physics::Update(const UpdateInfo& _info, EntityComponentManager& _ecm) << "s]. System may not work properly." << std::endl; } + // Update the component with the time in seconds that the simulation will + // have after the step + _ecm.Each<components::World, components::SimulatedTime>( + [&](const Entity& worldEntity, const components::World*, components::SimulatedTime*) { + scenario::gazebo::utils::setExistingComponentData< + ignition::gazebo::components::SimulatedTime>(&_ecm, worldEntity, _info.simTime); + return true; + }); + if (this->pImpl->engine) { this->pImpl->CreatePhysicsEntities(_ecm); + this->pImpl->UpdatePhysics(_info, _ecm); + // Only step if not paused. if (!_info.paused) { - this->pImpl->UpdatePhysics(_ecm); this->pImpl->Step(_info.dt); - this->pImpl->UpdateSim(_ecm); } + this->pImpl->UpdateSim(_info, _ecm); + // Entities scheduled to be removed should be removed from physics after // the simulation step. Otherwise, since the to-be-removed entity still // shows up in the ECM::Each the UpdatePhysics and UpdateSim calls will @@ -303,123 +310,111 @@ void Physics::Update(const UpdateInfo& _info, EntityComponentManager& _ecm) void Physics::Impl::CreatePhysicsEntities(const EntityComponentManager& _ecm) { - // Get all the new worlds - _ecm.EachNew<components::World, components::Name, components::Gravity>( - [&](const Entity& _entity, - const components::World* /* _world */, - const components::Name* _name, - const components::Gravity* _gravity) -> bool { - // Check if world already exists - if (this->entityWorldMap.find(_entity) != this->entityWorldMap.end()) { - ignwarn << "World entity [" << _entity - << "] marked as new, but it's already on the map." << std::endl; - return true; - } + auto processWorld = [&](const Entity& _entity, + const components::World* /* _world */, + const components::Name* _name, + const components::Gravity* _gravity) -> bool { + if (this->entityWorldMap.find(_entity) != this->entityWorldMap.end()) { + ignwarn << "World entity [" << _entity + << "] marked as new, but it's already on the map." << std::endl; + return true; + } - sdf::World world; - world.SetName(_name->Data()); - world.SetGravity(_gravity->Data()); - auto worldPtrPhys = this->engine->ConstructWorld(world); - this->entityWorldMap.insert(std::make_pair(_entity, worldPtrPhys)); + sdf::World world; + world.SetName(_name->Data()); + world.SetGravity(_gravity->Data()); + auto worldPtrPhys = this->engine->ConstructWorld(world); + this->entityWorldMap.insert(std::make_pair(_entity, worldPtrPhys)); + return true; + }; + + auto processModel = [&](const Entity& _entity, + const components::Model*, + const components::Name* _name, + const components::Pose* _pose, + const components::ParentEntity* _parent) -> bool { + // ignerr << "model " << _name->Data() << std::endl; + // Check if model already exists + if (this->entityModelMap.find(_entity) != this->entityModelMap.end()) { + ignwarn << "Model entity [" << _entity + << "] marked as new, but it's already on the map." << std::endl; return true; - }); - - _ecm.EachNew<components::Model, components::Name, components::Pose, components::ParentEntity>( - [&](const Entity& _entity, - const components::Model*, - const components::Name* _name, - const components::Pose* _pose, - const components::ParentEntity* _parent) -> bool { - // Check if model already exists - if (this->entityModelMap.find(_entity) != this->entityModelMap.end()) { - ignwarn << "Model entity [" << _entity - << "] marked as new, but it's already on the map." << std::endl; - return true; - } + } - // TODO(anyone) Don't load models unless they have collisions + // TODO(anyone) Don't load models unless they have collisions - // Check if parent world exists - // TODO(louise): Support nested models, see - // https://bitbucket.org/ignitionrobotics/ign-physics/issues/10 - if (this->entityWorldMap.find(_parent->Data()) == this->entityWorldMap.end()) { - ignwarn << "Model's parent entity [" << _parent->Data() - << "] not found on world map." << std::endl; - return true; - } - auto worldPtrPhys = this->entityWorldMap.at(_parent->Data()); + // Check if parent world exists + // TODO(louise): Support nested models, see + // https://bitbucket.org/ignitionrobotics/ign-physics/issues/10 + if (this->entityWorldMap.find(_parent->Data()) == this->entityWorldMap.end()) { + ignwarn << "Model's parent entity [" << _parent->Data() << "] not found on world map." + << std::endl; + return true; + } + auto worldPtrPhys = this->entityWorldMap.at(_parent->Data()); - sdf::Model model; - model.SetName(_name->Data()); - model.SetRawPose(_pose->Data()); + sdf::Model model; + model.SetName(_name->Data()); + model.SetRawPose(_pose->Data()); - auto staticComp = _ecm.Component<components::Static>(_entity); - if (staticComp && staticComp->Data()) { - model.SetStatic(staticComp->Data()); - } + auto staticComp = _ecm.Component<components::Static>(_entity); + if (staticComp && staticComp->Data()) { + model.SetStatic(staticComp->Data()); + } - auto modelPtrPhys = worldPtrPhys->ConstructModel(model); - this->entityModelMap.insert(std::make_pair(_entity, modelPtrPhys)); + auto modelPtrPhys = worldPtrPhys->ConstructModel(model); + this->entityModelMap.insert(std::make_pair(_entity, modelPtrPhys)); + return true; + }; + + auto processLink = [&](const Entity& _entity, + const components::Link* /* _link */, + const components::Name* _name, + const components::Pose* _pose, + const components::ParentEntity* _parent) -> bool { + // Check if link already exists + if (this->entityLinkMap.find(_entity) != this->entityLinkMap.end()) { + ignwarn << "Link entity [" << _entity << "] marked as new, but it's already on the map." + << std::endl; return true; - }); - - _ecm.EachNew<components::Link, components::Name, components::Pose, components::ParentEntity>( - [&](const Entity& _entity, - const components::Link* /* _link */, - const components::Name* _name, - const components::Pose* _pose, - const components::ParentEntity* _parent) -> bool { - // Check if link already exists - if (this->entityLinkMap.find(_entity) != this->entityLinkMap.end()) { - ignwarn << "Link entity [" << _entity - << "] marked as new, but it's already on the map." << std::endl; - return true; - } - - // TODO(anyone) Don't load links unless they have collisions + } - // Check if parent model exists - if (this->entityModelMap.find(_parent->Data()) == this->entityModelMap.end()) { - ignwarn << "Link's parent entity [" << _parent->Data() - << "] not found on model map." << std::endl; - return true; - } - auto modelPtrPhys = this->entityModelMap.at(_parent->Data()); + // TODO(anyone) Don't load links unless they have collisions - sdf::Link link; - link.SetName(_name->Data()); - link.SetRawPose(_pose->Data()); + // Check if parent model exists + if (this->entityModelMap.find(_parent->Data()) == this->entityModelMap.end()) { + ignwarn << "Link's parent entity [" << _parent->Data() << "] not found on model map." + << std::endl; + return true; + } + auto modelPtrPhys = this->entityModelMap.at(_parent->Data()); - // get link inertial - auto inertial = _ecm.Component<components::Inertial>(_entity); - if (inertial) { - link.SetInertial(inertial->Data()); - } + sdf::Link link; + link.SetName(_name->Data()); + link.SetRawPose(_pose->Data()); - auto linkPtrPhys = modelPtrPhys->ConstructLink(link); - this->entityLinkMap.insert(std::make_pair(_entity, linkPtrPhys)); - this->linkEntityMap.insert(std::make_pair(linkPtrPhys, _entity)); + // get link inertial + auto inertial = _ecm.Component<components::Inertial>(_entity); + if (inertial) { + link.SetInertial(inertial->Data()); + } - return true; - }); + auto linkPtrPhys = modelPtrPhys->ConstructLink(link); + this->entityLinkMap.insert(std::make_pair(_entity, linkPtrPhys)); + this->linkEntityMap.insert(std::make_pair(linkPtrPhys, _entity)); - // We don't need to add visuals to the physics engine. - - // collisions - _ecm.EachNew<components::Collision, - components::Name, - components::Pose, - components::Geometry, - components::CollisionElement, - components::ParentEntity>([&](const Entity& _entity, - const components::Collision*, - const components::Name* _name, - const components::Pose* _pose, - const components::Geometry* _geom, - const components::CollisionElement* _collElement, - const components::ParentEntity* _parent) -> bool { + return true; + }; + + auto processCollision = [&](const Entity& _entity, + const components::Collision*, + const components::Name* _name, + const components::Pose* _pose, + const components::Geometry* _geom, + const components::CollisionElement* _collElement, + const components::ParentEntity* _parent) -> bool { if (this->entityCollisionMap.find(_entity) != this->entityCollisionMap.end()) { ignwarn << "Collision entity [" << _entity << "] marked as new, but it's already on the map." << std::endl; @@ -471,78 +466,137 @@ void Physics::Impl::CreatePhysicsEntities(const EntityComponentManager& _ecm) this->entityCollisionMap.insert(std::make_pair(_entity, collisionPtrPhys)); this->collisionEntityMap.insert(std::make_pair(collisionPtrPhys, _entity)); return true; - }); - - // joints - _ecm.EachNew<components::Joint, - components::Name, - components::JointType, - components::Pose, - components::ThreadPitch, - components::ParentEntity, - components::ParentLinkName, - components::ChildLinkName>( - [&](const Entity& _entity, - const components::Joint* /* _joint */, - const components::Name* _name, - const components::JointType* _jointType, - const components::Pose* _pose, - const components::ThreadPitch* _threadPitch, - const components::ParentEntity* _parentModel, - const components::ParentLinkName* _parentLinkName, - const components::ChildLinkName* _childLinkName) -> bool { - // Check if joint already exists - if (this->entityJointMap.find(_entity) != this->entityJointMap.end()) { - ignwarn << "Joint entity [" << _entity - << "] marked as new, but it's already on the map." << std::endl; - return true; - } - - // Check if parent model exists - if (this->entityModelMap.find(_parentModel->Data()) == this->entityModelMap.end()) { - ignwarn << "Joint's parent entity [" << _parentModel->Data() - << "] not found on model map." << std::endl; - return true; - } - auto modelPtrPhys = this->entityModelMap.at(_parentModel->Data()); - - sdf::Joint joint; - joint.SetName(_name->Data()); - joint.SetType(_jointType->Data()); - joint.SetRawPose(_pose->Data()); - joint.SetThreadPitch(_threadPitch->Data()); - - joint.SetParentLinkName(_parentLinkName->Data()); - joint.SetChildLinkName(_childLinkName->Data()); - - auto jointAxis = _ecm.Component<components::JointAxis>(_entity); - auto jointAxis2 = _ecm.Component<components::JointAxis2>(_entity); - - // Since we're making copies of the joint axes that were created using - // `Model::Load`, frame semantics should work for resolving their xyz - // axis - if (jointAxis) - joint.SetAxis(0, jointAxis->Data()); - if (jointAxis2) - joint.SetAxis(1, jointAxis2->Data()); - - // Use the parent link's parent model as the model of this joint - auto jointPtrPhys = modelPtrPhys->ConstructJoint(joint); - - if (jointPtrPhys.Valid()) { - // Some joints may not be supported, so only add them to the map if - // the physics entity is valid - this->entityJointMap.insert(std::make_pair(_entity, jointPtrPhys)); - } + }; + + auto processJoint = [&](const Entity& _entity, + const components::Joint* /* _joint */, + const components::Name* _name, + const components::JointType* _jointType, + const components::Pose* _pose, + const components::ThreadPitch* _threadPitch, + const components::ParentEntity* _parentModel, + const components::ParentLinkName* _parentLinkName, + const components::ChildLinkName* _childLinkName) -> bool { + // Check if joint already exists + if (this->entityJointMap.find(_entity) != this->entityJointMap.end()) { + ignwarn << "Joint entity [" << _entity + << "] marked as new, but it's already on the map." << std::endl; return true; - }); + } - _ecm.EachNew<components::BatterySoC>( - [&](const Entity& _entity, const components::BatterySoC*) -> bool { - // Parent entity of battery is model entity - this->entityOffMap.insert(std::make_pair(_ecm.ParentEntity(_entity), false)); + // Check if parent model exists + if (this->entityModelMap.find(_parentModel->Data()) == this->entityModelMap.end()) { + ignwarn << "Joint's parent entity [" << _parentModel->Data() + << "] not found on model map." << std::endl; return true; - }); + } + auto modelPtrPhys = this->entityModelMap.at(_parentModel->Data()); + + sdf::Joint joint; + joint.SetName(_name->Data()); + joint.SetType(_jointType->Data()); + joint.SetRawPose(_pose->Data()); + joint.SetThreadPitch(_threadPitch->Data()); + + joint.SetParentLinkName(_parentLinkName->Data()); + joint.SetChildLinkName(_childLinkName->Data()); + + auto jointAxis = _ecm.Component<components::JointAxis>(_entity); + auto jointAxis2 = _ecm.Component<components::JointAxis2>(_entity); + + // Since we're making copies of the joint axes that were created using + // `Model::Load`, frame semantics should work for resolving their xyz + // axis + if (jointAxis) + joint.SetAxis(0, jointAxis->Data()); + if (jointAxis2) + joint.SetAxis(1, jointAxis2->Data()); + + // Use the parent link's parent model as the model of this joint + auto jointPtrPhys = modelPtrPhys->ConstructJoint(joint); + + if (jointPtrPhys.Valid()) { + // Some joints may not be supported, so only add them to the map if + // the physics entity is valid + this->entityJointMap.insert(std::make_pair(_entity, jointPtrPhys)); + } + return true; + }; + + if (this->firstRun) { + this->firstRun = false; + + _ecm.Each<components::World, components::Name, components::Gravity>(processWorld); + + _ecm.Each<components::Model, components::Name, components::Pose, components::ParentEntity>( + processModel); + + _ecm.Each<components::Link, components::Name, components::Pose, components::ParentEntity>( + processLink); + + // We don't need to add visuals to the physics engine. + + _ecm.Each<components::Collision, + components::Name, + components::Pose, + components::Geometry, + components::CollisionElement, + components::ParentEntity>(processCollision); + + _ecm.Each<components::Joint, + components::Name, + components::JointType, + components::Pose, + components::ThreadPitch, + components::ParentEntity, + components::ParentLinkName, + components::ChildLinkName>(processJoint); + + _ecm.Each<components::BatterySoC>( + [&](const Entity& _entity, const components::BatterySoC*) -> bool { + // Parent entity of battery is model entity + this->entityOffMap.insert(std::make_pair(_ecm.ParentEntity(_entity), false)); + return true; + }); + } + else { + _ecm.EachNew<components::World, components::Name, components::Gravity>(processWorld); + + _ecm.EachNew<components::Model, + components::Name, + components::Pose, + components::ParentEntity>(processModel); + + _ecm.EachNew<components::Link, + components::Name, + components::Pose, + components::ParentEntity>(processLink); + + // We don't need to add visuals to the physics engine. + + _ecm.EachNew<components::Collision, + components::Name, + components::Pose, + components::Geometry, + components::CollisionElement, + components::ParentEntity>(processCollision); + + _ecm.EachNew<components::Joint, + components::Name, + components::JointType, + components::Pose, + components::ThreadPitch, + components::ParentEntity, + components::ParentLinkName, + components::ChildLinkName>(processJoint); + + _ecm.EachNew<components::BatterySoC>( + [&](const Entity& _entity, const components::BatterySoC*) -> bool { + // Parent entity of battery is model entity + this->entityOffMap.insert(std::make_pair(_ecm.ParentEntity(_entity), false)); + return true; + }); + } } void Physics::Impl::RemovePhysicsEntities(const EntityComponentManager& _ecm) @@ -590,7 +644,8 @@ void Physics::Impl::RemovePhysicsEntities(const EntityComponentManager& _ecm) }); } -void Physics::Impl::UpdatePhysics(EntityComponentManager& _ecm) +void Physics::Impl::UpdatePhysics(const ignition::gazebo::UpdateInfo& _info, + EntityComponentManager& _ecm) { // Battery state _ecm.Each<components::BatterySoC>( @@ -728,6 +783,31 @@ void Physics::Impl::UpdatePhysics(EntityComponentManager& _ecm) return true; }); + // Link wrenches with duration + if (!_info.paused) { + _ecm.Each<components::ExternalWorldWrenchCmdWithDuration>( + [&](const Entity& _entity, + components::ExternalWorldWrenchCmdWithDuration* _wrenchWithDurComp) { + auto linkIt = this->entityLinkMap.find(_entity); + if (linkIt == this->entityLinkMap.end()) + return true; + + auto totalWrench = _wrenchWithDurComp->Data().totalWrench(); + math::Vector3 force = msgs::Convert(totalWrench.force()); + math::Vector3 torque = msgs::Convert(totalWrench.torque()); + + linkIt->second->AddExternalForce(math::eigen3::convert(force)); + linkIt->second->AddExternalTorque(math::eigen3::convert(torque)); + + // NOTE: Cleaning could be moved to UpdateSim, but let's + // keep things all together for now + auto simTimeAfterStep = _info.simTime; + _wrenchWithDurComp->Data().cleanExpired(simTimeAfterStep); + + return true; + }); + } + _ecm.Each<components::Model, components::WorldPoseCmd>( [&](const Entity& _entity, const components::Model*, @@ -849,7 +929,8 @@ void Physics::Impl::Step(const std::chrono::steady_clock::duration& _dt) } } -void Physics::Impl::UpdateSim(EntityComponentManager& _ecm) const +void Physics::Impl::UpdateSim(const ignition::gazebo::UpdateInfo& _info, + EntityComponentManager& _ecm) const { // local pose _ecm.Each<components::Link, components::Pose, components::ParentEntity>( @@ -1036,6 +1117,21 @@ void Physics::Impl::UpdateSim(EntityComponentManager& _ecm) const // Copy the force cmd jointForceData = jointForceCmdData; + // If the history is enabled, append the force command also there + auto historyComponent = + _ecm.Component<ignition::gazebo::components::HistoryOfAppliedJointForces>(_entity); + + // Since the operation is an append, we have to perform it only when + // the physics step is actually performed + if (!_info.paused && historyComponent) { + auto& history = scenario::gazebo::utils::getExistingComponentData< + ignition::gazebo::components::HistoryOfAppliedJointForces>(&_ecm, _entity); + + for (const auto& jointForce : jointForceData) { + history.push(jointForce); + } + } + return true; }); @@ -1288,5 +1384,4 @@ physics::FrameData3d Physics::Impl::LinkFrameDataAtOffset(const LinkPtrType& _li } IGNITION_ADD_PLUGIN(Physics, ignition::gazebo::System, Physics::ISystemUpdate) - IGNITION_ADD_PLUGIN_ALIAS(Physics, "ignition::gazebo::systems::Physics") diff --git a/plugins/Physics/Physics.h b/cpp/scenario/plugins/Physics/Physics.h similarity index 67% rename from plugins/Physics/Physics.h rename to cpp/scenario/plugins/Physics/Physics.h index 29280bbf0..395139b48 100644 --- a/plugins/Physics/Physics.h +++ b/cpp/scenario/plugins/Physics/Physics.h @@ -1,14 +1,7 @@ /* - * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2020 Open Source Robotics Foundation * 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. - * - * ================================================== - * - * Copyright (C) 2018 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 @@ -20,22 +13,23 @@ * 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 GYMPP_PLUGINS_PHYSICS -#define GYMPP_PLUGINS_PHYSICS +#ifndef SCENARIO_PLUGINS_GAZEBO_PHYSICS +#define SCENARIO_PLUGINS_GAZEBO_PHYSICS #include <ignition/gazebo/System.hh> #include <memory> -namespace gympp { +namespace scenario { namespace plugins { - class Physics; + namespace gazebo { + class Physics; + } // namespace gazebo } // namespace plugins -} // namespace gympp +} // namespace scenario -class gympp::plugins::Physics final +class scenario::plugins::gazebo::Physics final : public ignition::gazebo::System , public ignition::gazebo::ISystemUpdate { @@ -51,4 +45,4 @@ class gympp::plugins::Physics final std::unique_ptr<Impl> pImpl; }; -#endif // GYMPP_PLUGINS_PHYSICS +#endif // SCENARIO_PLUGINS_GAZEBO_PHYSICS diff --git a/cpp/scenario/plugins/RobotController/CMakeLists.txt b/cpp/scenario/plugins/RobotController/CMakeLists.txt new file mode 100644 index 000000000..ca8207f8d --- /dev/null +++ b/cpp/scenario/plugins/RobotController/CMakeLists.txt @@ -0,0 +1,46 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) +# All rights reserved. +# +# This project is dual licensed under LGPL v2.1+ or Apachi License. +# +# - - - - - - - - - - - - - - - - - - +# +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. +# +# - - - - - - - - - - - - - - - - - - +# +# 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. + +add_library(RobotController SHARED + RobotController.h + RobotController.cpp) + +target_link_libraries(RobotController + PUBLIC + ignition-gazebo3::core + PRIVATE + gympp + RobotSingleton) + +target_include_directories(RobotController PRIVATE + $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>) + +if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI") + install( + TARGETS RobotController + EXPORT scenario + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}/scenario/plugins + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}/scenario/plugins + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}/scenario/plugins) +endif() diff --git a/plugins/RobotController/RobotController.cpp b/cpp/scenario/plugins/RobotController/RobotController.cpp similarity index 79% rename from plugins/RobotController/RobotController.cpp rename to cpp/scenario/plugins/RobotController/RobotController.cpp index 4ae341bf1..512dd10f0 100644 --- a/plugins/RobotController/RobotController.cpp +++ b/cpp/scenario/plugins/RobotController/RobotController.cpp @@ -1,9 +1,27 @@ /* - * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * * This software may be modified and distributed under the terms of the * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * 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 "RobotController.h" @@ -22,7 +40,7 @@ #include <string> using namespace gympp::gazebo; -using namespace gympp::plugins; +using namespace scenario::plugins::gazebo; // ==== // IMPL @@ -63,7 +81,7 @@ gympp::base::RobotPtr RobotController::Impl::getRobotPtr(const std::string& robo RobotController::RobotController() : System() - , pImpl{new Impl(), [](Impl* impl) { delete impl; }} + , pImpl{new Impl()} {} RobotController::~RobotController() @@ -140,7 +158,7 @@ void RobotController::PreUpdate(const ignition::gazebo::UpdateInfo& info, } } -IGNITION_ADD_PLUGIN(gympp::plugins::RobotController, - gympp::plugins::RobotController::System, - gympp::plugins::RobotController::ISystemConfigure, - gympp::plugins::RobotController::ISystemPreUpdate) +IGNITION_ADD_PLUGIN(scenario::plugins::gazebo::RobotController, + scenario::plugins::gazebo::RobotController::System, + scenario::plugins::gazebo::RobotController::ISystemConfigure, + scenario::plugins::gazebo::RobotController::ISystemPreUpdate) diff --git a/cpp/scenario/plugins/RobotController/RobotController.h b/cpp/scenario/plugins/RobotController/RobotController.h new file mode 100644 index 000000000..23bb22acc --- /dev/null +++ b/cpp/scenario/plugins/RobotController/RobotController.h @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * 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 SCENARIO_PLUGINS_GAZEBO_ROBOTCONTROLLER +#define SCENARIO_PLUGINS_GAZEBO_ROBOTCONTROLLER + +#include <ignition/gazebo/Entity.hh> +#include <ignition/gazebo/EntityComponentManager.hh> +#include <ignition/gazebo/EventManager.hh> +#include <ignition/gazebo/System.hh> +#include <sdf/Element.hh> + +#include <memory> + +namespace scenario { + namespace plugins { + namespace gazebo { + class RobotController; + } // namespace gazebo + } // namespace plugins +} // namespace scenario + +class scenario::plugins::gazebo::RobotController final + : public ignition::gazebo::System + , public ignition::gazebo::ISystemPreUpdate + , public ignition::gazebo::ISystemConfigure +{ +private: + class Impl; + std::unique_ptr<Impl> pImpl = nullptr; + +public: + RobotController(); + ~RobotController() override; + + void Configure(const ignition::gazebo::Entity& entity, + const std::shared_ptr<const sdf::Element>& sdf, + ignition::gazebo::EntityComponentManager& ecm, + ignition::gazebo::EventManager& eventMgr) override; + + void PreUpdate(const ignition::gazebo::UpdateInfo& info, + ignition::gazebo::EntityComponentManager& ecm) override; +}; + +#endif // SCENARIO_PLUGINS_GAZEBO_ROBOTCONTROLLER diff --git a/gym_ignition/robots/base/gazebo_robot.py b/gym_ignition/robots/base/gazebo_robot.py index 8488c77fa..d60d655a4 100644 --- a/gym_ignition/robots/base/gazebo_robot.py +++ b/gym_ignition/robots/base/gazebo_robot.py @@ -116,7 +116,7 @@ def gympp_robot(self) -> bindings.Robot: # Initialize robot controller plugin plugin_data = bindings.PluginData() plugin_data.libName = "RobotController" - plugin_data.className = "gympp::plugins::RobotController" + plugin_data.className = "scenario::plugins::gazebo::RobotController" # Insert the model ok_model = self._gazebo.insertModel(model_data, plugin_data) diff --git a/gym_ignition_data/worlds/CartPole.world b/gym_ignition_data/worlds/CartPole.world index b2d7536f5..6f2bad6be 100644 --- a/gym_ignition_data/worlds/CartPole.world +++ b/gym_ignition_data/worlds/CartPole.world @@ -8,7 +8,7 @@ </physics> <plugin filename="libPhysicsSystem.so" - name="gympp::plugins::Physics"> + name="scenario::plugins::gazebo::Physics"> </plugin> <plugin filename="libignition-gazebo-user-commands-system.so" diff --git a/gym_ignition_data/worlds/DefaultEmptyWorld.world b/gym_ignition_data/worlds/DefaultEmptyWorld.world index 66e9a2175..42e3b2b72 100644 --- a/gym_ignition_data/worlds/DefaultEmptyWorld.world +++ b/gym_ignition_data/worlds/DefaultEmptyWorld.world @@ -5,7 +5,7 @@ </physics> <plugin filename="libPhysicsSystem.so" - name="gympp::plugins::Physics"> + name="scenario::plugins::gazebo::Physics"> </plugin> <plugin filename="libignition-gazebo-user-commands-system.so" @@ -17,10 +17,10 @@ </plugin> <plugin filename="libECMProvider.so" - name="gympp::plugins::ECMProvider"> + name="scenario::plugins::gazebo::ECMProvider"> </plugin> - <gui fullscreen="0"> + <gui fullscreen="0"> <!-- 3D scene --> <plugin filename="GzScene3D" name="3D View"> <ignition-gui> diff --git a/gym_ignition_data/worlds/Pendulum.world b/gym_ignition_data/worlds/Pendulum.world index beaa913bd..92c754bf4 100644 --- a/gym_ignition_data/worlds/Pendulum.world +++ b/gym_ignition_data/worlds/Pendulum.world @@ -8,7 +8,7 @@ </physics> <plugin filename="libPhysicsSystem.so" - name="gympp::plugins::Physics"> + name="scenario::plugins::gazebo::Physics"> </plugin> <plugin filename="libignition-gazebo-user-commands-system.so" diff --git a/gym_ignition_data/worlds/iCubGazebo.world b/gym_ignition_data/worlds/iCubGazebo.world index 153bae4ce..fcba11d6e 100644 --- a/gym_ignition_data/worlds/iCubGazebo.world +++ b/gym_ignition_data/worlds/iCubGazebo.world @@ -8,7 +8,7 @@ </physics> <plugin filename="libPhysicsSystem.so" - name="gympp::plugins::Physics"> + name="scenario::plugins::gazebo::Physics"> </plugin> <plugin filename="libignition-gazebo-user-commands-system.so" diff --git a/gym_ignition_data/worlds/iCubGazeboV2_5.world b/gym_ignition_data/worlds/iCubGazeboV2_5.world index 36889b319..75954a2a9 100644 --- a/gym_ignition_data/worlds/iCubGazeboV2_5.world +++ b/gym_ignition_data/worlds/iCubGazeboV2_5.world @@ -8,7 +8,7 @@ </physics> <plugin filename="libPhysicsSystem.so" - name="gympp::plugins::Physics"> + name="scenario::plugins::gazebo::Physics"> </plugin> <plugin filename="libignition-gazebo-user-commands-system.so" diff --git a/ignition/CMakeLists.txt b/ignition/CMakeLists.txt index 6355bf95d..a5c654965 100644 --- a/ignition/CMakeLists.txt +++ b/ignition/CMakeLists.txt @@ -2,29 +2,6 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -# ============= -# ECM SINGLETON -# ============= - -add_library(ECMSingleton - include/gympp/gazebo/ECMSingleton.h - src/ECMSingleton.cpp) - -target_include_directories(ECMSingleton PUBLIC - $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> - $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>) - -target_link_libraries(ECMSingleton - PUBLIC - ignition-gazebo3::core - PRIVATE - gympp) - -if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI") - set_target_properties(ECMSingleton PROPERTIES - PUBLIC_HEADER include/gympp/gazebo/ECMSingleton.h) -endif() - # ============== # GAZEBO WRAPPER # ============== @@ -58,7 +35,6 @@ if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI") install( TARGETS GazeboWrapper - ECMSingleton EXPORT gympp LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} diff --git a/ignition/include/gympp/gazebo/ECMSingleton.h b/ignition/include/gympp/gazebo/ECMSingleton.h deleted file mode 100644 index 85c121b1d..000000000 --- a/ignition/include/gympp/gazebo/ECMSingleton.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * 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_GAZEBO_ECMSINGLETON_H -#define GYMPP_GAZEBO_ECMSINGLETON_H - -#include "ignition/gazebo/EntityComponentManager.hh" -#include "ignition/gazebo/EventManager.hh" - -#include <functional> -#include <memory> -#include <string> - -namespace gympp { - namespace gazebo { - class ECMSingleton; - } // namespace gazebo -} // namespace gympp - -class gympp::gazebo::ECMSingleton -{ -private: - class Impl; - std::unique_ptr<Impl, std::function<void(Impl*)>> pImpl; - -public: - ECMSingleton(); - ~ECMSingleton() = default; - ECMSingleton(ECMSingleton&) = delete; - void operator=(const ECMSingleton&) = delete; - - static ECMSingleton& get(); - - void clean(const std::string& worldName); - bool valid(const std::string& worldName) const; - - bool exist(const std::string& worldName) const; - - ignition::gazebo::EventManager* getEventManager(const std::string& worldName) const; - ignition::gazebo::EntityComponentManager* getECM(const std::string& worldName) const; - - bool storePtrs(const std::string& worldName, - ignition::gazebo::EntityComponentManager* ecm, - ignition::gazebo::EventManager* eventMgr); - - void notifyExecutorFinished(const std::string& worldName); - void notifyAndWaitPreUpdate(const std::string& worldName); - std::unique_lock<std::mutex> waitPreUpdate(const std::string& worldName); -}; - -#endif // GYMPP_GAZEBO_ECMSINGLETON_H diff --git a/ignition/src/ECMSingleton.cpp b/ignition/src/ECMSingleton.cpp deleted file mode 100644 index e4db34181..000000000 --- a/ignition/src/ECMSingleton.cpp +++ /dev/null @@ -1,184 +0,0 @@ -/* - * 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. - */ - -#include "gympp/gazebo/ECMSingleton.h" -#include "gympp/base/Log.h" - -#include <mutex> -#include <ostream> -#include <unordered_map> - -using namespace gympp::gazebo; -using RobotName = std::string; - -struct Pointers -{ - std::atomic<ignition::gazebo::EventManager*> eventMgr = nullptr; - std::atomic<ignition::gazebo::EntityComponentManager*> ecm = nullptr; -}; - -struct PreUpdateSynchronizationData -{ - bool isPreUpdate; - std::mutex preUpdateMutex; - std::mutex processingMutex; - std::condition_variable executorsCV; - std::condition_variable preUpdateCV; - std::atomic<size_t> nrExecutorsWaiting; -}; - -class ECMSingleton::Impl -{ -public: - using WorldName = std::string; - std::unordered_map<WorldName, Pointers> resources; - std::unordered_map<WorldName, PreUpdateSynchronizationData> synchronizationData; -}; - -ECMSingleton::ECMSingleton() - : pImpl{new Impl(), [](Impl* impl) { delete impl; }} -{} - -ECMSingleton& ECMSingleton::get() -{ - static ECMSingleton instance; - return instance; -} - -bool ECMSingleton::valid(const std::string& worldName) const -{ - return exist(worldName) && pImpl->resources.at(worldName).ecm - && pImpl->resources.at(worldName).eventMgr; -} - -bool ECMSingleton::exist(const std::string& worldName) const -{ - if (pImpl->resources.find(worldName) != pImpl->resources.end()) { - return true; - } - else { - return false; - } -} - -ignition::gazebo::EventManager* ECMSingleton::getEventManager(const std::string& worldName) const -{ - if (!exist(worldName)) { - gymppError << "The event manager was never stored" << std::endl; - return nullptr; - } - - if (!valid(worldName)) { - gymppError << "The pointers are not valid" << std::endl; - return nullptr; - } - - return pImpl->resources.at(worldName).eventMgr; -} - -bool ECMSingleton::storePtrs(const std::string& worldName, - ignition::gazebo::EntityComponentManager* ecm, - ignition::gazebo::EventManager* eventMgr) -{ - if (!ecm || !eventMgr) { - gymppError << "The pointer to the ECM or EventManager is null" << std::endl; - return false; - } - - if (exist(worldName)) { - gymppWarning << "The pointers for world '" << worldName << "' have already been stored." - << " This method will do nothing" << std::endl; - return true; - } - - gymppDebug << "Storing the ECM and the EventManager in the singleton" << std::endl; - pImpl->resources[worldName].ecm = ecm; - pImpl->resources[worldName].eventMgr = eventMgr; - - return true; -} - -void ECMSingleton::notifyExecutorFinished(const std::string& worldName) -{ - pImpl->synchronizationData[worldName].executorsCV.notify_all(); -} - -void ECMSingleton::notifyAndWaitPreUpdate(const std::string& worldName) -{ - auto& syncroData = pImpl->synchronizationData[worldName]; - - // Temporarily lock the executors mutex (see the waitPreUpdate method). - std::unique_lock processingLock(syncroData.processingMutex); - - // Notify to all executors that the simulation reached the PreUpdate step - syncroData.isPreUpdate = true; - syncroData.preUpdateCV.notify_all(); - - // If the are pending executors, they are waiting in their condition variable lambda. - // Unlock the executors mutex and wait that all executors are processed. - syncroData.executorsCV.wait(processingLock, [&] { return syncroData.nrExecutorsWaiting == 0; }); - syncroData.isPreUpdate = false; -} - -std::unique_lock<std::mutex> ECMSingleton::waitPreUpdate(const std::string& worldName) -{ - // Let's define 'executor' every function that calls this method. - // Executors want to wait to reach the PreUpdate step of the simulation, block its execution, - // and run some custom code (e.g. adding entities in the ECM). - // Executors run syncronously, one after the other. The simulation starts again when all - // executors finished. - - auto& syncroData = pImpl->synchronizationData[worldName]; - - // Initialize the lock returned to the executor to wait for their processing - std::unique_lock processingLock(syncroData.processingMutex, std::defer_lock); - - // Executors can arrive here in parallel. When this variable is back to 0, the simulation - // starts again. - syncroData.nrExecutorsWaiting++; - - // Process one executor at time and wait to reach the PreUpdate step - std::unique_lock lock(syncroData.preUpdateMutex); - syncroData.preUpdateCV.wait(lock, [&] { - if (syncroData.isPreUpdate) { - // When the simulation reaches the PreUpdate step, this lambda is called. - // One executors acquires the lock and continues. The others are blocked here. - processingLock.lock(); - return true; - } - return false; - }); - - // Decrease the counter - syncroData.nrExecutorsWaiting--; - - // Return the lock. When it goes out of executor scope, the next executors (if any) will get - // unlocked from the condition variable lambda. - return processingLock; -} - -ignition::gazebo::EntityComponentManager* ECMSingleton::getECM(const std::string& worldName) const -{ - if (!exist(worldName)) { - gymppError << "The ECM of world '" << worldName << "' was never stored" << std::endl; - return nullptr; - } - - if (!valid(worldName)) { - gymppError << "The pointers are not valid" << std::endl; - return nullptr; - } - - return pImpl->resources.at(worldName).ecm; -} - -void ECMSingleton::clean(const std::string& worldName) -{ - pImpl->resources.erase(worldName); - pImpl->synchronizationData.erase(worldName); -} diff --git a/ignition/src/GazeboWrapper.cpp b/ignition/src/GazeboWrapper.cpp index 9fab3a4b0..e4debb4bb 100644 --- a/ignition/src/GazeboWrapper.cpp +++ b/ignition/src/GazeboWrapper.cpp @@ -8,10 +8,10 @@ #include "gympp/gazebo/GazeboWrapper.h" #include "gympp/base/Log.h" -#include "gympp/gazebo/ECMSingleton.h" #include "gympp/gazebo/IgnitionRobot.h" #include "gympp/gazebo/RobotSingleton.h" #include "process.hpp" +#include "scenario/plugins/gazebo/ECMSingleton.h" #include <ignition/common/Console.hh> #include <ignition/common/SystemPaths.hh> @@ -127,7 +127,7 @@ GazeboWrapper::Impl::getSdfEntityCreator(const std::string& worldName) return sdfEntityCreator; } - if (!ECMSingleton::get().valid(worldName)) { + if (!scenario::plugins::gazebo::ECMSingleton::get().valid(worldName)) { gymppError << "ECMSingleton not yet initialized. Failed to get the SdfEntityCreator" << std::endl; return {}; @@ -135,7 +135,8 @@ GazeboWrapper::Impl::getSdfEntityCreator(const std::string& worldName) // Create the SdfEntityCreator sdfEntityCreator = std::make_unique<ignition::gazebo::SdfEntityCreator>( - *ECMSingleton::get().getECM(worldName), *ECMSingleton::get().getEventManager(worldName)); + *scenario::plugins::gazebo::ECMSingleton::get().getECM(worldName), + *scenario::plugins::gazebo::ECMSingleton::get().getEventManager(worldName)); return sdfEntityCreator; } @@ -368,10 +369,10 @@ bool GazeboWrapper::close() } // Remove the ECM from the singleton - if (ECMSingleton::get().valid(getWorldName())) { + if (scenario::plugins::gazebo::ECMSingleton::get().valid(getWorldName())) { gymppDebug << "Cleaning the ECM singleton from world '" << getWorldName() << "'" << std::endl; - ECMSingleton::get().clean(getWorldName()); + scenario::plugins::gazebo::ECMSingleton::get().clean(getWorldName()); } return true; @@ -460,15 +461,16 @@ bool GazeboWrapper::insertModel(const gympp::gazebo::ModelInitData& modelData, // ==================== // Check that the ECM has been stored by the plugin - if (!ECMSingleton::get().valid(getWorldName())) { + if (!scenario::plugins::gazebo::ECMSingleton::get().valid(getWorldName())) { gymppError << "No ECM found for world '" << getWorldName() << "'. Does your world file have the ECMProvider plugin?" << std::endl; return false; } // Get the ECM and the EventManager - auto* ecm = ECMSingleton::get().getECM(getWorldName()); - auto* eventManager = ECMSingleton::get().getEventManager(getWorldName()); + auto* ecm = scenario::plugins::gazebo::ECMSingleton::get().getECM(getWorldName()); + auto* eventManager = + scenario::plugins::gazebo::ECMSingleton::get().getEventManager(getWorldName()); // Get the SdfEntityCreator that abstracts the ECM to easily create entities auto sdfEntityCreator = pImpl->getSdfEntityCreator(getWorldName()); @@ -597,7 +599,7 @@ bool GazeboWrapper::insertModel(const gympp::gazebo::ModelInitData& modelData, ignition::gazebo::Entity modelEntity; { - auto& ecmSingleton = ECMSingleton::get(); + auto& ecmSingleton = scenario::plugins::gazebo::ECMSingleton::get(); // The first iteration is quite delicate for two reasons: // @@ -823,14 +825,15 @@ bool GazeboWrapper::removeModel(const std::string& modelName) // ======================= // Check that the ECM has been stored by the plugin - if (!ECMSingleton::get().valid(getWorldName())) { + if (!scenario::plugins::gazebo::ECMSingleton::get().valid(getWorldName())) { gymppError << "No ECM found for world '" << getWorldName() << "'. Does your world file have the ECMProvider plugin?" << std::endl; return false; } // Get the ECM - ignition::gazebo::EntityComponentManager* ecm = ECMSingleton::get().getECM(getWorldName()); + ignition::gazebo::EntityComponentManager* ecm = + scenario::plugins::gazebo::ECMSingleton::get().getECM(getWorldName()); // Get the SdfEntityCreator that abstracts the ECM to easily create entities auto sdfEntityCreator = pImpl->getSdfEntityCreator(getWorldName()); diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt deleted file mode 100644 index c26eb6d00..000000000 --- a/plugins/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -# 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. - -add_subdirectory(Physics) -add_subdirectory(ECMProvider) -add_subdirectory(RobotController) diff --git a/plugins/ECMProvider/CMakeLists.txt b/plugins/ECMProvider/CMakeLists.txt deleted file mode 100644 index 956fd7043..000000000 --- a/plugins/ECMProvider/CMakeLists.txt +++ /dev/null @@ -1,30 +0,0 @@ -# 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. - -# ================== -# ECMProvider PLUGIN -# ================== - -add_library(ECMProvider SHARED - ECMProvider.h - ECMProvider.cpp) - -target_link_libraries(ECMProvider - PUBLIC - ignition-gazebo3::core - PRIVATE - gympp - ECMSingleton) - -target_include_directories(ECMProvider PRIVATE - $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>) - -if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI") -install( - TARGETS ECMProvider - EXPORT gympp - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}/gympp/plugins - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}/gympp/plugins - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}/gympp/plugins) -endif() diff --git a/plugins/ECMProvider/ECMProvider.cpp b/plugins/ECMProvider/ECMProvider.cpp deleted file mode 100644 index 083a0e55e..000000000 --- a/plugins/ECMProvider/ECMProvider.cpp +++ /dev/null @@ -1,88 +0,0 @@ -/* - * 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. - */ - -#include "ECMProvider.h" -#include "gympp/base/Log.h" -#include "gympp/gazebo/ECMSingleton.h" - -#include <ignition/gazebo/Entity.hh> -#include <ignition/gazebo/EntityComponentManager.hh> -#include <ignition/gazebo/components/Name.hh> -#include <ignition/gazebo/components/World.hh> -#include <ignition/plugin/Register.hh> - -#include <cassert> -#include <chrono> -#include <ostream> -#include <string> -#include <unordered_map> - -using namespace gympp::gazebo; -using namespace gympp::plugins; - -class ECMProvider::Impl -{ -public: - std::string worldName; -}; - -ECMProvider::ECMProvider() - : System() - , pImpl{new Impl(), [](Impl* impl) { delete impl; }} -{} - -ECMProvider::~ECMProvider() -{ - gymppDebug << "Destroying the ECMProvider" << std::endl; -}; - -void ECMProvider::Configure(const ignition::gazebo::Entity& /*entity*/, - const std::shared_ptr<const sdf::Element>& /*sdf*/, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& eventMgr) -{ - auto worldEntities = ecm.EntitiesByComponents(ignition::gazebo::components::World()); - - if (worldEntities.size() == 0) { - gymppError << "Didn't find any world in the context of the ECMProvider plugin" << std::endl; - assert(false); - return; - } - - assert(worldEntities.size() == 1); - auto worldEntity = worldEntities[0]; - - auto nameComponent = ecm.Component<ignition::gazebo::components::Name>(worldEntity); - pImpl->worldName = nameComponent->Data(); - assert(!pImpl->worldName.empty()); - - // Register the EntityComponentManager and the EventManager in the singleton - if (!ECMSingleton::get().valid(pImpl->worldName)) { - gymppDebug << "Inserting ECM for world '" << pImpl->worldName << "'" << std::endl; - - if (!ECMSingleton::get().storePtrs(pImpl->worldName, &ecm, &eventMgr)) { - gymppError << "Failed to store ECM in the singleton for world '" << pImpl->worldName - << "'" << std::endl; - return; - } - } -} - -void ECMProvider::PreUpdate(const ignition::gazebo::UpdateInfo& info, - ignition::gazebo::EntityComponentManager& /*ecm*/) -{ - // Syncronize entity creation only when the simulation is paused - if (info.paused) { - ECMSingleton::get().notifyAndWaitPreUpdate(pImpl->worldName); - } -} - -IGNITION_ADD_PLUGIN(gympp::plugins::ECMProvider, - gympp::plugins::ECMProvider::System, - gympp::plugins::ECMProvider::ISystemConfigure, - gympp::plugins::ECMProvider::ISystemPreUpdate) diff --git a/plugins/ECMProvider/ECMProvider.h b/plugins/ECMProvider/ECMProvider.h deleted file mode 100644 index f034e42f5..000000000 --- a/plugins/ECMProvider/ECMProvider.h +++ /dev/null @@ -1,50 +0,0 @@ -/* - * 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_PLUGINS_ECMPROVIDER -#define GYMPP_PLUGINS_ECMPROVIDER - -#include <ignition/gazebo/Entity.hh> -#include <ignition/gazebo/EntityComponentManager.hh> -#include <ignition/gazebo/EventManager.hh> -#include <ignition/gazebo/System.hh> -#include <sdf/Element.hh> - -#include <functional> -#include <memory> -#include <optional> - -namespace gympp { - namespace plugins { - class ECMProvider; - } // namespace plugins -} // namespace gympp - -class gympp::plugins::ECMProvider final - : public ignition::gazebo::System - , public ignition::gazebo::ISystemConfigure - , public ignition::gazebo::ISystemPreUpdate -{ -private: - class Impl; - std::unique_ptr<Impl, std::function<void(Impl*)>> pImpl = nullptr; - -public: - ECMProvider(); - ~ECMProvider() override; - - void Configure(const ignition::gazebo::Entity& entity, - const std::shared_ptr<const sdf::Element>& sdf, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& eventMgr) override; - - void PreUpdate(const ignition::gazebo::UpdateInfo& info, - ignition::gazebo::EntityComponentManager& ecm) override; -}; - -#endif // GYMPP_PLUGINS_ECMPROVIDER diff --git a/plugins/Physics/CMakeLists.txt b/plugins/Physics/CMakeLists.txt deleted file mode 100644 index e8cde530d..000000000 --- a/plugins/Physics/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -# 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. - -# ============== -# Physics PLUGIN -# ============== - -find_package(ignition-physics2 COMPONENTS dartsim REQUIRED) - -add_library(PhysicsSystem SHARED - Physics.h - Physics.cpp) - -target_link_libraries(PhysicsSystem - PUBLIC - ignition-gazebo3::core - ignition-physics2 - PRIVATE - ExtraComponents) - -target_include_directories(PhysicsSystem PRIVATE - $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>) - -target_compile_definitions(PhysicsSystem PRIVATE - ""dartsim_plugin_LIB=\"$<TARGET_SONAME_FILE:ignition-physics2::ignition-physics2-dartsim-plugin>\""") - -if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI") - install( - TARGETS PhysicsSystem - EXPORT gympp - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}/gympp/plugins - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}/gympp/plugins - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}/gympp/plugins) -endif() diff --git a/plugins/RobotController/CMakeLists.txt b/plugins/RobotController/CMakeLists.txt deleted file mode 100644 index 27d1f86e3..000000000 --- a/plugins/RobotController/CMakeLists.txt +++ /dev/null @@ -1,30 +0,0 @@ -# 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. - -# ====================== -# RobotController PLUGIN -# ====================== - -add_library(RobotController SHARED - RobotController.h - RobotController.cpp) - -target_link_libraries(RobotController - PUBLIC - ignition-gazebo3::core - PRIVATE - gympp - RobotSingleton) - -target_include_directories(RobotController PRIVATE - $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>) - -if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI") - install( - TARGETS RobotController - EXPORT gympp - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}/gympp/plugins - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}/gympp/plugins - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}/gympp/plugins) -endif() diff --git a/plugins/RobotController/RobotController.h b/plugins/RobotController/RobotController.h deleted file mode 100644 index 822976c51..000000000 --- a/plugins/RobotController/RobotController.h +++ /dev/null @@ -1,50 +0,0 @@ -/* - * 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_PLUGINS_ROBOTCONTROLLER -#define GYMPP_PLUGINS_ROBOTCONTROLLER - -#include <ignition/gazebo/Entity.hh> -#include <ignition/gazebo/EntityComponentManager.hh> -#include <ignition/gazebo/EventManager.hh> -#include <ignition/gazebo/System.hh> -#include <sdf/Element.hh> - -#include <functional> -#include <memory> -#include <optional> - -namespace gympp { - namespace plugins { - class RobotController; - } // namespace plugins -} // namespace gympp - -class gympp::plugins::RobotController final - : public ignition::gazebo::System - , public ignition::gazebo::ISystemPreUpdate - , public ignition::gazebo::ISystemConfigure -{ -private: - class Impl; - std::unique_ptr<Impl, std::function<void(Impl*)>> pImpl = nullptr; - -public: - RobotController(); - ~RobotController() override; - - void Configure(const ignition::gazebo::Entity& entity, - const std::shared_ptr<const sdf::Element>& sdf, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& eventMgr) override; - - void PreUpdate(const ignition::gazebo::UpdateInfo& info, - ignition::gazebo::EntityComponentManager& ecm) override; -}; - -#endif // GYMPP_PLUGINS_ROBOTCONTROLLER diff --git a/tests/python/test_robot_controller.py b/tests/python/test_robot_controller.py index c265ef317..158c02c7d 100644 --- a/tests/python/test_robot_controller.py +++ b/tests/python/test_robot_controller.py @@ -19,7 +19,7 @@ def test_joint_controller(): plugin_data = bindings.PluginData() plugin_data.libName = "RobotController" - plugin_data.className = "gympp::plugins::RobotController" + plugin_data.className = "scenario::plugins::gazebo::RobotController" # Find and load the model SDF file model_sdf_file = resource_finder.find_resource("CartPole/CartPole.urdf")