From bc8745abe5d55dc20604276cd7f68b176b1ff2ac Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Tue, 17 Sep 2024 20:40:38 +0300 Subject: [PATCH] refactor env_manager and rbs_gym to work with rbs_runtime --- env_manager/env_interface/CMakeLists.txt | 59 - .../include/env_interface/env_interface.hpp | 16 - .../env_interface/env_interface_base.hpp | 40 - env_manager/env_interface/package.xml | 19 - .../env_interface/src/env_interface.cpp | 6 - .../env_interface/src/env_interface_base.cpp | 45 - env_manager/env_manager/CMakeLists.txt | 52 +- .../env_manager}/__init__.py | 0 .../env_manager}/models/__init__.py | 0 .../env_manager/models/configs/__init__.py | 13 + .../env_manager/models/configs/camera.py | 42 + .../env_manager/models/configs/light.py | 12 + .../env_manager/models/configs/objects.py | 64 + .../env_manager/models/configs/robot.py | 25 + .../env_manager/models/configs/scene.py | 30 + .../env_manager/models/configs/terrain.py | 11 + .../env_manager/models/lights/__init__.py | 34 + .../env_manager}/models/lights/random_sun.py | 0 .../env_manager}/models/lights/sun.py | 33 +- .../env_manager}/models/objects/__init__.py | 0 .../env_manager}/models/objects/model.py | 0 .../models/objects/primitives/__init__.py | 0 .../models/objects/primitives/box.py | 0 .../models/objects/primitives/cylinder.py | 0 .../models/objects/primitives/plane.py | 0 .../models/objects/primitives/sphere.py | 0 .../models/objects/random_object.py | 2 +- .../models/objects/random_primitive.py | 0 .../env_manager/models/robots/__init__.py | 21 + .../env_manager}/models/robots/panda.py | 2 +- .../env_manager}/models/robots/rbs_arm.py | 6 +- .../env_manager/models/robots/rbs_arm_b.py | 90 + .../env_manager/models/robots/robot.py | 32 + .../env_manager}/models/sensors/__init__.py | 0 .../env_manager}/models/sensors/camera.py | 2 +- .../env_manager/models/terrains/__init__.py | 25 + .../env_manager}/models/terrains/ground.py | 14 +- .../env_manager}/models/utils/__init__.py | 0 .../utils/model_collection_randomizer.py | 0 .../env_manager}/models/utils/xacro2sdf.py | 0 .../env_manager/env_manager/scene/__init__.py | 1 + .../env_manager/env_manager/scene/scene.py | 805 ++++++++ .../env_manager/scene/scene_randomizer.py | 4 + .../env_manager}/utils/__init__.py | 2 +- .../env_manager}/utils/conversions.py | 0 .../env_manager}/utils/gazebo.py | 74 +- .../env_manager/env_manager/utils/helper.py | 488 +++++ .../env_manager}/utils/logging.py | 0 .../env_manager}/utils/math.py | 14 +- .../env_manager}/utils/tf2_broadcaster.py | 2 +- .../env_manager}/utils/tf2_listener.py | 0 .../env_manager/env_manager/utils/types.py | 5 + .../env_manager}/worlds/default.sdf | 0 .../include/env_manager/env_manager.hpp | 100 - env_manager/env_manager/package.xml | 7 +- env_manager/env_manager/src/env_manager.cpp | 190 -- .../env_manager/src/run_env_manager.cpp | 21 - .../env_manager_interfaces/CMakeLists.txt | 1 + .../env_manager_interfaces/srv/ResetEnv.srv | 3 + env_manager/gz_enviroment/CMakeLists.txt | 78 - env_manager/gz_enviroment/LICENSE | 202 -- env_manager/gz_enviroment/gz_enviroment.xml | 7 - .../include/gz_enviroment/gz_enviroment.hpp | 70 - env_manager/gz_enviroment/package.xml | 25 - .../scripts/publish_asm_config.py | 114 -- env_manager/gz_enviroment/scripts/test_tf.py | 75 - .../gz_enviroment/src/gz_enviroment.cpp | 208 -- env_manager/planning_scene_manager/LICENSE | 202 -- .../planning_scene_manager.hpp | 29 - .../src/planning_scene_manager.cpp | 31 - env_manager/rbs_gym/CMakeLists.txt | 2 +- env_manager/rbs_gym/launch/test_env.launch.py | 262 ++- env_manager/rbs_gym/rbs_gym/envs/__init__.py | 272 +-- .../envs/control/grippper_controller.py | 4 + .../rbs_gym/envs/models/lights/__init__.py | 20 - .../rbs_gym/envs/models/robots/__init__.py | 17 - .../rbs_gym/envs/models/terrains/__init__.py | 29 - .../envs/models/terrains/lunar_heightmap.py | 52 - .../envs/models/terrains/lunar_surface.py | 53 - .../envs/models/terrains/random_ground.py | 135 -- .../models/terrains/random_lunar_surface.py | 57 - .../rbs_gym/envs/randomizers/manipulation.py | 1684 +---------------- .../rbs_gym/rbs_gym/envs/tasks/__init__.py | 2 +- .../rbs_gym/envs/tasks/manipulation.py | 646 ++++--- .../rbs_gym/rbs_gym/envs/tasks/reach/reach.py | 68 +- .../envs/tasks/reach/reach_color_image.py | 2 +- .../envs/tasks/reach/reach_depth_image.py | 2 +- env_manager/rbs_gym/scripts/test_agent.py | 2 +- .../CMakeLists.txt | 12 +- .../{env_interface => rbs_runtime}/LICENSE | 0 .../rbs_runtime/config/scene_config.yaml | 139 ++ .../rbs_runtime/launch/runtime.launch.py | 278 +++ .../package.xml | 4 +- .../rbs_runtime/rbs_runtime/__init__.py | 0 env_manager/rbs_runtime/scripts/runtime.py | 81 + 95 files changed, 2972 insertions(+), 4304 deletions(-) delete mode 100644 env_manager/env_interface/CMakeLists.txt delete mode 100644 env_manager/env_interface/include/env_interface/env_interface.hpp delete mode 100644 env_manager/env_interface/include/env_interface/env_interface_base.hpp delete mode 100644 env_manager/env_interface/package.xml delete mode 100644 env_manager/env_interface/src/env_interface.cpp delete mode 100644 env_manager/env_interface/src/env_interface_base.cpp rename env_manager/{rbs_gym/runtime => env_manager/env_manager}/__init__.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/__init__.py (100%) create mode 100644 env_manager/env_manager/env_manager/models/configs/__init__.py create mode 100644 env_manager/env_manager/env_manager/models/configs/camera.py create mode 100644 env_manager/env_manager/env_manager/models/configs/light.py create mode 100644 env_manager/env_manager/env_manager/models/configs/objects.py create mode 100644 env_manager/env_manager/env_manager/models/configs/robot.py create mode 100644 env_manager/env_manager/env_manager/models/configs/scene.py create mode 100644 env_manager/env_manager/env_manager/models/configs/terrain.py create mode 100644 env_manager/env_manager/env_manager/models/lights/__init__.py rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/lights/random_sun.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/lights/sun.py (83%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/objects/__init__.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/objects/model.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/objects/primitives/__init__.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/objects/primitives/box.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/objects/primitives/cylinder.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/objects/primitives/plane.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/objects/primitives/sphere.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/objects/random_object.py (96%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/objects/random_primitive.py (100%) create mode 100644 env_manager/env_manager/env_manager/models/robots/__init__.py rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/robots/panda.py (99%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/robots/rbs_arm.py (98%) create mode 100644 env_manager/env_manager/env_manager/models/robots/rbs_arm_b.py create mode 100644 env_manager/env_manager/env_manager/models/robots/robot.py rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/sensors/__init__.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/sensors/camera.py (99%) create mode 100644 env_manager/env_manager/env_manager/models/terrains/__init__.py rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/terrains/ground.py (87%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/utils/__init__.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/utils/model_collection_randomizer.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/models/utils/xacro2sdf.py (100%) create mode 100644 env_manager/env_manager/env_manager/scene/__init__.py create mode 100644 env_manager/env_manager/env_manager/scene/scene.py create mode 100644 env_manager/env_manager/env_manager/scene/scene_randomizer.py rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/utils/__init__.py (70%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/utils/conversions.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/utils/gazebo.py (77%) create mode 100644 env_manager/env_manager/env_manager/utils/helper.py rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/utils/logging.py (100%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/utils/math.py (75%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/utils/tf2_broadcaster.py (97%) rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/utils/tf2_listener.py (100%) create mode 100644 env_manager/env_manager/env_manager/utils/types.py rename env_manager/{rbs_gym/rbs_gym/envs => env_manager/env_manager}/worlds/default.sdf (100%) delete mode 100644 env_manager/env_manager/include/env_manager/env_manager.hpp delete mode 100644 env_manager/env_manager/src/env_manager.cpp delete mode 100644 env_manager/env_manager/src/run_env_manager.cpp create mode 100644 env_manager/env_manager_interfaces/srv/ResetEnv.srv delete mode 100644 env_manager/gz_enviroment/CMakeLists.txt delete mode 100644 env_manager/gz_enviroment/LICENSE delete mode 100644 env_manager/gz_enviroment/gz_enviroment.xml delete mode 100644 env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp delete mode 100644 env_manager/gz_enviroment/package.xml delete mode 100755 env_manager/gz_enviroment/scripts/publish_asm_config.py delete mode 100755 env_manager/gz_enviroment/scripts/test_tf.py delete mode 100644 env_manager/gz_enviroment/src/gz_enviroment.cpp delete mode 100644 env_manager/planning_scene_manager/LICENSE delete mode 100644 env_manager/planning_scene_manager/include/planning_scene_manager/planning_scene_manager.hpp delete mode 100644 env_manager/planning_scene_manager/src/planning_scene_manager.cpp delete mode 100644 env_manager/rbs_gym/rbs_gym/envs/models/lights/__init__.py delete mode 100644 env_manager/rbs_gym/rbs_gym/envs/models/robots/__init__.py delete mode 100644 env_manager/rbs_gym/rbs_gym/envs/models/terrains/__init__.py delete mode 100644 env_manager/rbs_gym/rbs_gym/envs/models/terrains/lunar_heightmap.py delete mode 100644 env_manager/rbs_gym/rbs_gym/envs/models/terrains/lunar_surface.py delete mode 100644 env_manager/rbs_gym/rbs_gym/envs/models/terrains/random_ground.py delete mode 100644 env_manager/rbs_gym/rbs_gym/envs/models/terrains/random_lunar_surface.py rename env_manager/{planning_scene_manager => rbs_runtime}/CMakeLists.txt (84%) rename env_manager/{env_interface => rbs_runtime}/LICENSE (100%) create mode 100644 env_manager/rbs_runtime/config/scene_config.yaml create mode 100644 env_manager/rbs_runtime/launch/runtime.launch.py rename env_manager/{planning_scene_manager => rbs_runtime}/package.xml (82%) create mode 100644 env_manager/rbs_runtime/rbs_runtime/__init__.py create mode 100755 env_manager/rbs_runtime/scripts/runtime.py diff --git a/env_manager/env_interface/CMakeLists.txt b/env_manager/env_interface/CMakeLists.txt deleted file mode 100644 index d7789f9..0000000 --- a/env_manager/env_interface/CMakeLists.txt +++ /dev/null @@ -1,59 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(env_interface) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -set(INCLUDE_DEPENDS - rclcpp_lifecycle - rbs_utils - tf2_ros - tf2_eigen -) - -# find dependencies -find_package(ament_cmake REQUIRED) -foreach(Dependency IN LISTS INCLUDE_DEPENDS) - find_package(${Dependency} REQUIRED) -endforeach() - -add_library(${PROJECT_NAME} SHARED - src/env_interface_base.cpp) - -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ -) - -ament_target_dependencies(${PROJECT_NAME} PUBLIC ${INCLUDE_DEPENDS}) - -target_compile_definitions(${PROJECT_NAME} PRIVATE "ENVIROMENT_INTERFACE_BUILDING_DLL") - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -install( - DIRECTORY include/ - DESTINATION include/env_interface -) -install(TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(${INCLUDE_DEPENDS}) - -ament_package() diff --git a/env_manager/env_interface/include/env_interface/env_interface.hpp b/env_manager/env_interface/include/env_interface/env_interface.hpp deleted file mode 100644 index 807bdf3..0000000 --- a/env_manager/env_interface/include/env_interface/env_interface.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#include "env_interface/env_interface_base.hpp" -#include "rbs_utils/rbs_utils.hpp" -#include -#include - -namespace env_interface -{ -class EnvInterface : public EnvInterfaceBase -{ -public: - EnvInterface() = default; - virtual ~EnvInterface() = default; -protected: - std::shared_ptr> m_env_models; -}; -} // namespace env_interface diff --git a/env_manager/env_interface/include/env_interface/env_interface_base.hpp b/env_manager/env_interface/include/env_interface/env_interface_base.hpp deleted file mode 100644 index 5977b71..0000000 --- a/env_manager/env_interface/include/env_interface/env_interface_base.hpp +++ /dev/null @@ -1,40 +0,0 @@ -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include -#include - -namespace env_interface -{ - -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -// template -class EnvInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface -{ -public: - EnvInterfaceBase() = default; - - virtual ~EnvInterfaceBase() = default; - - virtual CallbackReturn init(const std::string& t_env_name, const std::string& t_namespace = "", - const rclcpp::NodeOptions& t_node_options = rclcpp::NodeOptions()); - - const rclcpp_lifecycle::State& configure(); - - virtual CallbackReturn on_init() = 0; - - std::shared_ptr getNode(); - std::shared_ptr getNode() const; - - const rclcpp_lifecycle::State& getState() const; - - // virtual void setActionSpace(const A_space& action, const std::size_t& size); - -protected: - // A_space action_space; - // S_space observation_space; - -private: - std::shared_ptr m_node; -}; -} // namespace env_interface diff --git a/env_manager/env_interface/package.xml b/env_manager/env_interface/package.xml deleted file mode 100644 index 603f2cc..0000000 --- a/env_manager/env_interface/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - env_interface - 0.0.0 - TODO: Package description - bill-finger - Apache-2.0 - - ament_cmake - - rbs_utils - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/env_manager/env_interface/src/env_interface.cpp b/env_manager/env_interface/src/env_interface.cpp deleted file mode 100644 index 45c7874..0000000 --- a/env_manager/env_interface/src/env_interface.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "env_interface/env_interface.hpp" - -namespace env_interface { - - -} \ No newline at end of file diff --git a/env_manager/env_interface/src/env_interface_base.cpp b/env_manager/env_interface/src/env_interface_base.cpp deleted file mode 100644 index c4e2d26..0000000 --- a/env_manager/env_interface/src/env_interface_base.cpp +++ /dev/null @@ -1,45 +0,0 @@ -#include "env_interface/env_interface_base.hpp" - -namespace env_interface -{ - -CallbackReturn EnvInterfaceBase::init(const std::string& t_env_name, const std::string& t_namespace, - const rclcpp::NodeOptions& t_node_options) -{ - m_node = std::make_shared(t_env_name, t_namespace, t_node_options, false); - - if (on_init() == CallbackReturn::FAILURE) - { - return CallbackReturn::FAILURE; - } - - m_node->register_on_configure(std::bind(&EnvInterfaceBase::on_configure, this, std::placeholders::_1)); - m_node->register_on_activate(std::bind(&EnvInterfaceBase::on_activate, this, std::placeholders::_1)); - m_node->register_on_cleanup(std::bind(&EnvInterfaceBase::on_cleanup, this, std::placeholders::_1)); - m_node->register_on_deactivate(std::bind(&EnvInterfaceBase::on_deactivate, this, std::placeholders::_1)); - m_node->register_on_error(std::bind(&EnvInterfaceBase::on_error, this, std::placeholders::_1)); - m_node->register_on_shutdown(std::bind(&EnvInterfaceBase::on_shutdown, this, std::placeholders::_1)); - - return CallbackReturn::SUCCESS; -} - -const rclcpp_lifecycle::State& EnvInterfaceBase::configure() -{ - return getNode()->configure(); -} - -std::shared_ptr EnvInterfaceBase::getNode() -{ - if (!m_node.get()) - { - throw std::runtime_error("Lifecycle node hasn't been initialized yet"); - } - return m_node; -} - -const rclcpp_lifecycle::State& EnvInterfaceBase::getState() const -{ - return m_node->get_current_state(); -} - -} // namespace env_interface diff --git a/env_manager/env_manager/CMakeLists.txt b/env_manager/env_manager/CMakeLists.txt index 5f2b07c..8e754ce 100644 --- a/env_manager/env_manager/CMakeLists.txt +++ b/env_manager/env_manager/CMakeLists.txt @@ -5,52 +5,13 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(INCLUDE_DEPENDS - rclcpp - rclcpp_lifecycle - env_manager_interfaces - pluginlib - env_interface -) - # find dependencies find_package(ament_cmake REQUIRED) -foreach(Dep IN ITEMS ${INCLUDE_DEPENDS}) - find_package(${Dep} REQUIRED) -endforeach() # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) - - -add_library(${PROJECT_NAME} SHARED - src/env_manager.cpp -) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} ${INCLUDE_DEPENDS}) -target_compile_definitions(${PROJECT_NAME} PRIVATE "ENVIRONMENT_MANAGER_BUILDING_DLL") - -add_executable(run_env_manager src/run_env_manager.cpp) -target_include_directories(run_env_manager PRIVATE include) -target_link_libraries(run_env_manager ${PROJECT_NAME}) -ament_target_dependencies(run_env_manager ${INCLUDE_DEPENDS}) - -install(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - -install(TARGETS run_env_manager - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY include/ - DESTINATION include -) - - +ament_python_install_package(${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -64,14 +25,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_include_directories( - include -) -ament_export_dependencies( - ${INCLUDE_DEPENDS} -) + +install(DIRECTORY env_manager/worlds DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/env_manager/rbs_gym/runtime/__init__.py b/env_manager/env_manager/env_manager/__init__.py similarity index 100% rename from env_manager/rbs_gym/runtime/__init__.py rename to env_manager/env_manager/env_manager/__init__.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/__init__.py b/env_manager/env_manager/env_manager/models/__init__.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/__init__.py rename to env_manager/env_manager/env_manager/models/__init__.py diff --git a/env_manager/env_manager/env_manager/models/configs/__init__.py b/env_manager/env_manager/env_manager/models/configs/__init__.py new file mode 100644 index 0000000..98b1dac --- /dev/null +++ b/env_manager/env_manager/env_manager/models/configs/__init__.py @@ -0,0 +1,13 @@ +from .camera import CameraData +from .light import LightData +from .objects import ( + BoxObjectData, + CylinderObjectData, + MeshObjectData, + ObjectData, + PlaneObjectData, + SphereObjectData, +) +from .robot import RobotData +from .terrain import TerrainData +from .scene import SceneData diff --git a/env_manager/env_manager/env_manager/models/configs/camera.py b/env_manager/env_manager/env_manager/models/configs/camera.py new file mode 100644 index 0000000..8cb424e --- /dev/null +++ b/env_manager/env_manager/env_manager/models/configs/camera.py @@ -0,0 +1,42 @@ +from dataclasses import dataclass, field +import numpy as np + +@dataclass +class CameraData: + name: str = field(default_factory=str) + enable: bool = field(default=True) + type: str = field(default="rgbd_camera") + relative_to: str = field(default="base_link") + + width: int = field(default=128) + height: int = field(default=128) + image_format: str = field(default="R8G8B8") + update_rate: int = field(default=10) + horizontal_fov: float = field(default=np.pi / 3.0) + vertical_fov: float = field(default=np.pi / 3.0) + + clip_color: tuple[float, float] = field(default=(0.01, 1000.0)) + clip_depth: tuple[float, float] = field(default=(0.05, 10.0)) + + noise_mean: float | None = None + noise_stddev: float | None = None + + publish_color: bool = field(default=False) + publish_depth: bool = field(default=False) + publish_points: bool = field(default=False) + + spawn_position: tuple[float, float, float] = field(default=(0, 0, 1)) + spawn_quat_xyzw: tuple[float, float, float, float] = field( + default=(0, 0.70710678118, 0, 0.70710678118) + ) + + random_pose_rollouts_num: int = field(default=1) + random_pose_mode: str = field(default="orbit") + random_pose_orbit_distance: float = field(default=1.0) + random_pose_orbit_height_range: tuple[float, float] = field(default=(0.1, 0.7)) + random_pose_orbit_ignore_arc_behind_robot: float = field(default=np.pi / 8) + random_pose_select_position_options: list[tuple[float, float, float]] = field( + default_factory=list + ) + random_pose_focal_point_z_offset: float = field(default=0.0) + random_pose_rollout_counter: float = field(default=0.0) diff --git a/env_manager/env_manager/env_manager/models/configs/light.py b/env_manager/env_manager/env_manager/models/configs/light.py new file mode 100644 index 0000000..b6e7bb3 --- /dev/null +++ b/env_manager/env_manager/env_manager/models/configs/light.py @@ -0,0 +1,12 @@ +from dataclasses import dataclass, field + +@dataclass +class LightData: + type: str = field(default="sun") + direction: tuple[float, float, float] = field(default=(0.5, -0.25, -0.75)) + random_minmax_elevation: tuple[float, float] = field(default=(-0.15, -0.65)) + color: tuple[float, float, float, float] = field(default=(1.0, 1.0, 1.0, 1.0)) + distance: float = field(default=1000.0) + visual: bool = field(default=True) + radius: float = field(default=25.0) + model_rollouts_num: int = field(default=1) diff --git a/env_manager/env_manager/env_manager/models/configs/objects.py b/env_manager/env_manager/env_manager/models/configs/objects.py new file mode 100644 index 0000000..e970d12 --- /dev/null +++ b/env_manager/env_manager/env_manager/models/configs/objects.py @@ -0,0 +1,64 @@ +from dataclasses import dataclass, field + + +@dataclass +class ObjectRandomizerData: + count: int = field(default=0) + random_pose: bool = field(default=False) + random_position: bool = field(default=False) + random_orientation: bool = field(default=False) + random_model: bool = field(default=False) + random_spawn_position_segments: list = field(default_factory=list) + random_spawn_position_update_workspace_centre: bool = field(default=False) + random_spawn_volume: tuple[float, float, float] = field(default=(0.5, 0.5, 0.5)) + models_rollouts_num: int = field(default=0) + + +@dataclass +class ObjectData: + name: str = field(default="object") + type: str = field(default_factory=str) + relative_to: str = field(default="world") + position: tuple[float, float, float] = field(default=(0.0, 0.0, 0.0)) + orientation: tuple[float, float, float, float] = field(default=(1.0, 0.0, 0.0, 0.0)) + static: bool = field(default=False) + randomize: ObjectRandomizerData = field(default_factory=ObjectRandomizerData) + + +@dataclass +class PrimitiveObjectData(ObjectData): + collision: bool = field(default=True) + visual: bool = field(default=True) + color: tuple = field(default=(0.8, 0.8, 0.8, 1.0)) + mass: float = field(default=0.1) + + +@dataclass +class SphereObjectData(PrimitiveObjectData): + radius: float = field(default=0.025) + friction: float = field(default=1.0) + + +@dataclass +class PlaneObjectData(PrimitiveObjectData): + size: tuple = field(default=(1.0, 1.0)) + direction: tuple = field(default=(0.0, 0.0, 1.0)) + friction: float = field(default=1.0) + + +@dataclass +class CylinderObjectData(PrimitiveObjectData): + radius: float = field(default=0.025) + length: float = field(default=0.1) + friction: float = field(default=1.0) + + +@dataclass +class BoxObjectData(PrimitiveObjectData): + size: tuple = field(default=(0.05, 0.05, 0.05)) + friction: float = field(default=1.0) + + +@dataclass +class MeshObjectData(ObjectData): + texture: list[float] = field(default_factory=list) diff --git a/env_manager/env_manager/env_manager/models/configs/robot.py b/env_manager/env_manager/env_manager/models/configs/robot.py new file mode 100644 index 0000000..66f817e --- /dev/null +++ b/env_manager/env_manager/env_manager/models/configs/robot.py @@ -0,0 +1,25 @@ +from dataclasses import dataclass, field + +@dataclass +class RobotRandomizerData: + pose: bool = field(default=False) + spawn_volume: tuple[float, float, float] = field(default=(1.0, 1.0, 0.0)) + joint_positions: bool = field(default=False) + joint_positions_std: float = field(default=0.1) + joint_positions_above_object_spawn: bool = field(default=False) + joint_positions_above_object_spawn_elevation: float = field(default=0.2) + joint_positions_above_object_spawn_xy_randomness: float = field(default=0.2) + + +@dataclass +class RobotData: + name: str = field(default_factory=str) + urdf_string: str = field(default_factory=str) + spawn_position: tuple[float, float, float] = field(default=(0.0, 0.0, 0.0)) + spawn_quat_xyzw: tuple[float, float, float, float] = field( + default=(0.0, 0.0, 0.0, 1.0) + ) + joint_positioins: list[float] = field(default_factory=list) + with_gripper: bool = field(default=False) + gripper_jont_positions: list[float] = field(default_factory=list) + randomizer: RobotRandomizerData = field(default_factory=RobotRandomizerData) diff --git a/env_manager/env_manager/env_manager/models/configs/scene.py b/env_manager/env_manager/env_manager/models/configs/scene.py new file mode 100644 index 0000000..011c53c --- /dev/null +++ b/env_manager/env_manager/env_manager/models/configs/scene.py @@ -0,0 +1,30 @@ + +from dataclasses import dataclass, field +from .objects import ObjectData +from .robot import RobotData +from .terrain import TerrainData +from .light import LightData +from .camera import CameraData + +@dataclass +class PluginsData: + scene_broadcaster: bool = field(default_factory=bool) + user_commands: bool = field(default_factory=bool) + fts_broadcaster: bool = field(default_factory=bool) + sensors_render_engine: str = field(default="ogre2") + +@dataclass +class SceneData: + physics_rollouts_num: int = field(default=0) + gravity: tuple[float, float, float] = field(default=(0.0, 0.0, -9.80665)) + gravity_std: tuple[float, float, float] = field(default=(0.0, 0.0, 0.0232)) + robot: RobotData = field(default_factory=RobotData) + terrain: TerrainData = field(default_factory=TerrainData) + light: LightData = field(default_factory=LightData) + objects: list[ObjectData] = field(default_factory=list) + camera: list[CameraData] = field(default_factory=list) + plugins: PluginsData = field(default_factory=PluginsData) + + + + diff --git a/env_manager/env_manager/env_manager/models/configs/terrain.py b/env_manager/env_manager/env_manager/models/configs/terrain.py new file mode 100644 index 0000000..0dfd3c8 --- /dev/null +++ b/env_manager/env_manager/env_manager/models/configs/terrain.py @@ -0,0 +1,11 @@ + +from dataclasses import dataclass, field + + +@dataclass +class TerrainData: + type: str = field(default="flat") + spawn_position: tuple[float, float, float] = field(default=(0, 0, 0)) + spawn_quat_xyzw: tuple[float, float, float, float] = field(default=(0, 0, 0, 1)) + size: tuple[float, float] = field(default=(1.5, 1.5)) + model_rollouts_num: int = field(default=1) diff --git a/env_manager/env_manager/env_manager/models/lights/__init__.py b/env_manager/env_manager/env_manager/models/lights/__init__.py new file mode 100644 index 0000000..f791750 --- /dev/null +++ b/env_manager/env_manager/env_manager/models/lights/__init__.py @@ -0,0 +1,34 @@ +from enum import Enum +from gym_gz.scenario.model_wrapper import ModelWrapper + +from .random_sun import RandomSun +from .sun import Sun + + +# Enum для типов света +class LightType(Enum): + SUN = "sun" + RANDOM_SUN = "random_sun" + + +LIGHT_MODEL_CLASSES = { + LightType.SUN: Sun, + LightType.RANDOM_SUN: RandomSun, +} + + +def get_light_model_class(light_type: str) -> type[ModelWrapper]: + try: + light_enum = LightType(light_type) + return LIGHT_MODEL_CLASSES[light_enum] + except KeyError: + raise ValueError(f"Unsupported light type: {light_type}") + + +def is_light_type_randomizable(light_type: str) -> bool: + try: + light_enum = LightType(light_type) + return light_enum == LightType.RANDOM_SUN + except ValueError: + return False + diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/lights/random_sun.py b/env_manager/env_manager/env_manager/models/lights/random_sun.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/lights/random_sun.py rename to env_manager/env_manager/env_manager/models/lights/random_sun.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/lights/sun.py b/env_manager/env_manager/env_manager/models/lights/sun.py similarity index 83% rename from env_manager/rbs_gym/rbs_gym/envs/models/lights/sun.py rename to env_manager/env_manager/env_manager/models/lights/sun.py index 6c6ff45..ba6c374 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/models/lights/sun.py +++ b/env_manager/env_manager/env_manager/models/lights/sun.py @@ -1,18 +1,17 @@ -from typing import List, Tuple - import numpy as np from gym_gz.scenario import model_wrapper from gym_gz.utils.scenario import get_unique_model_name -from scenario import core as scenario +from scenario import core as scenario_core +from scenario import gazebo as scenario_gazebo class Sun(model_wrapper.ModelWrapper): def __init__( self, - world: scenario.World, + world: scenario_gazebo.World, name: str = "sun", - direction: Tuple[float, float, float] = (0.5, -0.25, -0.75), - color: List[float] = (1.0, 1.0, 1.0, 1.0), + direction: tuple[float, float, float] = (0.5, -0.25, -0.75), + color: tuple[float, float, float, float] = (1.0, 1.0, 1.0, 1.0), distance: float = 800.0, visual: bool = True, radius: float = 20.0, @@ -23,20 +22,19 @@ class Sun(model_wrapper.ModelWrapper): attenuation_quadratic: float = 0.001, **kwargs, ): - # Get a unique model name model_name = get_unique_model_name(world, name) # Normalize direction - direction = np.array(direction) - direction = direction / np.linalg.norm(direction) + np_direction = np.array(direction) + np_direction = np_direction / np.linalg.norm(np_direction) # Initial pose - initial_pose = scenario.Pose( + initial_pose = scenario_core.Pose( ( - -direction[0] * distance, - -direction[1] * distance, - -direction[2] * distance, + -np_direction[0] * distance, + -np_direction[1] * distance, + -np_direction[2] * distance, ), (1, 0, 0, 0), ) @@ -44,7 +42,7 @@ class Sun(model_wrapper.ModelWrapper): # Create SDF string for the model sdf = self.get_sdf( model_name=model_name, - direction=direction, + direction=tuple(np_direction), color=color, visual=visual, radius=radius, @@ -70,10 +68,10 @@ class Sun(model_wrapper.ModelWrapper): @classmethod def get_sdf( - self, + cls, model_name: str, - direction: Tuple[float, float, float], - color: Tuple[float, float, float, float], + direction: tuple[float, float, float], + color: tuple[float, float, float, float], visual: bool, radius: float, specular: float, @@ -82,7 +80,6 @@ class Sun(model_wrapper.ModelWrapper): attenuation_linear: float, attenuation_quadratic: float, ) -> str: - return f''' true diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/objects/__init__.py b/env_manager/env_manager/env_manager/models/objects/__init__.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/objects/__init__.py rename to env_manager/env_manager/env_manager/models/objects/__init__.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/objects/model.py b/env_manager/env_manager/env_manager/models/objects/model.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/objects/model.py rename to env_manager/env_manager/env_manager/models/objects/model.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/objects/primitives/__init__.py b/env_manager/env_manager/env_manager/models/objects/primitives/__init__.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/objects/primitives/__init__.py rename to env_manager/env_manager/env_manager/models/objects/primitives/__init__.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/objects/primitives/box.py b/env_manager/env_manager/env_manager/models/objects/primitives/box.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/objects/primitives/box.py rename to env_manager/env_manager/env_manager/models/objects/primitives/box.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/objects/primitives/cylinder.py b/env_manager/env_manager/env_manager/models/objects/primitives/cylinder.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/objects/primitives/cylinder.py rename to env_manager/env_manager/env_manager/models/objects/primitives/cylinder.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/objects/primitives/plane.py b/env_manager/env_manager/env_manager/models/objects/primitives/plane.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/objects/primitives/plane.py rename to env_manager/env_manager/env_manager/models/objects/primitives/plane.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/objects/primitives/sphere.py b/env_manager/env_manager/env_manager/models/objects/primitives/sphere.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/objects/primitives/sphere.py rename to env_manager/env_manager/env_manager/models/objects/primitives/sphere.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/objects/random_object.py b/env_manager/env_manager/env_manager/models/objects/random_object.py similarity index 96% rename from env_manager/rbs_gym/rbs_gym/envs/models/objects/random_object.py rename to env_manager/env_manager/env_manager/models/objects/random_object.py index 600b26a..244b1eb 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/models/objects/random_object.py +++ b/env_manager/env_manager/env_manager/models/objects/random_object.py @@ -5,7 +5,7 @@ from gym_gz.utils.scenario import get_unique_model_name from numpy.random import RandomState from scenario import core as scenario -from rbs_gym.envs.models.utils import ModelCollectionRandomizer +from env_manager.models.utils import ModelCollectionRandomizer class RandomObject(model_wrapper.ModelWrapper): diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/objects/random_primitive.py b/env_manager/env_manager/env_manager/models/objects/random_primitive.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/objects/random_primitive.py rename to env_manager/env_manager/env_manager/models/objects/random_primitive.py diff --git a/env_manager/env_manager/env_manager/models/robots/__init__.py b/env_manager/env_manager/env_manager/models/robots/__init__.py new file mode 100644 index 0000000..8518940 --- /dev/null +++ b/env_manager/env_manager/env_manager/models/robots/__init__.py @@ -0,0 +1,21 @@ +from .robot import RobotWrapper +# from .rbs_arm import RbsArm +from .rbs_arm_b import RbsArm as RbsArmB + +# from .panda import Panda +from enum import Enum + + +class RobotEnum(Enum): + RBS_ARM = "rbs_arm" + # PANDA = "panda" # Uncomment when Panda is implemented + + +def get_robot_model_class(robot_model: str) -> type[RobotWrapper]: + # Используем enum для проверки модели робота + model_mapping = { + RobotEnum.RBS_ARM.value: RbsArmB, + # RobotModel.PANDA.value: Panda, # Uncomment when Panda is implemented + } + + return model_mapping.get(robot_model, RbsArmB) diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/robots/panda.py b/env_manager/env_manager/env_manager/models/robots/panda.py similarity index 99% rename from env_manager/rbs_gym/rbs_gym/envs/models/robots/panda.py rename to env_manager/env_manager/env_manager/models/robots/panda.py index fde0d49..0a36b72 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/models/robots/panda.py +++ b/env_manager/env_manager/env_manager/models/robots/panda.py @@ -7,7 +7,7 @@ from gym_gz.utils.scenario import get_unique_model_name from scenario import core as scenario from scenario import gazebo as scenario_gazebo -from rbs_gym.envs.models.utils import xacro2sdf +from env_manager.models.utils import xacro2sdf class Panda(model_wrapper.ModelWrapper, model_with_file.ModelWithFile): diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/robots/rbs_arm.py b/env_manager/env_manager/env_manager/models/robots/rbs_arm.py similarity index 98% rename from env_manager/rbs_gym/rbs_gym/envs/models/robots/rbs_arm.py rename to env_manager/env_manager/env_manager/models/robots/rbs_arm.py index f7942ba..45bf565 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/models/robots/rbs_arm.py +++ b/env_manager/env_manager/env_manager/models/robots/rbs_arm.py @@ -8,11 +8,13 @@ import numpy as np from scenario import core as scenario from scenario import gazebo as scenario_gazebo +from .robot import RobotWrapper + # from robot_builder.elements.robot import Robot # from robot_builder.writer.urdf import URDF_writer # from robot_builder.parser.urdf import URDF_parser -class RbsArm(model_wrapper.ModelWrapper, model_with_file.ModelWithFile): +class RbsArm(RobotWrapper): DEFAULT_ARM_JOINT_POSITIONS: List[float] = ( 0.0, 0.5, 3.14159, 1.5, 0.0, 1.4, 0.0, @@ -261,5 +263,5 @@ class RbsArm(model_wrapper.ModelWrapper, model_with_file.ModelWithFile): @classmethod def get_model_file(cls) -> str: - return "/home/bill-finger/rbs_ws/current.urdf" + return "/home/narmak/rbs_ws/current.urdf" diff --git a/env_manager/env_manager/env_manager/models/robots/rbs_arm_b.py b/env_manager/env_manager/env_manager/models/robots/rbs_arm_b.py new file mode 100644 index 0000000..6ef4f2b --- /dev/null +++ b/env_manager/env_manager/env_manager/models/robots/rbs_arm_b.py @@ -0,0 +1,90 @@ +from gym_gz.utils.scenario import get_unique_model_name +from rclpy.node import Node +from robot_builder.parser.urdf import URDF_parser +from robot_builder.elements.robot import Robot + +from scenario import core as scenario_core +from scenario import gazebo as scenario_gazebo + +from .robot import RobotWrapper + + +class RbsArm(RobotWrapper): + """ + Experimental class with `robot_builder` + """ + DEFAULT_ARM_JOINT_POSITIONS: list[float] = [ + 0.0, + 0.5, + 3.14159, + 1.5, + 0.0, + 1.4, + 0.0, + ] + + OPEN_GRIPPER_JOINT_POSITIONS: list[float] = [ + 0.064, + ] + + CLOSED_GRIPPER_JOINT_POSITIONS: list[float] = [ + 0.0, + ] + + DEFAULT_GRIPPER_JOINT_POSITIONS: list[float] = CLOSED_GRIPPER_JOINT_POSITIONS + + def __init__( + self, + world: scenario_gazebo.World, + node: Node, + urdf_string: str, + name: str = "rbs_arm", + position: tuple[float, float, float] = (0.0, 0.0, 0.0), + orientation: tuple[float, float, float, float] = (1.0, 0, 0, 0), + initial_arm_joint_positions: list[float] = DEFAULT_ARM_JOINT_POSITIONS, + initial_gripper_joint_positions: list[float] = DEFAULT_GRIPPER_JOINT_POSITIONS, + ): + self.__initial_arm_joint_positions = initial_arm_joint_positions + self.__initial_gripper_joint_positions = initial_gripper_joint_positions + + # Get a unique model name + model_name = get_unique_model_name(world, name) + + # Setup initial pose + initial_pose = scenario_core.Pose(position, orientation) + + # Insert the model + ok_model = world.insert_model_from_string(urdf_string, initial_pose, model_name) + if not ok_model: + raise RuntimeError("Failed to insert " + model_name) + + # Get the model + model = world.get_model(model_name) + + # Parse robot to get metadata + self._robot: Robot = URDF_parser.load_string(urdf_string) + node.get_logger().info("{}".format(self._robot.actuated_joint_names)) + node.get_logger().info("{}".format(self._robot.get_gripper_mimic_joint_name)) + node.get_logger().info("{}".format(self._robot.gripper_actuated_joint_names)) + + node.get_logger().info("{}".format(model.joint_names())) + + # Set initial joint configuration + self.set_initial_joint_positions(model) + super().__init__(model=model) + + @property + def robot(self) -> Robot: + return self._robot + + def set_initial_joint_positions(self, model): + model = model.to_gazebo() + if not model.reset_joint_positions( + self.__initial_arm_joint_positions, self._robot.actuated_joint_names + ): + raise RuntimeError("Failed to set initial positions of arm's joints") + if not model.reset_joint_positions( + self.__initial_gripper_joint_positions, [self._robot.get_gripper_mimic_joint_name] + ): + raise RuntimeError("Failed to set initial positions of gripper's joints") + diff --git a/env_manager/env_manager/env_manager/models/robots/robot.py b/env_manager/env_manager/env_manager/models/robots/robot.py new file mode 100644 index 0000000..33df834 --- /dev/null +++ b/env_manager/env_manager/env_manager/models/robots/robot.py @@ -0,0 +1,32 @@ +from gym_gz.scenario import model_wrapper +from robot_builder.elements.robot import Robot +from scenario import gazebo as scenario_gazebo +from abc import abstractmethod + + +class RobotWrapper(model_wrapper.ModelWrapper): + def __init__( + self, + world: scenario_gazebo.World | None = None, + urdf_string: str | None = None, + name: str | None = None, + position: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + initial_arm_joint_positions: list[float] = [], + initial_gripper_joint_positions: list[float] = [], + model: model_wrapper.ModelWrapper | None = None, + **kwargs, + ): + if model is not None: + super().__init__(model=model) + else: + raise ValueError("Model should be defined for the parent class") + + @property + @abstractmethod + def robot(self) -> Robot: + pass + + @abstractmethod + def set_initial_joint_positions(self, model): + pass diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/sensors/__init__.py b/env_manager/env_manager/env_manager/models/sensors/__init__.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/sensors/__init__.py rename to env_manager/env_manager/env_manager/models/sensors/__init__.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/sensors/camera.py b/env_manager/env_manager/env_manager/models/sensors/camera.py similarity index 99% rename from env_manager/rbs_gym/rbs_gym/envs/models/sensors/camera.py rename to env_manager/env_manager/env_manager/models/sensors/camera.py index d5b20d4..fa0b58b 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/models/sensors/camera.py +++ b/env_manager/env_manager/env_manager/models/sensors/camera.py @@ -6,7 +6,7 @@ from gym_gz.utils.scenario import get_unique_model_name from scenario import core as scenario_core from scenario import gazebo as scenario_gz -from rbs_gym.envs.models.utils import ModelCollectionRandomizer +from env_manager.models.utils import ModelCollectionRandomizer class Camera(model_wrapper.ModelWrapper): diff --git a/env_manager/env_manager/env_manager/models/terrains/__init__.py b/env_manager/env_manager/env_manager/models/terrains/__init__.py new file mode 100644 index 0000000..9829bf3 --- /dev/null +++ b/env_manager/env_manager/env_manager/models/terrains/__init__.py @@ -0,0 +1,25 @@ +# from gym_gz.scenario.model_wrapper import ModelWrapper + +from .ground import Ground + + +# def get_terrain_model_class(terrain_type: str) -> type[ModelWrapper]: +# # TODO: Refactor into enum +# +# if "flat" == terrain_type: +# return Ground +# elif "random_flat" == terrain_type: +# return RandomGround +# elif "lunar_heightmap" == terrain_type: +# return LunarHeightmap +# elif "lunar_surface" == terrain_type: +# return LunarSurface +# elif "random_lunar_surface" == terrain_type: +# return RandomLunarSurface +# else: +# raise AttributeError(f"Unsupported terrain [{terrain_type}]") +# +# +# def is_terrain_type_randomizable(terrain_type: str) -> bool: +# +# return "random_flat" == terrain_type or "random_lunar_surface" == terrain_type diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/terrains/ground.py b/env_manager/env_manager/env_manager/models/terrains/ground.py similarity index 87% rename from env_manager/rbs_gym/rbs_gym/envs/models/terrains/ground.py rename to env_manager/env_manager/env_manager/models/terrains/ground.py index 2069efb..712e011 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/models/terrains/ground.py +++ b/env_manager/env_manager/env_manager/models/terrains/ground.py @@ -3,17 +3,17 @@ from typing import List from gym_gz.scenario import model_wrapper from gym_gz.utils import misc from gym_gz.utils.scenario import get_unique_model_name -from scenario import core as scenario - +from scenario import core as scenario_core +from scenario import gazebo as scenario_gazebo class Ground(model_wrapper.ModelWrapper): def __init__( self, - world: scenario.World, + world: scenario_gazebo.World, name: str = "ground", - position: List[float] = (0, 0, 0), - orientation: List[float] = (1, 0, 0, 0), - size: List[float] = (1.5, 1.5), + position: tuple[float, float, float] = (0, 0, 0), + orientation: tuple[float, float, float, float] = (1, 0, 0, 0), + size: tuple[float, float] = (1.5, 1.5), collision_thickness=0.05, friction: float = 5.0, **kwargs, @@ -23,7 +23,7 @@ class Ground(model_wrapper.ModelWrapper): model_name = get_unique_model_name(world, name) # Initial pose - initial_pose = scenario.Pose(position, orientation) + initial_pose = scenario_core.Pose(position, orientation) # Create SDF string for the model sdf = f""" diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/utils/__init__.py b/env_manager/env_manager/env_manager/models/utils/__init__.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/utils/__init__.py rename to env_manager/env_manager/env_manager/models/utils/__init__.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/utils/model_collection_randomizer.py b/env_manager/env_manager/env_manager/models/utils/model_collection_randomizer.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/utils/model_collection_randomizer.py rename to env_manager/env_manager/env_manager/models/utils/model_collection_randomizer.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/utils/xacro2sdf.py b/env_manager/env_manager/env_manager/models/utils/xacro2sdf.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/models/utils/xacro2sdf.py rename to env_manager/env_manager/env_manager/models/utils/xacro2sdf.py diff --git a/env_manager/env_manager/env_manager/scene/__init__.py b/env_manager/env_manager/env_manager/scene/__init__.py new file mode 100644 index 0000000..421848f --- /dev/null +++ b/env_manager/env_manager/env_manager/scene/__init__.py @@ -0,0 +1 @@ +from .scene import Scene diff --git a/env_manager/env_manager/env_manager/scene/scene.py b/env_manager/env_manager/env_manager/scene/scene.py new file mode 100644 index 0000000..f7fd82d --- /dev/null +++ b/env_manager/env_manager/env_manager/scene/scene.py @@ -0,0 +1,805 @@ +from dataclasses import asdict + +import numpy as np +from rclpy.node import Node +from scenario.bindings.gazebo import GazeboSimulator, World, PhysicsEngine_dart +from scipy.spatial.transform import Rotation +from scipy.spatial import distance + +from ..models import Box, Camera, Cylinder, Ground, Mesh, Plane, Sphere, Sun +from ..models.configs import CameraData, ObjectData, SceneData +from ..models.robots import get_robot_model_class +from ..utils import Tf2Broadcaster +from ..utils.conversions import quat_to_wxyz +from ..utils.gazebo import ( + transform_move_to_model_pose, + transform_move_to_model_position, +) +from ..utils.types import Point, Pose + + +# TODO: Split scene randomizer and scene +class Scene: + """ + Manager scene during runtime, including randomization. + """ + + def __init__( + self, + node: Node, + gazebo: GazeboSimulator, + scene: SceneData, + robot_urdf_string: str, + ) -> None: + self.gazebo = gazebo + self.robot = scene.robot + self.cameras = scene.camera + self.objects = scene.objects + self.node = node + self.light = scene.light + self.terrain = scene.terrain + self.__scene_initialized = False + self.__plugin_scene_broadcaster = True + self.__plugin_user_commands = True + self.__fts_enable = True + self.__plugint_sensor_render_engine = "ogre2" + self.world: World = gazebo.get_world() + + # if not node.has_parameter("robot_description"): + # node.declare_parameter("robot_description") + + print(node._parameters) + self.__objects_unique_names = {} + self.__object_positions: dict[str, Point] = {} + self.__objects_workspace_centre: Point = (0.0, 0.0, 0.0) + scene.robot.urdf_string = robot_urdf_string + + self.tf2_broadcaster = Tf2Broadcaster(node=node) + + self.__workspace_center = (0.0, 0.0, 0.0) + self.__workspace_dimensions = (1.5, 1.5, 1.5) + + half_width = self.__workspace_dimensions[0] / 2 + half_height = self.__workspace_dimensions[1] / 2 + half_depth = self.__workspace_dimensions[2] / 2 + + self.__workspace_min_bound = ( + self.__workspace_center[0] - half_width, + self.__workspace_center[1] - half_height, + self.__workspace_center[2] - half_depth, + ) + self.__workspace_max_bound = ( + self.__workspace_center[0] + half_width, + self.__workspace_center[1] + half_height, + self.__workspace_center[2] + half_depth, + ) + + def init_scene(self): + self.gazebo_paused_run() + + self.init_world_plugins() + + self.world.set_physics_engine(PhysicsEngine_dart) + + self.add_terrain() + self.add_light() + self.add_robot() + for obj in self.objects: + self.add_object(obj) + for camera in self.cameras: + self.add_camera(camera) + + self.__scene_initialized = True + + def reset_scene(self): + self.reset_robot_joint_positions() + + object_randomized = set() + self.__object_positions.clear() + for object in self.objects: + if object.name in object_randomized: + continue + if object.randomize.random_pose: + self.randomize_object_position(object) + else: + self.reset_objects_pose(object) + object_randomized.add(object.name) + + for camera in self.cameras: + if camera.enable and self.is_camera_pose_expired(camera): + self.randomize_camera_pose(camera=camera) + + def init_world_plugins(self): + # SceneBroadcaster + if self.__plugin_scene_broadcaster: + if not self.gazebo.scene_broadcaster_active(self.world.to_gazebo().name()): + self.node.get_logger().info( + "Inserting world plugins for broadcasting scene to GUI clients..." + ) + self.world.to_gazebo().insert_world_plugin( + "ignition-gazebo-scene-broadcaster-system", + "ignition::gazebo::systems::SceneBroadcaster", + ) + + self.gazebo_paused_run() + + # UserCommands + if self.__plugin_user_commands: + self.node.get_logger().info( + "Inserting world plugins to enable user commands..." + ) + self.world.to_gazebo().insert_world_plugin( + "ignition-gazebo-user-commands-system", + "ignition::gazebo::systems::UserCommands", + ) + + self.gazebo_paused_run() + + # Sensors + self._camera_enable = False + for camera in self.cameras: + if camera.enable: + self._camera_enable = True + + if self._camera_enable: + self.node.get_logger().info( + f"Inserting world plugins for sensors with {self.__plugint_sensor_render_engine} rendering engine..." + ) + self.world.to_gazebo().insert_world_plugin( + "libignition-gazebo-sensors-system.so", + "ignition::gazebo::systems::Sensors", + "" + f"{self.__plugint_sensor_render_engine}" + "", + ) + + if self.__fts_enable: + self.world.to_gazebo().insert_world_plugin( + "ignition-gazebo-forcetorque-system", + "ignition::gazebo::systems::ForceTorque", + ) + + self.gazebo_paused_run() + + def add_robot(self): + """ + Configure and insert robot into the simulation + """ + + robot_model_class = get_robot_model_class(self.robot.name) + + # Instantiate robot class based on the selected model + self.robot_wrapper = robot_model_class( + world=self.world, + name=self.robot.name, + node=self.node, + position=self.robot.spawn_position, + orientation=quat_to_wxyz(self.robot.spawn_quat_xyzw), + urdf_string=self.robot.urdf_string, + initial_arm_joint_positions=self.robot.joint_positioins, + initial_gripper_joint_positions=self.robot.gripper_jont_positions, + ) + + # The desired name is passed as arg on creation, however, a different name might be selected to be unique + # Therefore, return the name back to the task + self.robot_name = self.robot_wrapper.name() + + # Enable contact detection for all gripper links (fingers) + robot_gazebo = self.robot_wrapper.to_gazebo() + # for gripper_link_name in self.robot.gripper_link_names: + # finger = robot_gazebo.get_link(link_name=gripper_link_name) + # finger.enable_contact_detection(True) + robot_gazebo.enable_contacts() + + # for arm_link_name in self.robot_wrapper.arm_link_names: + # link = robot_gazebo.get_link(link_name=arm_link_name) + # link.enable_contact_detection(True) + + # # If mobile, enable contact detection also for the wheels + # if self.robot.is_mobile: + # for wheel_link_name in self.robot.wheel_link_names: + # wheel = robot_gazebo.get_link(link_name=wheel_link_name) + # wheel.enable_contact_detection(True) + + self.gazebo_paused_run() + + # Reset robot joints to the defaults + self.reset_robot_joint_positions() + + def add_camera(self, camera: CameraData): + """ + Configure and insert camera into the simulation. Camera is places with respect to the robot + """ + + if self.world.to_gazebo().name() == camera.relative_to: + camera_position = camera.spawn_position + camera_quat_wxyz = quat_to_wxyz(camera.spawn_quat_xyzw) + else: + # Transform the pose of camera to be with respect to robot - but still represented in world reference frame for insertion into the world + camera_position, camera_quat_wxyz = transform_move_to_model_pose( + world=self.world, + position=camera.spawn_position, + quat=quat_to_wxyz(camera.spawn_quat_xyzw), + target_model=self.robot_wrapper, + target_link=camera.relative_to, + xyzw=False, + ) + + # Create camera + self.camera_wrapper = Camera( + name=camera.name, + world=self.world, + position=camera_position, + orientation=camera_quat_wxyz, + camera_type=camera.type, + width=camera.width, + height=camera.height, + image_format=camera.image_format, + update_rate=camera.update_rate, + horizontal_fov=camera.horizontal_fov, + vertical_fov=camera.vertical_fov, + clip_color=camera.clip_color, + clip_depth=camera.clip_depth, + noise_mean=camera.noise_mean, + noise_stddev=camera.noise_stddev, + ros2_bridge_color=camera.publish_color, + ros2_bridge_depth=camera.publish_depth, + ros2_bridge_points=camera.publish_points, + ) + + self.gazebo_paused_run() + + # Attach to robot if needed + if self.world.to_gazebo().name() != camera.relative_to: + if not self.robot_wrapper.to_gazebo().attach_link( + camera.relative_to, + self.camera_wrapper.name(), + self.camera_wrapper.link_name, + ): + raise Exception("Cannot attach camera link to robot") + + self.__is_camera_attached = True + + self.gazebo_paused_run() + + # Broadcast tf + self.tf2_broadcaster.broadcast_tf( + parent_frame_id=camera.relative_to, + child_frame_id=self.camera_wrapper.frame_id, + translation=camera.spawn_position, + rotation=camera.spawn_quat_xyzw, + xyzw=True, + ) + + def add_object(self, obj: ObjectData): + obj_dict = asdict(obj) + obj_dict["world"] = self.world + try: + if obj.type: + match obj.type: + case "box": + object_wrapper = Box(**obj_dict) + case "plane": + object_wrapper = Plane(**obj_dict) + case "sphere": + object_wrapper = Sphere(**obj_dict) + case "cylinder": + object_wrapper = Cylinder(**obj_dict) + case "mesh": + object_wrapper = Mesh(**obj_dict) + case _: + raise NotImplementedError("Type is not supported") + else: + from ..models.configs import ( + BoxObjectData, + CylinderObjectData, + MeshObjectData, + PlaneObjectData, + SphereObjectData, + ) + + match obj: + case BoxObjectData(): + object_wrapper = Box(**obj_dict) + case PlaneObjectData(): + object_wrapper = Plane(**obj_dict) + case SphereObjectData(): + object_wrapper = Sphere(**obj_dict) + case CylinderObjectData(): + object_wrapper = Cylinder(**obj_dict) + case MeshObjectData(): + object_wrapper = Mesh(**obj_dict) + case _: + raise NotImplementedError("Type is not supported") + + model_name = object_wrapper.name() + if obj.name not in self.__objects_unique_names: + self.__objects_unique_names[obj.name] = [] + self.__objects_unique_names[obj.name].append(model_name) + self.__object_positions[model_name] = obj.position + + # Enable contact detection + for link_name in object_wrapper.link_names(): + link = object_wrapper.to_gazebo().get_link(link_name=link_name) + link.enable_contact_detection(True) + + except Exception as ex: + self.node.get_logger().warn(f"Model could not be inserted. Reason: {ex}") + + self.gazebo_paused_run + + def add_light(self): + """ + Configure and insert default light into the simulation + """ + + # Create light + match self.light.type: + case "sun": + self.light = Sun( + self.world, + direction=self.light.direction, + color=self.light.color, + distance=self.light.distance, + visual=self.light.visual, + radius=self.light.radius, + ) + case "random_sun": + raise RuntimeError("random_sun is not supported yet") + case _: + raise RuntimeError( + f"Type of light [{self.light.type}] currently not supported" + ) + + self.gazebo_paused_run() + + def add_terrain(self): + """ + Configure and insert default terrain into the simulation + """ + + # TODO: add another terrains + self.terrain_wrapper = Ground( + self.world, + position=self.terrain.spawn_position, + orientation=quat_to_wxyz(self.terrain.spawn_quat_xyzw), + size=self.terrain.size, + ) + + # Enable contact detection + for link_name in self.terrain_wrapper.link_names(): + link = self.terrain_wrapper.to_gazebo().get_link(link_name=link_name) + link.enable_contact_detection(True) + + self.gazebo_paused_run() + + def randomize_object_position(self, object: ObjectData): + task_object_names = self.__objects_unique_names[object.name] + for object_name in task_object_names: + position, quat_random = self.get_random_object_pose(object) + obj = self.world.to_gazebo().get_model(object_name).to_gazebo() + if object.randomize.random_pose: + obj.reset_base_pose(position, quat_random) + elif object.randomize.random_orientation: + obj.reset_base_pose(object.position, quat_random) + elif object.randomize.random_position: + obj.reset_base_pose(position, object.orientation) + + self.__object_positions[object_name] = position + + self.gazebo_paused_run() + + def randomize_camera_pose(self, camera: CameraData): + # Get random camera pose, centred at object position (or centre of object spawn box) + if "orbit" == camera.random_pose_mode: + camera_position, camera_quat_xyzw = self.get_random_camera_pose_orbit( + camera + ) + elif "select_random" == camera.random_pose_mode: + ( + camera_position, + camera_quat_xyzw, + ) = self.get_random_camera_pose_sample_random(camera) + elif "select_nearest" == camera.random_pose_mode: + ( + camera_position, + camera_quat_xyzw, + ) = self.get_random_camera_pose_sample_nearest(camera) + else: + raise TypeError("Invalid mode for camera pose randomization.") + + if self.world.to_gazebo().name() == camera.relative_to: + transformed_camera_position = camera_position + transformed_camera_quat_wxyz = quat_to_wxyz(camera_quat_xyzw) + else: + # Transform the pose of camera to be with respect to robot - but represented in world reference frame for insertion into the world + ( + transformed_camera_position, + transformed_camera_quat_wxyz, + ) = transform_move_to_model_pose( + world=self.world, + position=camera_position, + quat=quat_to_wxyz(camera_quat_xyzw), + target_model=self.robot_wrapper, + target_link=camera.relative_to, + xyzw=False, + ) + + # Detach camera if needed + if self.__is_camera_attached: + if not self.robot_wrapper.to_gazebo().detach_link( + camera.relative_to, + self.camera_wrapper.name(), + self.camera_wrapper.link_name, + ): + raise Exception("Cannot detach camera link from robot") + + self.gazebo_paused_run() + + # Move pose of the camera + camera_gazebo = self.camera_wrapper.to_gazebo() + camera_gazebo.reset_base_pose( + transformed_camera_position, transformed_camera_quat_wxyz + ) + + self.gazebo_paused_run() + + # Attach to robot again if needed + if self.__is_camera_attached: + if not self.robot_wrapper.to_gazebo().attach_link( + camera.relative_to, + self.camera_wrapper.name(), + self.camera_wrapper.link_name, + ): + raise Exception("Cannot attach camera link to robot") + + self.gazebo_paused_run() + + # Broadcast tf + self.tf2_broadcaster.broadcast_tf( + parent_frame_id=camera.relative_to, + child_frame_id=self.camera_wrapper.frame_id, + translation=camera_position, + rotation=camera_quat_xyzw, + xyzw=True, + ) + + def get_random_camera_pose_orbit(self, camera: CameraData) -> Pose: + rng = np.random.default_rng() + + while True: + position = rng.uniform( + low=(-1.0, -1.0, camera.random_pose_orbit_height_range[0]), + high=(1.0, 1.0, camera.random_pose_orbit_height_range[1]), + ) + + norm = np.linalg.norm(position) + position /= norm + + if ( + abs(np.arctan2(position[0], position[1]) + np.pi / 2) + > camera.random_pose_orbit_ignore_arc_behind_robot + ): + break + + rpy = np.array( + [ + 0.0, + np.arctan2( + position[2] - camera.random_pose_focal_point_z_offset, + np.linalg.norm(position[:2]), + ), + np.arctan2(position[1], position[0]) + np.pi, + ] + ) + quat_xyzw = Rotation.from_euler("xyz", rpy).as_quat() + + position *= camera.random_pose_orbit_distance + position[:2] += self.__objects_workspace_centre[:2] + + return tuple(position), tuple(quat_xyzw) + + def get_random_camera_pose_sample_random(self, camera: CameraData) -> Pose: + # Select a random entry from the options + camera.random_pose_select_position_options + + selection = camera.random_pose_select_position_options[ + np.random.randint(len(camera.random_pose_select_position_options)) + ] + + return self.get_random_camera_pose_sample_process(camera, selection) + + def get_random_camera_pose_sample_nearest(self, camera: CameraData) -> Pose: + # Select the nearest entry + dist_sqr = np.sum( + ( + np.array(camera.random_pose_select_position_options) + - np.array(camera.random_pose_select_position_options) + ) + ** 2, + axis=1, + ) + nearest = camera.random_pose_select_position_options[np.argmin(dist_sqr)] + + return self.get_random_camera_pose_sample_process(camera, nearest) + + def get_random_camera_pose_sample_process( + self, camera: CameraData, position: Point + ) -> Pose: + # Determine orientation such that camera faces the centre + rpy = [ + 0.0, + np.arctan2( + position[2] - camera.random_pose_focal_point_z_offset, + np.linalg.norm( + ( + position[0] - self.__objects_workspace_centre[0], + position[1] - self.__objects_workspace_centre[1], + ), + 2, + ), + ), + np.arctan2( + position[1] - self.__objects_workspace_centre[1], + position[0] - self.__objects_workspace_centre[0], + ) + + np.pi, + ] + quat_xyzw = Rotation.from_euler("xyz", rpy).as_quat() + + return camera.spawn_position, tuple(quat_xyzw) + + def reset_robot_pose(self): + if self.robot.randomizer.pose: + position = [ + self.robot.spawn_position[0] + + np.random.RandomState().uniform( + -self.robot.randomizer.spawn_volume[0] / 2, + self.robot.randomizer.spawn_volume[0] / 2, + ), + self.robot.spawn_position[1] + + np.random.RandomState().uniform( + -self.robot.randomizer.spawn_volume[1] / 2, + self.robot.randomizer.spawn_volume[1] / 2, + ), + self.robot.spawn_position[2] + + np.random.RandomState().uniform( + -self.robot.randomizer.spawn_volume[2] / 2, + self.robot.randomizer.spawn_volume[2] / 2, + ), + ] + quat_xyzw = Rotation.from_euler( + "xyz", (0, 0, np.random.RandomState().uniform(-np.pi, np.pi)) + ).as_quat() + quat_xyzw = tuple(quat_xyzw) + else: + position = self.robot.spawn_position + quat_xyzw = self.robot.spawn_quat_xyzw + + gazebo_robot = self.robot_wrapper.to_gazebo() + gazebo_robot.reset_base_pose(position, quat_to_wxyz(quat_xyzw)) + + self.gazebo_paused_run() + + def reset_robot_joint_positions(self): + gazebo_robot = self.robot_wrapper.to_gazebo() + + arm_joint_positions = self.robot.joint_positioins + + # Add normal noise if desired + if self.robot.randomizer.joint_positions: + for joint_position in arm_joint_positions: + joint_position += np.random.RandomState().normal( + loc=0.0, scale=self.robot.randomizer.joint_positions_std + ) + + # Arm joints - apply joint positions zero out velocities + if not gazebo_robot.reset_joint_positions( + arm_joint_positions, self.robot_wrapper.robot.actuated_joint_names + ): + raise RuntimeError("Failed to reset robot joint positions") + if not gazebo_robot.reset_joint_velocities( + [0.0] * len(self.robot_wrapper.robot.actuated_joint_names), + self.robot_wrapper.robot.actuated_joint_names, + ): + raise RuntimeError("Failed to reset robot joint velocities") + + # Gripper joints - apply joint positions zero out velocities + if ( + self.robot.with_gripper + and self.robot_wrapper.robot.gripper_actuated_joint_names + ): + if not gazebo_robot.reset_joint_positions( + self.robot.gripper_jont_positions, + self.robot_wrapper.robot.gripper_actuated_joint_names, + ): + raise RuntimeError("Failed to reset gripper joint positions") + if not gazebo_robot.reset_joint_velocities( + [0.0] * len(self.robot_wrapper.robot.gripper_actuated_joint_names), + self.robot_wrapper.robot.gripper_actuated_joint_names, + ): + raise RuntimeError("Failed to reset gripper joint velocities") + + # Execute an unpaused run to process model modification and get new JointStates + if not self.gazebo.step(): + raise RuntimeError("Failed to execute an unpaused Gazebo step") + + # if self.robot.with_gripper: + # if ( + # self.robot_wrapper.CLOSED_GRIPPER_JOINT_POSITIONS + # == self.robot.gripper_jont_positions + # ): + # self.gripper_controller.close() + # else: + # self.gripper.open() + + def reset_objects_pose(self, object: ObjectData): + task_object_names = self.__objects_unique_names[object.name] + for object_name in task_object_names: + obj = self.world.to_gazebo().get_model(object_name).to_gazebo() + obj.reset_base_pose(object.position, object.orientation) + + self.gazebo_paused_run() + + def get_random_object_pose( + self, + obj: ObjectData, + name: str = "", + min_distance_to_other_objects: float = 0.2, + min_distance_decay_factor: float = 0.95, + ) -> Pose: + is_too_close = True + # The default value for randomization + object_position = obj.position + while is_too_close: + object_position = ( + obj.position[0] + + np.random.uniform( + -obj.randomize.random_spawn_volume[0] / 2, + obj.randomize.random_spawn_volume[0] / 2, + ), + obj.position[1] + + np.random.uniform( + -obj.randomize.random_spawn_volume[1] / 2, + obj.randomize.random_spawn_volume[1] / 2, + ), + obj.position[2] + + np.random.uniform( + -obj.randomize.random_spawn_volume[2] / 2, + obj.randomize.random_spawn_volume[2] / 2, + ), + ) + + if self.world.to_gazebo().name() != obj.relative_to: + # Transform the pose of camera to be with respect to robot - but represented in world reference frame for insertion into the world + object_position = transform_move_to_model_position( + world=self.world, + position=object_position, + target_model=self.robot_wrapper, + target_link=obj.relative_to, + ) + + # Check if position is far enough from other + is_too_close = False + for ( + existing_object_name, + existing_object_position, + ) in self.__object_positions.items(): + if existing_object_name == name: + # Do not compare to itself + continue + if ( + distance.euclidean(object_position, existing_object_position) + < min_distance_to_other_objects + ): + min_distance_to_other_objects *= min_distance_decay_factor + is_too_close = True + break + + quat = np.random.uniform(-1, 1, 4) + quat /= np.linalg.norm(quat) + + return object_position, tuple(quat) + + def is_camera_pose_expired(self, camera: CameraData) -> bool: + """ + Checks if camera pose needs to be randomized. + + Return: + True if expired, false otherwise + """ + + if camera.random_pose_rollouts_num == 0: + return False + + camera.random_pose_rollout_counter += 1 + + if camera.random_pose_rollout_counter >= camera.random_pose_rollouts_num: + camera.random_pose_rollout_counter = 0 + return True + + return False + + def gazebo_paused_run(self): + if not self.gazebo.run(paused=True): + raise RuntimeError("Failed to execute a paused Gazebo run") + + def check_object_outside_workspace( + self, + object_position: Point, + ) -> bool: + """ + Returns true if the object is outside the workspace (parallelepiped defined by center and dimensions). + """ + + return ( + object_position[0] < self.__workspace_min_bound[0] + or object_position[1] < self.__workspace_min_bound[1] + or object_position[2] < self.__workspace_min_bound[2] + or object_position[0] > self.__workspace_max_bound[0] + or object_position[1] > self.__workspace_max_bound[1] + or object_position[2] > self.__workspace_max_bound[2] + ) + + def check_object_overlapping( + self, + object: ObjectData, + allowed_penetration_depth: float = 0.001, + terrain_allowed_penetration_depth: float = 0.002, + ) -> bool: + """ + Go through all objects and make sure that none of them are overlapping. + If an object is overlapping, reset its position. + Positions are reset also if object is in collision with robot right after reset. + Collisions/overlaps with terrain are ignored. + Returns True if all objects are okay, false if they had to be reset + """ + + # Update object positions + for object_name in self.__objects_unique_names[object.name]: + model = self.world.get_model(object_name).to_gazebo() + self.__object_positions[object_name] = model.get_link( + link_name=model.link_names()[0] + ).position() + + for object_name in self.__objects_unique_names[object.name]: + obj = self.world.get_model(object_name).to_gazebo() + + # Make sure the object is inside workspace + if self.check_object_outside_workspace( + self.__object_positions[object_name] + ): + position, quat_random = self.get_random_object_pose( + object, + name=object_name, + ) + obj.reset_base_pose(position, quat_random) + obj.reset_base_world_velocity([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]) + return False + + # Make sure the object is not intersecting other objects + try: + for contact in obj.contacts(): + depth = np.mean([point.depth for point in contact.points]) + if ( + self.terrain_wrapper.name() in contact.body_b + and depth < terrain_allowed_penetration_depth + ): + continue + if ( + self.robot_wrapper.name() in contact.body_b + or depth > allowed_penetration_depth + ): + position, quat_random = self.get_random_object_pose( + object, + name=object_name, + ) + obj.reset_base_pose(position, quat_random) + obj.reset_base_world_velocity([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]) + return False + except Exception as e: + self.node.get_logger().error( + f"Runtime error encountered while checking objects intersections: {e}" + ) + + return True diff --git a/env_manager/env_manager/env_manager/scene/scene_randomizer.py b/env_manager/env_manager/env_manager/scene/scene_randomizer.py new file mode 100644 index 0000000..b74ce54 --- /dev/null +++ b/env_manager/env_manager/env_manager/scene/scene_randomizer.py @@ -0,0 +1,4 @@ + +class SceneRandomizer: + def __init__(self) -> None: + pass diff --git a/env_manager/rbs_gym/rbs_gym/envs/utils/__init__.py b/env_manager/env_manager/env_manager/utils/__init__.py similarity index 70% rename from env_manager/rbs_gym/rbs_gym/envs/utils/__init__.py rename to env_manager/env_manager/env_manager/utils/__init__.py index 3d0497d..aa70740 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/utils/__init__.py +++ b/env_manager/env_manager/env_manager/utils/__init__.py @@ -1,3 +1,3 @@ -from . import conversions, gazebo, logging, math +from . import conversions, gazebo, logging, math, types from .tf2_broadcaster import Tf2Broadcaster, Tf2BroadcasterStandalone from .tf2_listener import Tf2Listener, Tf2ListenerStandalone diff --git a/env_manager/rbs_gym/rbs_gym/envs/utils/conversions.py b/env_manager/env_manager/env_manager/utils/conversions.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/utils/conversions.py rename to env_manager/env_manager/env_manager/utils/conversions.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/utils/gazebo.py b/env_manager/env_manager/env_manager/utils/gazebo.py similarity index 77% rename from env_manager/rbs_gym/rbs_gym/envs/utils/gazebo.py rename to env_manager/env_manager/env_manager/utils/gazebo.py index 8753c61..2786de8 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/utils/gazebo.py +++ b/env_manager/env_manager/env_manager/utils/gazebo.py @@ -2,19 +2,21 @@ from typing import Tuple, Union from gym_gz.scenario.model_wrapper import ModelWrapper from numpy import exp -from scenario.bindings.gazebo import Link, World +from scenario.bindings.core import Link, World from scipy.spatial.transform import Rotation -from rbs_gym.envs.utils.conversions import quat_to_wxyz, quat_to_xyzw -from rbs_gym.envs.utils.math import quat_mul +from .conversions import quat_to_wxyz, quat_to_xyzw +from .math import quat_mul +from .types import Pose, Point, Quat +#NOTE: Errors in pyright will be fixed only with pybind11 in the scenario module def get_model_pose( world: World, - model: Union[ModelWrapper, str], - link: Union[Link, str, None] = None, + model: ModelWrapper | str, + link: Link | str | None = None, xyzw: bool = False, -) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]: +) -> Pose: """ Return pose of model's link. Orientation is represented as wxyz quaternion or xyzw based on the passed argument `xyzw`. """ @@ -47,9 +49,9 @@ def get_model_pose( def get_model_position( world: World, - model: Union[ModelWrapper, str], - link: Union[Link, str, None] = None, -) -> Tuple[float, float, float]: + model: ModelWrapper | str, + link: Link | str | None = None, +) -> Point: """ Return position of model's link. """ @@ -71,10 +73,10 @@ def get_model_position( def get_model_orientation( world: World, - model: Union[ModelWrapper, str], - link: Union[Link, str, None] = None, + model: ModelWrapper | str, + link: Link | str | None = None, xyzw: bool = False, -) -> Tuple[float, float, float, float]: +) -> Quat: """ Return orientation of model's link that is represented as wxyz quaternion or xyzw based on the passed argument `xyzw`. """ @@ -103,12 +105,12 @@ def get_model_orientation( def transform_move_to_model_pose( world: World, - position: tuple[float, float, float], - quat: tuple[float, float, float, float], + position: Point, + quat: Quat, target_model: ModelWrapper | str, target_link: Link | str | None = None, xyzw: bool = False, -) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]: +) -> Pose: """ Transform such that original `position` and `quat` are represented with respect to `target_model::target_link`. The resulting pose is still represented in world coordinate system. @@ -137,10 +139,10 @@ def transform_move_to_model_pose( def transform_move_to_model_position( world: World, - position: Tuple[float, float, float], - target_model: Union[ModelWrapper, str], - target_link: Union[Link, str, None] = None, -) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]: + position: Point, + target_model: ModelWrapper | str, + target_link: Link | str | None = None, +) -> Point: target_frame_position, target_frame_quat_xyzw = get_model_pose( world, @@ -161,11 +163,11 @@ def transform_move_to_model_position( def transform_move_to_model_orientation( world: World, - quat: Tuple[float, float, float, float], - target_model: Union[ModelWrapper, str], - target_link: Union[Link, str, None] = None, + quat: Quat, + target_model: ModelWrapper | str, + target_link: Link | str | None = None, xyzw: bool = False, -) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]: +) -> Quat: target_frame_quat = get_model_orientation( world, @@ -181,12 +183,12 @@ def transform_move_to_model_orientation( def transform_change_reference_frame_pose( world: World, - position: Tuple[float, float, float], - quat: Tuple[float, float, float, float], - target_model: Union[ModelWrapper, str], - target_link: Union[Link, str, None] = None, + position: Point, + quat: Quat, + target_model: ModelWrapper | str, + target_link: Link | str | None, xyzw: bool = False, -) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]: +) -> Pose: """ Change reference frame of original `position` and `quat` from world coordinate system to `target_model::target_link` coordinate system. """ @@ -216,10 +218,10 @@ def transform_change_reference_frame_pose( def transform_change_reference_frame_position( world: World, - position: Tuple[float, float, float], - target_model: Union[ModelWrapper, str], - target_link: Union[Link, str, None] = None, -) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]: + position: Point, + target_model: ModelWrapper | str, + target_link: Link | str | None = None, +) -> Point: target_frame_position, target_frame_quat_xyzw = get_model_pose( world, @@ -242,11 +244,11 @@ def transform_change_reference_frame_position( def transform_change_reference_frame_orientation( world: World, - quat: Tuple[float, float, float, float], - target_model: Union[ModelWrapper, str], - target_link: Union[Link, str, None] = None, + quat: Quat, + target_model: ModelWrapper | str, + target_link: Link | str | None = None, xyzw: bool = False, -) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]: +) -> Quat: target_frame_quat = get_model_orientation( world, diff --git a/env_manager/env_manager/env_manager/utils/helper.py b/env_manager/env_manager/env_manager/utils/helper.py new file mode 100644 index 0000000..a5bfd08 --- /dev/null +++ b/env_manager/env_manager/env_manager/utils/helper.py @@ -0,0 +1,488 @@ +import numpy as np +from gym_gz.scenario.model_wrapper import ModelWrapper +from rclpy.node import Node +from scenario.bindings.core import World +from scipy.spatial.transform import Rotation + +from .conversions import orientation_6d_to_quat +from .gazebo import ( + get_model_orientation, + get_model_pose, + get_model_position, + transform_change_reference_frame_orientation, + transform_change_reference_frame_pose, + transform_change_reference_frame_position, +) +from .math import quat_mul +from .tf2_listener import Tf2Listener +from .types import Point, Pose, PoseRpy, Quat, Rpy + + +# Helper functions # +def get_relative_ee_position(self, translation: Point) -> Point: + # Scale relative action to metric units + translation = self.scale_relative_translation(translation) + # Get current position + current_position = self.get_ee_position() + # Compute target position + target_position = ( + current_position[0] + translation[0], + current_position[1] + translation[1], + current_position[2] + translation[2], + ) + + # Restrict target position to a limited workspace, if desired + if self._restrict_position_goal_to_workspace: + target_position = self.restrict_position_goal_to_workspace(target_position) + + return target_position + + +def get_relative_ee_orientation( + self, + rotation: float | Quat | PoseRpy, + representation: str = "quat", +) -> Quat: + current_quat_xyzw = self.get_ee_orientation() + + if representation == "z": + current_yaw = Rotation.from_quat(current_quat_xyzw).as_euler("xyz")[2] + current_quat_xyzw = Rotation.from_euler( + "xyz", [np.pi, 0, current_yaw] + ).as_quat() + + if isinstance(rotation, tuple): + if len(rotation) == 4 and representation == "quat": + relative_quat_xyzw = rotation + elif len(rotation) == 6 and representation == "6d": + vectors = (rotation[0:3], rotation[3:6]) + relative_quat_xyzw = orientation_6d_to_quat(vectors[0], vectors[1]) + else: + raise ValueError("Invalid rotation tuple length for representation.") + elif isinstance(rotation, float) and representation == "z": + rotation = self.scale_relative_rotation(rotation) + relative_quat_xyzw = Rotation.from_euler("xyz", [0, 0, rotation]).as_quat() + else: + raise TypeError("Invalid type for rotation or representation.") + + target_quat_xyzw = quat_mul(tuple(current_quat_xyzw), tuple(relative_quat_xyzw)) + + target_quat_xyzw = normalize_quaternion(tuple(relative_quat_xyzw)) + + return target_quat_xyzw + + +def normalize_quaternion( + target_quat_xyzw: tuple[float, float, float, float], +) -> tuple[float, float, float, float]: + quat_array = np.array(target_quat_xyzw) + normalized_quat = quat_array / np.linalg.norm(quat_array) + return tuple(normalized_quat) + + +def scale_relative_translation(self, translation: Point) -> Point: + return ( + self.__scaling_factor_translation * translation[0], + self.__scaling_factor_translation * translation[1], + self.__scaling_factor_translation * translation[2], + ) + + +def scale_relative_rotation( + self, rotation: float | Rpy | np.floating | np.ndarray +) -> float | Rpy: + scaling_factor = self.__scaling_factor_rotation + + if isinstance(rotation, (int, float, np.floating)): + return scaling_factor * rotation + + return tuple(scaling_factor * r for r in rotation) + + +def restrict_position_goal_to_workspace(self, position: Point) -> Point: + return ( + min( + self.workspace_max_bound[0], + max( + self.workspace_min_bound[0], + position[0], + ), + ), + min( + self.workspace_max_bound[1], + max( + self.workspace_min_bound[1], + position[1], + ), + ), + min( + self.workspace_max_bound[2], + max( + self.workspace_min_bound[2], + position[2], + ), + ), + ) + + +# def restrict_servo_translation_to_workspace( +# self, translation: tuple[float, float, float] +# ) -> tuple[float, float, float]: +# current_ee_position = self.get_ee_position() +# +# translation = tuple( +# 0.0 +# if ( +# current_ee_position[i] > self.workspace_max_bound[i] +# and translation[i] > 0.0 +# ) +# or ( +# current_ee_position[i] < self.workspace_min_bound[i] +# and translation[i] < 0.0 +# ) +# else translation[i] +# for i in range(3) +# ) +# +# return translation + + +def get_ee_pose( + node: Node, + world: World, + robot_ee_link_name: str, + robot_name: str, + robot_arm_base_link_name: str, + tf2_listener: Tf2Listener, +) -> Pose | None: + """ + Return the current pose of the end effector with respect to arm base link. + """ + + try: + robot_model = world.to_gazebo().get_model(robot_name).to_gazebo() + ee_position, ee_quat_xyzw = get_model_pose( + world=world, + model=robot_model, + link=robot_ee_link_name, + xyzw=True, + ) + return transform_change_reference_frame_pose( + world=world, + position=ee_position, + quat=ee_quat_xyzw, + target_model=robot_model, + target_link=robot_arm_base_link_name, + xyzw=True, + ) + except Exception as e: + node.get_logger().warn( + f"Cannot get end effector pose from Gazebo ({e}), using tf2..." + ) + transform = tf2_listener.lookup_transform_sync( + source_frame=robot_ee_link_name, + target_frame=robot_arm_base_link_name, + retry=False, + ) + if transform is not None: + return ( + ( + transform.translation.x, + transform.translation.y, + transform.translation.z, + ), + ( + transform.rotation.x, + transform.rotation.y, + transform.rotation.z, + transform.rotation.w, + ), + ) + else: + node.get_logger().error( + "Cannot get pose of the end effector (default values are returned)" + ) + return ( + (0.0, 0.0, 0.0), + (0.0, 0.0, 0.0, 1.0), + ) + + +def get_ee_position( + world: World, + robot_name: str, + robot_ee_link_name: str, + robot_arm_base_link_name: str, + node: Node, + tf2_listener: Tf2Listener, +) -> Point: + """ + Return the current position of the end effector with respect to arm base link. + """ + + try: + robot_model = world.to_gazebo().get_model(robot_name).to_gazebo() + ee_position = get_model_position( + world=world, + model=robot_model, + link=robot_ee_link_name, + ) + return transform_change_reference_frame_position( + world=world, + position=ee_position, + target_model=robot_model, + target_link=robot_arm_base_link_name, + ) + except Exception as e: + node.get_logger().debug( + f"Cannot get end effector position from Gazebo ({e}), using tf2..." + ) + transform = tf2_listener.lookup_transform_sync( + source_frame=robot_ee_link_name, + target_frame=robot_arm_base_link_name, + retry=False, + ) + if transform is not None: + return ( + transform.translation.x, + transform.translation.y, + transform.translation.z, + ) + else: + node.get_logger().error( + "Cannot get position of the end effector (default values are returned)" + ) + return (0.0, 0.0, 0.0) + + +def get_ee_orientation( + world: World, + robot_name: str, + robot_ee_link_name: str, + robot_arm_base_link_name: str, + node: Node, + tf2_listener: Tf2Listener, +) -> Quat: + """ + Return the current xyzw quaternion of the end effector with respect to arm base link. + """ + + try: + robot_model = world.to_gazebo().get_model(robot_name).to_gazebo() + ee_quat_xyzw = get_model_orientation( + world=world, + model=robot_model, + link=robot_ee_link_name, + xyzw=True, + ) + return transform_change_reference_frame_orientation( + world=world, + quat=ee_quat_xyzw, + target_model=robot_model, + target_link=robot_arm_base_link_name, + xyzw=True, + ) + except Exception as e: + node.get_logger().warn( + f"Cannot get end effector orientation from Gazebo ({e}), using tf2..." + ) + transform = tf2_listener.lookup_transform_sync( + source_frame=robot_ee_link_name, + target_frame=robot_arm_base_link_name, + retry=False, + ) + if transform is not None: + return ( + transform.rotation.x, + transform.rotation.y, + transform.rotation.z, + transform.rotation.w, + ) + else: + node.get_logger().error( + "Cannot get orientation of the end effector (default values are returned)" + ) + return (0.0, 0.0, 0.0, 1.0) + + +def get_object_position( + object_model: ModelWrapper | str, + node: Node, + world: World, + robot_name: str, + robot_arm_base_link_name: str, +) -> Point: + """ + Return the current position of an object with respect to arm base link. + Note: Only simulated objects are currently supported. + """ + + try: + object_position = get_model_position( + world=world, + model=object_model, + ) + return transform_change_reference_frame_position( + world=world, + position=object_position, + target_model=robot_name, + target_link=robot_arm_base_link_name, + ) + except Exception as e: + node.get_logger().error( + f"Cannot get position of {object_model} object (default values are returned): {e}" + ) + return (0.0, 0.0, 0.0) + + +def get_object_positions( + node: Node, + world: World, + object_names: list[str], + robot_name: str, + robot_arm_base_link_name: str, +) -> dict[str, tuple[float, float, float]]: + """ + Return the current position of all objects with respect to arm base link. + Note: Only simulated objects are currently supported. + """ + + object_positions = {} + + try: + robot_model = world.to_gazebo().get_model(robot_name).to_gazebo() + robot_arm_base_link = robot_model.get_link(link_name=robot_arm_base_link_name) + for object_name in object_names: + object_position = get_model_position( + world=world, + model=object_name, + ) + object_positions[object_name] = transform_change_reference_frame_position( + world=world, + position=object_position, + target_model=robot_model, + target_link=robot_arm_base_link, + ) + except Exception as e: + node.get_logger().error( + f"Cannot get positions of all objects (empty Dict is returned): {e}" + ) + + return object_positions + + +# def substitute_special_frame(self, frame_id: str) -> str: +# if "arm_base_link" == frame_id: +# return self.robot_arm_base_link_name +# elif "base_link" == frame_id: +# return self.robot_base_link_name +# elif "end_effector" == frame_id: +# return self.robot_ee_link_name +# elif "world" == frame_id: +# try: +# # In Gazebo, where multiple worlds are allowed +# return self.world.to_gazebo().name() +# except Exception as e: +# self.get_logger().warn(f"") +# # Otherwise (e.g. real world) +# return "rbs_gym_world" +# else: +# return frame_id + + +def move_to_initial_joint_configuration(self): + pass + + # self.moveit2.move_to_configuration(self.initial_arm_joint_positions) + + # if ( + # self.robot_model_class.CLOSED_GRIPPER_JOINT_POSITIONS + # == self.initial_gripper_joint_positions + # ): + # self.gripper.reset_close() + # else: + # self.gripper.reset_open() + + +def check_terrain_collision( + world: World, + robot_name: str, + terrain_name: str, + robot_base_link_name: str, + robot_arm_link_names: list[str], + robot_gripper_link_names: list[str], +) -> bool: + """ + Returns true if robot links are in collision with the ground. + """ + robot_name_len = len(robot_name) + terrain_model = world.get_model(terrain_name) + + for contact in terrain_model.contacts(): + body_b = contact.body_b + + if body_b.startswith(robot_name) and len(body_b) > robot_name_len: + link = body_b[robot_name_len + 2 :] + + if link != robot_base_link_name and ( + link in robot_arm_link_names or link in robot_gripper_link_names + ): + return True + + return False + + +def check_all_objects_outside_workspace( + workspace: Pose, + object_positions: dict[str, Point], +) -> bool: + """ + Returns True if all objects are outside the workspace. + """ + + return all( + [ + check_object_outside_workspace(workspace, object_position) + for object_position in object_positions.values() + ] + ) + + +def check_object_outside_workspace(workspace: Pose, object_position: Point) -> bool: + """ + Returns True if the object is outside the workspace. + """ + workspace_min_bound, workspace_max_bound = workspace + return any( + coord < min_bound or coord > max_bound + for coord, min_bound, max_bound in zip( + object_position, workspace_min_bound, workspace_max_bound + ) + ) + + +# def add_parameter_overrides(self, parameter_overrides: Dict[str, any]): +# self.add_task_parameter_overrides(parameter_overrides) +# self.add_randomizer_parameter_overrides(parameter_overrides) +# +# +# def add_task_parameter_overrides(self, parameter_overrides: Dict[str, any]): +# self.__task_parameter_overrides.update(parameter_overrides) +# +# +# def add_randomizer_parameter_overrides(self, parameter_overrides: Dict[str, any]): +# self._randomizer_parameter_overrides.update(parameter_overrides) +# +# +# def __consume_parameter_overrides(self): +# for key, value in self.__task_parameter_overrides.items(): +# if hasattr(self, key): +# setattr(self, key, value) +# elif hasattr(self, f"_{key}"): +# setattr(self, f"_{key}", value) +# elif hasattr(self, f"__{key}"): +# setattr(self, f"__{key}", value) +# else: +# self.get_logger().error(f"Override '{key}' is not supperted by the task.") +# +# self.__task_parameter_overrides.clear() diff --git a/env_manager/rbs_gym/rbs_gym/envs/utils/logging.py b/env_manager/env_manager/env_manager/utils/logging.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/utils/logging.py rename to env_manager/env_manager/env_manager/utils/logging.py diff --git a/env_manager/rbs_gym/rbs_gym/envs/utils/math.py b/env_manager/env_manager/env_manager/utils/math.py similarity index 75% rename from env_manager/rbs_gym/rbs_gym/envs/utils/math.py rename to env_manager/env_manager/env_manager/utils/math.py index cd776b1..291cdf5 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/utils/math.py +++ b/env_manager/env_manager/env_manager/utils/math.py @@ -1,13 +1,11 @@ -from typing import List, Tuple - import numpy as np def quat_mul( - quat_0: Tuple[float, float, float, float], - quat_1: Tuple[float, float, float, float], + quat_0: tuple[float, float, float, float], + quat_1: tuple[float, float, float, float], xyzw: bool = True, -) -> Tuple[float, float, float, float]: +) -> tuple[float, float, float, float]: """ Multiply two quaternions """ @@ -32,15 +30,15 @@ def quat_mul( def distance_to_nearest_point( - origin: Tuple[float, float, float], points: List[Tuple[float, float, float]] + origin: tuple[float, float, float], points: list[tuple[float, float, float]] ) -> float: return np.linalg.norm(np.array(points) - np.array(origin), axis=1).min() def get_nearest_point( - origin: Tuple[float, float, float], points: List[Tuple[float, float, float]] -) -> Tuple[float, float, float]: + origin: tuple[float, float, float], points: list[tuple[float, float, float]] +) -> tuple[float, float, float]: target_distances = np.linalg.norm(np.array(points) - np.array(origin), axis=1) return points[target_distances.argmin()] diff --git a/env_manager/rbs_gym/rbs_gym/envs/utils/tf2_broadcaster.py b/env_manager/env_manager/env_manager/utils/tf2_broadcaster.py similarity index 97% rename from env_manager/rbs_gym/rbs_gym/envs/utils/tf2_broadcaster.py rename to env_manager/env_manager/env_manager/utils/tf2_broadcaster.py index 71216f5..27fb52a 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/utils/tf2_broadcaster.py +++ b/env_manager/env_manager/env_manager/utils/tf2_broadcaster.py @@ -56,7 +56,7 @@ class Tf2Broadcaster: class Tf2BroadcasterStandalone(Node, Tf2Broadcaster): def __init__( self, - node_name: str = "rbs_gym_tf_broadcaster", + node_name: str = "env_manager_tf_broadcaster", use_sim_time: bool = True, ): diff --git a/env_manager/rbs_gym/rbs_gym/envs/utils/tf2_listener.py b/env_manager/env_manager/env_manager/utils/tf2_listener.py similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/utils/tf2_listener.py rename to env_manager/env_manager/env_manager/utils/tf2_listener.py diff --git a/env_manager/env_manager/env_manager/utils/types.py b/env_manager/env_manager/env_manager/utils/types.py new file mode 100644 index 0000000..a502bd6 --- /dev/null +++ b/env_manager/env_manager/env_manager/utils/types.py @@ -0,0 +1,5 @@ +Point = tuple[float, float, float] +Quat = tuple[float, float, float, float] +Rpy = tuple[float, float, float] +Pose = tuple[Point, Quat] +PoseRpy = tuple[Point, Rpy] diff --git a/env_manager/rbs_gym/rbs_gym/envs/worlds/default.sdf b/env_manager/env_manager/env_manager/worlds/default.sdf similarity index 100% rename from env_manager/rbs_gym/rbs_gym/envs/worlds/default.sdf rename to env_manager/env_manager/env_manager/worlds/default.sdf diff --git a/env_manager/env_manager/include/env_manager/env_manager.hpp b/env_manager/env_manager/include/env_manager/env_manager.hpp deleted file mode 100644 index 912c168..0000000 --- a/env_manager/env_manager/include/env_manager/env_manager.hpp +++ /dev/null @@ -1,100 +0,0 @@ -#ifndef __ENV_MANAGER_HPP__ -#define __ENV_MANAGER_HPP__ - -#include "env_manager_interfaces/srv/configure_env.hpp" -#include "env_manager_interfaces/srv/load_env.hpp" -#include "env_manager_interfaces/srv/unload_env.hpp" -#include "env_manager_interfaces/srv/start_env.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "env_interface/env_interface.hpp" - -#include "pluginlib/class_loader.hpp" - -namespace env_manager { -using EnvInterface = env_interface::EnvInterface; -using EnvInterfaceSharedPtr = std::shared_ptr; -using EnvStateReturnType = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -struct EnvSpec { - std::string name; - std::string type; - EnvInterfaceSharedPtr env_ptr; -}; - -class EnvManager : public rclcpp::Node { -public: - // EnvManager(rclcpp::Executor::SharedPtr executor, - // const std::string &asm_config, - // const std::string &node_name = "env_manager", - // const std::string &node_namespace = "", - // rclcpp::NodeOptions &node_options = - // rclcpp::NodeOptions() - // .allow_undeclared_parameters(true) - // .automatically_declare_parameters_from_overrides(true)); - EnvManager(rclcpp::Executor::SharedPtr executor, - const std::string &node_name = "env_manager", - const std::string &node_namespace = "", - rclcpp::NodeOptions &node_options = - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); - virtual ~EnvManager() = default; - - // EnvInterfaceSharedPtr loadEnv(const std::string& env_name); - EnvInterfaceSharedPtr loadEnv(const std::string &env_name, - const std::string &env_class_type); - EnvStateReturnType unloadEnv(const std::string &env_name); - EnvStateReturnType configureEnv(const std::string &env_name); - EnvInterfaceSharedPtr addEnv(const EnvSpec &enviment); - - // TODO: Define the input data format - // Set Target for RL enviroment - EnvInterfaceSharedPtr setTarget(); - EnvInterfaceSharedPtr switchTarget(); - EnvInterfaceSharedPtr unsetTarget(); - // Load Constraints for RL enviroment - EnvInterfaceSharedPtr loadConstraints(); - EnvInterfaceSharedPtr switchConstraints(); - EnvInterfaceSharedPtr unloadConstraints(); - -protected: - void initServices(); - rclcpp::Node::SharedPtr getNode(); - void startEnv_cb( - const env_manager_interfaces::srv::StartEnv::Request::SharedPtr request, - env_manager_interfaces::srv::StartEnv::Response::SharedPtr response); - void loadEnv_cb( - const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, - env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response); - - void configureEnv_cb( - const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr - request, - env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response); - - void unloadEnv_cb( - const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, - env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response); - -private: - rclcpp::Node::SharedPtr m_node; - std::mutex m_env_mutex; - std::vector m_active_envs; - rclcpp::Service::SharedPtr - m_load_env_srv; - rclcpp::Service::SharedPtr - m_configure_env_srv; - rclcpp::Service::SharedPtr - m_unload_env_srv; - rclcpp::Service::SharedPtr m_start_env_srv; - - rclcpp::CallbackGroup::SharedPtr m_callback_group; - rclcpp::Executor::SharedPtr m_executor; - - std::shared_ptr> m_loader; -}; -} // namespace env_manager -#endif // __ENV_MANAGER_HPP__ diff --git a/env_manager/env_manager/package.xml b/env_manager/env_manager/package.xml index 7973f5f..64e3f20 100644 --- a/env_manager/env_manager/package.xml +++ b/env_manager/env_manager/package.xml @@ -4,15 +4,10 @@ env_manager 0.0.0 TODO: Package description - banana + narmak Apache-2.0 ament_cmake - - env_interface - env_manager_interfaces - rclcpp - rclcpp_lifecycle ament_lint_auto ament_lint_common diff --git a/env_manager/env_manager/src/env_manager.cpp b/env_manager/env_manager/src/env_manager.cpp deleted file mode 100644 index 28a1fc9..0000000 --- a/env_manager/env_manager/src/env_manager.cpp +++ /dev/null @@ -1,190 +0,0 @@ -#include "env_manager/env_manager.hpp" -#include - -static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE = - "env_interface"; -static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAME = - "env_interface::EnvInterface"; - -namespace env_manager { -EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor, - const std::string &node_name, - const std::string &node_namespace, - rclcpp::NodeOptions &options) - : rclcpp::Node(node_name, node_namespace, options), m_executor(executor), - m_loader( - std::make_shared>( - ENV_INTERFACE_BASE_CLASS_NAMESPACE, - ENV_INTERFACE_BASE_CLASS_NAME)) { - initServices(); -} - -// EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor, -// const std::string &asm_config, -// const std::string &node_name, -// const std::string &node_namespace, -// rclcpp::NodeOptions &options) -// : rclcpp::Node(node_name, node_namespace, options), m_executor(executor), -// m_loader( -// std::make_shared>( -// ENV_INTERFACE_BASE_CLASS_NAMESPACE, -// ENV_INTERFACE_BASE_CLASS_NAME)) { -// initServices(); -// } - -void EnvManager::initServices() { - using namespace std::placeholders; - - m_load_env_srv = create_service( - "~/load_env", std::bind(&EnvManager::loadEnv_cb, this, _1, _2)); - m_configure_env_srv = - create_service( - "~/configure_env", - std::bind(&EnvManager::configureEnv_cb, this, _1, _2)); - m_unload_env_srv = create_service( - "~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2)); - m_start_env_srv = create_service( - "~/start_env", std::bind(&EnvManager::startEnv_cb, this, _1, _2)); -} - -rclcpp::Node::SharedPtr EnvManager::getNode() { return m_node; } - -void EnvManager::startEnv_cb( - const env_manager_interfaces::srv::StartEnv::Request::SharedPtr request, - env_manager_interfaces::srv::StartEnv::Response::SharedPtr response) { - - auto it = std::find_if( - m_active_envs.begin(), m_active_envs.end(), - [&request](const EnvSpec &env) { return env.name == request->name; }); - - if (it != m_active_envs.end()) { - RCLCPP_INFO(get_logger(), "Unloading existing environment '%s'", - request->name.c_str()); - unloadEnv(request->name); - } - - auto loadedEnv = loadEnv(request->name, request->type); - auto configuredEnv = configureEnv(request->name); - response->ok = (loadedEnv.get() != nullptr && - configuredEnv == EnvStateReturnType::SUCCESS) - ? true - : false; -} -void EnvManager::loadEnv_cb( - const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, - env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) { - response->ok = loadEnv(request->name, request->type).get() != nullptr; -} - -void EnvManager::configureEnv_cb( - const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, - env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) { - response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS; -} - -void EnvManager::unloadEnv_cb( - const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, - env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) { - response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS; -} - -EnvInterfaceSharedPtr EnvManager::addEnv(const EnvSpec &enviroment) { - // generate list of active enviroments - // std::string unique_env_name = enviroment.name;// + "_" + - // std::to_string(m_active_envs.size()); m_active_envs[unique_env_name] = - // enviroment.env_ptr; - if (enviroment.env_ptr->init(enviroment.name, get_namespace()) == - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: - CallbackReturn::ERROR) { - RCLCPP_ERROR(get_logger(), - "Enviroment with name %s cannot been initialized", - enviroment.name.c_str()); - return nullptr; - } - - m_active_envs.push_back(enviroment); - m_executor->add_node( - enviroment.env_ptr->getNode()->get_node_base_interface()); - return enviroment.env_ptr; -} - -EnvInterfaceSharedPtr EnvManager::loadEnv(const std::string &env_name, - const std::string &env_class_type) { - EnvInterfaceSharedPtr loaded_env = nullptr; - RCLCPP_INFO(this->get_logger(), "Loading enviroment '%s'", env_name.c_str()); - try { - if (m_loader->isClassAvailable(env_class_type)) { - loaded_env = m_loader->createSharedInstance(env_class_type); - } else { - RCLCPP_ERROR( - this->get_logger(), - "Loading enviroment is not available '%s' with class type '%s'", - env_name.c_str(), env_class_type.c_str()); - RCLCPP_INFO(this->get_logger(), "Available classes"); - for (const auto &available_class : m_loader->getDeclaredClasses()) { - RCLCPP_INFO(this->get_logger(), "\t%s", available_class.c_str()); - } - return nullptr; - } - } catch (const pluginlib::PluginlibException &e) { - RCLCPP_ERROR( - get_logger(), - "Error happened during creation of enviroment '%s' with type '%s':\n%s", - env_name.c_str(), env_class_type.c_str(), e.what()); - return nullptr; - } - EnvSpec enviroment; - enviroment.name = env_name; - enviroment.type = env_class_type; - enviroment.env_ptr = loaded_env; - return addEnv(enviroment); -} - -EnvStateReturnType EnvManager::unloadEnv(const std::string &env_name) { - auto it = std::find_if( - m_active_envs.begin(), m_active_envs.end(), - [&env_name](const EnvSpec &env) { return env.name == env_name; }); - - if (it != m_active_envs.end()) { - it->env_ptr->getNode()->deactivate(); - it->env_ptr->getNode()->cleanup(); - m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface()); - m_active_envs.erase(it); - return EnvStateReturnType::SUCCESS; - } else { - RCLCPP_ERROR(this->get_logger(), - "Environment '%s' not found in the active environments list.", - env_name.c_str()); - return EnvStateReturnType::ERROR; - } - return EnvStateReturnType::FAILURE; -} - -// EnvInterfaceSharedPtr -// EnvManager::loadEnv(const std::string env_name) -// { -// std::string env_class_type = "some_default_namespace::" + env_name; - -// return loadEnv(env_name, env_class_type); -// } - -EnvStateReturnType EnvManager::configureEnv(const std::string &env_name) { - RCLCPP_INFO(this->get_logger(), "Configuring enviroment '%s'", env_name.c_str()); - - auto it = std::find_if( - m_active_envs.begin(), m_active_envs.end(), - [&env_name](const EnvSpec &env) { return env.name == env_name; }); - - if (it == m_active_envs.end()) { - RCLCPP_ERROR(this->get_logger(), - "Environment '%s' not found in the active environments list.", - env_name.c_str()); - return EnvStateReturnType::FAILURE; - } - it->env_ptr->configure(); - it->env_ptr->getNode()->activate(); - - return EnvStateReturnType::SUCCESS; -} - -} // namespace env_manager diff --git a/env_manager/env_manager/src/run_env_manager.cpp b/env_manager/env_manager/src/run_env_manager.cpp deleted file mode 100644 index 1506122..0000000 --- a/env_manager/env_manager/src/run_env_manager.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#include "rclcpp/rclcpp.hpp" -#include "env_manager/env_manager.hpp" - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - std::shared_ptr executor = std::make_shared(); - std::string manager_node_name = "env_manager"; - - auto em = std::make_shared(executor, manager_node_name); - - RCLCPP_INFO(em->get_logger(), "Env manager is starting"); - - em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment"); - em->configureEnv("gz_enviroment"); - - executor->add_node(em); - executor->spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/env_manager/env_manager_interfaces/CMakeLists.txt b/env_manager/env_manager_interfaces/CMakeLists.txt index 5839aeb..a0a5c75 100644 --- a/env_manager/env_manager_interfaces/CMakeLists.txt +++ b/env_manager/env_manager_interfaces/CMakeLists.txt @@ -18,6 +18,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} srv/ConfigureEnv.srv srv/LoadEnv.srv srv/UnloadEnv.srv + srv/ResetEnv.srv msg/EnvState.msg DEPENDENCIES lifecycle_msgs ) diff --git a/env_manager/env_manager_interfaces/srv/ResetEnv.srv b/env_manager/env_manager_interfaces/srv/ResetEnv.srv new file mode 100644 index 0000000..5d0cbc4 --- /dev/null +++ b/env_manager/env_manager_interfaces/srv/ResetEnv.srv @@ -0,0 +1,3 @@ + +--- +bool ok diff --git a/env_manager/gz_enviroment/CMakeLists.txt b/env_manager/gz_enviroment/CMakeLists.txt deleted file mode 100644 index 42c7027..0000000 --- a/env_manager/gz_enviroment/CMakeLists.txt +++ /dev/null @@ -1,78 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(gz_enviroment) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - rclcpp_lifecycle - tf2_ros - tf2_msgs - tf2_eigen - ros_gz - pluginlib - ignition-transport11 - ignition-msgs8 - ros_gz_bridge - env_interface - ignition-gazebo6 -) - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -# TODO: generate_parameter_library - - - -add_library(${PROJECT_NAME} SHARED src/gz_enviroment.cpp) -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ -) -ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(${PROJECT_NAME} PRIVATE "GZENVIROMENT_BUILDING_DLL") -#TODO: replace rclcpp_lifecycle with custom interface class -pluginlib_export_plugin_description_file(env_interface gz_enviroment.xml) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -install(TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install(PROGRAMS - scripts/publish_asm_config.py - scripts/test_tf.py - DESTINATION lib/${PROJECT_NAME} -) - -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -# ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/env_manager/gz_enviroment/LICENSE b/env_manager/gz_enviroment/LICENSE deleted file mode 100644 index d645695..0000000 --- a/env_manager/gz_enviroment/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. diff --git a/env_manager/gz_enviroment/gz_enviroment.xml b/env_manager/gz_enviroment/gz_enviroment.xml deleted file mode 100644 index 59d0901..0000000 --- a/env_manager/gz_enviroment/gz_enviroment.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - Gazebo enviroment for env_manager - - \ No newline at end of file diff --git a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp deleted file mode 100644 index 34f84cb..0000000 --- a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp +++ /dev/null @@ -1,70 +0,0 @@ -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -// #include "ros_gz_bridge/convert.hpp" -#include "env_interface/env_interface.hpp" -#include "geometry_msgs/msg/pose_array.hpp" -#include "gz/msgs/pose_v.pb.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace gz_enviroment { -using CallbackReturn = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -class GzEnviroment : public env_interface::EnvInterface { -public: - GzEnviroment(); - CallbackReturn on_init() override; - CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State &) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; - CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override; - -protected: - void onGzPoseSub(const gz::msgs::Pose_V &pose_v); - - // bool doGzSpawn(); - -private: - std::unique_ptr m_tf2_broadcaster; - std::shared_ptr m_gz_node; - std::vector m_follow_frames; - std::string m_topic_name; - std::string m_service_spawn; - std::string m_world_name; - std::shared_ptr m_config_loader; - tf2_msgs::msg::TFMessage m_transforms; - tf2_msgs::msg::TFMessage m_target_places; - rclcpp::TimerBase::SharedPtr m_timer; - - std::vector m_all_transforms{}; - - std::string getGzWorldName(); - std::string createGzModelString(const std::vector &pose, - const std::vector &inertia, - const double &mass, - const std::string &mesh_filepath, - const std::string &name); - - void broadcast_timer_callback() { - if (!m_transforms.transforms.empty()) { - for (auto &transform : m_transforms.transforms) { - m_tf2_broadcaster->sendTransform(std::move(transform)); - } - // m_transforms.transforms.clear(); - } - } -}; -} // namespace gz_enviroment diff --git a/env_manager/gz_enviroment/package.xml b/env_manager/gz_enviroment/package.xml deleted file mode 100644 index 65f99ad..0000000 --- a/env_manager/gz_enviroment/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - gz_enviroment - 0.0.0 - TODO: Package description - bill-finger - Apache-2.0 - - ament_cmake - - rclcpp - rclcpp_lifecycle - tf2_ros - tf2_msgs - ros_gz - env_interface - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/env_manager/gz_enviroment/scripts/publish_asm_config.py b/env_manager/gz_enviroment/scripts/publish_asm_config.py deleted file mode 100755 index dbc1931..0000000 --- a/env_manager/gz_enviroment/scripts/publish_asm_config.py +++ /dev/null @@ -1,114 +0,0 @@ -#! /usr/bin/env python3 - -import yaml -import rclpy -from rclpy.node import Node -from rbs_utils_interfaces.msg import AssemblyConfig, NamedPose, RelativeNamedPose -from geometry_msgs.msg import Point -import os - - -class ConfigPublisherNode(Node): - - def __init__(self): - super().__init__('config_publisher_node') - self.publisher_ = self.create_publisher(AssemblyConfig, '/assembly_config', 10) - timer_period = 1 # seconds - self.timer = self.create_timer(timer_period, self.timer_callback) - if 'RBS_ASSEMBLY_DIR' in os.environ: - assembly_dir = os.environ['RBS_ASSEMBLY_DIR'] - else: - assembly_dir = '~/assembly/robossembler-stuff' - self.assembly_dir = os.path.expanduser(assembly_dir) - datafolder = "asp-example" - # Read data from YAML file - with open(f"{self.assembly_dir}/{datafolder}/rbs_db.yaml") as f: - self.data = yaml.safe_load(f) - - def timer_callback(self): - - # Create AssemblyConfig message - msg = AssemblyConfig() - # Fill in the data from the YAML file - msg.assets_db = self.data['assets_db'] - - # Populate workspace - for wp in self.data['workspace']: - point = Point() - point.x = wp["x"] - point.y = wp["y"] - point.z = wp["z"] - msg.workspace.append(point) - - # Populate absolute_part - for part in self.data['absolute_part']: - named_pose = NamedPose() - named_pose.name = part['name'] - pose = part['pose'] - named_pose.pose.position.x = pose['position']['x'] - named_pose.pose.position.y = pose['position']['y'] - named_pose.pose.position.z = pose['position']['z'] - named_pose.pose.orientation.x = pose['orientation']['x'] - named_pose.pose.orientation.y = pose['orientation']['y'] - named_pose.pose.orientation.z = pose['orientation']['z'] - named_pose.pose.orientation.w = pose['orientation']['w'] - msg.absolute_part.append(named_pose) - - # Populate relative_part - for part in self.data['relative_part']: - relative_named_pose = RelativeNamedPose() - relative_named_pose.name = part['name'] - relative_named_pose.relative_at = part['relative_at'] - pose = part['pose'] - relative_named_pose.pose.position.x = pose['position']['x'] - relative_named_pose.pose.position.y = pose['position']['y'] - relative_named_pose.pose.position.z = pose['position']['z'] - relative_named_pose.pose.orientation.x = pose['orientation']['x'] - relative_named_pose.pose.orientation.y = pose['orientation']['y'] - relative_named_pose.pose.orientation.z = pose['orientation']['z'] - relative_named_pose.pose.orientation.w = pose['orientation']['w'] - msg.relative_part.append(relative_named_pose) - - # Populate grasp_pose - for part in self.data['grasp_pose']: - relative_named_pose = RelativeNamedPose() - relative_named_pose.name = part['name'] - relative_named_pose.relative_at = part['relative_at'] - pose = part['pose'] - relative_named_pose.pose.position.x = pose['position']['x'] - relative_named_pose.pose.position.y = pose['position']['y'] - relative_named_pose.pose.position.z = pose['position']['z'] - relative_named_pose.pose.orientation.x = pose['orientation']['x'] - relative_named_pose.pose.orientation.y = pose['orientation']['y'] - relative_named_pose.pose.orientation.z = pose['orientation']['z'] - relative_named_pose.pose.orientation.w = pose['orientation']['w'] - msg.grasp_pose.append(relative_named_pose) - - # Populate extra_poses - for part in self.data['extra_poses']: - named_pose = NamedPose() - named_pose.name = part['name'] - pose = part['pose'] - named_pose.pose.position.x = pose['position']['x'] - named_pose.pose.position.y = pose['position']['y'] - named_pose.pose.position.z = pose['position']['z'] - named_pose.pose.orientation.x = pose['orientation']['x'] - named_pose.pose.orientation.y = pose['orientation']['y'] - named_pose.pose.orientation.z = pose['orientation']['z'] - named_pose.pose.orientation.w = pose['orientation']['w'] - msg.extra_poses.append(named_pose) - - # Publish the message - self.publisher_.publish(msg) - - -def main(args=None): - rclpy.init(args=args) - config_publisher_node = ConfigPublisherNode() - rclpy.spin(config_publisher_node) - config_publisher_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/env_manager/gz_enviroment/scripts/test_tf.py b/env_manager/gz_enviroment/scripts/test_tf.py deleted file mode 100755 index d7f7ebb..0000000 --- a/env_manager/gz_enviroment/scripts/test_tf.py +++ /dev/null @@ -1,75 +0,0 @@ -#! /usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from tf2_msgs.msg import TFMessage -from geometry_msgs.msg import TransformStamped -from rbs_utils_interfaces.msg import AssemblyConfig -from tf2_ros.transform_broadcaster import TransformBroadcaster - - -class TF2PublisherNode(Node): - - def __init__(self): - super().__init__('tf2_publisher_node') - self.tfb_ = TransformBroadcaster(self) - self.subscription = self.create_subscription( - AssemblyConfig, '/assembly_config', self.config_callback, 10) - - def config_callback(self, msg): - # Populate transforms for absolute_part - for part in msg.absolute_part: - ts = TransformStamped() - ts.header.stamp = self.get_clock().now().to_msg() - ts.header.frame_id = "world" - ts.child_frame_id = part.name - ts.transform.translation.x = part.pose.position.x - ts.transform.translation.y = part.pose.position.y - ts.transform.translation.z = part.pose.position.z - ts.transform.rotation.x = part.pose.orientation.x - ts.transform.rotation.y = part.pose.orientation.y - ts.transform.rotation.z = part.pose.orientation.z - ts.transform.rotation.w = part.pose.orientation.w - self.tfb_.sendTransform(ts) - - # Populate transforms for relative_part - for part in msg.relative_part: - ts = TransformStamped() - ts.header.stamp = self.get_clock().now().to_msg() - ts.header.frame_id = part.relative_at - ts.child_frame_id = part.name - ts.transform.translation.x = part.pose.position.x - ts.transform.translation.y = part.pose.position.y - ts.transform.translation.z = part.pose.position.z - ts.transform.rotation.x = part.pose.orientation.x - ts.transform.rotation.y = part.pose.orientation.y - ts.transform.rotation.z = part.pose.orientation.z - ts.transform.rotation.w = part.pose.orientation.w - self.tfb_.sendTransform(ts) - - # Populate transforms for grasp_pose - for part in msg.grasp_pose: - ts = TransformStamped() - ts.header.stamp = self.get_clock().now().to_msg() - ts.header.frame_id = part.relative_at - ts.child_frame_id = part.name - ts.transform.translation.x = part.pose.position.x - ts.transform.translation.y = part.pose.position.y - ts.transform.translation.z = part.pose.position.z - ts.transform.rotation.x = part.pose.orientation.x - ts.transform.rotation.y = part.pose.orientation.y - ts.transform.rotation.z = part.pose.orientation.z - ts.transform.rotation.w = part.pose.orientation.w - self.tfb_.sendTransform(ts) - - -def main(args=None): - rclpy.init(args=args) - tf2_publisher_node = TF2PublisherNode() - rclpy.spin(tf2_publisher_node) - tf2_publisher_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/env_manager/gz_enviroment/src/gz_enviroment.cpp b/env_manager/gz_enviroment/src/gz_enviroment.cpp deleted file mode 100644 index 8de7b3e..0000000 --- a/env_manager/gz_enviroment/src/gz_enviroment.cpp +++ /dev/null @@ -1,208 +0,0 @@ -#include "gz_enviroment/gz_enviroment.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "ros_gz_bridge/convert.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace gz_enviroment { - -GzEnviroment::GzEnviroment() : env_interface::EnvInterface() {} - -CallbackReturn GzEnviroment::on_init() { - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()"); - gz::sim::EntityComponentManager ecm; - - const auto modelEntities = - ecm.EntitiesByComponents(gz::sim::components::Model()); - - for (const auto entity : modelEntities) { - const auto modelName = ecm.Component(entity); - if (modelName) { - RCLCPP_INFO_STREAM(getNode()->get_logger(), - "Model Name: " << modelName->Data()); - } - } - - // getNode()->declare_parameter("use_sim_time", true); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) { - // Configuration of parameters - using namespace std::chrono_literals; - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure"); - m_gz_node = std::make_shared(); - m_world_name = getGzWorldName(); - m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info"; - m_service_spawn = std::string("/world/") + m_world_name + "/create"; - m_config_loader = std::make_shared( - "asp-example", getNode()); - m_follow_frames = m_config_loader->getUniqueSceneModelNames(); - m_transforms = m_config_loader->getGraspTfData("bishop"); - // m_transforms = m_config_loader->getAllPossibleTfData(); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State &) { - // Setting up the subscribers and publishers - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate"); - - // Initialize tf broadcaster before use in subscriber - m_tf2_broadcaster = - std::make_unique(getNode()); - m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) { - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup"); - // Clear variables on_cleanup - m_gz_node.reset(); - m_follow_frames.clear(); - m_topic_name.clear(); - m_service_spawn.clear(); - m_world_name.clear(); - m_config_loader.reset(); - m_transforms = tf2_msgs::msg::TFMessage(); - m_target_places = tf2_msgs::msg::TFMessage(); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State &) { - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate"); - return CallbackReturn::SUCCESS; -} - -// TODO: Check to do this via EntityComponentManager -void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) { - std::vector all_transforms{}; - - for (const auto &it : pose_v.pose()) { - if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) == - m_follow_frames.end()) - continue; - - geometry_msgs::msg::PoseStamped rpose; - ros_gz_bridge::convert_gz_to_ros(it, rpose); - - geometry_msgs::msg::TransformStamped t; - t.header.stamp = getNode()->get_clock()->now(); - t.header.frame_id = "world"; - t.child_frame_id = it.name(); - - t.transform.translation.x = rpose.pose.position.x; - t.transform.translation.y = rpose.pose.position.y; - t.transform.translation.z = rpose.pose.position.z; - - t.transform.rotation.x = rpose.pose.orientation.x; - t.transform.rotation.y = rpose.pose.orientation.y; - t.transform.rotation.z = rpose.pose.orientation.z; - t.transform.rotation.w = rpose.pose.orientation.w; - - all_transforms.push_back(t); - } - - for (auto &place : m_transforms.transforms) { - place.header.stamp = getNode()->get_clock()->now(); - all_transforms.push_back(place); - } - - // m_all_transforms.swap(all_transforms); -} - -std::string GzEnviroment::getGzWorldName() { - bool executed{false}; - bool result{false}; - unsigned int timeout{5000}; - std::string service{"/gazebo/worlds"}; - gz::msgs::StringMsg_V worlds_msg; - - while (rclcpp::ok() && !executed) { - RCLCPP_INFO(getNode()->get_logger(), "Requesting list of world names."); - executed = m_gz_node->Request(service, timeout, worlds_msg, result); - } - - if (!executed) { - RCLCPP_INFO(getNode()->get_logger(), "Timed out when getting world names."); - return ""; - } - - if (!result || worlds_msg.data().empty()) { - RCLCPP_INFO(getNode()->get_logger(), "Failed to get world names."); - return ""; - } - RCLCPP_INFO(getNode()->get_logger(), "%s", worlds_msg.data(0).c_str()); - return worlds_msg.data(0); -} - -// bool GzEnviroment::doGzSpawn() { -// gz::msgs::EntityFactory req; -// gz::msgs::Boolean rep; -// bool result; -// unsigned int timeout = 5000; -// bool executed; -// -// auto env_models = m_config_loader->getEnvModels(); -// for (auto &model : *env_models) { -// std::string sdf_string = -// createGzModelString(model.model_pose, model.model_inertia, model.mass, -// model.mesh_path, model.model_name); -// req.set_sdf(sdf_string); -// executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result); -// } -// return executed; -// } - -// std::string GzEnviroment::createGzModelString( -// const std::vector &pose, const std::vector &inertia, -// const double &mass, const std::string &mesh_filepath, -// const std::string &name) { -// std::string model_template = -// std::string("") + "" + -// "" + "1" + "" + -// std::to_string(pose[0]) + " " + std::to_string(pose[1]) + " " + -// std::to_string(pose[2]) + " " + std::to_string(pose[3]) + " " + -// std::to_string(pose[4]) + " " + std::to_string(pose[5]) + -// "" -// // + "" -// // + "" -// // + "" + std::to_string(inertia[0]) + "" -// // + "" + std::to_string(inertia[1]) + "" -// // + "" + std::to_string(inertia[2]) + "" -// // + "" + std::to_string(inertia[3]) + "" -// // + "" + std::to_string(inertia[4]) + "" -// // + "" + std::to_string(inertia[5]) + "" -// // + "" -// // + "" + std::to_string(mass) + "" -// // + "" -// + "" + "file://" + -// mesh_filepath + "" + "" + -// "" + -// "file://" + mesh_filepath + -// "" + "" + "" + "" + -// ""; -// return model_template; -// } - -} // namespace gz_enviroment - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment, - env_interface::EnvInterface); diff --git a/env_manager/planning_scene_manager/LICENSE b/env_manager/planning_scene_manager/LICENSE deleted file mode 100644 index d645695..0000000 --- a/env_manager/planning_scene_manager/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. diff --git a/env_manager/planning_scene_manager/include/planning_scene_manager/planning_scene_manager.hpp b/env_manager/planning_scene_manager/include/planning_scene_manager/planning_scene_manager.hpp deleted file mode 100644 index a3023e8..0000000 --- a/env_manager/planning_scene_manager/include/planning_scene_manager/planning_scene_manager.hpp +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef __PLANNING_SCENE_MANAGER_H__ -#define __PLANNING_SCENE_MANAGER_H__ - -#include "rclcpp/rclcpp.hpp" -#include "moveit/planning_scene_monitor/planning_scene_monitor.h" - -namespace planning_scene_manager -{ -class PlanningSceneManager : public planning_scene_monitor::PlanningSceneMonitor -{ -public: - PlanningSceneManager(); - - bool GetPlanningObject(); - bool SetPlanningObject(); - - bool SetPlanningScene(); - bool GetPlanningScene(); - - bool UpdatePlanningScene(); - -private: - -}; - - -} // namespace planning_scene_manager - -#endif // __PLANNING_SCENE_MANAGER_H__ \ No newline at end of file diff --git a/env_manager/planning_scene_manager/src/planning_scene_manager.cpp b/env_manager/planning_scene_manager/src/planning_scene_manager.cpp deleted file mode 100644 index 8c9ab4a..0000000 --- a/env_manager/planning_scene_manager/src/planning_scene_manager.cpp +++ /dev/null @@ -1,31 +0,0 @@ -#include "planning_scene_manager/planning_scene_manager.hpp" -#include "moveit_msgs/msg/collision_object.hpp" -#include "moveit/planning_scene_interface/planning_scene_interface.h" - -namespace planning_scene_manager -{ - -bool PlanningSceneManager::SetPlanningObject() -{ - moveit_msgs::msg::CollisionObject object; - std::shared_ptr pcm_; - planning_scene_monitor::LockedPlanningSceneRW scene(pcm_); - scene->removeAllCollisionObjects(); - RCLCPP_INFO(pnode_->get_logger(), "All object has been delete"); - return true; -} - -} // namespace planning_scene_manager - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - - // Добавляем объект в сцену коллизии - node->SetPlanningObject(); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/env_manager/rbs_gym/CMakeLists.txt b/env_manager/rbs_gym/CMakeLists.txt index 022f4ba..150862f 100644 --- a/env_manager/rbs_gym/CMakeLists.txt +++ b/env_manager/rbs_gym/CMakeLists.txt @@ -34,6 +34,6 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -install(DIRECTORY rbs_gym/envs/worlds launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/env_manager/rbs_gym/launch/test_env.launch.py b/env_manager/rbs_gym/launch/test_env.launch.py index 0b7b641..146ef71 100644 --- a/env_manager/rbs_gym/launch/test_env.launch.py +++ b/env_manager/rbs_gym/launch/test_env.launch.py @@ -1,18 +1,21 @@ +import os +from os import cpu_count + +import xacro +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, SetEnvironmentVariable, - TimerAction + TimerAction, ) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare from launch_ros.actions import Node -import os -from os import cpu_count -from ament_index_python.packages import get_package_share_directory +from launch_ros.substitutions import FindPackageShare + def launch_setup(context, *args, **kwargs): # Initialize Arguments @@ -45,33 +48,50 @@ def launch_setup(context, *args, **kwargs): log_level = LaunchConfiguration("log_level") env_kwargs = LaunchConfiguration("env_kwargs") - sim_gazebo = LaunchConfiguration("sim_gazebo") launch_simulation = LaunchConfiguration("launch_sim") initial_joint_controllers_file_path = os.path.join( - get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml' + get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml" ) + xacro_file = os.path.join( + get_package_share_directory(description_package.perform(context)), + "urdf", + description_file.perform(context), + ) + + robot_description_doc = xacro.process_file( + xacro_file, + mappings={ + "gripper_name": gripper_name.perform(context), + "hardware": hardware.perform(context), + "simulation_controllers": initial_joint_controllers_file_path, + "namespace": "", + }, + ) + + robot_description_content = robot_description_doc.toprettyxml(indent=" ") + robot_description = {"robot_description": robot_description_content} + single_robot_setup = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('rbs_bringup'), - "launch", - "rbs_robot.launch.py" - ]) - ]), + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [FindPackageShare("rbs_bringup"), "launch", "rbs_robot.launch.py"] + ) + ] + ), launch_arguments={ "env_manager": env_manager, "with_gripper": with_gripper_condition, "gripper_name": gripper_name, - "controllers_file": controllers_file, - "robot_type": robot_type, "controllers_file": initial_joint_controllers_file_path, + "robot_type": robot_type, "cartesian_controllers": cartesian_controllers, "description_package": description_package, "description_file": description_file, - "robot_name": robot_name, + "robot_name": robot_type, "start_joint_controller": start_joint_controller, "initial_joint_controller": initial_joint_controller, "launch_simulation": launch_simulation, @@ -83,9 +103,9 @@ def launch_setup(context, *args, **kwargs): "use_sim_time": use_sim_time, "sim_gazebo": sim_gazebo, "hardware": hardware, - "launch_controllers": launch_controllers, - # "gazebo_gui": gazebo_gui - }.items() + "launch_controllers": "true", + "robot_description": robot_description_content, + }.items(), ) args = [ @@ -103,26 +123,23 @@ def launch_setup(context, *args, **kwargs): executable="test_agent.py", output="log", arguments=args, - parameters=[{"use_sim_time": True}] + parameters=[{"use_sim_time": True}, robot_description], ) clock_bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], - output='screen') - - - delay_robot_control_stack = TimerAction( - period=10.0, - actions=[single_robot_setup] + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"], + output="screen", ) + delay_robot_control_stack = TimerAction(period=10.0, actions=[single_robot_setup]) + nodes_to_start = [ # env, rl_task, clock_bridge, - delay_robot_control_stack + delay_robot_control_stack, ] return nodes_to_start @@ -133,7 +150,7 @@ def generate_launch_description(): DeclareLaunchArgument( "robot_type", description="Type of robot by name", - choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], + choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], default_value="rbs_arm", ) ) @@ -213,62 +230,72 @@ def generate_launch_description(): ) ) declared_arguments.append( - DeclareLaunchArgument("with_gripper", - default_value="true", - description="With gripper or not?") + DeclareLaunchArgument( + "with_gripper", default_value="true", description="With gripper or not?" + ) ) declared_arguments.append( - DeclareLaunchArgument("sim_gazebo", - default_value="true", - description="Gazebo Simulation") + DeclareLaunchArgument( + "sim_gazebo", default_value="true", description="Gazebo Simulation" + ) ) declared_arguments.append( - DeclareLaunchArgument("env_manager", - default_value="false", - description="Launch env_manager?") + DeclareLaunchArgument( + "env_manager", default_value="false", description="Launch env_manager?" + ) ) declared_arguments.append( - DeclareLaunchArgument("launch_sim", - default_value="true", - description="Launch simulator (Gazebo)?\ - Most general arg") + DeclareLaunchArgument( + "launch_sim", + default_value="true", + description="Launch simulator (Gazebo)?\ + Most general arg", + ) ) declared_arguments.append( - DeclareLaunchArgument("launch_moveit", - default_value="false", - description="Launch moveit?") + DeclareLaunchArgument( + "launch_moveit", default_value="false", description="Launch moveit?" + ) ) declared_arguments.append( - DeclareLaunchArgument("launch_perception", - default_value="false", - description="Launch perception?") + DeclareLaunchArgument( + "launch_perception", default_value="false", description="Launch perception?" + ) ) declared_arguments.append( - DeclareLaunchArgument("launch_task_planner", - default_value="false", - description="Launch task_planner?") + DeclareLaunchArgument( + "launch_task_planner", + default_value="false", + description="Launch task_planner?", + ) ) declared_arguments.append( - DeclareLaunchArgument("cartesian_controllers", - default_value="true", - description="Load cartesian\ - controllers?") + DeclareLaunchArgument( + "cartesian_controllers", + default_value="true", + description="Load cartesian\ + controllers?", + ) ) declared_arguments.append( - DeclareLaunchArgument("hardware", - choices=["gazebo", "mock"], - default_value="gazebo", - description="Choose your harware_interface") + DeclareLaunchArgument( + "hardware", + choices=["gazebo", "mock"], + default_value="gazebo", + description="Choose your harware_interface", + ) ) declared_arguments.append( - DeclareLaunchArgument("launch_controllers", - default_value="true", - description="Launch controllers?") + DeclareLaunchArgument( + "launch_controllers", + default_value="true", + description="Launch controllers?", + ) ) declared_arguments.append( - DeclareLaunchArgument("gazebo_gui", - default_value="true", - description="Launch gazebo with gui?") + DeclareLaunchArgument( + "gazebo_gui", default_value="true", description="Launch gazebo with gui?" + ) ) # training arguments declared_arguments.append( @@ -276,140 +303,165 @@ def generate_launch_description(): "env", default_value="Reach-Gazebo-v0", description="Environment ID", - )) + ) + ) declared_arguments.append( DeclareLaunchArgument( "env_kwargs", default_value="", description="Optional keyword argument to pass to the env constructor.", - )) + ) + ) declared_arguments.append( DeclareLaunchArgument( "vec_env", default_value="dummy", description="Type of VecEnv to use (dummy or subproc).", - )) - # Algorithm and training + ) + ) + # Algorithm and training declared_arguments.append( DeclareLaunchArgument( "algo", default_value="sac", description="RL algorithm to use during the training.", - )) + ) + ) declared_arguments.append( DeclareLaunchArgument( "n_timesteps", default_value="-1", description="Overwrite the number of timesteps.", - )) + ) + ) declared_arguments.append( DeclareLaunchArgument( "hyperparams", default_value="", description="Optional RL hyperparameter overwrite (e.g. learning_rate:0.01 train_freq:10).", - )) + ) + ) declared_arguments.append( DeclareLaunchArgument( "num_threads", default_value="-1", description="Number of threads for PyTorch (-1 to use default).", - )) - # Continue training an already trained agent + ) + ) + # Continue training an already trained agent declared_arguments.append( DeclareLaunchArgument( "trained_agent", default_value="", description="Path to a pretrained agent to continue training.", - )) - # Random seed + ) + ) + # Random seed declared_arguments.append( DeclareLaunchArgument( "seed", default_value="-1", description="Random generator seed.", - )) - # Saving of model + ) + ) + # Saving of model declared_arguments.append( DeclareLaunchArgument( "save_freq", default_value="10000", description="Save the model every n steps (if negative, no checkpoint).", - )) + ) + ) declared_arguments.append( DeclareLaunchArgument( "save_replay_buffer", default_value="False", description="Save the replay buffer too (when applicable).", - )) - # Pre-load a replay buffer and start training on it + ) + ) + # Pre-load a replay buffer and start training on it declared_arguments.append( DeclareLaunchArgument( "preload_replay_buffer", default_value="", description="Path to a replay buffer that should be preloaded before starting the training process.", - )) - # Logging + ) + ) + # Logging declared_arguments.append( DeclareLaunchArgument( "log_folder", default_value="logs", description="Path to the log directory.", - )) + ) + ) declared_arguments.append( DeclareLaunchArgument( "tensorboard_log", default_value="tensorboard_logs", description="Tensorboard log dir.", - )) + ) + ) declared_arguments.append( DeclareLaunchArgument( "log_interval", default_value="-1", description="Override log interval (default: -1, no change).", - )) + ) + ) declared_arguments.append( DeclareLaunchArgument( "uuid", default_value="False", description="Ensure that the run has a unique ID.", - )) - # Evaluation + ) + ) + # Evaluation declared_arguments.append( DeclareLaunchArgument( "eval_freq", default_value="-1", description="Evaluate the agent every n steps (if negative, no evaluation).", - )) + ) + ) declared_arguments.append( DeclareLaunchArgument( "eval_episodes", default_value="5", description="Number of episodes to use for evaluation.", - )) - # Verbosity + ) + ) + # Verbosity declared_arguments.append( DeclareLaunchArgument( "verbose", default_value="1", description="Verbose mode (0: no output, 1: INFO).", - )) - # HER specifics - declared_arguments.append( - DeclareLaunchArgument( - "truncate_last_trajectory", - default_value="True", - description="When using HER with online sampling the last trajectory in the replay buffer will be truncated after) reloading the replay buffer." - )), + ) + ) + # HER specifics + ( + declared_arguments.append( + DeclareLaunchArgument( + "truncate_last_trajectory", + default_value="True", + description="When using HER with online sampling the last trajectory in the replay buffer will be truncated after) reloading the replay buffer.", + ) + ), + ) declared_arguments.append( DeclareLaunchArgument( "log_level", default_value="error", description="The level of logging that is applied to all ROS 2 nodes launched by this script.", - )) + ) + ) env_variables = [ SetEnvironmentVariable(name="OMP_DYNAMIC", value="TRUE"), - SetEnvironmentVariable(name="OMP_NUM_THREADS", value=str(cpu_count() // 2)) + SetEnvironmentVariable(name="OMP_NUM_THREADS", value=str(cpu_count() // 2)), ] - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)] + env_variables) + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + env_variables + ) diff --git a/env_manager/rbs_gym/rbs_gym/envs/__init__.py b/env_manager/rbs_gym/rbs_gym/envs/__init__.py index fd29d15..4528d2f 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/__init__.py +++ b/env_manager/rbs_gym/rbs_gym/envs/__init__.py @@ -1,4 +1,13 @@ # Note: The `open3d` and `stable_baselines3` modules must be imported prior to `gym_gz` +from env_manager.models.configs import ( + RobotData, + SceneData, + SphereObjectData, + TerrainData, +) +from env_manager.models.configs.objects import ObjectRandomizerData +from env_manager.models.configs.robot import RobotRandomizerData +from env_manager.models.configs.scene import PluginsData import open3d # isort:skip import stable_baselines3 # isort:skip @@ -13,16 +22,16 @@ except: pass from os import environ, path -from typing import Dict, Tuple, Any +from typing import Any, Dict, Tuple import numpy as np from ament_index_python.packages import get_package_share_directory from gymnasium.envs.registration import register +from rbs_assets_library import get_world_file from rbs_gym.utils.utils import str2bool from . import tasks -from dataclasses import dataclass, field ###################### # Runtime Entrypoint # @@ -48,15 +57,15 @@ RBS_ENVS_WORLDS_DIR: str = path.join(get_package_share_directory("rbs_gym"), "wo # Presets # ########### # Gravity preset for Earth -GRAVITY_EARTH: Tuple[float, float, float] = (0.0, 0.0, -9.80665) -GRAVITY_EARTH_STD: Tuple[float, float, float] = (0.0, 0.0, 0.0232) +# GRAVITY_EARTH: Tuple[float, float, float] = (0.0, 0.0, -9.80665) +# GRAVITY_EARTH_STD: Tuple[float, float, float] = (0.0, 0.0, 0.0232) ############################ # Additional Configuration # ############################ -BROADCAST_GUI: bool = str2bool( - environ.get("RBS_ENVS_BROADCAST_INTERACTIVE_GUI", default=True) -) +# BROADCAST_GUI: bool = str2bool( +# environ.get("RBS_ENVS_BROADCAST_INTERACTIVE_GUI", default=True) +# ) ######### @@ -84,224 +93,38 @@ REACH_KWARGS: Dict[str, Any] = { REACH_KWARGS_SIM: Dict[str, Any] = { "physics_rate": 1000.0, "real_time_factor": float(np.finfo(np.float32).max), - "world": path.join(RBS_ENVS_WORLDS_DIR, "default.sdf"), + "world": get_world_file("default"), } -@dataclass -class ObjectRandomizerData: - count: int = field(default=0) - random_pose: bool = field(default=False) - random_position: bool = field(default=False) - random_orientation: bool = field(default=False) - random_model: bool = field(default=False) - random_spawn_position_segments: list = field(default_factory=list) - random_spawn_position_update_workspace_centre: bool = field(default=False) - random_spawn_volume: tuple = field(default=(0.5, 0.5, 0.5)) - models_rollouts_num: int = field(default=0) - - -@dataclass -class ObjectData: - name: str = field(default="object") - relative_to: str = field(default="world") - position: tuple = field(default=(0.0, 0.0, 0.0)) - orientation: tuple = field(default=(1.0, 0.0, 0.0, 0.0)) - static: bool = field(default=False) - randomize: ObjectRandomizerData = field(default_factory=ObjectRandomizerData) - - -@dataclass -class PrimitiveObjectData(ObjectData): - collision: bool = field(default=True) - visual: bool = field(default=True) - color: tuple = field(default=(0.8, 0.8, 0.8, 1.0)) - mass: float = field(default=0.1) - - -@dataclass -class SphereObjectData(PrimitiveObjectData): - radius: float = field(default=0.025) - friction: float = field(default=1.0) - - -@dataclass -class PlaneObjectData(PrimitiveObjectData): - size: tuple = field(default=(1.0, 1.0)) - direction: tuple = field(default=(0.0, 0.0, 1.0)) - friction: float = field(default=1.0) - - -@dataclass -class CylinderObjectData(PrimitiveObjectData): - radius: float = field(default=0.025) - length: float = field(default=0.1) - friction: float = field(default=1.0) - - -@dataclass -class BoxObjectData(PrimitiveObjectData): - size: tuple = field(default=(0.05, 0.05, 0.05)) - friction: float = field(default=1.0) - - -@dataclass -class MeshObjectData(ObjectData): - texture: list[float] = field(default_factory=list) - - REACH_RANDOMIZER: str = "rbs_gym.envs.randomizers:ManipulationGazeboEnvRandomizer" -REACH_KWARGS_RANDOMIZER: Dict[str, Any] = { - "gravity": GRAVITY_EARTH, - "gravity_std": GRAVITY_EARTH_STD, - "plugin_scene_broadcaster": BROADCAST_GUI, - "plugin_user_commands": BROADCAST_GUI, - "plugin_sensors_render_engine": "ogre2", - "robot_random_pose": False, - "robot_random_joint_positions": True, # FIXME: - "robot_random_joint_positions_std": 0.2, - "robot_random_joint_positions_above_object_spawn": False, - "robot_random_joint_positions_above_object_spawn_elevation": 0.0, - "robot_random_joint_positions_above_object_spawn_xy_randomness": 0.2, - "terrain_enable": True, - "light_type": "sun", - "light_direction": (0.5, 0.4, -0.2), - "light_random_minmax_elevation": (-0.15, -0.5), - "light_distance": 1000.0, - "light_visual": False, - "light_radius": 25.0, - "light_model_rollouts_num": 1, - "underworld_collision_plane": False, -} -REACH_KWARGS_OBJECT_RANDOMIZER: list = [ - MeshObjectData( - "bishop", - "base_link", - (-0.3, 0.0, 0.0), - randomize=ObjectRandomizerData( - random_position=True, random_spawn_volume=(0.2, 0.5, 0.0) - ), + +SCENE_CONFIGURATION: SceneData = SceneData( + physics_rollouts_num=0, + robot=RobotData( + name="rbs_arm", + joint_positioins=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + with_gripper=True, + gripper_jont_positions=[0], + randomizer=RobotRandomizerData(joint_positions=True), ), - SphereObjectData( - "sphere", - "base_link", - (-0.3, 0.0, 0.3), - mass=1.0, - color=(0.0, 0.0, 1.0, 1.0), - # randomize=ObjectRandomizerData( - # random_pose=True, random_spawn_volume=(0.2, 0.2, 0.0) - # ), + objects=[ + SphereObjectData( + name="sphere", + relative_to="base_link", + position=(0.0, 0.4, 1.0), + static=True, + collision=False, + color=(0.0, 1.0, 0.0, 0.8), + randomize=ObjectRandomizerData(random_pose=True, models_rollouts_num=2), + ) + ], + plugins=PluginsData( + scene_broadcaster=True, user_commands=True, fts_broadcaster=True ), -] -# REACH_KWARGS_RANDOMIZER_CAMERA: Dict[str, Any] = { -# "camera_enable": True, -# "camera_width": 64, -# "camera_height": 64, -# "camera_update_rate": 1.2 * REACH_KWARGS["agent_rate"], -# "camera_horizontal_fov": np.pi / 3.0, -# "camera_vertical_fov": np.pi / 3.0, -# "camera_noise_mean": 0.0, -# "camera_noise_stddev": 0.001, -# "camera_relative_to": "base_link", -# "camera_spawn_position": (0.85, -0.4, 0.45), -# "camera_spawn_quat_xyzw": (-0.0402991, -0.0166924, 0.9230002, 0.3823192), -# "camera_random_pose_rollouts_num": 0, -# "camera_random_pose_mode": "orbit", -# "camera_random_pose_orbit_distance": 1.0, -# "camera_random_pose_orbit_height_range": (0.1, 0.7), -# "camera_random_pose_orbit_ignore_arc_behind_robot": np.pi / 8, -# "camera_random_pose_select_position_options": [], -# "camera_random_pose_focal_point_z_offset": 0.0, -# } +) -@dataclass -class CameraData: - name: str | None = None - enable: bool = field(default=True) - type: str = field(default="rgbd_camera") - relative_to: str = field(default="base_link") - - width: int = field(default=128) - height: int = field(default=128) - image_format: str = field(default="R8G8B8") - update_rate: int = field(default=10) - horizontal_fov: float = field(default=np.pi / 3.0) - vertical_fov: float = field(default=np.pi / 3.0) - - clip_color: tuple[float, float] = field(default=(0.01, 1000.0)) - clip_depth: tuple[float, float] = field(default=(0.05, 10.0)) - - noise_mean: float | None = None - noise_stddev: float | None = None - - publish_color: bool = field(default=False) - publish_depth: bool = field(default=False) - publish_points: bool = field(default=False) - - spawn_position: tuple[float, float, float] = field(default=(0, 0, 1)) - spawn_quat_xyzw: tuple[float, float, float, float] = field( - default=(0, 0.70710678118, 0, 0.70710678118) - ) - - random_pose_rollouts_num: int = field(default=1) - random_pose_mode: str = field(default="orbit") - random_pose_orbit_distance: float = field(default=1.0) - random_pose_orbit_height_range: tuple[float, float] = field(default=(0.1, 0.7)) - random_pose_orbit_ignore_arc_behind_robot: float = field(default=np.pi / 8) - random_pose_select_position_options: list[tuple[float, float, float]] = field( - default_factory=list - ) - random_pose_focal_point_z_offset: float = field(default=0.0) - - -REACH_KWARGS_RANDOMIZER_CAMERAS: list[CameraData] = [ - CameraData( - name="inner_robot_camera", - enable=True, - type="rgbd_camera", - relative_to="rbs_gripper_rot_base_link", - width=320, - height=240, - update_rate=1.2 * REACH_KWARGS["agent_rate"], - horizontal_fov=np.pi / 3.0, - vertical_fov=np.pi / 3.0, - noise_mean=0.0, - publish_color=True, - noise_stddev=0.001, - spawn_position=(-0.02, 0.0, 0.03), - spawn_quat_xyzw=(0.0, -0.707, 0.0, 0.707), - random_pose_mode="orbit", - random_pose_rollouts_num=0, - random_pose_orbit_distance=1.0, - random_pose_orbit_height_range=(0.1, 0.7), - random_pose_focal_point_z_offset=0.0, - random_pose_orbit_ignore_arc_behind_robot=np.pi / 8, - ), - CameraData( - name="outer_robot_camera", - enable=True, - type="rgbd_camera", - relative_to="base_link", - width=320, - height=240, - update_rate=1.2 * REACH_KWARGS["agent_rate"], - publish_color=True, - horizontal_fov=np.pi / 3.0, - vertical_fov=np.pi / 3.0, - noise_mean=0.0, - noise_stddev=0.001, - spawn_position=(-0.2, 1.0, 1.0), - spawn_quat_xyzw=(0.183, 0.183, -0.683, 0.683), - random_pose_mode="orbit", - random_pose_rollouts_num=0, - random_pose_orbit_distance=1.0, - random_pose_orbit_height_range=(0.1, 0.7), - random_pose_focal_point_z_offset=0.0, - random_pose_orbit_ignore_arc_behind_robot=np.pi / 8, - ), -] - # Task register( id="Reach-v0", @@ -336,12 +159,7 @@ register( id="Reach-Gazebo-v0", entry_point=REACH_RANDOMIZER, max_episode_steps=REACH_MAX_EPISODE_STEPS, - kwargs={ - "env": "Reach-v0", - **REACH_KWARGS_SIM, - **REACH_KWARGS_RANDOMIZER, - "object_args": REACH_KWARGS_OBJECT_RANDOMIZER, - }, + kwargs={"env": "Reach-v0", **REACH_KWARGS_SIM, "scene_args": SCENE_CONFIGURATION}, ) register( id="Reach-ColorImage-Gazebo-v0", @@ -350,9 +168,7 @@ register( kwargs={ "env": "Reach-ColorImage-v0", **REACH_KWARGS_SIM, - **REACH_KWARGS_RANDOMIZER, - "camera_args": REACH_KWARGS_RANDOMIZER_CAMERAS, - "object_args": REACH_KWARGS_OBJECT_RANDOMIZER, + "scene_args": SCENE_CONFIGURATION, }, ) register( @@ -362,8 +178,6 @@ register( kwargs={ "env": "Reach-DepthImage-v0", **REACH_KWARGS_SIM, - **REACH_KWARGS_RANDOMIZER, - "camera_args": REACH_KWARGS_RANDOMIZER_CAMERAS, - "object_args": REACH_KWARGS_OBJECT_RANDOMIZER, + "scene_args": SCENE_CONFIGURATION, }, ) diff --git a/env_manager/rbs_gym/rbs_gym/envs/control/grippper_controller.py b/env_manager/rbs_gym/rbs_gym/envs/control/grippper_controller.py index 88ed6cc..79a9e2a 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/control/grippper_controller.py +++ b/env_manager/rbs_gym/rbs_gym/envs/control/grippper_controller.py @@ -14,6 +14,7 @@ class GripperController: self._open_position = open_position self._close_position = close_position self._max_effort = max_effort + self.is_executed = False def open(self): self.send_goal(self._open_position) @@ -44,3 +45,6 @@ class GripperController: result = future.result().result self.get_logger().info(f"Gripper position: {result.position}") + def wait_until_executed(self): + while not self.is_executed: + pass diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/lights/__init__.py b/env_manager/rbs_gym/rbs_gym/envs/models/lights/__init__.py deleted file mode 100644 index 74163b3..0000000 --- a/env_manager/rbs_gym/rbs_gym/envs/models/lights/__init__.py +++ /dev/null @@ -1,20 +0,0 @@ -from gym_gz.scenario.model_wrapper import ModelWrapper - -from .random_sun import RandomSun -from .sun import Sun - - -def get_light_model_class(light_type: str) -> ModelWrapper: - # TODO: Refactor into enum - - if "sun" == light_type: - return Sun - elif "random_sun" == light_type: - return RandomSun - - -def is_light_type_randomizable(light_type: str) -> bool: - - if "random_sun" == light_type: - return True - return False diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/robots/__init__.py b/env_manager/rbs_gym/rbs_gym/envs/models/robots/__init__.py deleted file mode 100644 index 2a117fa..0000000 --- a/env_manager/rbs_gym/rbs_gym/envs/models/robots/__init__.py +++ /dev/null @@ -1,17 +0,0 @@ -from gym_gz.scenario.model_wrapper import ModelWrapper - -from .rbs_arm import RbsArm -# from .panda import Panda - -# TODO: When adding new a robot, create abstract classes to simplify such process - - -def get_robot_model_class(robot_model: str) -> type[RbsArm]: - # TODO: Refactor into enum - - if "rbs_arm" == robot_model: - return RbsArm - else: - return RbsArm - # elif "panda" == robot_model: - # return Panda diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/terrains/__init__.py b/env_manager/rbs_gym/rbs_gym/envs/models/terrains/__init__.py deleted file mode 100644 index 69d8435..0000000 --- a/env_manager/rbs_gym/rbs_gym/envs/models/terrains/__init__.py +++ /dev/null @@ -1,29 +0,0 @@ -from gym_gz.scenario.model_wrapper import ModelWrapper - -from .ground import Ground -from .lunar_heightmap import LunarHeightmap -from .lunar_surface import LunarSurface -from .random_ground import RandomGround -from .random_lunar_surface import RandomLunarSurface - - -def get_terrain_model_class(terrain_type: str) -> type[ModelWrapper]: - # TODO: Refactor into enum - - if "flat" == terrain_type: - return Ground - elif "random_flat" == terrain_type: - return RandomGround - elif "lunar_heightmap" == terrain_type: - return LunarHeightmap - elif "lunar_surface" == terrain_type: - return LunarSurface - elif "random_lunar_surface" == terrain_type: - return RandomLunarSurface - else: - raise AttributeError(f"Unsupported terrain [{terrain_type}]") - - -def is_terrain_type_randomizable(terrain_type: str) -> bool: - - return "random_flat" == terrain_type or "random_lunar_surface" == terrain_type diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/terrains/lunar_heightmap.py b/env_manager/rbs_gym/rbs_gym/envs/models/terrains/lunar_heightmap.py deleted file mode 100644 index 1327c9e..0000000 --- a/env_manager/rbs_gym/rbs_gym/envs/models/terrains/lunar_heightmap.py +++ /dev/null @@ -1,52 +0,0 @@ -from typing import List - -from gym_gz.scenario import model_with_file, model_wrapper -from gym_gz.utils.scenario import get_unique_model_name -from scenario import core as scenario -from scenario import gazebo as scenario_gazebo - - -class LunarHeightmap(model_wrapper.ModelWrapper, model_with_file.ModelWithFile): - def __init__( - self, - world: scenario.World, - name: str = "lunar_heightmap", - position: List[float] = (0, 0, 0), - orientation: List[float] = (1, 0, 0, 0), - model_file: str = None, - use_fuel: bool = False, - **kwargs, - ): - - # Allow passing of custom model file as an argument - if model_file is None: - model_file = self.get_model_file(fuel=use_fuel) - - # Get a unique model name - model_name = get_unique_model_name(world, name) - - # Setup initial pose - initial_pose = scenario.Pose(position, orientation) - - # Insert the model - ok_model = world.to_gazebo().insert_model_from_file( - model_file, initial_pose, model_name - ) - if not ok_model: - raise RuntimeError("Failed to insert " + model_name) - - # Get the model - model = world.get_model(model_name) - - # Initialize base class - super().__init__(model=model) - - @classmethod - def get_model_file(self, fuel: bool = False) -> str: - if fuel: - raise NotImplementedError - return scenario_gazebo.get_model_file_from_fuel( - "https://fuel.ignitionrobotics.org/1.0/AndrejOrsula/models/lunar_heightmap" - ) - else: - return "lunar_heightmap" diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/terrains/lunar_surface.py b/env_manager/rbs_gym/rbs_gym/envs/models/terrains/lunar_surface.py deleted file mode 100644 index 100a65f..0000000 --- a/env_manager/rbs_gym/rbs_gym/envs/models/terrains/lunar_surface.py +++ /dev/null @@ -1,53 +0,0 @@ -from typing import List - -from gym_gz.scenario import model_with_file, model_wrapper -from gym_gz.utils.scenario import get_unique_model_name -from scenario import core as scenario -from scenario import gazebo as scenario_gazebo - - -class LunarSurface(model_wrapper.ModelWrapper, model_with_file.ModelWithFile): - def __init__( - self, - world: scenario.World, - name: str = "lunar_surface", - position: List[float] = (0, 0, 0), - orientation: List[float] = (1, 0, 0, 0), - model_file: str = None, - use_fuel: bool = False, - variant: str = "tycho", - **kwargs, - ): - - # Allow passing of custom model file as an argument - if model_file is None: - model_file = self.get_model_file(fuel=use_fuel, variant=variant) - - # Get a unique model name - model_name = get_unique_model_name(world, name) - - # Setup initial pose - initial_pose = scenario.Pose(position, orientation) - - # Insert the model - ok_model = world.to_gazebo().insert_model_from_file( - model_file, initial_pose, model_name - ) - if not ok_model: - raise RuntimeError("Failed to insert " + model_name) - - # Get the model - model = world.get_model(model_name) - - # Initialize base class - super().__init__(model=model) - - @classmethod - def get_model_file(self, fuel: bool = False, variant: str = "tycho") -> str: - if fuel: - raise NotImplementedError - return scenario_gazebo.get_model_file_from_fuel( - f"https://fuel.ignitionrobotics.org/1.0/AndrejOrsula/models/lunar_surface_{variant}" - ) - else: - return f"lunar_surface_{variant}" diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/terrains/random_ground.py b/env_manager/rbs_gym/rbs_gym/envs/models/terrains/random_ground.py deleted file mode 100644 index 45a9957..0000000 --- a/env_manager/rbs_gym/rbs_gym/envs/models/terrains/random_ground.py +++ /dev/null @@ -1,135 +0,0 @@ -import os -from typing import List, Optional - -import numpy as np -from gym_gz.scenario import model_wrapper -from gym_gz.utils.scenario import get_unique_model_name -from numpy.random import RandomState -from scenario import core as scenario - - -class RandomGround(model_wrapper.ModelWrapper): - def __init__( - self, - world: scenario.World, - name: str = "random_ground", - position: List[float] = (0, 0, 0), - orientation: List[float] = (1, 0, 0, 0), - size: List[float] = (1.0, 1.0), - collision_thickness: float = 0.05, - friction: float = 5.0, - texture_dir: Optional[str] = None, - np_random: Optional[RandomState] = None, - **kwargs, - ): - - if np_random is None: - np_random = np.random.default_rng() - - # Get a unique model name - model_name = get_unique_model_name(world, name) - - # Initial pose - initial_pose = scenario.Pose(position, orientation) - - # Get textures from ENV variable if not directly specified - if not texture_dir: - texture_dir = os.environ.get("TEXTURE_DIRS", default="") - - # Find random PBR texture - albedo_map = None - normal_map = None - roughness_map = None - metalness_map = None - if texture_dir: - if ":" in texture_dir: - textures = [] - for d in texture_dir.split(":"): - textures.extend([os.path.join(d, f) for f in os.listdir(d)]) - else: - # Get list of the available textures - textures = os.listdir(texture_dir) - - # Choose a random texture from these - random_texture_dir = str(np_random.choice(textures)) - - # List all files - texture_files = os.listdir(random_texture_dir) - - # Extract the appropriate files - for texture in texture_files: - texture_lower = texture.lower() - if "color" in texture_lower or "albedo" in texture_lower: - albedo_map = os.path.join(random_texture_dir, texture) - elif "normal" in texture_lower: - normal_map = os.path.join(random_texture_dir, texture) - elif "roughness" in texture_lower: - roughness_map = os.path.join(random_texture_dir, texture) - elif "specular" in texture_lower or "metalness" in texture_lower: - metalness_map = os.path.join(random_texture_dir, texture) - - # Create SDF string for the model - sdf = f""" - - true - - - - - 0 0 1 - {size[0]} {size[1]} - - - - - - {friction} - {friction} - 0 0 0 - 0.0 - 0.0 - - - - - - - - 0 0 1 - {size[0]} {size[1]} - - - - 1 1 1 1 - 1 1 1 1 - 1 1 1 1 - - - {"%s" - % albedo_map if albedo_map is not None else ""} - {"%s" - % normal_map if normal_map is not None else ""} - {"%s" - % roughness_map if roughness_map is not None else ""} - {"%s" - % metalness_map if metalness_map is not None else ""} - - - - - - - """ - - # Insert the model - ok_model = world.to_gazebo().insert_model_from_string( - sdf, initial_pose, model_name - ) - if not ok_model: - raise RuntimeError("Failed to insert " + model_name) - - # Get the model - model = world.get_model(model_name) - - # Initialize base class - model_wrapper.ModelWrapper.__init__(self, model=model) diff --git a/env_manager/rbs_gym/rbs_gym/envs/models/terrains/random_lunar_surface.py b/env_manager/rbs_gym/rbs_gym/envs/models/terrains/random_lunar_surface.py deleted file mode 100644 index a3646eb..0000000 --- a/env_manager/rbs_gym/rbs_gym/envs/models/terrains/random_lunar_surface.py +++ /dev/null @@ -1,57 +0,0 @@ -import os -from typing import List, Optional, Tuple - -import numpy as np -from gym_gz.scenario import model_wrapper -from gym_gz.utils.scenario import get_unique_model_name -from numpy.random import RandomState -from scenario import core as scenario - - -class RandomLunarSurface(model_wrapper.ModelWrapper): - def __init__( - self, - world: scenario.World, - name: str = "lunar_surface", - position: List[float] = (0, 0, 0), - orientation: List[float] = (1, 0, 0, 0), - models_dir: Optional[str] = None, - np_random: Optional[RandomState] = None, - **kwargs, - ): - - if np_random is None: - np_random = np.random.default_rng() - - # Get a unique model name - model_name = get_unique_model_name(world, name) - - # Setup initial pose - initial_pose = scenario.Pose(position, orientation) - - # Get path to all lunar surface models - if not models_dir: - models_dir = os.environ.get("SDF_PATH_LUNAR_SURFACE", default="") - - # Make sure the path exists - if not os.path.exists(models_dir): - raise ValueError( - f"Invalid path '{models_dir}' pointed by 'SDF_PATH_LUNAR_SURFACE' environment variable." - ) - - # Select a single model at random - model_dir = np_random.choice(os.listdir(models_dir)) - sdf_filepath = os.path.join(model_dir, "model.sdf") - - # Insert the model - ok_model = world.to_gazebo().insert_model_from_file( - sdf_filepath, initial_pose, model_name - ) - if not ok_model: - raise RuntimeError("Failed to insert " + model_name) - - # Get the model - model = world.get_model(model_name) - - # Initialize base class - model_wrapper.ModelWrapper.__init__(self, model=model) diff --git a/env_manager/rbs_gym/rbs_gym/envs/randomizers/manipulation.py b/env_manager/rbs_gym/rbs_gym/envs/randomizers/manipulation.py index 5de5e3c..5777a2c 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/randomizers/manipulation.py +++ b/env_manager/rbs_gym/rbs_gym/envs/randomizers/manipulation.py @@ -1,33 +1,17 @@ import abc -from typing import List, Tuple -from gym_gz.scenario.model_wrapper import ModelWrapper -import numpy as np +from env_manager.models.configs import ( + SceneData, +) +from env_manager.scene import Scene +from env_manager.utils.logging import set_log_level from gym_gz import randomizers from gym_gz.randomizers import gazebo_env_randomizer from gym_gz.randomizers.gazebo_env_randomizer import MakeEnvCallable from scenario import gazebo as scenario -from scipy.spatial import distance -from scipy.spatial.transform import Rotation -from dataclasses import asdict +from scenario.bindings.gazebo import GazeboSimulator -from rbs_gym.envs import ( - BoxObjectData, - CylinderObjectData, - MeshObjectData, - PlaneObjectData, - PrimitiveObjectData, - SphereObjectData, - models, - tasks, -) -from rbs_gym.envs.utils.conversions import quat_to_wxyz -from rbs_gym.envs.utils.gazebo import ( - transform_move_to_model_pose, - transform_move_to_model_position, -) -from rbs_gym.envs.utils.logging import set_log_level -from rbs_gym.envs import CameraData, ObjectData +from rbs_gym.envs import tasks SupportedTasks = tasks.Reach | tasks.ReachColorImage | tasks.ReachDepthImage @@ -50,87 +34,21 @@ class ManipulationGazeboEnvRandomizer( def __init__( self, env: MakeEnvCallable, - # Physics - physics_rollouts_num: int = 0, - gravity: Tuple[float, float, float] = (0.0, 0.0, -9.80665), - gravity_std: Tuple[float, float, float] = (0.0, 0.0, 0.0232), - # World plugins - plugin_scene_broadcaster: bool = False, - plugin_user_commands: bool = False, - plugin_sensors_render_engine: str = "ogre2", - # Robot - robot_spawn_position: Tuple[float, float, float] = (0.0, 0.0, 0.0), - robot_spawn_quat_xyzw: Tuple[float, float, float, float] = ( - 0.0, - 0.0, - 0.0, - 1.0, - ), - robot_random_pose: bool = False, - robot_random_spawn_volume: Tuple[float, float, float] = (1.0, 1.0, 0.0), - robot_random_joint_positions: bool = False, - robot_random_joint_positions_std: float = 0.1, - robot_random_joint_positions_above_object_spawn: bool = False, - robot_random_joint_positions_above_object_spawn_elevation: float = 0.2, - robot_random_joint_positions_above_object_spawn_xy_randomness: float = 0.2, - # Camera # - camera_args: list[CameraData] = [], - # Terrain - terrain_enable: bool = True, - terrain_type: str = "flat", - terrain_spawn_position: Tuple[float, float, float] = (0, 0, 0), - terrain_spawn_quat_xyzw: Tuple[float, float, float, float] = (0, 0, 0, 1), - terrain_size: Tuple[float, float] = (1.5, 1.5), - terrain_model_rollouts_num: int = 1, - # Light - light_enable: bool = True, - light_type: str = "sun", - light_direction: Tuple[float, float, float] = (0.5, -0.25, -0.75), - light_random_minmax_elevation: Tuple[float, float] = (-0.15, -0.65), - light_color: Tuple[float, float, float, float] = (1.0, 1.0, 1.0, 1.0), - light_distance: float = 1000.0, - light_visual: bool = True, - light_radius: float = 25.0, - light_model_rollouts_num: int = 1, - # Objects - object_args: list = [], - # object_enable: bool = True, - # object_type: str = "box", - # objects_relative_to: str = "base_link", - # object_static: bool = False, - # object_collision: bool = True, - # object_visual: bool = True, - # object_color: Tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0), - # object_dimensions: List[float] = [0.05, 0.05, 0.05], - # object_mass: float = 0.1, - # object_count: int = 1, - # object_randomize_count: bool = False, - # object_spawn_position: Tuple[float, float, float] = (0.0, 0.0, 0.0), - # object_random_pose: bool = True, - # object_random_spawn_position_segments: List[Tuple[float, float, float]] = [], - # object_random_spawn_position_update_workspace_centre: bool = False, - # object_random_spawn_volume: Tuple[float, float, float] = (0.5, 0.5, 0.5), - # object_models_rollouts_num: int = 1, - # Collision plane below terrain - underworld_collision_plane: bool = True, - boundary_collision_walls: bool = False, - collision_plane_offset: float = 1.0, - # Visual debugging - visualise_workspace: bool = False, - visualise_spawn_volume: bool = False, + scene_args: SceneData = SceneData(), **kwargs, ): + self._scene_data = scene_args # TODO (a lot of work): Implement proper physics randomization. - if physics_rollouts_num != 0: + if scene_args.physics_rollouts_num != 0: raise TypeError( "Proper physics randomization at each reset is not yet implemented. Please set `physics_rollouts_num=0`." ) # Update kwargs before passing them to the task constructor (some tasks might need them) # TODO: update logic when will need choose between cameras - cameras = [] - for camera in camera_args: - camera_info = {} + cameras: list[dict[str, str | int]] = [] + for camera in self._scene_data.camera: + camera_info: dict[str, str | int] = {} camera_info["name"] = camera.name camera_info["type"] = camera.type camera_info["width"] = camera.width @@ -142,109 +60,14 @@ class ManipulationGazeboEnvRandomizer( # Initialize base classes randomizers.abc.TaskRandomizer.__init__(self) randomizers.abc.PhysicsRandomizer.__init__( - self, randomize_after_rollouts_num=physics_rollouts_num + self, randomize_after_rollouts_num=scene_args.physics_rollouts_num ) gazebo_env_randomizer.GazeboEnvRandomizer.__init__( self, env=env, physics_randomizer=self, **kwargs ) - # Store parameters for later use # - # Physics - self._gravity = gravity - self._gravity_std = gravity_std - - # World plugins - self._plugin_scene_broadcaster = plugin_scene_broadcaster - self._plugin_user_commands = plugin_user_commands - self._plugin_sensors_render_engine = plugin_sensors_render_engine - - # Robot - self._robot_spawn_position = robot_spawn_position - self._robot_spawn_quat_xyzw = robot_spawn_quat_xyzw - self._robot_random_pose = robot_random_pose - self._robot_random_spawn_volume = robot_random_spawn_volume - self._robot_random_joint_positions = robot_random_joint_positions - self._robot_random_joint_positions_std = robot_random_joint_positions_std - self._robot_random_joint_positions_above_object_spawn = ( - robot_random_joint_positions_above_object_spawn - ) - self._robot_random_joint_positions_above_object_spawn_elevation = ( - robot_random_joint_positions_above_object_spawn_elevation - ) - self._robot_random_joint_positions_above_object_spawn_xy_randomness = ( - robot_random_joint_positions_above_object_spawn_xy_randomness - ) - - # Camera - self._cameras = camera_args - self._camera_enable = False - - # FT sensor - self._ft_sensor_enable = True # TODO: add external param - - # Terrain - self._terrain_enable = terrain_enable - self._terrain_spawn_position = terrain_spawn_position - self._terrain_spawn_quat_xyzw = terrain_spawn_quat_xyzw - self._terrain_size = terrain_size - self._terrain_model_rollouts_num = terrain_model_rollouts_num - - # Light - self._light_enable = light_enable - self._light_direction = light_direction - self._light_random_minmax_elevation = light_random_minmax_elevation - self._light_color = light_color - self._light_distance = light_distance - self._light_visual = light_visual - self._light_radius = light_radius - self._light_model_rollouts_num = light_model_rollouts_num - - # Objects - self._objects = object_args - - # Collision plane beneath the terrain (prevent objects from falling forever) - self._underworld_collision_plane = underworld_collision_plane - self._boundary_collision_walls = boundary_collision_walls - self._collision_plane_offset = collision_plane_offset - if self._collision_plane_offset < 0.0: - self._collision_plane_offset *= -1.0 - - # Visual debugging - self._visualise_workspace = visualise_workspace - self._visualise_spawn_volume = visualise_spawn_volume - - # Derived variables # - # Model classes and whether these are randomizable - self.__terrain_model_class = models.get_terrain_model_class(terrain_type) - self.__is_terrain_type_randomizable = models.is_terrain_type_randomizable( - terrain_type - ) - self.__light_model_class = models.get_light_model_class(light_type) - self.__is_light_type_randomizable = models.is_light_type_randomizable( - light_type - ) - # self.__object_model_classes: list[ModelWrapper] = [] - # self.__is_object_type_randomizable: list[bool] = [] - # self.__objects_max_count: list[int] = [] - - # Variable initialisation # - # Rollout counters - self.__objects_unique_names: dict[str, list[str]] = {} - self.__camera_pose_rollout_counter: int = 0 - self.__terrain_model_rollout_counter = terrain_model_rollouts_num - self.__light_model_rollout_counter = light_model_rollouts_num - self.__object_models_rollout_counter: int = 0 - - # Flag that determines whether the camera is attached to the robot via detachable joint - self.__is_camera_attached = False - - # Flag that determines whether environment has already been initialised self.__env_initialised = False - # Dict to keep track of set object positions - without stepping (faster than lookup through gazebo) - # It is used to make sure that objects are not spawned inside each other - self.__object_positions = {} - ########################## # PhysicsRandomizer impl # ########################## @@ -281,13 +104,10 @@ class ManipulationGazeboEnvRandomizer( # Get gazebo instance associated with the task if "gazebo" not in kwargs: raise ValueError("Randomizer does not have access to the gazebo interface") - gazebo = kwargs["gazebo"] - - # Perform internal overrides of parameters - self.internal_overrides(task=task) - - # Perform external overrides (e.g. from curriculum) - self.external_overrides(task=task) + if isinstance(kwargs["gazebo"], GazeboSimulator): + gazebo: GazeboSimulator = kwargs["gazebo"] + else: + raise RuntimeError("Provide GazeboSimulator instance") # Initialise the environment on the first iteration if not self.__env_initialised: @@ -297,8 +117,7 @@ class ManipulationGazeboEnvRandomizer( # Perform pre-randomization steps self.pre_randomization(task=task) - # Randomize models if needed - self.randomize_models(task=task, gazebo=gazebo) + self._scene.reset_scene() # Perform post-randomization steps # TODO: Something in post-randomization causes GUI client to freeze during reset (the simulation server still works fine) @@ -314,1129 +133,19 @@ class ManipulationGazeboEnvRandomizer( Initialise an instance of the environment before the very first iteration """ + self._scene = Scene( + task, + gazebo, + self._scene_data, + task.get_parameter("robot_description").get_parameter_value().string_value, + ) + + # Init Scene for task + task.scene = self._scene # Set log level for (Gym) Ignition set_log_level(log_level=task.get_logger().get_effective_level().name) - # Execute a paused run for the first time before initialising everything - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - # Offset some parameters by the robot base offset - for object in self._objects: - object.position = ( - object.position[0], - object.position[1], - object.position[2] + task.robot_model_class.BASE_LINK_Z_OFFSET, - ) - - for camera in self._cameras: - camera.random_pose_focal_point_z_offset += ( - task.robot_model_class.BASE_LINK_Z_OFFSET - ) - - # Initialise custom physics preset - self.init_physics_preset(task=task) - - # Insert world plugins needed by the task or selected by user - self.init_world_plugins(task=task, gazebo=gazebo) - - # Initialise all models that are persustent throughout the entire training - self.init_models(task=task, gazebo=gazebo) - - def init_world_plugins( - self, task: SupportedTasks, gazebo: scenario.GazeboSimulator - ): - # SceneBroadcaster - if self._plugin_scene_broadcaster: - if not gazebo.scene_broadcaster_active( - task.substitute_special_frame("world") - ): - task.get_logger().info( - "Inserting world plugins for broadcasting scene to GUI clients..." - ) - task.world.to_gazebo().insert_world_plugin( - "ignition-gazebo-scene-broadcaster-system", - "ignition::gazebo::systems::SceneBroadcaster", - ) - - # Execute a paused run to process world plugin insertion - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - # UserCommands - if self._plugin_user_commands: - task.get_logger().info("Inserting world plugins to enable user commands...") - task.world.to_gazebo().insert_world_plugin( - "ignition-gazebo-user-commands-system", - "ignition::gazebo::systems::UserCommands", - ) - - # Execute a paused run to process world plugin insertion - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - # Sensors - for camera in self._cameras: - if camera.enable: - self._camera_enable = True - - if self._camera_enable: - task.get_logger().info( - f"Inserting world plugins for sensors with {self._plugin_sensors_render_engine} rendering engine..." - ) - task.world.to_gazebo().insert_world_plugin( - "libignition-gazebo-sensors-system.so", - "ignition::gazebo::systems::Sensors", - "" - f"{self._plugin_sensors_render_engine}" - "", - ) - - if self._ft_sensor_enable: - task.world.to_gazebo().insert_world_plugin( - "ignition-gazebo-forcetorque-system", - "ignition::gazebo::systems::ForceTorque", - ) - - # Execute a paused run to process world plugin insertion - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - def init_models(self, task: SupportedTasks, gazebo: scenario.GazeboSimulator): - """ - Initialise all models that are persistent throughout the entire training (they do not need to be re-spawned). - All other models that need to be re-spawned on each reset are ignored here - """ - - model_names = task.world.to_gazebo().model_names() - if len(model_names) > 0: - task.get_logger().warn( - "Before initialisation, the world already contains the following models:" - f"\n\t{model_names}" - ) - - # Insert default light if enabled and light randomization is disabled - if self._light_enable and not self.__light_model_randomizer_enabled(): - task.get_logger().info("Inserting default light into the environment...") - self.add_default_light(task=task, gazebo=gazebo) - - # Insert default terrain if enabled and terrain randomization is disabled - if self._terrain_enable and not self.__terrain_model_randomizer_enabled(): - task.get_logger().info("Inserting default terrain into the environment...") - self.add_default_terrain(task=task, gazebo=gazebo) - - # Insert robot - task.get_logger().info("Inserting robot into the environment...") - self.add_robot(task=task, gazebo=gazebo) - - # Insert camera - for camera in self._cameras: - if camera.enable: - task.get_logger().info( - f"Inserting [{camera.name}] into the environment ..." - ) - self.add_camera(task=task, gazebo=gazebo, camera=camera) - - # Insert default object if enabled and object randomization is disabled - for object in self._objects: - # TODO: support for random models - # this code is not executable for objects with random model - task.get_logger().info( - f"Inserting default object [{object.name}] to the environment" - ) - self.add_default_objects(task=task, gazebo=gazebo, obj=object) - - # TODO (medium): Consider replacing invisible planes with removal of all objects that are too low along a certain axis - # Insert invisible plane below the terrain to prevent objects from falling into the abyss and causing physics errors - if self._underworld_collision_plane: - task.get_logger().info( - "Inserting invisible plane below the terrain into the environment..." - ) - self.add_underworld_collision_plane(task=task, gazebo=gazebo) - # Insert invisible planes around the environment to prevent objects from going into the abyss and causing physics errors - if self._boundary_collision_walls: - task.get_logger().info( - "Inserting invisible planes around the terrain into the environment..." - ) - self.add_boundary_collision_walls(task=task, gazebo=gazebo) - - # TODO: Visualization must follow the robot - consider using RViZ geometry markers instead of this approach - # Visualise volumes in GUI if desired - if self._visualise_workspace: - self.visualise_workspace(task=task, gazebo=gazebo) - if self._visualise_spawn_volume: - self.visualise_spawn_volume(task=task, gazebo=gazebo) - - def add_robot(self, task: SupportedTasks, gazebo: scenario.GazeboSimulator): - """ - Configure and insert robot into the simulation - """ - - # Instantiate robot class based on the selected model - self.robot = task.robot_model_class( - world=task.world, - name=task.robot_name, - prefix=task.robot_prefix, - position=self._robot_spawn_position, - orientation=quat_to_wxyz(self._robot_spawn_quat_xyzw), - initial_arm_joint_positions=task.initial_arm_joint_positions, - initial_gripper_joint_positions=task.initial_gripper_joint_positions, - ) - - # The desired name is passed as arg on creation, however, a different name might be selected to be unique - # Therefore, return the name back to the task - task.robot_name = self.robot.name() - - # Enable contact detection for all gripper links (fingers) - robot_gazebo = self.robot.to_gazebo() - # for gripper_link_name in self.robot.gripper_link_names: - # finger = robot_gazebo.get_link(link_name=gripper_link_name) - # finger.enable_contact_detection(True) - - for arm_link_name in self.robot.arm_link_names: - link = robot_gazebo.get_link(link_name=arm_link_name) - link.enable_contact_detection(True) - - # # If mobile, enable contact detection also for the wheels - # if self.robot.is_mobile: - # for wheel_link_name in self.robot.wheel_link_names: - # wheel = robot_gazebo.get_link(link_name=wheel_link_name) - # wheel.enable_contact_detection(True) - - # Execute a paused run to process robot model insertion - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - # Reset robot joints to the defaults - self.reset_robot_joint_positions( - task=task, gazebo=gazebo, above_object_spawn=False, randomize=False - ) - - def add_camera( - self, task: SupportedTasks, gazebo: scenario.GazeboSimulator, camera: CameraData - ): - """ - Configure and insert camera into the simulation. Camera is places with respect to the robot - """ - - if task.world.to_gazebo().name() == camera.relative_to: - camera_position = camera.spawn_position - camera_quat_wxyz = quat_to_wxyz(camera.spawn_quat_xyzw) - else: - # Transform the pose of camera to be with respect to robot - but still represented in world reference frame for insertion into the world - camera_position, camera_quat_wxyz = transform_move_to_model_pose( - world=task.world, - position=camera.spawn_position, - quat=quat_to_wxyz(camera.spawn_quat_xyzw), - target_model=self.robot, - target_link=camera.relative_to, - xyzw=False, - ) - - # Create camera - self.camera_wrapper = models.Camera( - name=camera.name, - world=task.world, - position=camera_position, - orientation=camera_quat_wxyz, - camera_type=camera.type, - width=camera.width, - height=camera.height, - image_format=camera.image_format, - update_rate=camera.update_rate, - horizontal_fov=camera.horizontal_fov, - vertical_fov=camera.vertical_fov, - clip_color=camera.clip_color, - clip_depth=camera.clip_depth, - noise_mean=camera.noise_mean, - noise_stddev=camera.noise_stddev, - ros2_bridge_color=camera.publish_color, - ros2_bridge_depth=camera.publish_depth, - ros2_bridge_points=camera.publish_points, - ) - - # Execute a paused run to process camera model insertion - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - # Attach to robot if needed - if task.world.to_gazebo().name() != camera.relative_to: - if not self.robot.to_gazebo().attach_link( - camera.relative_to, - self.camera_wrapper.name(), - self.camera_wrapper.link_name, - ): - raise Exception("Cannot attach camera link to robot") - - self.__is_camera_attached = True - - # Execute a paused run to process camera link attachment - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - # Broadcast tf - task.tf2_broadcaster.broadcast_tf( - parent_frame_id=camera.relative_to, - child_frame_id=self.camera_wrapper.frame_id, - translation=camera.spawn_position, - rotation=camera.spawn_quat_xyzw, - xyzw=True, - ) - - def add_default_terrain( - self, task: SupportedTasks, gazebo: scenario.GazeboSimulator - ): - """ - Configure and insert default terrain into the simulation - """ - - # Create terrain - self.terrain = self.__terrain_model_class( - world=task.world, - position=self._terrain_spawn_position, - orientation=quat_to_wxyz(self._terrain_spawn_quat_xyzw), - size=self._terrain_size, - np_random=task.np_random, - ) - - # The desired name is passed as arg on creation, however, a different name might be selected to be unique - # Therefore, return the name back to the task - task.terrain_name = self.terrain.name() - - # Enable contact detection - for link_name in self.terrain.link_names(): - link = self.terrain.to_gazebo().get_link(link_name=link_name) - link.enable_contact_detection(True) - - # Execute a paused run to process terrain model insertion - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - def add_default_light(self, task: SupportedTasks, gazebo: scenario.GazeboSimulator): - """ - Configure and insert default light into the simulation - """ - - # Create light - self.light = self.__light_model_class( - world=task.world, - direction=self._light_direction, - minmax_elevation=self._light_random_minmax_elevation, - color=self._light_color, - distance=self._light_distance, - visual=self._light_visual, - radius=self._light_radius, - np_random=task.np_random, - ) - - # Execute a paused run to process light model insertion - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - def add_default_objects( - self, task: SupportedTasks, gazebo: scenario.GazeboSimulator, obj: ObjectData - ): - """ - Configure and insert default object into the simulation - """ - - # TODO: add randomization when spawning - # Insert new models with random pose - # while len(self.task.object_names) < self._object_count: - # if self._object_count > 1: - # object_position, object_quat_wxyz = self.get_random_object_pose( - # task=task, - # centre=self._object_spawn_position, - # volume=self._object_random_spawn_volume, - # ) - # else: - # object_position = self._object_spawn_position - # object_quat_wxyz = (1.0, 0.0, 0.0, 0.0) - # - # if task.world.to_gazebo().name() != self._objects_relative_to: - # # Transform the pose of camera to be with respect to robot - but represented in world reference frame for insertion into the world - # object_position, object_quat_wxyz = transform_move_to_model_pose( - # world=task.world, - # position=object_position, - # quat=object_quat_wxyz, - # target_model=self.robot, - # target_link=self._objects_relative_to, - # xyzw=False, - # ) - try: - obj_dict = asdict(obj) - obj_dict["world"] = task.world - match obj: - case BoxObjectData(): - object_wrapper = models.Box(**obj_dict) - case PlaneObjectData(): - object_wrapper = models.Plane(**obj_dict) - case SphereObjectData(): - object_wrapper = models.Sphere(**obj_dict) - case CylinderObjectData(): - object_wrapper = models.Cylinder(**obj_dict) - case MeshObjectData(): - object_wrapper = models.Mesh(**obj_dict) - case _: - raise NotImplementedError("Type is not supported") - - model_name = object_wrapper.name() - if obj.name not in self.__objects_unique_names: - self.__objects_unique_names[obj.name] = [] - self.__objects_unique_names[obj.name].append(model_name) - - # Expose name of the object for task (append in case of more) - task.object_names.append(model_name) - - # Enable contact detection - for link_name in object_wrapper.link_names(): - link = object_wrapper.to_gazebo().get_link(link_name=link_name) - link.enable_contact_detection(True) - - except Exception as ex: - task.get_logger().warn(f"Model could not be inserted. Reason: {ex}") - - # Execute a paused run to process insertion of object model - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - def add_underworld_collision_plane( - self, task: SupportedTasks, gazebo: scenario.GazeboSimulator - ): - """ - Add an infinitely large collision plane below the terrain in order to prevent object from falling into the abyss forever - """ - - models.Plane( - name="_collision_plane_B", - world=task.world, - position=( - 0.0, - 0.0, - self._terrain_spawn_position[2] - self._collision_plane_offset, - ), - orientation=(1.0, 0.0, 0.0, 0.0), - direction=(0.0, 0.0, 1.0), - visual=False, - collision=True, - friction=1000.0, - ) - - # Execute a paused run to process model insertion of underworld collision plane - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - def add_boundary_collision_walls( - self, task: SupportedTasks, gazebo: scenario.GazeboSimulator - ): - """ - Add an infinitely large collision planes around the terrain in order to prevent object from going into the abyss forever - """ - - models.Plane( - name="_collision_plane_N", - world=task.world, - position=( - self._terrain_spawn_position[0] - + self._terrain_size[0] / 2 - + self._collision_plane_offset, - 0.0, - 0.0, - ), - orientation=(1.0, 0.0, 0.0, 0.0), - direction=(-1.0, 0.0, 0.0), - visual=False, - collision=True, - friction=1000.0, - ) - - models.Plane( - name="_collision_plane_S", - world=task.world, - position=( - self._terrain_spawn_position[0] - - self._terrain_size[0] / 2 - - self._collision_plane_offset, - 0.0, - 0.0, - ), - orientation=(1.0, 0.0, 0.0, 0.0), - direction=(1.0, 0.0, 0.0), - visual=False, - collision=True, - friction=1000.0, - ) - - models.Plane( - name="_collision_plane_E", - world=task.world, - position=( - 0.0, - self._terrain_spawn_position[1] - + self._terrain_size[1] / 2 - + self._collision_plane_offset, - 0.0, - ), - orientation=(1.0, 0.0, 0.0, 0.0), - direction=(0.0, -1.0, 0.0), - visual=False, - collision=True, - friction=1000.0, - ) - - models.Plane( - name="_collision_plane_W", - world=task.world, - position=( - 0.0, - self._terrain_spawn_position[1] - - self._terrain_size[1] / 2 - - self._collision_plane_offset, - 0.0, - ), - orientation=(1.0, 0.0, 0.0, 0.0), - direction=(0.0, 1.0, 0.0), - visual=False, - collision=True, - friction=1000.0, - ) - - # Execute a paused run to process model insertion of boundary walls - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - # Randomization # - def randomize_models(self, task: SupportedTasks, gazebo: scenario.GazeboSimulator): - """ - Randomize models if needed - """ - - # Randomize light if needed - if self._light_enable and self._light_model_expired(): - self.randomize_light(task=task, gazebo=gazebo) - - # Randomize robot model pose if needed - if self.robot.is_mobile: - self.reset_robot_pose( - task=task, gazebo=gazebo, randomize=self._robot_random_pose - ) - - # Reset/randomize robot joint positions - self.reset_robot_joint_positions( - task=task, - gazebo=gazebo, - above_object_spawn=self._robot_random_joint_positions_above_object_spawn, - randomize=self._robot_random_joint_positions, - ) - - # Randomize cameras if needed - for camera in self._cameras: - if camera.enable and self._camera_pose_expired(camera): - self.randomize_camera_pose( - task=task, - gazebo=gazebo, - mode=camera.random_pose_mode, - camera=camera, - ) - - # Randomize objects if needed - # NOTE: No need to randomize pose of new models because they are already spawned randomly - randomized_objects = set() - if self._objects: - self.__object_positions.clear() - for obj in self._objects: - if obj.name in randomized_objects: - continue - # NOTE: currently unimplemented well - # if obj.randomize.random_model: - # if self._object_models_expired(obj): - # self.randomize_object_models(task=task, gazebo=gazebo) - if ( - obj.randomize.random_pose - or obj.randomize.random_position - or obj.randomize.random_orientation - ): - self.object_random_pose(task=task, gazebo=gazebo, object=obj) - else: - self.reset_default_object_pose(task=task, gazebo=gazebo, object=obj) - randomized_objects.add(obj.name) - - # Randomize terrain if needed - if self._terrain_enable and self._terrain_model_expired(): - self.randomize_terrain(task=task, gazebo=gazebo) - - def reset_robot_pose( - self, - task: SupportedTasks, - gazebo: scenario.GazeboSimulator, - randomize: bool = False, - ): - if randomize: - position = [ - self._robot_spawn_position[0] - + task.np_random.uniform( - -self._robot_random_spawn_volume[0] / 2, - self._robot_random_spawn_volume[0] / 2, - ), - self._robot_spawn_position[1] - + task.np_random.uniform( - -self._robot_random_spawn_volume[1] / 2, - self._robot_random_spawn_volume[1] / 2, - ), - self._robot_spawn_position[2] - + task.np_random.uniform( - -self._robot_random_spawn_volume[2] / 2, - self._robot_random_spawn_volume[2] / 2, - ), - ] - quat_xyzw = Rotation.from_euler( - "xyz", (0, 0, task.np_random.uniform(-np.pi, np.pi)) - ).as_quat() - else: - position = self._robot_spawn_position - quat_xyzw = self._robot_spawn_quat_xyzw - - gazebo_robot = self.robot.to_gazebo() - gazebo_robot.reset_base_pose(position, quat_to_wxyz(quat_xyzw)) - - # Execute a paused run to process model modification - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - def reset_robot_joint_positions( - self, - task: SupportedTasks, - gazebo: scenario.GazeboSimulator, - above_object_spawn: bool = False, - randomize: bool = False, - ): - # TODO: remove moveit - # Stop servoing - # if task._use_servo: - # if task.servo.is_enabled: - # task.servo.servo() - # task.servo.disable(sync=True) - - # task.controller.publish = False - - gazebo_robot = self.robot.to_gazebo() - - #TODO: select object - if above_object_spawn: - # If desired, compute IK above object spawn - if randomize: - rnd_displacement = ( - self._robot_random_joint_positions_above_object_spawn_xy_randomness - * task.np_random.uniform( - ( - -self._object_random_spawn_volume[0], - -self._object_random_spawn_volume[1], - ), - self._object_random_spawn_volume[:2], - ) - ) - position = ( - self._object_spawn_position[0] + rnd_displacement[0], - self._object_spawn_position[1] + rnd_displacement[1], - self._object_spawn_position[2] - + self._robot_random_joint_positions_above_object_spawn_elevation, - ) - quat_xyzw = Rotation.from_euler( - "xyz", (0, np.pi, task.np_random.uniform(-np.pi, np.pi)) - ).as_quat() - else: - position = ( - self._object_spawn_position[0], - self._object_spawn_position[1], - self._object_spawn_position[2] - + self._robot_random_joint_positions_above_object_spawn_elevation, - ) - quat_xyzw = (1.0, 0.0, 0.0, 0.0) - - # joint_configuration = task.moveit2.compute_ik( - # position=position, - # quat_xyzw=quat_xyzw, - # start_joint_state=task.initial_arm_joint_positions, - # ) - if joint_configuration is not None: - arm_joint_positions = joint_configuration.position[ - : len(task.initial_arm_joint_positions) - ] - else: - task.get_logger().warn( - "Robot configuration could not be reset above the object spawn. Using initial arm joint positions instead." - ) - arm_joint_positions = task.initial_arm_joint_positions - else: - # Otherwise get initial arm joint positions from the task (each task might need something different) - arm_joint_positions = task.initial_arm_joint_positions - - # Add normal noise if desired - if randomize: - for joint_position in arm_joint_positions: - joint_position += task.np_random.normal( - loc=0.0, scale=self._robot_random_joint_positions_std - ) - - # Arm joints - apply joint positions zero out velocities - if not gazebo_robot.reset_joint_positions( - arm_joint_positions, self.robot.arm_joint_names - ): - raise RuntimeError("Failed to reset robot joint positions") - if not gazebo_robot.reset_joint_velocities( - [0.0] * len(self.robot.arm_joint_names), - self.robot.arm_joint_names, - ): - raise RuntimeError("Failed to reset robot joint velocities") - - # Gripper joints - apply joint positions zero out velocities - if task._enable_gripper and self.robot.gripper_joint_names: - if not gazebo_robot.reset_joint_positions( - task.initial_gripper_joint_positions, self.robot.gripper_joint_names - ): - raise RuntimeError("Failed to reset gripper joint positions") - if not gazebo_robot.reset_joint_velocities( - [0.0] * len(self.robot.gripper_joint_names), - self.robot.gripper_joint_names, - ): - raise RuntimeError("Failed to reset gripper joint velocities") - - # Passive joints - zero out velocities - if self.robot.passive_joint_names: - if not gazebo_robot.reset_joint_velocities( - [0.0] * len(self.robot.passive_joint_names), - self.robot.passive_joint_names, - ): - raise RuntimeError("Failed to reset passive joint velocities") - - # Execute an unpaused run to process model modification and get new JointStates - if not gazebo.step(): - raise RuntimeError("Failed to execute an unpaused Gazebo step") - - # Reset also the controllers - # task.moveit2.force_reset_executing_state() - # task.moveit2.reset_controller(joint_state=arm_joint_positions) - if task._enable_gripper: - if ( - self.robot.CLOSED_GRIPPER_JOINT_POSITIONS - == task.initial_gripper_joint_positions - ): - task.gripper.close() - else: - task.gripper.open() - - def randomize_camera_pose( - self, - task: SupportedTasks, - gazebo: scenario.GazeboSimulator, - mode: str, - camera: CameraData, - ): - # Get random camera pose, centred at object position (or centre of object spawn box) - if "orbit" == mode: - camera_position, camera_quat_xyzw = self.get_random_camera_pose_orbit( - task=task, - centre=self._object_spawn_position, - distance=camera.random_pose_orbit_distance, - height=camera.random_pose_orbit_height_range, - ignore_arc_behind_robot=camera.random_pose_orbit_ignore_arc_behind_robot, - focal_point_z_offset=camera.random_pose_focal_point_z_offset, - ) - # elif "off" == mode: - # (camera_position, camera_quat_xyzw) = (self) - elif "select_random" == mode: - ( - camera_position, - camera_quat_xyzw, - ) = self.get_random_camera_pose_sample_random( - task=task, - centre=self._object_spawn_position, - options=camera.random_pose_select_position_options, - ) - elif "select_nearest" == mode: - ( - camera_position, - camera_quat_xyzw, - ) = self.get_random_camera_pose_sample_nearest( - centre=self._object_spawn_position, - options=camera.random_pose_select_position_options, - ) - else: - raise TypeError("Invalid mode for camera pose randomization.") - - if task.world.to_gazebo().name() == camera.relative_to: - transformed_camera_position = camera_position - transformed_camera_quat_wxyz = quat_to_wxyz(camera_quat_xyzw) - else: - # Transform the pose of camera to be with respect to robot - but represented in world reference frame for insertion into the world - ( - transformed_camera_position, - transformed_camera_quat_wxyz, - ) = transform_move_to_model_pose( - world=task.world, - position=camera_position, - quat=quat_to_wxyz(camera_quat_xyzw), - target_model=self.robot, - target_link=camera.relative_to, - xyzw=False, - ) - - # Detach camera if needed - if self.__is_camera_attached: - if not self.robot.to_gazebo().detach_link( - camera.relative_to, - self.camera_wrapper.name(), - self.camera_wrapper.link_name, - ): - raise Exception("Cannot detach camera link from robot") - - # Execute a paused run to process camera detachment - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - # Move pose of the camera - camera_gazebo = self.camera.to_gazebo() - camera_gazebo.reset_base_pose( - transformed_camera_position, transformed_camera_quat_wxyz - ) - - # Execute a paused run to process change of camera pose - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - # Attach to robot again if needed - if self.__is_camera_attached: - if not self.robot.to_gazebo().attach_link( - camera.relative_to, - self.camera_wrapper.name(), - self.camera_wrapper.link_name, - ): - raise Exception("Cannot attach camera link to robot") - - # Execute a paused run to process link attachment - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - # Broadcast tf - task.tf2_broadcaster.broadcast_tf( - parent_frame_id=camera.relative_to, - child_frame_id=self.camera_wrapper.frame_id, - translation=camera_position, - rotation=camera_quat_xyzw, - xyzw=True, - ) - - def get_random_camera_pose_orbit( - self, - task: SupportedTasks, - centre: Tuple[float, float, float], - distance: float, - height: Tuple[float, float], - ignore_arc_behind_robot: float, - focal_point_z_offset: float, - ) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]: - # Select a random 3D position (with restricted min height) - while True: - position = task.np_random.uniform( - low=(-1.0, -1.0, height[0]), high=(1.0, 1.0, height[1]) - ) - # Normalize - position /= np.linalg.norm(position) - - # Make sure it does not get spawned directly behind the robot - if ( - abs(np.arctan2(position[0], position[1]) + np.pi / 2) - > ignore_arc_behind_robot - ): - break - - # Determine orientation such that camera faces the origin - rpy = [ - 0.0, - np.arctan2( - position[2] - focal_point_z_offset, np.linalg.norm(position[:2], 2) - ), - np.arctan2(position[1], position[0]) + np.pi, - ] - quat_xyzw = Rotation.from_euler("xyz", rpy).as_quat() - - # Scale normal vector by distance and translate camera to point at the workspace centre - position *= distance - position[:2] += centre[:2] - - return position, quat_xyzw - - def get_random_camera_pose_sample_random( - self, - task: SupportedTasks, - centre: Tuple[float, float, float], - options: List[Tuple[float, float, float]], - ) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]: - # Select a random entry from the options - selection = options[task.np_random.randint(len(options))] - - # Process it and return - return self.get_random_camera_pose_sample_process( - centre=centre, - position=selection, - focal_point_z_offset=self._camera_random_pose_focal_point_z_offset, - ) - - def get_random_camera_pose_sample_nearest( - self, - centre: Tuple[float, float, float], - options: List[Tuple[float, float, float]], - ) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]: - # Select the nearest entry - dist_sqr = np.sum((np.array(options) - np.array(centre)) ** 2, axis=1) - nearest = options[np.argmin(dist_sqr)] - - # Process it and return - return self.get_random_camera_pose_sample_process( - centre=centre, - position=nearest, - focal_point_z_offset=self._camera_random_pose_focal_point_z_offset, - ) - - def get_random_camera_pose_sample_process( - self, - centre: Tuple[float, float, float], - position: Tuple[float, float, float], - focal_point_z_offset: float, - ) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]: - # Determine orientation such that camera faces the centre - rpy = [ - 0.0, - np.arctan2( - position[2] - focal_point_z_offset, - np.linalg.norm((position[0] - centre[0], position[1] - centre[1]), 2), - ), - np.arctan2(position[1] - centre[1], position[0] - centre[0]) + np.pi, - ] - quat_xyzw = Rotation.from_euler("xyz", rpy).as_quat() - - return position, quat_xyzw - - def randomize_terrain(self, task: SupportedTasks, gazebo: scenario.GazeboSimulator): - # Remove existing terrain - if hasattr(self, "terrain"): - if not task.world.to_gazebo().remove_model(self.terrain.name()): - raise RuntimeError(f"Failed to remove {self.terrain.name()}") - - # Choose one of the random orientations for the texture (4 directions) - orientation = [ - (1, 0, 0, 0), - (0, 0, 0, 1), - (0.70710678118, 0, 0, 0.70710678118), - (0.70710678118, 0, 0, -0.70710678118), - ][task.np_random.randint(4)] - - # Create terrain - self.terrain = self.__terrain_model_class( - world=task.world, - position=self._terrain_spawn_position, - orientation=orientation, - size=self._terrain_size, - np_random=task.np_random, - ) - - # Expose name of the terrain for task - task.terrain_name = self.terrain.name() - - # Enable contact detection - for link_name in self.terrain.link_names(): - link = self.terrain.to_gazebo().get_link(link_name=link_name) - link.enable_contact_detection(True) - - # Execute an unpaused step to process terrain removal and insertion - if not gazebo.step(): - raise RuntimeError("Failed to execute an unpaused Gazebo run") - - def randomize_light(self, task: SupportedTasks, gazebo: scenario.GazeboSimulator): - # Remove existing light - if hasattr(self, "light"): - if not task.world.to_gazebo().remove_model(self.light.name()): - raise RuntimeError(f"Failed to remove {self.light.name()}") - - # Create light - self.light = self.__light_model_class( - world=task.world, - direction=self._light_direction, - minmax_elevation=self._light_random_minmax_elevation, - color=self._light_color, - distance=self._light_distance, - visual=self._light_visual, - radius=self._light_radius, - np_random=task.np_random, - ) - - # Execute a paused run to process model removal and insertion - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - def reset_default_object_pose( - self, task: SupportedTasks, gazebo: scenario.GazeboSimulator, object: ObjectData - ): - task_object_names = self.__objects_unique_names[object.name] - for object_name in task_object_names: - obj = task.world.to_gazebo().get_model(object_name).to_gazebo() - obj.reset_base_pose(object.position, object.orientation) - - # Execute a paused run to process model modification - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - def randomize_object_models( - self, task: SupportedTasks, gazebo: scenario.GazeboSimulator - ): - # Remove all existing models - if len(self.task.object_names) > 0: - for object_name in self.task.object_names: - if not task.world.to_gazebo().remove_model(object_name): - raise RuntimeError(f"Failed to remove {object_name}") - self.task.object_names.clear() - - # Insert new models with random pose - i = 0 - while len(self.task.object_names) < len(self._objects): - position, quat_random = self.get_random_object_pose( - task=task, obj=self._objects[i] - ) - try: - obj_dict = asdict(self._objects[i]) - obj_dict["world"] = task.world - obj_dict["position"] = position - obj_dict["orientation"] = quat_random - obj_dict["np_random"] = task.np_random - match self._objects[i]: - case BoxObjectData(): - object_wrapper = models.Box(**obj_dict) - case PlaneObjectData(): - object_wrapper = models.Plane(**obj_dict) - case SphereObjectData(): - object_wrapper = models.Sphere(**obj_dict) - case CylinderObjectData(): - object_wrapper = models.Cylinder(**obj_dict) - case MeshObjectData(): - object_wrapper = models.Mesh(**obj_dict) - case _: - raise NotImplementedError("Type is not supported") - - model_name = object_wrapper.name() - self.task.object_names.append(model_name) - self.__object_positions[model_name] = position - # Enable contact detection - for link_name in object_wrapper.link_names(): - link = object_wrapper.to_gazebo().get_link(link_name=link_name) - link.enable_contact_detection(True) - - i += 1 - - except Exception as ex: - task.get_logger().warn(f"Model could not be inserted: {ex}") - - # Execute a paused run to process model insertion - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - def object_random_pose( - self, task: SupportedTasks, gazebo: scenario.GazeboSimulator, object: ObjectData - ): - task_object_names = self.__objects_unique_names[object.name] - for object_name in task_object_names: - position, quat_random = self.get_random_object_pose(task=task, obj=object) - obj = task.world.to_gazebo().get_model(object_name).to_gazebo() - if object.randomize.random_pose: - obj.reset_base_pose(position, quat_random) - elif object.randomize.random_orientation: - obj.reset_base_pose(object.position, quat_random) - elif object.randomize.random_position: - obj.reset_base_pose(position, object.orientation) - - self.__object_positions[object_name] = position - - # Execute a paused run to process model modification - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - def get_random_object_pose( - self, - task: SupportedTasks, - obj: ObjectData, - name: str = "", - min_distance_to_other_objects: float = 0.2, - min_distance_decay_factor: float = 0.95, - ): - is_too_close = True - while is_too_close: - object_position = ( - obj.position[0] - + task.np_random.uniform( - -obj.randomize.random_spawn_volume[0] / 2, - obj.randomize.random_spawn_volume[0] / 2, - ), - obj.position[1] - + task.np_random.uniform( - -obj.randomize.random_spawn_volume[1] / 2, - obj.randomize.random_spawn_volume[1] / 2, - ), - obj.position[2] - + task.np_random.uniform( - -obj.randomize.random_spawn_volume[2] / 2, - obj.randomize.random_spawn_volume[2] / 2, - ), - ) - - if task.world.to_gazebo().name() != obj.relative_to: - # Transform the pose of camera to be with respect to robot - but represented in world reference frame for insertion into the world - object_position = transform_move_to_model_position( - world=task.world, - position=object_position, - target_model=self.robot, - target_link=obj.relative_to, - ) - - # Check if position is far enough from other - is_too_close = False - for ( - existing_object_name, - existing_object_position, - ) in self.__object_positions.items(): - if existing_object_name == name: - # Do not compare to itself - continue - if ( - distance.euclidean(object_position, existing_object_position) - < min_distance_to_other_objects - ): - min_distance_to_other_objects *= min_distance_decay_factor - is_too_close = True - break - - quat = task.np_random.uniform(-1, 1, 4) - quat /= np.linalg.norm(quat) - - return object_position, quat - - # Internal overrides # - def internal_overrides(self, task: SupportedTasks): - """ - Perform internal overrides if parameters - """ - pass - - # if self._object_randomize_count: - # self._object_count = task.np_random.randint( - # low=1, high=self.__object_max_count + 1 - # ) - - # External overrides # - def external_overrides(self, task: SupportedTasks): - """ - Perform external overrides from either task level or environment before initialising/randomising the task. - """ - - self.__consume_parameter_overrides(task=task) + self._scene.init_scene() # Pre-randomization # def pre_randomization(self, task: SupportedTasks): @@ -1444,7 +153,7 @@ class ManipulationGazeboEnvRandomizer( Perform steps that are required before randomization is performed. """ - for obj in self._objects: + for obj in self._scene.objects: # If desired, select random spawn position from the segments # It is performed here because object spawn position might be of interest also for robot and camera randomization @@ -1539,7 +248,7 @@ class ManipulationGazeboEnvRandomizer( return # Ensure no objects are overlapping - for obj in self._objects: + for obj in self._scene.objects: if not obj.randomize.random_pose or obj.name in processed_objects: continue @@ -1548,7 +257,7 @@ class ManipulationGazeboEnvRandomizer( task.get_logger().debug( f"Checking overlap for {obj.name}, attempt {attempts + 1}" ) - if self.check_object_overlapping(task=task, object=obj): + if self._scene.check_object_overlapping(obj): processed_objects.add(obj.name) break # No overlap, move to next object else: @@ -1572,278 +281,65 @@ class ManipulationGazeboEnvRandomizer( if self.POST_RANDOMIZATION_MAX_STEPS == attempts: task.get_logger().error("Cannot obtain new observation.") - def check_object_overlapping( - self, - task: SupportedTasks, - object: ObjectData, - allowed_penetration_depth: float = 0.001, - terrain_allowed_penetration_depth: float = 0.002, - ) -> bool: - """ - Go through all objects and make sure that none of them are overlapping. - If an object is overlapping, reset its position. - Positions are reset also if object is in collision with robot right after reset. - Collisions/overlaps with terrain are ignored. - Returns True if all objects are okay, false if they had to be reset - """ - - # Update object positions - task_object_names = self.__objects_unique_names[object.name] - for object_name in task_object_names: - model = task.world.get_model(object_name).to_gazebo() - self.__object_positions[object_name] = model.get_link( - link_name=model.link_names()[0] - ).position() - - for object_name in task_object_names: - obj = task.world.get_model(object_name).to_gazebo() - - # Make sure the object is inside workspace - if task.check_object_outside_workspace( - self.__object_positions[object_name] - ): - position, quat_random = self.get_random_object_pose( - task=task, obj=object, name=object_name - ) - obj.reset_base_pose(position, quat_random) - return False - - # Make sure the object is not intersecting other objects - try: - for contact in obj.contacts(): - depth = np.mean([point.depth for point in contact.points]) - if ( - self.terrain.name() in contact.body_b - and depth < terrain_allowed_penetration_depth - ): - continue - if ( - task.robot_name in contact.body_b - or depth > allowed_penetration_depth - ): - position, quat_random = self.get_random_object_pose( - task=task, - obj=object, - name=object_name, - ) - obj.reset_base_pose(position, quat_random) - return False - except Exception as e: - task.get_logger().error( - f"Runtime error encountered while checking objects intersections: {e}" - ) - - return True - - # ============================ - # Randomizer rollouts checking - # ============================ - - def __camera_pose_randomizer_enabled(self, camera: CameraData) -> bool: - """ - Checks if camera pose randomizer is enabled. - - Return: - True if enabled, false otherwise - """ - - if camera.random_pose_rollouts_num == 0: - return False - else: - return True - - def _camera_pose_expired(self, camera: CameraData) -> bool: - """ - Checks if camera pose needs to be randomized. - - Return: - True if expired, false otherwise - """ - - if not self.__camera_pose_randomizer_enabled(camera): - return False - - self.__camera_pose_rollout_counter += 1 - - if self.__camera_pose_rollout_counter >= camera.random_pose_rollouts_num: - self.__camera_pose_rollout_counter = 0 - return True - - return False - - def __terrain_model_randomizer_enabled(self) -> bool: - """ - Checks if terrain randomizer is enabled. - - Return: - True if enabled, false otherwise - """ - - if self._terrain_model_rollouts_num == 0: - return False - else: - return self.__is_terrain_type_randomizable - - def _terrain_model_expired(self) -> bool: - """ - Checks if terrain model needs to be randomized. - - Return: - True if expired, false otherwise - """ - - if not self.__terrain_model_randomizer_enabled(): - return False - - self.__terrain_model_rollout_counter += 1 - - if self.__terrain_model_rollout_counter >= self._terrain_model_rollouts_num: - self.__terrain_model_rollout_counter = 0 - return True - - return False - - def __light_model_randomizer_enabled(self) -> bool: - """ - Checks if light model randomizer is enabled. - - Return: - True if enabled, false otherwise - """ - - if self._light_model_rollouts_num == 0: - return False - else: - return self.__is_light_type_randomizable - - def _light_model_expired(self) -> bool: - """ - Checks if light models need to be randomized. - - Return: - True if expired, false otherwise - """ - - if not self.__light_model_randomizer_enabled(): - return False - - self.__light_model_rollout_counter += 1 - - if self.__light_model_rollout_counter >= self._light_model_rollouts_num: - self.__light_model_rollout_counter = 0 - return True - - return False - - def __object_models_randomizer_enabled(self, obj: ObjectData) -> bool: - """ - Checks if object model randomizer is enabled. - - Return: - True if enabled, false otherwise - """ - - if obj.randomize: - if obj.randomize.models_rollouts_num == 0: - return False - else: - return True - else: - return False - - def _object_models_expired(self, obj: ObjectData) -> bool: - """ - Checks if object models need to be randomized. - - Return: - True if expired, false otherwise - """ - - if not self.__object_models_randomizer_enabled(obj): - return False - - self.__object_models_rollout_counter += 1 - - if self.__object_models_rollout_counter >= obj.randomize.models_rollouts_num: - self.__object_models_rollout_counter = 0 - return True - - return False - - def __consume_parameter_overrides(self, task: SupportedTasks): - for key, value in task._randomizer_parameter_overrides.items(): - if hasattr(self, key): - setattr(self, key, value) - elif hasattr(self, f"_{key}"): - setattr(self, f"_{key}", value) - elif hasattr(self, f"__{key}"): - setattr(self, f"__{key}", value) - else: - task.get_logger().error( - f"Override '{key}' is not supperted by the randomizer." - ) - - task._randomizer_parameter_overrides.clear() - # ============================= # Additional features and debug # ============================= - def visualise_workspace( - self, - task: SupportedTasks, - gazebo: scenario.GazeboSimulator, - color: Tuple[float, float, float, float] = (0, 1, 0, 0.8), - ): - # Insert a translucent box visible only in simulation with no physical interactions - models.Box( - world=task.world, - name="_workspace_volume", - position=self._object_spawn_position, - orientation=(0, 0, 0, 1), - size=task.workspace_volume, - collision=False, - visual=True, - gui_only=True, - static=True, - color=color, - ) - # Execute a paused run to process model insertion - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") - - def visualise_spawn_volume( - self, - task: SupportedTasks, - gazebo: scenario.GazeboSimulator, - color: Tuple[float, float, float, float] = (0, 0, 1, 0.8), - color_with_height: Tuple[float, float, float, float] = (1, 0, 1, 0.7), - ): - # Insert translucent boxes visible only in simulation with no physical interactions - models.Box( - world=task.world, - name="_object_random_spawn_volume", - position=self._object_spawn_position, - orientation=(0, 0, 0, 1), - size=self._object_random_spawn_volume, - collision=False, - visual=True, - gui_only=True, - static=True, - color=color, - ) - models.Box( - world=task.world, - name="_object_random_spawn_volume_with_height", - position=self._object_spawn_position, - orientation=(0, 0, 0, 1), - size=self._object_random_spawn_volume, - collision=False, - visual=True, - gui_only=True, - static=True, - color=color_with_height, - ) - # Execute a paused run to process model insertion - if not gazebo.run(paused=True): - raise RuntimeError("Failed to execute a paused Gazebo run") + # def visualise_workspace( + # self, + # task: SupportedTasks, + # gazebo: scenario.GazeboSimulator, + # color: Tuple[float, float, float, float] = (0, 1, 0, 0.8), + # ): + # # Insert a translucent box visible only in simulation with no physical interactions + # models.Box( + # world=task.world, + # name="_workspace_volume", + # position=self._object_spawn_position, + # orientation=(0, 0, 0, 1), + # size=task.workspace_volume, + # collision=False, + # visual=True, + # gui_only=True, + # static=True, + # color=color, + # ) + # # Execute a paused run to process model insertion + # if not gazebo.run(paused=True): + # raise RuntimeError("Failed to execute a paused Gazebo run") + # + # def visualise_spawn_volume( + # self, + # task: SupportedTasks, + # gazebo: scenario.GazeboSimulator, + # color: Tuple[float, float, float, float] = (0, 0, 1, 0.8), + # color_with_height: Tuple[float, float, float, float] = (1, 0, 1, 0.7), + # ): + # # Insert translucent boxes visible only in simulation with no physical interactions + # models.Box( + # world=task.world, + # name="_object_random_spawn_volume", + # position=self._object_spawn_position, + # orientation=(0, 0, 0, 1), + # size=self._object_random_spawn_volume, + # collision=False, + # visual=True, + # gui_only=True, + # static=True, + # color=color, + # ) + # models.Box( + # world=task.world, + # name="_object_random_spawn_volume_with_height", + # position=self._object_spawn_position, + # orientation=(0, 0, 0, 1), + # size=self._object_random_spawn_volume, + # collision=False, + # visual=True, + # gui_only=True, + # static=True, + # color=color_with_height, + # ) + # # Execute a paused run to process model insertion + # if not gazebo.run(paused=True): + # raise RuntimeError("Failed to execute a paused Gazebo run") diff --git a/env_manager/rbs_gym/rbs_gym/envs/tasks/__init__.py b/env_manager/rbs_gym/rbs_gym/envs/tasks/__init__.py index d6f0932..15d114f 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/tasks/__init__.py +++ b/env_manager/rbs_gym/rbs_gym/envs/tasks/__init__.py @@ -1,4 +1,4 @@ -from .curriculums import * +# from .curriculums import * # from .grasp import * # from .grasp_planetary import * from .reach import * diff --git a/env_manager/rbs_gym/rbs_gym/envs/tasks/manipulation.py b/env_manager/rbs_gym/rbs_gym/envs/tasks/manipulation.py index 3194f2e..c8d6b6b 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/tasks/manipulation.py +++ b/env_manager/rbs_gym/rbs_gym/envs/tasks/manipulation.py @@ -3,11 +3,29 @@ import multiprocessing import sys from itertools import count from threading import Thread -from typing import Dict, Optional, Tuple, Union +from typing import Any, Dict, Optional, Tuple, Union import numpy as np import rclpy +from env_manager.models.configs import RobotData +from env_manager.models.robots import RobotWrapper, get_robot_model_class +from env_manager.scene import Scene +from env_manager.utils import Tf2Broadcaster, Tf2Listener +from env_manager.utils.conversions import orientation_6d_to_quat +from env_manager.utils.gazebo import ( + Point, + Pose, + Quat, + get_model_orientation, + get_model_pose, + get_model_position, + transform_change_reference_frame_orientation, + transform_change_reference_frame_pose, + transform_change_reference_frame_position, +) +from env_manager.utils.math import quat_mul from gym_gz.base.task import Task +from gym_gz.scenario.model_wrapper import ModelWrapper from gym_gz.utils.typing import ( Action, ActionSpace, @@ -18,14 +36,14 @@ from gym_gz.utils.typing import ( from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor from rclpy.node import Node +from robot_builder.elements.robot import Robot from scipy.spatial.transform import Rotation -from rbs_gym.envs.control import * -from rbs_gym.envs.models.robots import get_robot_model_class -from rbs_gym.envs.utils import Tf2Broadcaster, Tf2Listener -from rbs_gym.envs.utils.conversions import orientation_6d_to_quat -from rbs_gym.envs.utils.gazebo import * -from rbs_gym.envs.utils.math import quat_mul +from rbs_gym.envs.control import ( + CartesianForceController, + GripperController, + JointEffortController, +) class Manipulation(Task, Node, abc.ABC): @@ -47,7 +65,6 @@ class Manipulation(Task, Node, abc.ABC): num_threads: int, **kwargs, ): - # Get next ID for this task instance self.id = next(self._ids) @@ -64,6 +81,8 @@ class Manipulation(Task, Node, abc.ABC): # Initialize ROS 2 Node base class Node.__init__(self, f"rbs_gym_{self.id}") + self._allow_undeclared_parameters = True + self.declare_parameter("robot_description", "") # Create callback group that allows execution of callbacks in parallel without restrictions self._callback_group = ReentrantCallbackGroup() @@ -85,92 +104,92 @@ class Manipulation(Task, Node, abc.ABC): self._executor_thread.start() # Get class of the robot model based on passed argument - self.robot_model_class = get_robot_model_class(robot_model) + # self.robot_model_class = get_robot_model_class(robot_model) # Store passed arguments for later use - self.workspace_centre = ( - workspace_centre[0], - workspace_centre[1], - workspace_centre[2] + self.robot_model_class.BASE_LINK_Z_OFFSET, - ) - self.workspace_volume = workspace_volume - self._restrict_position_goal_to_workspace = restrict_position_goal_to_workspace - self._use_servo = use_servo - self.__scaling_factor_translation = scaling_factor_translation - self.__scaling_factor_rotation = scaling_factor_rotation + # self.workspace_centre = ( + # workspace_centre[0], + # workspace_centre[1], + # workspace_centre[2] + self.robot_model_class.BASE_LINK_Z_OFFSET, + # ) + # self.workspace_volume = workspace_volume + # self._restrict_position_goal_to_workspace = restrict_position_goal_to_workspace + # self._use_servo = use_servo + # self.__scaling_factor_translation = scaling_factor_translation + # self.__scaling_factor_rotation = scaling_factor_rotation self._enable_gripper = enable_gripper - - # Get workspace bounds, useful is many computations - workspace_volume_half = ( - workspace_volume[0] / 2, - workspace_volume[1] / 2, - workspace_volume[2] / 2, - ) - self.workspace_min_bound = ( - self.workspace_centre[0] - workspace_volume_half[0], - self.workspace_centre[1] - workspace_volume_half[1], - self.workspace_centre[2] - workspace_volume_half[2], - ) - self.workspace_max_bound = ( - self.workspace_centre[0] + workspace_volume_half[0], - self.workspace_centre[1] + workspace_volume_half[1], - self.workspace_centre[2] + workspace_volume_half[2], - ) + self._scene: Scene | None = None + # + # # Get workspace bounds, useful is many computations + # workspace_volume_half = ( + # workspace_volume[0] / 2, + # workspace_volume[1] / 2, + # workspace_volume[2] / 2, + # ) + # self.workspace_min_bound = ( + # self.workspace_centre[0] - workspace_volume_half[0], + # self.workspace_centre[1] - workspace_volume_half[1], + # self.workspace_centre[2] - workspace_volume_half[2], + # ) + # self.workspace_max_bound = ( + # self.workspace_centre[0] + workspace_volume_half[0], + # self.workspace_centre[1] + workspace_volume_half[1], + # self.workspace_centre[2] + workspace_volume_half[2], + # ) # Determine robot name and prefix based on current ID of the task - self.robot_prefix: str = self.robot_model_class.DEFAULT_PREFIX - if 0 == self.id: - self.robot_name = self.robot_model_class.ROBOT_MODEL_NAME - else: - self.robot_name = f"{self.robot_model_class.ROBOT_MODEL_NAME}{self.id}" - if self.robot_prefix.endswith("_"): - self.robot_prefix = f"{self.robot_prefix[:-1]}{self.id}_" - elif self.robot_prefix == "": - self.robot_prefix = f"robot{self.id}_" + # self.robot_prefix: str = self.robot_model_class.DEFAULT_PREFIX + # if 0 == self.id: + # self.robot_name = self.robot_model_class.ROBOT_MODEL_NAME + # else: + # self.robot_name = f"{self.robot_model_class.ROBOT_MODEL_NAME}{self.id}" + # if self.robot_prefix.endswith("_"): + # self.robot_prefix = f"{self.robot_prefix[:-1]}{self.id}_" + # elif self.robot_prefix == "": + # self.robot_prefix = f"robot{self.id}_" # Names of specific robot links, useful all around the code - self.robot_base_link_name = self.robot_model_class.get_robot_base_link_name( - self.robot_prefix - ) - self.robot_arm_base_link_name = self.robot_model_class.get_arm_base_link_name( - self.robot_prefix - ) - self.robot_ee_link_name = self.robot_model_class.get_ee_link_name( - self.robot_prefix - ) - self.robot_arm_link_names = self.robot_model_class.get_arm_link_names( - self.robot_prefix - ) - self.robot_gripper_link_names = self.robot_model_class.get_gripper_link_names( - self.robot_prefix - ) - self.robot_arm_joint_names = self.robot_model_class.get_arm_joint_names( - self.robot_prefix - ) - self.robot_gripper_joint_names = self.robot_model_class.get_gripper_joint_names( - self.robot_prefix - ) + # self.robot_base_link_name = self.robot_model_class.get_robot_base_link_name( + # self.robot_prefix + # ) + # self.robot_arm_base_link_name = self.robot_model_class.get_arm_base_link_name( + # self.robot_prefix + # ) + # self.robot_ee_link_name = self.robot_model_class.get_ee_link_name( + # self.robot_prefix + # ) + # self.robot_arm_link_names = self.robot_model_class.get_arm_link_names( + # self.robot_prefix + # ) + # self.robot_gripper_link_names = self.robot_model_class.get_gripper_link_names( + # self.robot_prefix + # ) + # self.robot_arm_joint_names = self.robot_model_class.get_arm_joint_names( + # self.robot_prefix + # ) + # self.robot_gripper_joint_names = self.robot_model_class.get_gripper_joint_names( + # self.robot_prefix + # ) # Get exact name substitution of the frame for workspace self.workspace_frame_id = self.substitute_special_frame(workspace_frame_id) # Specify initial positions (default configuration is used here) - self.initial_arm_joint_positions = ( - self.robot_model_class.DEFAULT_ARM_JOINT_POSITIONS - ) - self.initial_gripper_joint_positions = ( - self.robot_model_class.DEFAULT_GRIPPER_JOINT_POSITIONS - ) + # self.initial_arm_joint_positions = ( + # self.robot_model_class.DEFAULT_ARM_JOINT_POSITIONS + # ) + # self.initial_gripper_joint_positions = ( + # self.robot_model_class.DEFAULT_GRIPPER_JOINT_POSITIONS + # ) # Names of important models (in addition to robot model) self.terrain_name = "terrain" - self.object_names = [] # Setup listener and broadcaster of transforms via tf2 self.tf2_listener = Tf2Listener(node=self) self.tf2_broadcaster = Tf2Broadcaster(node=self) - self.cartesian_control = True #TODO: make it as an external parameter + self.cartesian_control = True # TODO: make it as an external parameter # Setup controllers self.controller = CartesianForceController(self) @@ -181,195 +200,181 @@ class Manipulation(Task, Node, abc.ABC): # Initialize task and randomizer overrides (e.g. from curriculum) # Both of these are consumed at the beginning of reset - self.__task_parameter_overrides: Dict[str, any] = {} - self._randomizer_parameter_overrides: Dict[str, any] = {} + self.__task_parameter_overrides: dict[str, Any] = {} + self._randomizer_parameter_overrides: dict[str, Any] = {} def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: - action_space = self.create_action_space() observation_space = self.create_observation_space() return action_space, observation_space def create_action_space(self) -> ActionSpace: - raise NotImplementedError() def create_observation_space(self) -> ObservationSpace: - raise NotImplementedError() def set_action(self, action: Action): - raise NotImplementedError() def get_observation(self) -> Observation: - raise NotImplementedError() def get_reward(self) -> Reward: - raise NotImplementedError() def is_done(self) -> bool: - raise NotImplementedError() def reset_task(self): - self.__consume_parameter_overrides() - # Helper functions # - def get_relative_ee_position( - self, translation: Tuple[float, float, float] - ) -> Tuple[float, float, float]: + # # Helper functions # + # def get_relative_ee_position( + # self, translation: Tuple[float, float, float] + # ) -> Tuple[float, float, float]: + # # Scale relative action to metric units + # translation = self.scale_relative_translation(translation) + # # Get current position + # current_position = self.get_ee_position() + # # Compute target position + # target_position = ( + # current_position[0] + translation[0], + # current_position[1] + translation[1], + # current_position[2] + translation[2], + # ) + # + # # Restrict target position to a limited workspace, if desired + # if self._restrict_position_goal_to_workspace: + # target_position = self.restrict_position_goal_to_workspace(target_position) + # + # return target_position + # + # def get_relative_ee_orientation( + # self, + # rotation: Union[ + # float, + # Tuple[float, float, float, float], + # Tuple[float, float, float, float, float, float], + # ], + # representation: str = "quat", + # ) -> Tuple[float, float, float, float]: + # # Get current orientation + # current_quat_xyzw = self.get_ee_orientation() + # + # # For 'z' representation, result should always point down + # # Therefore, create a new quatertnion that contains only yaw component + # if "z" == representation: + # current_yaw = Rotation.from_quat(current_quat_xyzw).as_euler("xyz")[2] + # current_quat_xyzw = Rotation.from_euler( + # "xyz", [np.pi, 0, current_yaw] + # ).as_quat() + # + # # Convert relative orientation representation to quaternion + # relative_quat_xyzw = None + # if "quat" == representation: + # relative_quat_xyzw = rotation + # elif "6d" == representation: + # vectors = tuple( + # rotation[x : x + 3] for x, _ in enumerate(rotation) if x % 3 == 0 + # ) + # relative_quat_xyzw = orientation_6d_to_quat(vectors[0], vectors[1]) + # elif "z" == representation: + # rotation = self.scale_relative_rotation(rotation) + # relative_quat_xyzw = Rotation.from_euler("xyz", [0, 0, rotation]).as_quat() + # + # # Compute target position (combine quaternions) + # target_quat_xyzw = quat_mul(current_quat_xyzw, relative_quat_xyzw) + # + # # Normalise quaternion (should not be needed, but just to be safe) + # target_quat_xyzw /= np.linalg.norm(target_quat_xyzw) + # + # return target_quat_xyzw - # Scale relative action to metric units - translation = self.scale_relative_translation(translation) - # Get current position - current_position = self.get_ee_position() - # Compute target position - target_position = ( - current_position[0] + translation[0], - current_position[1] + translation[1], - current_position[2] + translation[2], - ) + # def scale_relative_translation( + # self, translation: Tuple[float, float, float] + # ) -> Tuple[float, float, float]: + # return ( + # self.__scaling_factor_translation * translation[0], + # self.__scaling_factor_translation * translation[1], + # self.__scaling_factor_translation * translation[2], + # ) - # Restrict target position to a limited workspace, if desired - if self._restrict_position_goal_to_workspace: - target_position = self.restrict_position_goal_to_workspace(target_position) + # def scale_relative_rotation( + # self, + # rotation: Union[float, Tuple[float, float, float], np.floating, np.ndarray], + # ) -> float: + # if not hasattr(rotation, "__len__"): + # return self.__scaling_factor_rotation * rotation + # else: + # return ( + # self.__scaling_factor_rotation * rotation[0], + # self.__scaling_factor_rotation * rotation[1], + # self.__scaling_factor_rotation * rotation[2], + # ) + # + # def restrict_position_goal_to_workspace( + # self, position: Tuple[float, float, float] + # ) -> Tuple[float, float, float]: + # return ( + # min( + # self.workspace_max_bound[0], + # max( + # self.workspace_min_bound[0], + # position[0], + # ), + # ), + # min( + # self.workspace_max_bound[1], + # max( + # self.workspace_min_bound[1], + # position[1], + # ), + # ), + # min( + # self.workspace_max_bound[2], + # max( + # self.workspace_min_bound[2], + # position[2], + # ), + # ), + # ) - return target_position - - def get_relative_ee_orientation( - self, - rotation: Union[ - float, - Tuple[float, float, float, float], - Tuple[float, float, float, float, float, float], - ], - representation: str = "quat", - ) -> Tuple[float, float, float, float]: - - # Get current orientation - current_quat_xyzw = self.get_ee_orientation() - - # For 'z' representation, result should always point down - # Therefore, create a new quatertnion that contains only yaw component - if "z" == representation: - current_yaw = Rotation.from_quat(current_quat_xyzw).as_euler("xyz")[2] - current_quat_xyzw = Rotation.from_euler( - "xyz", [np.pi, 0, current_yaw] - ).as_quat() - - # Convert relative orientation representation to quaternion - relative_quat_xyzw = None - if "quat" == representation: - relative_quat_xyzw = rotation - elif "6d" == representation: - vectors = tuple( - rotation[x : x + 3] for x, _ in enumerate(rotation) if x % 3 == 0 - ) - relative_quat_xyzw = orientation_6d_to_quat(vectors[0], vectors[1]) - elif "z" == representation: - rotation = self.scale_relative_rotation(rotation) - relative_quat_xyzw = Rotation.from_euler("xyz", [0, 0, rotation]).as_quat() - - # Compute target position (combine quaternions) - target_quat_xyzw = quat_mul(current_quat_xyzw, relative_quat_xyzw) - - # Normalise quaternion (should not be needed, but just to be safe) - target_quat_xyzw /= np.linalg.norm(target_quat_xyzw) - - return target_quat_xyzw - - def scale_relative_translation( - self, translation: Tuple[float, float, float] - ) -> Tuple[float, float, float]: - - return ( - self.__scaling_factor_translation * translation[0], - self.__scaling_factor_translation * translation[1], - self.__scaling_factor_translation * translation[2], - ) - - def scale_relative_rotation( - self, - rotation: Union[float, Tuple[float, float, float], np.floating, np.ndarray], - ) -> float: - - if not hasattr(rotation, "__len__"): - return self.__scaling_factor_rotation * rotation - else: - return ( - self.__scaling_factor_rotation * rotation[0], - self.__scaling_factor_rotation * rotation[1], - self.__scaling_factor_rotation * rotation[2], - ) - - def restrict_position_goal_to_workspace( - self, position: Tuple[float, float, float] - ) -> Tuple[float, float, float]: - - return ( - min( - self.workspace_max_bound[0], - max( - self.workspace_min_bound[0], - position[0], - ), - ), - min( - self.workspace_max_bound[1], - max( - self.workspace_min_bound[1], - position[1], - ), - ), - min( - self.workspace_max_bound[2], - max( - self.workspace_min_bound[2], - position[2], - ), - ), - ) - - def restrict_servo_translation_to_workspace( - self, translation: Tuple[float, float, float] - ) -> Tuple[float, float, float]: - - current_ee_position = self.get_ee_position() - - translation = tuple( - 0.0 - if ( - current_ee_position[i] > self.workspace_max_bound[i] - and translation[i] > 0.0 - ) - or ( - current_ee_position[i] < self.workspace_min_bound[i] - and translation[i] < 0.0 - ) - else translation[i] - for i in range(3) - ) - - return translation + # def restrict_servo_translation_to_workspace( + # self, translation: tuple[float, float, float] + # ) -> tuple[float, float, float]: + # current_ee_position = self.get_ee_position() + # + # translation = tuple( + # 0.0 + # if ( + # current_ee_position[i] > self.workspace_max_bound[i] + # and translation[i] > 0.0 + # ) + # or ( + # current_ee_position[i] < self.workspace_min_bound[i] + # and translation[i] < 0.0 + # ) + # else translation[i] + # for i in range(3) + # ) + # + # return translation def get_ee_pose( self, - ) -> Optional[Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]]: + ) -> Pose: """ Return the current pose of the end effector with respect to arm base link. """ try: - robot_model = self.world.to_gazebo().get_model(self.robot_name).to_gazebo() + robot_model = self.world.to_gazebo().get_model(self.robot.name).to_gazebo() ee_position, ee_quat_xyzw = get_model_pose( world=self.world, model=robot_model, - link=self.robot_ee_link_name, + link=self.robot.ee_link, xyzw=True, ) return transform_change_reference_frame_pose( @@ -377,7 +382,7 @@ class Manipulation(Task, Node, abc.ABC): position=ee_position, quat=ee_quat_xyzw, target_model=robot_model, - target_link=self.robot_arm_base_link_name, + target_link=self.robot.base_link, xyzw=True, ) except Exception as e: @@ -385,8 +390,8 @@ class Manipulation(Task, Node, abc.ABC): f"Cannot get end effector pose from Gazebo ({e}), using tf2..." ) transform = self.tf2_listener.lookup_transform_sync( - source_frame=self.robot_ee_link_name, - target_frame=self.robot_arm_base_link_name, + source_frame=self.robot.ee_link, + target_frame=self.robot.base_link, retry=False, ) if transform is not None: @@ -412,31 +417,31 @@ class Manipulation(Task, Node, abc.ABC): (0.0, 0.0, 0.0, 1.0), ) - def get_ee_position(self) -> Tuple[float, float, float]: + def get_ee_position(self) -> Point: """ Return the current position of the end effector with respect to arm base link. """ try: - robot_model = self.world.to_gazebo().get_model(self.robot_name).to_gazebo() + robot_model = self.robot_wrapper ee_position = get_model_position( world=self.world, model=robot_model, - link=self.robot_ee_link_name, + link=self.robot.ee_link, ) return transform_change_reference_frame_position( world=self.world, position=ee_position, target_model=robot_model, - target_link=self.robot_arm_base_link_name, + target_link=self.robot.base_link, ) except Exception as e: self.get_logger().debug( f"Cannot get end effector position from Gazebo ({e}), using tf2..." ) transform = self.tf2_listener.lookup_transform_sync( - source_frame=self.robot_ee_link_name, - target_frame=self.robot_arm_base_link_name, + source_frame=self.robot.ee_link, + target_frame=self.robot.base_link, retry=False, ) if transform is not None: @@ -451,24 +456,24 @@ class Manipulation(Task, Node, abc.ABC): ) return (0.0, 0.0, 0.0) - def get_ee_orientation(self) -> Tuple[float, float, float, float]: + def get_ee_orientation(self) -> Quat: """ Return the current xyzw quaternion of the end effector with respect to arm base link. """ try: - robot_model = self.world.to_gazebo().get_model(self.robot_name).to_gazebo() + robot_model = self.robot_wrapper ee_quat_xyzw = get_model_orientation( world=self.world, model=robot_model, - link=self.robot_ee_link_name, + link=self.robot.ee_link, xyzw=True, ) return transform_change_reference_frame_orientation( world=self.world, quat=ee_quat_xyzw, target_model=robot_model, - target_link=self.robot_arm_base_link_name, + target_link=self.robot.base_link, xyzw=True, ) except Exception as e: @@ -476,8 +481,8 @@ class Manipulation(Task, Node, abc.ABC): f"Cannot get end effector orientation from Gazebo ({e}), using tf2..." ) transform = self.tf2_listener.lookup_transform_sync( - source_frame=self.robot_ee_link_name, - target_frame=self.robot_arm_base_link_name, + source_frame=self.robot.ee_link, + target_frame=self.robot.base_link, retry=False, ) if transform is not None: @@ -494,8 +499,8 @@ class Manipulation(Task, Node, abc.ABC): return (0.0, 0.0, 0.0, 1.0) def get_object_position( - self, object_model: Union[ModelWrapper, str] - ) -> Tuple[float, float, float]: + self, object_model: ModelWrapper | str + ) -> Point: """ Return the current position of an object with respect to arm base link. Note: Only simulated objects are currently supported. @@ -509,8 +514,8 @@ class Manipulation(Task, Node, abc.ABC): return transform_change_reference_frame_position( world=self.world, position=object_position, - target_model=self.robot_name, - target_link=self.robot_arm_base_link_name, + target_model=self.robot_wrapper.name(), + target_link=self.robot.base_link, ) except Exception as e: self.get_logger().error( @@ -518,7 +523,7 @@ class Manipulation(Task, Node, abc.ABC): ) return (0.0, 0.0, 0.0) - def get_object_positions(self) -> Dict[str, Tuple[float, float, float]]: + def get_object_positions(self) -> dict[str, Point]: """ Return the current position of all objects with respect to arm base link. Note: Only simulated objects are currently supported. @@ -527,22 +532,20 @@ class Manipulation(Task, Node, abc.ABC): object_positions = {} try: - robot_model = self.world.to_gazebo().get_model(self.robot_name).to_gazebo() - robot_arm_base_link = robot_model.get_link( - link_name=self.robot_arm_base_link_name - ) - for object_name in self.object_names: + robot_model = self.robot_wrapper + robot_arm_base_link = robot_model.get_link(link_name=self.robot.base_link) + for object_name in self.scene.__objects_unique_names: object_position = get_model_position( world=self.world, model=object_name, ) - object_positions[ - object_name - ] = transform_change_reference_frame_position( - world=self.world, - position=object_position, - target_model=robot_model, - target_link=robot_arm_base_link, + object_positions[object_name] = ( + transform_change_reference_frame_position( + world=self.world, + position=object_position, + target_model=robot_model, + target_link=robot_arm_base_link, + ) ) except Exception as e: self.get_logger().error( @@ -552,30 +555,30 @@ class Manipulation(Task, Node, abc.ABC): return object_positions def substitute_special_frame(self, frame_id: str) -> str: - if "arm_base_link" == frame_id: - return self.robot_arm_base_link_name + return self.robot.base_link elif "base_link" == frame_id: - return self.robot_base_link_name + return self.robot.base_link elif "end_effector" == frame_id: - return self.robot_ee_link_name + return self.robot.ee_link elif "world" == frame_id: try: # In Gazebo, where multiple worlds are allowed return self.world.to_gazebo().name() except Exception as e: - self.get_logger().warn(f"") + self.get_logger().warn(f"{e}") # Otherwise (e.g. real world) return "rbs_gym_world" else: return frame_id def wait_until_action_executed(self): - if self._enable_gripper: + #FIXME: code is not tested self.gripper.wait_until_executed() def move_to_initial_joint_configuration(self): + #TODO: Write this code for cartesian_control pass # self.moveit2.move_to_configuration(self.initial_arm_joint_positions) @@ -592,69 +595,66 @@ class Manipulation(Task, Node, abc.ABC): """ Returns true if robot links are in collision with the ground. """ - robot_name_len = len(self.robot_name) + robot_name_len = len(self.robot.name) terrain_model = self.world.get_model(self.terrain_name) for contact in terrain_model.contacts(): body_b = contact.body_b - - if body_b.startswith(self.robot_name) and len(body_b) > robot_name_len: - link = body_b[robot_name_len + 2:] - if link != self.robot_base_link_name and ( - link in self.robot_arm_link_names or link in self.robot_gripper_link_names + if body_b.startswith(self.robot.name) and len(body_b) > robot_name_len: + link = body_b[robot_name_len + 2 :] + + if link != self.robot.base_link and ( + link in self.robot.actuated_joint_names + or link in self.robot.gripper_actuated_joint_names ): return True return False - def check_all_objects_outside_workspace( - self, - object_positions: Dict[str, Tuple[float, float, float]], - ) -> bool: - """ - Returns true if all objects are outside the workspace - """ - - return all( - [ - self.check_object_outside_workspace(object_position) - for object_position in object_positions.values() - ] - ) - - def check_object_outside_workspace( - self, - object_position: Tuple[float, float, float], - ) -> bool: - """ - Returns true if the object is outside the workspace - """ - - return ( - object_position[0] < self.workspace_min_bound[0] - or object_position[1] < self.workspace_min_bound[1] - or object_position[2] < self.workspace_min_bound[2] - or object_position[0] > self.workspace_max_bound[0] - or object_position[1] > self.workspace_max_bound[1] - or object_position[2] > self.workspace_max_bound[2] - ) - - def add_parameter_overrides(self, parameter_overrides: Dict[str, any]): + # def check_all_objects_outside_workspace( + # self, + # object_positions: Dict[str, Tuple[float, float, float]], + # ) -> bool: + # """ + # Returns true if all objects are outside the workspace + # """ + # + # return all( + # [ + # self.check_object_outside_workspace(object_position) + # for object_position in object_positions.values() + # ] + # ) + # + # def check_object_outside_workspace( + # self, + # object_position: Tuple[float, float, float], + # ) -> bool: + # """ + # Returns true if the object is outside the workspace + # """ + # + # return ( + # object_position[0] < self.workspace_min_bound[0] + # or object_position[1] < self.workspace_min_bound[1] + # or object_position[2] < self.workspace_min_bound[2] + # or object_position[0] > self.workspace_max_bound[0] + # or object_position[1] > self.workspace_max_bound[1] + # or object_position[2] > self.workspace_max_bound[2] + # ) + def add_parameter_overrides(self, parameter_overrides: Dict[str, Any]): self.add_task_parameter_overrides(parameter_overrides) self.add_randomizer_parameter_overrides(parameter_overrides) - - def add_task_parameter_overrides(self, parameter_overrides: Dict[str, any]): - + # + def add_task_parameter_overrides(self, parameter_overrides: Dict[str, Any]): self.__task_parameter_overrides.update(parameter_overrides) - - def add_randomizer_parameter_overrides(self, parameter_overrides: Dict[str, any]): - + # + def add_randomizer_parameter_overrides(self, parameter_overrides: Dict[str, Any]): self._randomizer_parameter_overrides.update(parameter_overrides) def __consume_parameter_overrides(self): - for key, value in self.__task_parameter_overrides.items(): if hasattr(self, key): setattr(self, key, value) @@ -668,3 +668,39 @@ class Manipulation(Task, Node, abc.ABC): ) self.__task_parameter_overrides.clear() + + @property + def robot(self) -> Robot: + """The robot property.""" + if self._scene: + return self._scene.robot_wrapper.robot + else: + raise ValueError("R") + + @property + def robot_data(self) -> RobotData: + """The robot_data property.""" + if self._scene: + return self._scene.robot + else: + raise ValueError("RD") + + @property + def robot_wrapper(self) -> RobotWrapper: + """The robot_wrapper property.""" + if self._scene: + return self._scene.robot_wrapper + else: + raise ValueError("RW") + + @property + def scene(self) -> Scene: + """The scene property.""" + if self._scene: + return self._scene + else: + raise ValueError("S") + + @scene.setter + def scene(self, value: Scene): + self._scene = value diff --git a/env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach.py b/env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach.py index 8c595a9..5cba474 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach.py +++ b/env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach.py @@ -13,9 +13,10 @@ from gym_gz.utils.typing import ( ) from rbs_gym.envs.tasks.manipulation import Manipulation -from rbs_gym.envs.utils.math import distance_to_nearest_point +from env_manager.utils.math import distance_to_nearest_point from std_msgs.msg import Float64MultiArray from rbs_gym.envs.observation import TwistSubscriber, JointStates +from env_manager.utils.helper import get_object_position class Reach(Manipulation, abc.ABC): @@ -27,7 +28,6 @@ class Reach(Manipulation, abc.ABC): required_accuracy: float, **kwargs, ): - # Initialize the Task base class Manipulation.__init__( self, @@ -50,30 +50,30 @@ class Reach(Manipulation, abc.ABC): self._is_terminated: bool = False # Distance to target in the previous step (or after reset) - self._previous_distance: float = None + self._previous_distance: float = 0.0 # self._collision_reward: float = collision_reward - self.initial_gripper_joint_positions = ( - self.robot_model_class.CLOSED_GRIPPER_JOINT_POSITIONS + # self.initial_gripper_joint_positions = self.robot_data.gripper_jont_positions + self.twist = TwistSubscriber( + self, + topic="/cartesian_force_controller/current_twist", + callback_group=self._callback_group, ) - self.twist = TwistSubscriber(self, - topic="/cartesian_force_controller/current_twist", - callback_group=self._callback_group) - self.joint_states = JointStates(self, topic="/joint_states", callback_group=self._callback_group) + self.joint_states = JointStates( + self, topic="/joint_states", callback_group=self._callback_group + ) self._action = WrenchStamped() - self._action_array: Action = [] - + self._action_array: Action = np.array([]) + self.followed_object_name = "sphere" def create_action_space(self) -> ActionSpace: - # 0:3 - (x, y, z) force # 3:6 - (x, y, z) torque return gym.spaces.Box(low=-1.0, high=1.0, shape=(3,), dtype=np.float32) def create_observation_space(self) -> ObservationSpace: - # 0:3 - (x, y, z) end effector position # 3:6 - (x, y, z) target position # 6:9 - (x, y, z) current twist @@ -81,18 +81,19 @@ class Reach(Manipulation, abc.ABC): return gym.spaces.Box(low=-np.inf, high=np.inf, shape=(12,), dtype=np.float32) def set_action(self, action: Action): - # self.get_logger().debug(f"action: {action}") # act = Float64MultiArray() # act.data = action # self.joint_controller.publisher.publish(act) - + if isinstance(action, np.number): + raise RuntimeError("For the task Reach the action space should be array") + # Store action for reward function self._action_array = action # self._action.header.frame_id = self.robot_ee_link_name self._action.header.stamp = self.get_clock().now().to_msg() - self._action.header.frame_id = self.robot_ee_link_name + self._action.header.frame_id = self.robot.ee_link self._action.wrench.force.x = float(action[0]) * 30.0 self._action.wrench.force.y = float(action[1]) * 30.0 self._action.wrench.force.z = float(action[2]) * 30.0 @@ -102,21 +103,24 @@ class Reach(Manipulation, abc.ABC): self.controller.publisher.publish(self._action) def get_observation(self) -> Observation: - # Get current end-effector and target positions + self.object_names = [] ee_position = self.get_ee_position() - target_position = self.get_object_position(object_model=self.object_names[0]) + target_position = self.get_object_position(self.followed_object_name) + # target_position = self.get_object_position(object_model=self.object_names[0]) # joint_states = tuple(self.joint_states.get_positions()) # self.get_logger().warn(f"joint_states: {joint_states[:7]}") twist = self.twist.get_observation() - twt = (twist.twist.linear.x, - twist.twist.linear.y, - twist.twist.linear.z, - twist.twist.angular.x, - twist.twist.angular.y, - twist.twist.angular.z) + twt: tuple[float, float, float, float, float, float] = ( + twist.twist.linear.x, + twist.twist.linear.y, + twist.twist.linear.z, + twist.twist.angular.x, + twist.twist.angular.y, + twist.twist.angular.z, + ) # Create the observation observation = Observation( @@ -129,7 +133,6 @@ class Reach(Manipulation, abc.ABC): return observation def get_reward(self) -> Reward: - reward = 0.0 # Compute the current distance to the target @@ -138,7 +141,7 @@ class Reach(Manipulation, abc.ABC): # Mark the episode done if target is reached if current_distance < self._required_accuracy: self._is_terminated = True - reward += 1.0 if self._sparse_reward else 0.0 # 100.0 + reward += 1.0 if self._sparse_reward else 0.0 # 100.0 self.get_logger().debug(f"reward_target: {reward}") # Give reward based on how much closer robot got relative to the target for dense reward @@ -167,12 +170,12 @@ class Reach(Manipulation, abc.ABC): def is_terminated(self) -> bool: self.get_logger().debug(f"terminated: {self._is_terminated}") return self._is_terminated + def is_truncated(self) -> bool: self.get_logger().debug(f"truncated: {self._is_truncated}") return self._is_truncated def reset_task(self): - Manipulation.reset_task(self) self._is_done = False @@ -183,12 +186,13 @@ class Reach(Manipulation, abc.ABC): if not self._sparse_reward: self._previous_distance = self.get_distance_to_target() - self.get_logger().debug(f"\ntask reset") - - def get_distance_to_target(self) -> Tuple[float, float, float]: + self.get_logger().debug("\ntask reset") + def get_distance_to_target(self) -> float: ee_position = self.get_ee_position() - object_position = self.get_object_position(object_model=self.object_names[0]) - self.tf2_broadcaster.broadcast_tf("world", "object", object_position, (0.0,0.0,0.0,1.0), xyzw=True) + object_position = self.get_object_position(object_model=self.followed_object_name) + self.tf2_broadcaster.broadcast_tf( + "world", "object", object_position, (0.0, 0.0, 0.0, 1.0), xyzw=True + ) return distance_to_nearest_point(origin=ee_position, points=[object_position]) diff --git a/env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach_color_image.py b/env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach_color_image.py index 2c934d4..32ea90a 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach_color_image.py +++ b/env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach_color_image.py @@ -4,7 +4,7 @@ import gymnasium as gym import numpy as np from gym_gz.utils.typing import Observation, ObservationSpace -from rbs_gym.envs.models.sensors import Camera +from env_manager.models.sensors import Camera from rbs_gym.envs.observation import CameraSubscriber from rbs_gym.envs.tasks.reach import Reach diff --git a/env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach_depth_image.py b/env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach_depth_image.py index bf77475..00d64b9 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach_depth_image.py +++ b/env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach_depth_image.py @@ -4,7 +4,7 @@ import gymnasium as gym import numpy as np from gym_gz.utils.typing import Observation, ObservationSpace -from rbs_gym.envs.models.sensors import Camera +from env_manager.models.sensors import Camera from rbs_gym.envs.observation import CameraSubscriber from rbs_gym.envs.tasks.reach import Reach diff --git a/env_manager/rbs_gym/scripts/test_agent.py b/env_manager/rbs_gym/scripts/test_agent.py index 285a68a..96ac550 100755 --- a/env_manager/rbs_gym/scripts/test_agent.py +++ b/env_manager/rbs_gym/scripts/test_agent.py @@ -61,7 +61,7 @@ if __name__ == "__main__": # Environment and its parameters parser.add_argument( - "--env", type=str, default="Reach-ColorImage-Gazebo-v0", help="Environment ID" + "--env", type=str, default="Reach-Gazebo-v0", help="Environment ID" ) parser.add_argument( "--env-kwargs", diff --git a/env_manager/planning_scene_manager/CMakeLists.txt b/env_manager/rbs_runtime/CMakeLists.txt similarity index 84% rename from env_manager/planning_scene_manager/CMakeLists.txt rename to env_manager/rbs_runtime/CMakeLists.txt index 4a46988..cf2b965 100644 --- a/env_manager/planning_scene_manager/CMakeLists.txt +++ b/env_manager/rbs_runtime/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(planning_scene_manager) +project(rbs_runtime) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -11,6 +11,14 @@ find_package(ament_cmake REQUIRED) # further dependencies manually. # find_package( REQUIRED) +ament_python_install_package(${PROJECT_NAME}) + + +install(PROGRAMS + scripts/runtime.py + DESTINATION lib/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -23,4 +31,6 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + ament_package() diff --git a/env_manager/env_interface/LICENSE b/env_manager/rbs_runtime/LICENSE similarity index 100% rename from env_manager/env_interface/LICENSE rename to env_manager/rbs_runtime/LICENSE diff --git a/env_manager/rbs_runtime/config/scene_config.yaml b/env_manager/rbs_runtime/config/scene_config.yaml new file mode 100644 index 0000000..f3af7fa --- /dev/null +++ b/env_manager/rbs_runtime/config/scene_config.yaml @@ -0,0 +1,139 @@ +robot: + name: rbs_arm + urdf_string: "" + spawn_position: + - 0.0 + - 0.0 + - 0.0 + spawn_quat_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + joint_positioins: + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + with_gripper: true + gripper_jont_positions: + - 0 + randomizer: + pose: false + spawn_volume: + - 1.0 + - 1.0 + - 0.0 + joint_positions: false + joint_positions_std: 0.1 + joint_positions_above_object_spawn: false + joint_positions_above_object_spawn_elevation: 0.2 + joint_positions_above_object_spawn_xy_randomness: 0.2 + +terrain: + type: flat + spawn_position: + - 0 + - 0 + - 0 + spawn_quat_xyzw: + - 0 + - 0 + - 0 + - 1 + size: + - 1.5 + - 1.5 + model_rollouts_num: 1 + +light: + type: sun + direction: + - 0.5 + - -0.25 + - -0.75 + random_minmax_elevation: + - -0.15 + - -0.65 + color: + - 1.0 + - 1.0 + - 1.0 + - 1.0 + distance: 1000.0 + visual: true + radius: 25.0 + model_rollouts_num: 1 + +objects: + - name: bishop + type: "" + relative_to: world + position: + - 0.0 + - 0.0 + - 0.0 + orientation: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + static: false + randomize: + count: 0 + random_pose: false + random_position: false + random_orientation: false + random_model: false + random_spawn_position_segments: [] + random_spawn_position_update_workspace_centre: false + random_spawn_volume: + - 0.5 + - 0.5 + - 0.5 + models_rollouts_num: 0 + texture: [] + +camera: + - name: robot_camera + enable: true + type: rgbd_camera + relative_to: base_link + width: 128 + height: 128 + image_format: R8G8B8 + update_rate: 10 + horizontal_fov: 1.0471975511965976 + vertical_fov: 1.0471975511965976 + clip_color: + - 0.01 + - 1000.0 + clip_depth: + - 0.05 + - 10.0 + noise_mean: null + noise_stddev: null + publish_color: false + publish_depth: false + publish_points: false + spawn_position: + - 0 + - 0 + - 1 + spawn_quat_xyzw: + - 0 + - 0.70710678118 + - 0 + - 0.70710678118 + random_pose_rollouts_num: 1 + random_pose_mode: orbit + random_pose_orbit_distance: 1.0 + random_pose_orbit_height_range: + - 0.1 + - 0.7 + random_pose_orbit_ignore_arc_behind_robot: 0.39269908169872414 + random_pose_select_position_options: [] + random_pose_focal_point_z_offset: 0.0 + random_pose_rollout_counter: 0.0 diff --git a/env_manager/rbs_runtime/launch/runtime.launch.py b/env_manager/rbs_runtime/launch/runtime.launch.py new file mode 100644 index 0000000..32ef1ea --- /dev/null +++ b/env_manager/rbs_runtime/launch/runtime.launch.py @@ -0,0 +1,278 @@ +import os + +import xacro +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, + TimerAction, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + # Initialize Arguments + robot_type = LaunchConfiguration("robot_type") + # General arguments + with_gripper_condition = LaunchConfiguration("with_gripper") + cartesian_controllers = LaunchConfiguration("cartesian_controllers") + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + robot_name = LaunchConfiguration("robot_name") + start_joint_controller = LaunchConfiguration("start_joint_controller") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + launch_simulation = LaunchConfiguration("launch_sim") + launch_moveit = LaunchConfiguration("launch_moveit") + launch_task_planner = LaunchConfiguration("launch_task_planner") + launch_perception = LaunchConfiguration("launch_perception") + moveit_config_package = LaunchConfiguration("moveit_config_package") + moveit_config_file = LaunchConfiguration("moveit_config_file") + use_sim_time = LaunchConfiguration("use_sim_time") + sim_gazebo = LaunchConfiguration("sim_gazebo") + hardware = LaunchConfiguration("hardware") + env_manager = LaunchConfiguration("env_manager") + gazebo_gui = LaunchConfiguration("gazebo_gui") + gripper_name = LaunchConfiguration("gripper_name") + + initial_joint_controllers_file_path = os.path.join( + get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml" + ) + + xacro_file = os.path.join( + get_package_share_directory(description_package.perform(context)), + "urdf", + description_file.perform(context), + ) + + robot_description_doc = xacro.process_file( + xacro_file, + mappings={ + "gripper_name": gripper_name.perform(context), + "hardware": hardware.perform(context), + "simulation_controllers": initial_joint_controllers_file_path, + "namespace": "", + }, + ) + + robot_description_content = robot_description_doc.toprettyxml(indent=" ") + robot_description = {"robot_description": robot_description_content} + + single_robot_setup = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [FindPackageShare("rbs_bringup"), "launch", "rbs_robot.launch.py"] + ) + ] + ), + launch_arguments={ + "env_manager": env_manager, + "with_gripper": with_gripper_condition, + "gripper_name": gripper_name, + "controllers_file": initial_joint_controllers_file_path, + "robot_type": robot_type, + # "controllers_file": controller_paramfile, + "cartesian_controllers": cartesian_controllers, + "description_package": description_package, + "description_file": description_file, + "robot_name": robot_type, + "start_joint_controller": start_joint_controller, + "initial_joint_controller": initial_joint_controller, + "launch_simulation": launch_simulation, + "launch_moveit": launch_moveit, + "launch_task_planner": launch_task_planner, + "launch_perception": launch_perception, + "moveit_config_package": moveit_config_package, + "moveit_config_file": moveit_config_file, + "use_sim_time": use_sim_time, + "sim_gazebo": sim_gazebo, + "hardware": hardware, + "launch_controllers": "true", + "gazebo_gui": gazebo_gui, + "robot_description": robot_description_content, + }.items(), + ) + + rbs_runtime = Node( + package="rbs_runtime", + executable="runtime.py", + parameters=[robot_description, {"use_sim_time": True}], + ) + + clock_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"], + output="screen", + ) + + delay_robot_control_stack = TimerAction(period=10.0, actions=[single_robot_setup]) + + nodes_to_start = [rbs_runtime, clock_bridge, + delay_robot_control_stack + ] + return nodes_to_start + + +def generate_launch_description(): + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_type", + description="Type of robot by name", + choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], + default_value="rbs_arm", + ) + ) + # General arguments + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="rbs_arm0_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="rbs_arm", + description="Description package with robot URDF/XACRO files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="rbs_arm_modular.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_name", + default_value="arm0", + description="Name for robot, used to apply namespace for specific robot in multirobot setup", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "start_joint_controller", + default_value="false", + description="Enable headless mode for robot control", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="joint_trajectory_controller", + description="Robot controller to start.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_config_package", + default_value="rbs_arm", + description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \ + is not set, it enables use of a custom moveit config.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_config_file", + default_value="rbs_arm.srdf.xacro", + description="MoveIt SRDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="Make MoveIt to use simulation time.\ + This is needed for the trajectory planing in simulation.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "gripper_name", + default_value="rbs_gripper", + choices=["rbs_gripper", ""], + description="choose gripper by name (leave empty if hasn't)", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "with_gripper", default_value="true", description="With gripper or not?" + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "sim_gazebo", default_value="true", description="Gazebo Simulation" + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "env_manager", default_value="true", description="Launch env_manager?" + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "launch_sim", + default_value="true", + description="Launch simulator (Gazebo)?\ + Most general arg", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "launch_moveit", default_value="false", description="Launch moveit?" + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "launch_perception", default_value="false", description="Launch perception?" + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "launch_task_planner", + default_value="false", + description="Launch task_planner?", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "cartesian_controllers", + default_value="true", + description="Load cartesian\ + controllers?", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "hardware", + choices=["gazebo", "mock"], + default_value="gazebo", + description="Choose your harware_interface", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "launch_controllers", + default_value="true", + description="Launch controllers?", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "gazebo_gui", default_value="true", description="Launch gazebo with gui?" + ) + ) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/env_manager/planning_scene_manager/package.xml b/env_manager/rbs_runtime/package.xml similarity index 82% rename from env_manager/planning_scene_manager/package.xml rename to env_manager/rbs_runtime/package.xml index ea47535..9e85d40 100644 --- a/env_manager/planning_scene_manager/package.xml +++ b/env_manager/rbs_runtime/package.xml @@ -1,10 +1,10 @@ - planning_scene_manager + rbs_runtime 0.0.0 TODO: Package description - Bill Finger + narmak Apache-2.0 ament_cmake diff --git a/env_manager/rbs_runtime/rbs_runtime/__init__.py b/env_manager/rbs_runtime/rbs_runtime/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/env_manager/rbs_runtime/scripts/runtime.py b/env_manager/rbs_runtime/scripts/runtime.py new file mode 100755 index 0000000..ad263e1 --- /dev/null +++ b/env_manager/rbs_runtime/scripts/runtime.py @@ -0,0 +1,81 @@ +#!/usr/bin/python3 + +from rclpy.lifecycle import LifecycleNode, LifecycleState, TransitionCallbackReturn +from rclpy.node import Node +from env_manager.scene import Scene +from env_manager.models.configs import ( + CameraData, + SceneData, + RobotData, + ObjectData, + MeshObjectData, + TerrainData, + LightData, +) +import rclpy +from scenario.bindings.gazebo import GazeboSimulator +from env_manager_interfaces.srv import ResetEnv +from rbs_assets_library import get_world_file + + +DEFAULT_SCENE: SceneData = SceneData( + robot=RobotData( + name="rbs_arm", + with_gripper=True, + joint_positioins=[0, 0, 0, 0, 0, 0, 0], + gripper_jont_positions=[0], + ), + objects=[MeshObjectData("bishop", position=(0.0, 1.0, 0.3))], + camera=[CameraData("robot_camera")], +) + + +class GazeboRuntime(Node): + def __init__(self) -> None: + super().__init__(node_name="rbs_gz_runtime") + self.declare_parameter("robot_description", "") + self.gazebo = GazeboSimulator(step_size=0.001, rtf=1.0, steps_per_run=1) + + self.gazebo.insert_world_from_sdf(get_world_file("default")) + if not self.gazebo.initialize(): + raise RuntimeError("Gazebo cannot be initialized") + + self.scene = Scene( + node=self, + gazebo=self.gazebo, + scene=DEFAULT_SCENE, + robot_urdf_string=self.get_parameter("robot_description") + .get_parameter_value() + .string_value, + ) + self.scene.init_scene() + + self.reset_env_srv = self.create_service( + ResetEnv, "/env_manager/reset_env", self.reset_env + ) + self.is_env_reset = False + self.timer = self.create_timer(self.gazebo.step_size(), self.gazebo_run) + + def gazebo_run(self): + if self.is_env_reset: + self.scene.reset_scene() + self.is_env_reset = False + self.gazebo.run() + + def reset_env(self, req, res): + self.is_env_reset = True + res.ok = self.is_env_reset + return res + + +def main(args=None): + rclpy.init(args=args) + runtime_node = GazeboRuntime() + rclpy.spin(runtime_node) + runtime_node.gazebo.close() + runtime_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main()