ADD pcg-gazebo

.gitignore

@@ -4,6 +4,4 @@ ref
 *.vscode
 **/tensorboard_logs/**
 **/logs/**
-**/docs/_build/
-**/docs/build/
-**/__pycache__/**
+result

.gitlab-ci.yml

@@ -9,20 +9,15 @@ workflow:
 stages:
   - build
 
-variables:
-  GIT_SUBMODULE_STRATEGY: recursive
-
 build-colcon-job:
   stage: build
   script:
   - apt-get update
-  - apt-get install -y python3-pip libgeos-dev
   - mkdir -p .src/robossembler-ros2
   - mv * .src/robossembler-ros2
   - mv .git .src/robossembler-ros2
   - mv .src src
-  - vcs import src < src/robossembler-ros2/repos/all-deps.repos
-  - pip install -r src/robossembler-ros2/repos/requirements.txt
+  - vcs import src < src/robossembler-ros2/repos/sim.rbs.repos
   - rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
   - colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
   rules:

.gitmodules

@@ -1,4 +0,0 @@
-[submodule "env_manager"]
-	path = env_manager
-	url = https://gitlab.com/solid-sinusoid/env_manager.git
-	branch = main

Dockerfile

@@ -4,8 +4,6 @@ ARG WSDIR=rbs_ws
 ENV RBS_ASSEMBLY_DIR=/assembly
 ENV DEBIAN_FRONTEND=noninteractive
 
-SHELL ["/bin/bash", "-c"]
-
 # COPY /home/bill-finger/assembly /assembly
 ENV IGN_GAZEBO_RESOURCE_PATH=/${WSDIR}/install/rbs_simulation/share/rbs_simulation/
 
@@ -15,39 +13,53 @@ RUN apt-get update && apt-get upgrade -y && apt-get install -y \
     python3-pip \ 
     lsb-release \
     curl \
-    wget \
-    libgeos-dev
+    wget
+
+
+# WORKDIR /libs
+# RUN wget https://github.com/nlohmann/json/archive/refs/tags/v3.11.3.tar.gz &&\
+#     tar -xf v3.11.3.tar.gz &&\
+#     cd json-3.11.3 &&\
+#     mkdir build &&\
+#     cd build &&\
+#     cmake .. &&\
+#     make &&\
+#     make install
 
 RUN add-apt-repository universe
 RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
 RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null
 
 
-# Insatll ROS2 and tools
-RUN apt-get update &&\
-  apt-get upgrade &&\
-  apt-get install -y ros-humble-ros-base\
-  python3-rosdep\
-  python3-colcon-common-extensions
+RUN apt-get update && apt-get upgrade && apt-get install -y ros-humble-ros-base
 
-WORKDIR /${RBS_ASSEMBLY_DIR}
-RUN curl -s https://packagecloud.io/install/repositories/github/git-lfs/script.deb.sh | bash
-RUN apt-get install git-lfs
+# RUN git clone https://gitlab.com/robossembler/forks/megapose6d.git &&\
+#     cd megapose6d &&\
+#     pip install bokeh joblib pin torch transforms3d webdataset omegaconf tqdm &&\
+#     pip install -e .
+# RUN git clone https://github.com/thodan/bop_toolkit &&\
+#     cd bop_toolkit &&\
+#     pip install -e .
 
 WORKDIR /${WSDIR}
 
 COPY . src/robossembler-ros2/
 
-RUN pip install vcstool
+RUN pip install vcstool uv
 
 # Install framework and dependencies
 RUN vcs import src/. < src/robossembler-ros2/repos/all-deps.repos
-RUN pip install -r src/robossembler-ros2/repos/requirements.txt
-RUN apt-get update && source /opt/ros/humble/setup.bash && rosdep init && rosdep update && \
+RUN uv pip install --system -r src/robossembler-ros2/repos/requirements.txt
+RUN apt-get update && rosdep update && \
     rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
-RUN . /opt/ros/humble/setup.bash && \
+RUN . /opt/ros/humble/setup.sh && \
     colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=1
 
 
+WORKDIR /${RBS_ASSEMBLY_DIR}
+RUN curl -s https://packagecloud.io/install/repositories/github/git-lfs/script.deb.sh | bash
+RUN apt-get install git-lfs
+RUN git clone https://github.com/solid-sinusoid/rbs_assets_library.git 
+RUN cd rbs_assets_library && git lfs pull && pip install -e .
 
 WORKDIR /${WSDIR}

README.md

@@ -22,6 +22,65 @@ Repo for ROS2 packages related to Robossembler
 10. `rbs_utils` - working with a config containing capture positions for details
 11. `rbss_objectdetection` - Object Detection Skill Server using YOLOv8
 
-[Installation instructon RU](./docs/ru/installation.md)
+## Instructions
+### Requirements
+* OS: Ubuntu 22.04
+* ROS 2 Humble
 
-[Installation instructon EN](./docs/en/installation.md)
+### Dependencies
+These are the primary dependencies required to use this project.
+
+* MoveIt 2 
+    > Install/build a version based on the selected ROS 2 release
+* Gazebo Fortress
+
+### Build
+
+1. Clone the repository
+2. Build packages `colcon build`
+
+
+Prepare workspace & install dependencies (So far only tested with UR robot arm)
+```bash
+mkdir -p ~/robossembler_ws/src && cd ~/robossembler_ws/src
+git clone https://gitlab.com/robosphere/robossembler-ros2
+vcs import . < robossembler-ros2/repos/sim.rbs.repos
+cd ..
+rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
+colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install
+```
+
+Additionally, if you want to use Cartesian controllers, such as stiffness or others, you need to execute the following:
+```
+# in directory robossembler_ws/src
+vcs import . < robossembler-ros2/repos/cartesian_controllers.repos
+```
+This will also install `ros2_control` and `gz_ros2_control` as packages, so it is recommended to delete global packages if they have been installed.
+
+### Set Gazebo enviroment variables
+Replace `[WS_FOLDER]` with your workspace folder
+```bash
+echo "export IGN_GAZEBO_RESOURCE_PATH=${IGN_GAZEBO_RESOURCE_PATH}:~/[WS_FOLDER]/install/rbs_simulation/share/rbs_simulation/" >> ~/.bashrc
+# or if you have alredy built the workspace
+echo "export IGN_GAZEBO_RESOURCE_PATH=${IGN_GAZEBO_RESOURCE_PATH}:~/$(ros2 pkg prefix rbs_simulation)/share/rbs_simulation/" >> ~/.bashrc
+```
+
+### Examples
+Activate current ROS2 enviroment:
+```
+. install/setup.bash
+```
+
+Launch MoveIt2, Gazebo, RViz:
+```bash
+ros2 launch rbs_bringup bringup.launch.py
+```
+
+Start BT node in another terminal
+```
+ros2 launch rbs_bt_executor rbs_executor.launch.py
+```
+
+### Links
+* [bt_v3_cpp](https://www.behaviortree.dev)
+* [moveit2](https://moveit.picknik.ai/humble/index.html)

behaviortree-ros2.nix

@@ -0,0 +1,24 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchFromGitHub, ament-cmake, behaviortree-cpp, boost, btcpp-ros2-interfaces, fmt, generate-parameter-library, rclcpp, rclcpp-action }:
+buildRosPackage rec {
+  pname = "ros-jazzy-behaviortree-ros2";
+  version = "0.2.0";
+
+  src = fetchFromGitHub {
+    owner = "BehaviorTree";
+    repo = "BehaviorTree.ROS2";
+    rev = "cc31ea7b97947f1aac6e8c37df6cec379c84a7d9";
+    sha256 = "0m0n85jab14rfssyjf9sf05rq1rdzs6kqwb467953f26bvcnckkh";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/behaviortree_ros2/";
+  buildInputs = [ ament-cmake ];
+  propagatedBuildInputs = [ behaviortree-cpp boost btcpp-ros2-interfaces fmt generate-parameter-library rclcpp rclcpp-action ];
+  nativeBuildInputs = [ ament-cmake ];
+
+  meta = {
+    description = "This package provides a ROS2 wrapper, on top of BehaviorTree.CPP.";
+    license = with lib.licenses; [ mit ];
+  };
+}

btcpp-ros2-interfaces.nix

@@ -0,0 +1,24 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchFromGitHub, action-msgs, ament-cmake, rosidl-default-generators, rosidl-default-runtime }:
+buildRosPackage rec {
+  pname = "ros-jazzy-btcpp-ros2-interfaces";
+  version = "0.2.0";
+
+  src = fetchFromGitHub {
+    owner = "BehaviorTree";
+    repo = "BehaviorTree.ROS2";
+    rev = "cc31ea7b97947f1aac6e8c37df6cec379c84a7d9";
+    sha256 = "0m0n85jab14rfssyjf9sf05rq1rdzs6kqwb467953f26bvcnckkh";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/btcpp_ros2_interfaces/";
+  buildInputs = [ ament-cmake rosidl-default-generators ];
+  propagatedBuildInputs = [ action-msgs rosidl-default-runtime ];
+  nativeBuildInputs = [ ament-cmake rosidl-default-generators ];
+
+  meta = {
+    description = "ROS2 interfaces, mostly used to demonstrate behaviortree_ros2";
+    license = with lib.licenses; [ mit ];
+  };
+}

btcpp-ros2-samples.nix

@@ -0,0 +1,24 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchFromGitHub, ament-cmake, behaviortree-ros2, btcpp-ros2-interfaces, std-msgs, std-srvs }:
+buildRosPackage rec {
+  pname = "ros-jazzy-btcpp-ros2-samples";
+  version = "0.2.0";
+
+  src = fetchFromGitHub {
+    owner = "BehaviorTree";
+    repo = "BehaviorTree.ROS2";
+    rev = "cc31ea7b97947f1aac6e8c37df6cec379c84a7d9";
+    sha256 = "0m0n85jab14rfssyjf9sf05rq1rdzs6kqwb467953f26bvcnckkh";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/btcpp_ros2_samples/";
+  buildInputs = [ ament-cmake ];
+  propagatedBuildInputs = [ behaviortree-ros2 btcpp-ros2-interfaces std-msgs std-srvs ];
+  nativeBuildInputs = [ ament-cmake ];
+
+  meta = {
+    description = "Examples related to behaviortree_ros2";
+    license = with lib.licenses; [ mit ];
+  };
+}

cartesian-compliance-controller.nix

@@ -0,0 +1,24 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchgit, ament-cmake, ament-lint-common, cartesian-controller-base, cartesian-force-controller, cartesian-motion-controller, controller-interface, hardware-interface, pluginlib, rclcpp }:
+buildRosPackage rec {
+  pname = "ros-jazzy-cartesian-compliance-controller";
+  version = "0.0.0";
+
+  src = fetchgit {
+    url = "https://seed.solid-sinusoid.com/z4HmBYurXbXCtDwaHeizszqTMJT7R.git";
+    rev = "350b722a7f0d9f3431cdf8cd5704618895741edc";
+    sha256 = "sha256-AoAno/VKbvBwfOfYiv9umJohL9y87aN6Fq3F5aha970=";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/cartesian_compliance_controller/";
+  buildInputs = [ ament-cmake ];
+  checkInputs = [ ament-lint-common ];
+  propagatedBuildInputs = [ cartesian-controller-base cartesian-force-controller cartesian-motion-controller controller-interface hardware-interface pluginlib rclcpp ];
+  nativeBuildInputs = [ ament-cmake ];
+
+  meta = {
+    description = "Control your robot through Cartesian target poses and target wrenches";
+    license = with lib.licenses; [ bsd3 ];
+  };
+}

cartesian-controller-base.nix

@@ -0,0 +1,23 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchgit, ament-cmake, controller-interface, kdl-parser, pluginlib, rclcpp, realtime-tools, trajectory-msgs, ros-environment }:
+buildRosPackage rec {
+  pname = "ros-jazzy-cartesian-controller-base";
+  version = "0.0.0";
+
+  src = fetchgit {
+    url = "https://seed.solid-sinusoid.com/z4HmBYurXbXCtDwaHeizszqTMJT7R.git";
+    rev = "350b722a7f0d9f3431cdf8cd5704618895741edc";
+    sha256 = "sha256-AoAno/VKbvBwfOfYiv9umJohL9y87aN6Fq3F5aha970=";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/cartesian_controller_base/";
+  buildInputs = [ ament-cmake ];
+  propagatedBuildInputs = [ controller-interface kdl-parser pluginlib rclcpp realtime-tools trajectory-msgs ros-environment ];
+  nativeBuildInputs = [ ament-cmake ];
+
+  meta = {
+    description = "The cartesian_controller_base package";
+    license = with lib.licenses; [ bsd3 ];
+  };
+}

cartesian-controller-handles.nix

@@ -0,0 +1,23 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchgit, ament-cmake, cartesian-controller-base, controller-interface, geometry-msgs, interactive-markers, kdl-parser, pluginlib, rclcpp, urdf }:
+buildRosPackage rec {
+  pname = "ros-jazzy-cartesian-controller-handles";
+  version = "0.0.0";
+
+  src = fetchgit {
+    url = "https://seed.solid-sinusoid.com/z4HmBYurXbXCtDwaHeizszqTMJT7R.git";
+    rev = "350b722a7f0d9f3431cdf8cd5704618895741edc";
+    sha256 = "sha256-AoAno/VKbvBwfOfYiv9umJohL9y87aN6Fq3F5aha970=";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/cartesian_controller_handles/";
+  buildInputs = [ ament-cmake ];
+  propagatedBuildInputs = [ cartesian-controller-base controller-interface geometry-msgs interactive-markers kdl-parser pluginlib rclcpp urdf ];
+  nativeBuildInputs = [ ament-cmake ];
+
+  meta = {
+    description = "The cartesian_controller_handles package";
+    license = with lib.licenses; [ bsd3 ];
+  };
+}

cartesian-controller-utilities.nix

@@ -0,0 +1,23 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchgit, ament-cmake, geometry-msgs, launch, launch-ros, rclcpp, sensor-msgs }:
+buildRosPackage rec {
+  pname = "ros-jazzy-cartesian-controller-utilities";
+  version = "0.1.0";
+
+  src = fetchgit {
+    url = "https://seed.solid-sinusoid.com/z4HmBYurXbXCtDwaHeizszqTMJT7R.git";
+    rev = "350b722a7f0d9f3431cdf8cd5704618895741edc";
+    sha256 = "sha256-AoAno/VKbvBwfOfYiv9umJohL9y87aN6Fq3F5aha970=";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/cartesian_controller_utilities/";
+  buildInputs = [ ament-cmake ];
+  propagatedBuildInputs = [ geometry-msgs launch launch-ros rclcpp sensor-msgs ];
+  nativeBuildInputs = [ ament-cmake ];
+
+  meta = {
+    description = "Make the spacemouse a teach device for contact-based teleoperation";
+    license = with lib.licenses; [ bsd3 ];
+  };
+}

cartesian-force-controller.nix

@@ -0,0 +1,23 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchgit, ament-cmake, cartesian-controller-base, controller-interface, rclcpp }:
+buildRosPackage rec {
+  pname = "ros-jazzy-cartesian-force-controller";
+  version = "0.0.0";
+
+  src = fetchgit {
+    url = "https://seed.solid-sinusoid.com/z4HmBYurXbXCtDwaHeizszqTMJT7R.git";
+    rev = "350b722a7f0d9f3431cdf8cd5704618895741edc";
+    sha256 = "sha256-AoAno/VKbvBwfOfYiv9umJohL9y87aN6Fq3F5aha970=";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/cartesian_force_controller/";
+  buildInputs = [ ament-cmake ];
+  propagatedBuildInputs = [ cartesian-controller-base controller-interface rclcpp ];
+  nativeBuildInputs = [ ament-cmake ];
+
+  meta = {
+    description = "The cartesian_force_controller package";
+    license = with lib.licenses; [ bsd3 ];
+  };
+}

cartesian-motion-controller.nix

@@ -0,0 +1,21 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchgit, cartesian-controller-base, controller-interface, rclcpp }:
+buildRosPackage rec {
+  pname = "ros-jazzy-cartesian-motion-controller";
+  version = "0.0.0";
+
+  src = fetchgit {
+    url = "https://seed.solid-sinusoid.com/z4HmBYurXbXCtDwaHeizszqTMJT7R.git";
+    rev = "350b722a7f0d9f3431cdf8cd5704618895741edc";
+    sha256 = "sha256-AoAno/VKbvBwfOfYiv9umJohL9y87aN6Fq3F5aha970=";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/cartesian_motion_controller/";
+  propagatedBuildInputs = [ cartesian-controller-base controller-interface rclcpp ];
+
+  meta = {
+    description = "The cartesian_motion_controller package";
+    license = with lib.licenses; [ bsd3 ];
+  };
+}

cartesian-twist-controller.nix

@@ -0,0 +1,21 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchgit, cartesian-controller-base, controller-interface, rclcpp }:
+buildRosPackage rec {
+  pname = "ros-jazzy-cartesian-twist-controller";
+  version = "0.0.0";
+
+  src = fetchgit {
+    url = "https://seed.solid-sinusoid.com/z4HmBYurXbXCtDwaHeizszqTMJT7R.git";
+    rev = "350b722a7f0d9f3431cdf8cd5704618895741edc";
+    sha256 = "sha256-AoAno/VKbvBwfOfYiv9umJohL9y87aN6Fq3F5aha970=";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/cartesian_twist_controller/";
+  propagatedBuildInputs = [ cartesian-controller-base controller-interface rclcpp ];
+
+  meta = {
+    description = "The cartesian_twist_controller package";
+    license = with lib.licenses; [ bsd3 ];
+  };
+}

doc/Doxyfile

@@ -1,4 +1,4 @@
-# Doxyfile 1.8.17
+# Doxyfile 1.9.1
 
 # This file describes the settings to be used by the documentation system
 # doxygen (www.doxygen.org) for a project.
@@ -32,7 +32,7 @@ DOXYFILE_ENCODING      = UTF-8
 # title of most generated pages and in a few other places.
 # The default value is: My Project.
 
-PROJECT_NAME           = "Robossembler ROS2"
+PROJECT_NAME           = "Robossembler Framework ROS2"
 
 # The PROJECT_NUMBER tag can be used to enter a project or revision number. This
 # could be handy for archiving the generated documentation or if some version
@@ -58,7 +58,7 @@ PROJECT_LOGO           =
 # entered, it will be relative to the location where doxygen was started. If
 # left blank the current directory will be used.
 
-OUTPUT_DIRECTORY       = docs/_build/
+OUTPUT_DIRECTORY       = doc/_build/
 
 # If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
 # directories (in 2 levels) under the output directory of each output format and
@@ -195,7 +195,7 @@ SHORT_NAMES            = NO
 # description.)
 # The default value is: NO.
 
-JAVADOC_AUTOBRIEF      = YES
+JAVADOC_AUTOBRIEF      = NO
 
 # If the JAVADOC_BANNER tag is set to YES then doxygen will interpret a line
 # such as
@@ -227,6 +227,14 @@ QT_AUTOBRIEF           = NO
 
 MULTILINE_CPP_IS_BRIEF = NO
 
+# By default Python docstrings are displayed as preformatted text and doxygen's
+# special commands cannot be used. By setting PYTHON_DOCSTRING to NO the
+# doxygen's special commands can be used and the contents of the docstring
+# documentation blocks is shown as doxygen documentation.
+# The default value is: YES.
+
+PYTHON_DOCSTRING       = YES
+
 # If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
 # documentation from any documented member that it re-implements.
 # The default value is: YES.
@@ -263,12 +271,6 @@ TAB_SIZE               = 4
 
 ALIASES                =
 
-# This tag can be used to specify a number of word-keyword mappings (TCL only).
-# A mapping has the form "name=value". For example adding "class=itcl::class"
-# will allow you to use the command class in the itcl::class meaning.
-
-TCL_SUBST              =
-
 # Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
 # only. Doxygen will then generate output that is more tailored for C. For
 # instance, some of the names that are used will be different. The list of all
@@ -283,7 +285,7 @@ OPTIMIZE_OUTPUT_FOR_C  = NO
 # qualified scopes will look different, etc.
 # The default value is: NO.
 
-OPTIMIZE_OUTPUT_JAVA   = YES
+OPTIMIZE_OUTPUT_JAVA   = NO
 
 # Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
 # sources. Doxygen will then generate output that is tailored for Fortran.
@@ -310,18 +312,21 @@ OPTIMIZE_OUTPUT_SLICE  = NO
 # extension. Doxygen has a built-in mapping, but you can override or extend it
 # using this tag. The format is ext=language, where ext is a file extension, and
 # language is one of the parsers supported by doxygen: IDL, Java, JavaScript,
-# Csharp (C#), C, C++, D, PHP, md (Markdown), Objective-C, Python, Slice,
+# Csharp (C#), C, C++, D, PHP, md (Markdown), Objective-C, Python, Slice, VHDL,
 # Fortran (fixed format Fortran: FortranFixed, free formatted Fortran:
 # FortranFree, unknown formatted Fortran: Fortran. In the later case the parser
 # tries to guess whether the code is fixed or free formatted code, this is the
-# default for Fortran type files), VHDL, tcl. For instance to make doxygen treat
-# .inc files as Fortran files (default is PHP), and .f files as C (default is
-# Fortran), use: inc=Fortran f=C.
+# default for Fortran type files). For instance to make doxygen treat .inc files
+# as Fortran files (default is PHP), and .f files as C (default is Fortran),
+# use: inc=Fortran f=C.
 #
 # Note: For files without extension you can use no_extension as a placeholder.
 #
 # Note that for custom extensions you also need to set FILE_PATTERNS otherwise
-# the files are not read by doxygen.
+# the files are not read by doxygen. When specifying no_extension you should add
+# * to the FILE_PATTERNS.
+#
+# Note see also the list of default file extension mappings.
 
 EXTENSION_MAPPING      =
 
@@ -455,6 +460,19 @@ TYPEDEF_HIDES_STRUCT   = NO
 
 LOOKUP_CACHE_SIZE      = 0
 
+# The NUM_PROC_THREADS specifies the number threads doxygen is allowed to use
+# during processing. When set to 0 doxygen will based this on the number of
+# cores available in the system. You can set it explicitly to a value larger
+# than 0 to get more control over the balance between CPU load and processing
+# speed. At this moment only the input processing can be done using multiple
+# threads. Since this is still an experimental feature the default is set to 1,
+# which efficively disables parallel processing. Please report any issues you
+# encounter. Generating dot graphs in parallel is controlled by the
+# DOT_NUM_THREADS setting.
+# Minimum value: 0, maximum value: 32, default value: 1.
+
+NUM_PROC_THREADS       = 1
+
 #---------------------------------------------------------------------------
 # Build related configuration options
 #---------------------------------------------------------------------------
@@ -467,7 +485,7 @@ LOOKUP_CACHE_SIZE      = 0
 # normally produced when WARNINGS is set to YES.
 # The default value is: NO.
 
-EXTRACT_ALL            = YES
+EXTRACT_ALL            = NO
 
 # If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
 # be included in the documentation.
@@ -518,6 +536,13 @@ EXTRACT_LOCAL_METHODS  = NO
 
 EXTRACT_ANON_NSPACES   = NO
 
+# If this flag is set to YES, the name of an unnamed parameter in a declaration
+# will be determined by the corresponding definition. By default unnamed
+# parameters remain unnamed in the output.
+# The default value is: YES.
+
+RESOLVE_UNNAMED_PARAMS = YES
+
 # If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
 # undocumented members inside documented classes or files. If set to NO these
 # members will be included in the various overviews, but no documentation
@@ -555,11 +580,18 @@ HIDE_IN_BODY_DOCS      = NO
 
 INTERNAL_DOCS          = NO
 
-# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
-# names in lower-case letters. If set to YES, upper-case letters are also
-# allowed. This is useful if you have classes or files whose names only differ
-# in case and if your file system supports case sensitive file names. Windows
-# (including Cygwin) ands Mac users are advised to set this option to NO.
+# With the correct setting of option CASE_SENSE_NAMES doxygen will better be
+# able to match the capabilities of the underlying filesystem. In case the
+# filesystem is case sensitive (i.e. it supports files in the same directory
+# whose names only differ in casing), the option must be set to YES to properly
+# deal with such files in case they appear in the input. For filesystems that
+# are not case sensitive the option should be be set to NO to properly deal with
+# output files written for symbols that only differ in casing, such as for two
+# classes, one named CLASS and the other named Class, and to also support
+# references to files without having to specify the exact matching casing. On
+# Windows (including Cygwin) and MacOS, users should typically set this option
+# to NO, whereas on Linux or other Unix flavors it should typically be set to
+# YES.
 # The default value is: system dependent.
 
 CASE_SENSE_NAMES       = YES
@@ -569,7 +601,7 @@ CASE_SENSE_NAMES       = YES
 # scope will be hidden.
 # The default value is: NO.
 
-HIDE_SCOPE_NAMES       = YES
+HIDE_SCOPE_NAMES       = NO
 
 # If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will
 # append additional text to a page's title, such as Class Reference. If set to
@@ -616,7 +648,7 @@ SORT_MEMBER_DOCS       = YES
 # this will also influence the order of the classes in the class list.
 # The default value is: NO.
 
-SORT_BRIEF_DOCS        = YES
+SORT_BRIEF_DOCS        = NO
 
 # If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
 # (brief and detailed) documentation of class members so that constructors and
@@ -798,7 +830,10 @@ WARN_IF_DOC_ERROR      = YES
 WARN_NO_PARAMDOC       = NO
 
 # If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
-# a warning is encountered.
+# a warning is encountered. If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS
+# then doxygen will continue running as if WARN_AS_ERROR tag is set to NO, but
+# at the end of the doxygen process doxygen will return with a non-zero status.
+# Possible values are: NO, YES and FAIL_ON_WARNINGS.
 # The default value is: NO.
 
 WARN_AS_ERROR          = NO
@@ -829,13 +864,14 @@ WARN_LOGFILE           =
 # spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
 # Note: If this tag is empty the current directory is searched.
 
-INPUT                  = README.md .
+INPUT                  = README.md \
+                         .
 
 # This tag can be used to specify the character encoding of the source files
 # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
 # libiconv (or the iconv built into libc) for the transcoding. See the libiconv
-# documentation (see: https://www.gnu.org/software/libiconv/) for the list of
-# possible encodings.
+# documentation (see:
+# https://www.gnu.org/software/libiconv/) for the list of possible encodings.
 # The default value is: UTF-8.
 
 INPUT_ENCODING         = UTF-8
@@ -848,13 +884,15 @@ INPUT_ENCODING         = UTF-8
 # need to set EXTENSION_MAPPING for the extension otherwise the files are not
 # read by doxygen.
 #
+# Note the list of default checked file patterns might differ from the list of
+# default file extension mappings.
+#
 # If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
 # *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
 # *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc,
 # *.m, *.markdown, *.md, *.mm, *.dox (to be provided as doxygen C comment),
-# *.doc (to be provided as doxygen C comment), *.txt (to be provided as doxygen
-# C comment), *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, *.f, *.for, *.tcl, *.vhd,
-# *.vhdl, *.ucf, *.qsf and *.ice.
+# *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, *.f18, *.f, *.for, *.vhd, *.vhdl,
+# *.ucf, *.qsf and *.ice.
 
 FILE_PATTERNS          = *.c \
                          *.cc \
@@ -1115,16 +1153,22 @@ USE_HTAGS              = NO
 VERBATIM_HEADERS       = YES
 
 # If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the
-# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the
-# cost of reduced performance. This can be particularly helpful with template
-# rich C++ code for which doxygen's built-in parser lacks the necessary type
-# information.
+# clang parser (see:
+# http://clang.llvm.org/) for more accurate parsing at the cost of reduced
+# performance. This can be particularly helpful with template rich C++ code for
+# which doxygen's built-in parser lacks the necessary type information.
 # Note: The availability of this option depends on whether or not doxygen was
 # generated with the -Duse_libclang=ON option for CMake.
 # The default value is: NO.
 
 CLANG_ASSISTED_PARSING = NO
 
+# If clang assisted parsing is enabled and the CLANG_ADD_INC_PATHS tag is set to
+# YES then doxygen will add the directory of each input to the include path.
+# The default value is: YES.
+
+CLANG_ADD_INC_PATHS    = YES
+
 # If clang assisted parsing is enabled you can provide the compiler with command
 # line options that you would normally use when invoking the compiler. Note that
 # the include paths will already be set by doxygen for the files and directories
@@ -1134,10 +1178,13 @@ CLANG_ASSISTED_PARSING = NO
 CLANG_OPTIONS          =
 
 # If clang assisted parsing is enabled you can provide the clang parser with the
-# path to the compilation database (see:
-# http://clang.llvm.org/docs/HowToSetupToolingForLLVM.html) used when the files
-# were built. This is equivalent to specifying the "-p" option to a clang tool,
-# such as clang-check. These options will then be passed to the parser.
+# path to the directory containing a file called compile_commands.json. This
+# file is the compilation database (see:
+# http://clang.llvm.org/docs/HowToSetupToolingForLLVM.html) containing the
+# options used when the source files were built. This is equivalent to
+# specifying the -p option to a clang tool, such as clang-check. These options
+# will then be passed to the parser. Any options specified with CLANG_OPTIONS
+# will be added as well.
 # Note: The availability of this option depends on whether or not doxygen was
 # generated with the -Duse_libclang=ON option for CMake.
 
@@ -1154,13 +1201,6 @@ CLANG_DATABASE_PATH    =
 
 ALPHABETICAL_INDEX     = YES
 
-# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
-# which the alphabetical index list will be split.
-# Minimum value: 1, maximum value: 20, default value: 5.
-# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
-
-COLS_IN_ALPHA_INDEX    = 5
-
 # In case all classes in a project start with a common prefix, all classes will
 # be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
 # can be used to specify a prefix (or a list of prefixes) that should be ignored
@@ -1331,10 +1371,11 @@ HTML_INDEX_NUM_ENTRIES = 100
 
 # If the GENERATE_DOCSET tag is set to YES, additional index files will be
 # generated that can be used as input for Apple's Xcode 3 integrated development
-# environment (see: https://developer.apple.com/xcode/), introduced with OSX
-# 10.5 (Leopard). To create a documentation set, doxygen will generate a
-# Makefile in the HTML output directory. Running make will produce the docset in
-# that directory and running make install will install the docset in
+# environment (see:
+# https://developer.apple.com/xcode/), introduced with OSX 10.5 (Leopard). To
+# create a documentation set, doxygen will generate a Makefile in the HTML
+# output directory. Running make will produce the docset in that directory and
+# running make install will install the docset in
 # ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
 # startup. See https://developer.apple.com/library/archive/featuredarticles/Doxy
 # genXcode/_index.html for more information.
@@ -1376,8 +1417,8 @@ DOCSET_PUBLISHER_NAME  = Publisher
 # If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
 # additional HTML index files: index.hhp, index.hhc, and index.hhk. The
 # index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
-# (see: https://www.microsoft.com/en-us/download/details.aspx?id=21138) on
-# Windows.
+# (see:
+# https://www.microsoft.com/en-us/download/details.aspx?id=21138) on Windows.
 #
 # The HTML Help Workshop contains a compiler that can convert all HTML output
 # generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
@@ -1407,7 +1448,7 @@ CHM_FILE               =
 HHC_LOCATION           =
 
 # The GENERATE_CHI flag controls if a separate .chi index file is generated
-# (YES) or that it should be included in the master .chm file (NO).
+# (YES) or that it should be included in the main .chm file (NO).
 # The default value is: NO.
 # This tag requires that the tag GENERATE_HTMLHELP is set to YES.
 
@@ -1452,7 +1493,8 @@ QCH_FILE               =
 
 # The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
 # Project output. For more information please see Qt Help Project / Namespace
-# (see: https://doc.qt.io/archives/qt-4.8/qthelpproject.html#namespace).
+# (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#namespace).
 # The default value is: org.doxygen.Project.
 # This tag requires that the tag GENERATE_QHP is set to YES.
 
@@ -1460,8 +1502,8 @@ QHP_NAMESPACE          = org.doxygen.Project
 
 # The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
 # Help Project output. For more information please see Qt Help Project / Virtual
-# Folders (see: https://doc.qt.io/archives/qt-4.8/qthelpproject.html#virtual-
-# folders).
+# Folders (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#virtual-folders).
 # The default value is: doc.
 # This tag requires that the tag GENERATE_QHP is set to YES.
 
@@ -1469,16 +1511,16 @@ QHP_VIRTUAL_FOLDER     = doc
 
 # If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
 # filter to add. For more information please see Qt Help Project / Custom
-# Filters (see: https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-
-# filters).
+# Filters (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters).
 # This tag requires that the tag GENERATE_QHP is set to YES.
 
 QHP_CUST_FILTER_NAME   =
 
 # The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
 # custom filter to add. For more information please see Qt Help Project / Custom
-# Filters (see: https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-
-# filters).
+# Filters (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters).
 # This tag requires that the tag GENERATE_QHP is set to YES.
 
 QHP_CUST_FILTER_ATTRS  =
@@ -1490,9 +1532,9 @@ QHP_CUST_FILTER_ATTRS  =
 
 QHP_SECT_FILTER_ATTRS  =
 
-# The QHG_LOCATION tag can be used to specify the location of Qt's
-# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
-# generated .qhp file.
+# The QHG_LOCATION tag can be used to specify the location (absolute path
+# including file name) of Qt's qhelpgenerator. If non-empty doxygen will try to
+# run qhelpgenerator on the generated .qhp file.
 # This tag requires that the tag GENERATE_QHP is set to YES.
 
 QHG_LOCATION           =
@@ -1569,6 +1611,17 @@ TREEVIEW_WIDTH         = 250
 
 EXT_LINKS_IN_WINDOW    = NO
 
+# If the HTML_FORMULA_FORMAT option is set to svg, doxygen will use the pdf2svg
+# tool (see https://github.com/dawbarton/pdf2svg) or inkscape (see
+# https://inkscape.org) to generate formulas as SVG images instead of PNGs for
+# the HTML output. These images will generally look nicer at scaled resolutions.
+# Possible values are: png (the default) and svg (looks nicer but requires the
+# pdf2svg or inkscape tool).
+# The default value is: png.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FORMULA_FORMAT    = png
+
 # Use this tag to change the font size of LaTeX formulas included as images in
 # the HTML documentation. When you change the font size after a successful
 # doxygen run you need to manually remove any form_*.png images from the HTML
@@ -1608,7 +1661,7 @@ USE_MATHJAX            = NO
 
 # When MathJax is enabled you can set the default output format to be used for
 # the MathJax output. See the MathJax site (see:
-# http://docs.mathjax.org/en/latest/output.html) for more details.
+# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details.
 # Possible values are: HTML-CSS (which is slower, but has the best
 # compatibility), NativeMML (i.e. MathML) and SVG.
 # The default value is: HTML-CSS.
@@ -1624,7 +1677,7 @@ MATHJAX_FORMAT         = HTML-CSS
 # Content Delivery Network so you can quickly see the result without installing
 # MathJax. However, it is strongly recommended to install a local copy of
 # MathJax from https://www.mathjax.org before deployment.
-# The default value is: https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/.
+# The default value is: https://cdn.jsdelivr.net/npm/mathjax@2.
 # This tag requires that the tag USE_MATHJAX is set to YES.
 
 MATHJAX_RELPATH        = https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/
@@ -1638,7 +1691,8 @@ MATHJAX_EXTENSIONS     =
 
 # The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
 # of code that will be used on startup of the MathJax code. See the MathJax site
-# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
+# (see:
+# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details. For an
 # example see the documentation.
 # This tag requires that the tag USE_MATHJAX is set to YES.
 
@@ -1685,7 +1739,8 @@ SERVER_BASED_SEARCH    = NO
 #
 # Doxygen ships with an example indexer (doxyindexer) and search engine
 # (doxysearch.cgi) which are based on the open source search engine library
-# Xapian (see: https://xapian.org/).
+# Xapian (see:
+# https://xapian.org/).
 #
 # See the section "External Indexing and Searching" for details.
 # The default value is: NO.
@@ -1698,8 +1753,9 @@ EXTERNAL_SEARCH        = NO
 #
 # Doxygen ships with an example indexer (doxyindexer) and search engine
 # (doxysearch.cgi) which are based on the open source search engine library
-# Xapian (see: https://xapian.org/). See the section "External Indexing and
-# Searching" for details.
+# Xapian (see:
+# https://xapian.org/). See the section "External Indexing and Searching" for
+# details.
 # This tag requires that the tag SEARCHENGINE is set to YES.
 
 SEARCHENGINE_URL       =
@@ -1863,9 +1919,11 @@ LATEX_EXTRA_FILES      =
 
 PDF_HYPERLINKS         = YES
 
-# If the USE_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
-# the PDF file directly from the LaTeX files. Set this option to YES, to get a
-# higher quality PDF documentation.
+# If the USE_PDFLATEX tag is set to YES, doxygen will use the engine as
+# specified with LATEX_CMD_NAME to generate the PDF file directly from the LaTeX
+# files. Set this option to YES, to get a higher quality PDF documentation.
+#
+# See also section LATEX_CMD_NAME for selecting the engine.
 # The default value is: YES.
 # This tag requires that the tag GENERATE_LATEX is set to YES.
 
@@ -2376,10 +2434,32 @@ UML_LOOK               = NO
 # but if the number exceeds 15, the total amount of fields shown is limited to
 # 10.
 # Minimum value: 0, maximum value: 100, default value: 10.
-# This tag requires that the tag HAVE_DOT is set to YES.
+# This tag requires that the tag UML_LOOK is set to YES.
 
 UML_LIMIT_NUM_FIELDS   = 10
 
+# If the DOT_UML_DETAILS tag is set to NO, doxygen will show attributes and
+# methods without types and arguments in the UML graphs. If the DOT_UML_DETAILS
+# tag is set to YES, doxygen will add type and arguments for attributes and
+# methods in the UML graphs. If the DOT_UML_DETAILS tag is set to NONE, doxygen
+# will not generate fields with class member information in the UML graphs. The
+# class diagrams will look similar to the default class diagrams but using UML
+# notation for the relationships.
+# Possible values are: NO, YES and NONE.
+# The default value is: NO.
+# This tag requires that the tag UML_LOOK is set to YES.
+
+DOT_UML_DETAILS        = NO
+
+# The DOT_WRAP_THRESHOLD tag can be used to set the maximum number of characters
+# to display on a single line. If the actual line length exceeds this threshold
+# significantly it will wrapped across multiple lines. Some heuristics are apply
+# to avoid ugly line breaks.
+# Minimum value: 0, maximum value: 1000, default value: 17.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_WRAP_THRESHOLD     = 17
+
 # If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
 # collaboration graphs will show the relations between templates and their
 # instances.
@@ -2571,9 +2651,11 @@ DOT_MULTI_TARGETS      = NO
 
 GENERATE_LEGEND        = YES
 
-# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate dot
+# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate
 # files that are used to generate the various graphs.
+#
+# Note: This setting is not only used for dot files but also for msc and
+# plantuml temporary files.
 # The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
 
 DOT_CLEANUP            = YES

doc/ru/add_new_robot.md

@@ -1,11 +1,9 @@
 # Инструкция по добавлению нового робота в фреймворк Robossembler ROS 2
 
-Прежде всего, необходимо скачать пакет робота, содержащий файлы `xacro` или `urdf`, а также файлы геометрии робота в формате `.stl`, `.dae`, `.obj` и других.
+Прежде всего необходимо скачать пакет робота, содержащий файлы `xacro` или `urdf`, а также файлы геометрии робота в формате `.stl`, `.dae`, `.obj` и других.
 
 Перед началом работы важно ознакомиться с основными аспектами формата [xacro](https://github.com/ros/xacro/wiki). Этот формат позволяет переиспользовать существующие фрагменты URDF-описания робота, что упрощает создание и модификацию описания.
 
----
-
 ### Шаги по добавлению нового робота:
 
 1. **Установка пакета робота**  
@@ -26,6 +24,7 @@
        ),
        launch_arguments={
            "with_gripper": "true",
+           "gripper_name": "rbs_gripper",
            "robot_type": "rbs_arm",
            "description_package": "rbs_arm",
            "description_file": "rbs_arm_modular.xacro",
@@ -34,27 +33,26 @@
            "moveit_config_package": "rbs_arm",
            "moveit_config_file": "rbs_arm.srdf.xacro",
            "use_sim_time": "true",
+           "hardware": "gazebo",
            "use_controllers": "true",
            "scene_config_file": "",
            "base_link_name": "base_link",
            "ee_link_name": "gripper_grasp_point",
            "control_space": "task",
            "control_strategy": "position",
-           "interactive": "false",
-           "use_rbs_utils": "true",
-           "assembly_config_name": "board_pick_and_place"
        }.items(),
    )
    ```
    Здесь выполняется запуск другого launch-файла с указанными аргументами. Ниже приводится описание каждого аргумента.
 
----
-
 ### Описание параметров:
 
 - **`with_gripper`**  
   Указывает, есть ли на роботе захватное устройство (гриппер). Если значение `true`, будет настроен и запущен `gripper_controller`.
 
+- **`gripper_name`**  
+  Используется как ключевое слово для указания линков и джоинтов, относящихся к грипперу. Также применяется в xacro-аргументах.
+
 - **`robot_type`**  
   Обозначает группу роботов одного типа. Например, все роботы с разными именами, но одинаковой конструкцией.
 
@@ -79,6 +77,9 @@
 - **`use_sim_time`**  
   Обязательный параметр при работе в симуляции. Обеспечивает синхронизацию времени с симулятором.
 
+- **`hardware`**  
+  Указывает интерфейс для управления роботом. Например, `gazebo`. Используется в основном в xacro-файлах.
+
 - **`use_controllers`**  
   Указывает, нужно ли использовать стандартные контроллеры. Если значение `false`, робот не сможет двигаться. Влияет на запуск файла [control.launch.py](../../rbs_bringup/launch/control.launch.py).
 
@@ -108,13 +109,4 @@
 
 - **`interactive`**  
   Указывает, нужно ли запускать контроллер `motion_control_handle`.  
-  Значение по умолчанию: `false`.  
-
-- **`use_rbs_utils`**  
-  Указывает, нужно ли использовать вспомогательные модули фреймворка Robossembler.  
-  Значение по умолчанию: `true`.
-
-- **`assembly_config_name`**  
-  Имя конфигурации сборки для выполнения заданий.  
-
----
+  Значение по умолчанию: `true`.  

doc/ru/installation.md

@@ -0,0 +1,37 @@
+# Инструкция по установке фреймворка
+
+Первым делом необходимо установить [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html). Рекомендуется минимальная установка.
+
+Вторым шагом надо собрать [`ros2_control`](https://github.com/ros-controls/ros2_control) из исходников из этого [форка](https://github.com/solid-sinusoid/ros2_control/tree/gz-ros2-cartesian-controllers) стоит отметить, что также существует альтернативная установка с использованием [`vsctool`](https://github.com/dirk-thomas/vcstool) который поставляется с базовыми пакетами ROS2.
+
+Если устанавливать через `vcstool` тогда необходимые пакеты будут клонированы в тоже рабочее пространство, что и сам фреймворк. Сама команда будет выглядеть так
+```sh
+vcs import . < robossembler-ros2/repos/all-deps.repos
+```
+
+Заодно можно выполнить команду для установки всех требуемых библиотек Python
+```shell
+pip insatll -r robossembler-ros2/repos/requirements.txt
+```
+
+> [!IMPORTANT]
+> Стоит отметить, что для того, чтобы выполнить следующую команду вам необходимо
+> установить `git lfs` так как в `requirements.txt` есть модуль `rbs_assets_library` который содержит
+> в себе тяжелые файлы, но при этом устанавливается как модуль python.
+
+При этом команду надо выполнять в директории `{robossembler_ws}/src/`
+
+Более четкая последовательность команд кому лень:
+```sh
+cd 
+mkdir -p robossembler-ros2/src && cd robossembler-ros2
+git clone git clone https://seed.robossembler.org/z46gtVRpXaXrGQM7Fxiqu7pLy7kip.git robossembler-ros2
+# Или если вы предпочитаете radicle
+rad clone rad:z46gtVRpXaXrGQM7Fxiqu7pLy7kip
+cd src
+vcs import . < robossembler-ros2/repos/all-deps.repos
+pip insatll -r robossembler-ros2/repos/requirements.txt
+cd ..
+rosdep install --from-paths src -y --ignore-src
+colcon build --symlink-install
+```

docs/en/installation.md

@@ -1,92 +0,0 @@
-# Framework Installation Guide
-
-**Important!** Compatible only with Ubuntu 22.04.  
-
-Below are the steps to set up the framework.
-
----
-
-## Step 1: Install ROS2 Humble  
-
-Start by installing [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html).  
-A minimal installation of `ros-humble-ros-base` is recommended, along with the `ros-dev-tools` package.
-
----
-
-## Step 2: Initialize `rosdep` (if not done already)  
-
-Run the following commands:
-```sh
-sudo rosdep init
-rosdep update
-```
-
----
-
-## Step 3: Verify ROS2 Environment  
-
-Before continuing, ensure that the ROS2 environment is activated. Run:
-```sh
-source /opt/ros/humble/setup.bash
-```
-
----
-
-## Step 4: Framework Installation  
-
-```sh
-cd 
-mkdir -p robossembler-ws/src && cd robossembler-ws/src
-git clone --recurse-submodules https://gitlab.com/robossembler/robossembler-ros2.git
-```
-
-Next, build [`ros2_control`](https://github.com/ros-controls/ros2_control) from source using this [fork](https://github.com/solid-sinusoid/ros2_control/tree/gz-ros2-cartesian-controllers).  
-Alternatively, you can use [`vsctool`](https://github.com/dirk-thomas/vcstool), which is included in the basic ROS2 packages.
-
-If you choose to use `vcstool`, the necessary packages will be cloned into the same workspace as the framework. Use this command:
-```sh
-vcs import . < robossembler-ros2/repos/all-deps.repos
-```
-
-You can also install all required Python libraries by running:
-```shell
-pip install -r robossembler-ros2/repos/requirements.txt
-# If you encounter an error while installing Shapely:
-sudo apt install libgeos-dev
-```
-
-> **[!IMPORTANT]**  
-> Ensure that `git lfs` is installed. The `requirements.txt` file includes the `rbs_assets_library` module, which contains large files and is installed as a Python module.
-
-Run these commands in the `robossembler-ws/src/` directory.
-
-Install dependencies using `rosdep`:
-```sh
-cd ~/robossembler-ws
-rosdep install --from-paths src -y --ignore-src --rosdistro ${ROS_DISTRO}
-```
-
-Build the framework using `colcon`:
-```sh
-colcon build
-```
-
----
-
-## Full Command Sequence  
-
-Below is the full set of commands to set up the framework:
-```sh
-cd 
-mkdir -p robossembler-ws/src && cd robossembler-ws/src
-git clone --recurse-submodules https://gitlab.com/robossembler/robossembler-ros2.git
-# Or, if you prefer Radicle:
-git clone --recurse-submodules https://seed.robossembler.org/z46gtVRpXaXrGQM7Fxiqu7pLy7kip.git robossembler-ros2
-rad clone rad:z46gtVRpXaXrGQM7Fxiqu7pLy7kip
-
-vcs import . < robossembler-ros2/repos/all-deps.repos
-pip install -r robossembler-ros2/repos/requirements.txt
-cd ..
-rosdep install --from-paths src -y --ignore-src --rosdistro ${ROS_DISTRO}
-colcon build
-```

docs/ru/installation.md

@@ -1,87 +0,0 @@
-# Инструкция по установке фреймворка
-
-**Важно!** Совместимо только с операционной системой Ubuntu 22.04.  
-
-Ниже приведена последовательность шагов для настройки фреймворка.
-
----
-
-## Шаг 1: Установка ROS2 Humble  
-
-Для начала установите [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html).  
-Рекомендуется минимальная установка `ros-humble-ros-base`, а также пакет `ros-dev-tools`.
-
----
-
-## Шаг 2: Проверка среды ROS2  
-
-Перед продолжением убедитесь, что среда ROS2 активирована. Для этого выполните:
-```sh
-source /opt/ros/humble/setup.bash
-```
-
----
-## Шаг 3: Если Вы этого не делали то сделайте
-```sh
-sudo rosdep init
-rosdep update
-```
----
-
-## Шаг 4: Установка фреймворка
-
-```sh
-cd 
-mkdir -p robossembler-ws/src && cd robossembler-ws/src
-git clone --recurse-submodules https://gitlab.com/robossembler/robossembler-ros2.git
-```
-
-Далее необходимо собрать [`ros2_control`](https://github.com/ros-controls/ros2_control) из исходников, используя этот [форк](https://github.com/solid-sinusoid/ros2_control/tree/gz-ros2-cartesian-controllers).  
-Также доступна альтернатива с использованием [`vsctool`](https://github.com/dirk-thomas/vcstool), который входит в состав базовых пакетов ROS2.
-
-Если вы решили использовать `vcstool`, нужные пакеты будут клонированы в тоже рабочее пространство, что и сам фреймворк. Команда для этого выглядит следующим образом:
-```sh
-vcs import . < robossembler-ros2/repos/all-deps.repos
-```
-
-Вы также можете установить все необходимые библиотеки Python, выполнив команду:
-```shell
-pip install -r robossembler-ros2/repos/requirements.txt
-# Если Вы получили ошибку с установкой Shapely
-sudo apt install libgeos-dev
-```
-
-> **[!ВНИМАНИЕ]**  
-> Убедитесь, что у вас установлен `git lfs`. В файле `requirements.txt` указан модуль `rbs_assets_library`, который содержит большие файлы и устанавливается как Python-модуль.
-
-Эти команды нужно выполнять в директории `robossembler-ws/src/`.
-
-Установка зависимостей при помощи `rosdep`
-```sh
-cd ~/robossembler-ws
-rosdep install --from-paths src -y --ignore-src --rosdistro ${ROS_DISTRO}
-```
-
-Сборка фреймворка при помощи `colcon`
-```sh
-colcon build
-```
-
----
-## Полная последовательность команд  
-
-Ниже приведён полный набор команд для настройки фреймворка:
-```sh
-cd 
-mkdir -p robossembler-ws/src && cd robossembler-ws/src
-git clone --recurse-submodules https://gitlab.com/robossembler/robossembler-ros2.git
-# Или, если вы предпочитаете Radicle:
-git clone --recurse-submodules https://seed.robossembler.org/z46gtVRpXaXrGQM7Fxiqu7pLy7kip.git robossembler-ros2
-rad clone rad:z46gtVRpXaXrGQM7Fxiqu7pLy7kip
-
-vcs import . < robossembler-ros2/repos/all-deps.repos
-pip install -r robossembler-ros2/repos/requirements.txt
-cd ..
-rosdep install --from-paths src -y --ignore-src --rosdistro ${ROS_DISTRO}
-colcon build
-```

dynmsg-demo.nix

@@ -0,0 +1,25 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchFromGitHub, ament-cmake, ament-lint-auto, ament-lint-common, dynmsg, dynmsg-msgs, example-interfaces, geometry-msgs, rcl, rcl-action, std-msgs, test-msgs, yaml-cpp-vendor }:
+buildRosPackage rec {
+  pname = "ros-jazzy-dynmsg-demo";
+  version = "1.0.0";
+
+  src = fetchFromGitHub {
+    owner = "osrf";
+    repo = "dynamic_message_introspection";
+    rev = "50277e341f61455c42a6015b5718804a4f8588f6";
+    sha256 = "1kayqlmgpzz6lg4sfv6hlm1ak0jhmgqlarbj9wk3206as5iwhbhd";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/dynmsg_demo/";
+  buildInputs = [ ament-cmake ];
+  checkInputs = [ ament-lint-auto ament-lint-common dynmsg-msgs example-interfaces geometry-msgs std-msgs test-msgs ];
+  propagatedBuildInputs = [ dynmsg rcl rcl-action yaml-cpp-vendor ];
+  nativeBuildInputs = [ ament-cmake ];
+
+  meta = {
+    description = "Demo of dynamic message type loading";
+    license = with lib.licenses; [ asl20 ];
+  };
+}

dynmsg-msgs.nix

@@ -0,0 +1,25 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchFromGitHub, ament-cmake, ament-lint-auto, ament-lint-common, rosidl-default-generators, rosidl-default-runtime }:
+buildRosPackage rec {
+  pname = "ros-jazzy-dynmsg-msgs";
+  version = "1.0.0";
+
+  src = fetchFromGitHub {
+    owner = "osrf";
+    repo = "dynamic_message_introspection";
+    rev = "50277e341f61455c42a6015b5718804a4f8588f6";
+    sha256 = "1kayqlmgpzz6lg4sfv6hlm1ak0jhmgqlarbj9wk3206as5iwhbhd";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/dynmsg_msgs/";
+  buildInputs = [ ament-cmake rosidl-default-generators ];
+  checkInputs = [ ament-lint-auto ament-lint-common ];
+  propagatedBuildInputs = [ rosidl-default-runtime ];
+  nativeBuildInputs = [ ament-cmake ];
+
+  meta = {
+    description = "Test messages for dynamic message type loading";
+    license = with lib.licenses; [ asl20 ];
+  };
+}

dynmsg.nix

@@ -0,0 +1,25 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchFromGitHub, ament-cmake, ament-cmake-gtest, ament-lint-auto, ament-lint-common, rcutils, rosidl-runtime-c, rosidl-runtime-cpp, rosidl-typesupport-introspection-c, rosidl-typesupport-introspection-cpp, std-msgs, yaml-cpp-vendor }:
+buildRosPackage rec {
+  pname = "ros-jazzy-dynmsg";
+  version = "1.0.0";
+
+  src = fetchFromGitHub {
+    owner = "osrf";
+    repo = "dynamic_message_introspection";
+    rev = "50277e341f61455c42a6015b5718804a4f8588f6";
+    sha256 = "1kayqlmgpzz6lg4sfv6hlm1ak0jhmgqlarbj9wk3206as5iwhbhd";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/dynmsg/";
+  buildInputs = [ ament-cmake ];
+  checkInputs = [ ament-cmake-gtest ament-lint-auto ament-lint-common std-msgs ];
+  propagatedBuildInputs = [ rcutils rosidl-runtime-c rosidl-runtime-cpp rosidl-typesupport-introspection-c rosidl-typesupport-introspection-cpp yaml-cpp-vendor ];
+  nativeBuildInputs = [ ament-cmake ];
+
+  meta = {
+    description = "Introspection tools for message to/from YAML conversions";
+    license = with lib.licenses; [ asl20 ];
+  };
+}

env-manager-interfaces.nix

@@ -0,0 +1,24 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchgit, ament-cmake, ament-lint-auto, ament-lint-common, builtin-interfaces, lifecycle-msgs, rosidl-default-generators, rosidl-default-runtime }:
+buildRosPackage rec {
+  pname = "ros-jazzy-env-manager-interfaces";
+  version = "0.0.0";
+
+  src = fetchgit {
+    url = "https://gitlab.com/robossembler/robossembler-ros2";
+    rev = "b83a68f77724d83e1a0a014ffb4da4ef42ec3a42";
+    sha256 = "sha256-UdshWzp+WAThBieQPIqNgpy48bLO/+FEEX/higgMJH8=";
+  };
+
+  buildType = "ament_cmake";
+  sourceRoot = "${src.name}/env_manager/env_manager_interfaces/";
+  buildInputs = [ ament-cmake rosidl-default-generators ];
+  checkInputs = [ ament-lint-auto ament-lint-common ];
+  propagatedBuildInputs = [ builtin-interfaces lifecycle-msgs rosidl-default-runtime ];
+  nativeBuildInputs = [ ament-cmake rosidl-default-generators ];
+
+  meta = {
+    description = "TODO: Package description";
+    license = with lib.licenses; [ asl20 ];
+  };
+}

env-manager.nix

@@ -0,0 +1,21 @@
+# Automatically generated by: ros2nix --output-as-nix-pkg-name --fetch --distro jazzy
+{ lib, buildRosPackage, fetchgit, ament-copyright, ament-flake8, ament-pep257, pythonPackages }:
+buildRosPackage rec {
+  pname = "ros-jazzy-env-manager";
+  version = "0.0.0";
+
+  src = fetchgit {
+    url = "https://gitlab.com/robossembler/robossembler-ros2";
+    rev = "b83a68f77724d83e1a0a014ffb4da4ef42ec3a42";
+    sha256 = "sha256-UdshWzp+WAThBieQPIqNgpy48bLO/+FEEX/higgMJH8=";
+  };
+
+  buildType = "ament_python";
+  sourceRoot = "${src.name}/env_manager/env_manager/";
+  checkInputs = [ ament-copyright ament-flake8 ament-pep257 pythonPackages.pytest ];
+
+  meta = {
+    description = "TODO: Package description";
+    license = with lib.licenses; [ asl20 ];
+  };
+}

env_manager

@@ -1 +0,0 @@
-Subproject commit d3d19355683338c8c013fde5a7a5c6ebcd73d0f7

env_manager/env_manager/LICENSE

@@ -0,0 +1,202 @@
+
+                                 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.

env_manager/env_manager/env_manager/models/__init__.py

@@ -0,0 +1,5 @@
+from .lights import *
+from .objects import *
+from .robots import *
+from .sensors import *
+from .terrains import *

env_manager/env_manager/env_manager/models/configs/__init__.py

@@ -0,0 +1,15 @@
+from .camera import CameraData
+from .light import LightData
+from .objects import (
+    BoxObjectData,
+    CylinderObjectData,
+    MeshData,
+    ModelData,
+    ObjectData,
+    PlaneObjectData,
+    SphereObjectData,
+    ObjectRandomizerData,
+)
+from .robot import RobotData
+from .terrain import TerrainData
+from .scene import SceneData

env_manager/env_manager/env_manager/models/configs/camera.py

@@ -0,0 +1,111 @@
+from dataclasses import dataclass, field
+
+import numpy as np
+
+
+@dataclass
+class CameraData:
+    """
+    CameraData stores the parameters and configuration settings for an RGB-D camera.
+
+    Attributes:
+    ----------
+    name : str
+        The name of the camera instance. Default is an empty string.
+    enable : bool
+        Boolean flag to enable or disable the camera. Default is True.
+    type : str
+        Type of the camera. Default is "rgbd_camera".
+    relative_to : str
+        The reference frame relative to which the camera is placed. Default is "base_link".
+
+    width : int
+        The image width (in pixels) captured by the camera. Default is 128.
+    height : int
+        The image height (in pixels) captured by the camera. Default is 128.
+    image_format : str
+        The image format used (e.g., "R8G8B8"). Default is "R8G8B8".
+    update_rate : int
+        The rate (in Hz) at which the camera provides updates. Default is 10 Hz.
+    horizontal_fov : float
+        The horizontal field of view (in radians). Default is pi / 3.0.
+    vertical_fov : float
+        The vertical field of view (in radians). Default is pi / 3.0.
+
+    clip_color : tuple[float, float]
+        The near and far clipping planes for the color camera. Default is (0.01, 1000.0).
+    clip_depth : tuple[float, float]
+        The near and far clipping planes for the depth camera. Default is (0.05, 10.0).
+
+    noise_mean : float | None
+        The mean value of the Gaussian noise applied to the camera's data. Default is None (no noise).
+    noise_stddev : float | None
+        The standard deviation of the Gaussian noise applied to the camera's data. Default is None (no noise).
+
+    publish_color : bool
+        Whether or not to publish color data from the camera. Default is False.
+    publish_depth : bool
+        Whether or not to publish depth data from the camera. Default is False.
+    publish_points : bool
+        Whether or not to publish point cloud data from the camera. Default is False.
+
+    spawn_position : tuple[float, float, float]
+        The initial spawn position (x, y, z) of the camera relative to the reference frame. Default is (0, 0, 1).
+    spawn_quat_xyzw : tuple[float, float, float, float]
+        The initial spawn orientation of the camera in quaternion (x, y, z, w). Default is (0, 0.70710678118, 0, 0.70710678118).
+
+    random_pose_rollouts_num : int
+        The number of random pose rollouts. Default is 1.
+    random_pose_mode : str
+        The mode for random pose generation (e.g., "orbit"). Default is "orbit".
+    random_pose_orbit_distance : float
+        The fixed distance from the object in "orbit" mode. Default is 1.0.
+    random_pose_orbit_height_range : tuple[float, float]
+        The range of vertical positions (z-axis) in "orbit" mode. Default is (0.1, 0.7).
+    random_pose_orbit_ignore_arc_behind_robot : float
+        The arc angle (in radians) behind the robot to ignore when generating random poses. Default is pi / 8.
+    random_pose_select_position_options : list[tuple[float, float, float]]
+        A list of preset position options for the camera in random pose mode. Default is an empty list.
+    random_pose_focal_point_z_offset : float
+        The offset in the z-direction of the focal point when generating random poses. Default is 0.0.
+    random_pose_rollout_counter : float
+        A counter tracking the number of rollouts completed for random poses. Default is 0.0.
+    """
+
+    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, 0.0, 1.0))
+    spawn_quat_xyzw: tuple[float, float, float, float] = field(
+        default=(0.0, 0.70710678118, 0.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)

env_manager/env_manager/env_manager/models/configs/light.py

@@ -0,0 +1,39 @@
+from dataclasses import dataclass, field
+
+
+@dataclass
+class LightData:
+    """
+    LightData stores the configuration and settings for a light source in a simulation or rendering environment.
+
+    Attributes:
+    ----------
+    type : str
+        The type of the light source (e.g., "sun", "point", "spot"). Default is "sun".
+    direction : tuple[float, float, float]
+        The direction vector of the light source in (x, y, z). This is typically used for directional lights like the sun.
+        Default is (0.5, -0.25, -0.75).
+    random_minmax_elevation : tuple[float, float]
+        The minimum and maximum elevation angles (in radians) for randomizing the light's direction.
+        Default is (-0.15, -0.65).
+    color : tuple[float, float, float, float]
+        The RGBA color of the light source. Each value ranges from 0 to 1. Default is (1.0, 1.0, 1.0, 1.0), which represents white light.
+    distance : float
+        The effective distance of the light source. Default is 1000.0 units.
+    visual : bool
+        A flag indicating whether the light source is visually represented in the simulation (e.g., whether it casts visible rays).
+        Default is True.
+    radius : float
+        The radius of the light's influence (for point and spot lights). Default is 25.0 units.
+    model_rollouts_num : int
+        The number of rollouts for randomizing light configurations in different simulation runs. Default is 1.
+    """
+
+    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)

env_manager/env_manager/env_manager/models/configs/objects.py

@@ -0,0 +1,198 @@
+from dataclasses import dataclass, field
+
+from env_manager.utils.types import Pose
+
+
+@dataclass
+class ObjectRandomizerData:
+    """
+    ObjectRandomizerData contains parameters for randomizing object properties during simulation.
+
+    Attributes:
+    ----------
+    count : int
+        The number of objects to randomize. Default is 0.
+    random_pose : bool
+        Flag indicating whether to randomize both the position and orientation of objects. Default is False.
+    random_position : bool
+        Flag indicating whether to randomize the position of objects. Default is False.
+    random_orientation : bool
+        Flag indicating whether to randomize the orientation of objects. Default is False.
+    random_model : bool
+        Flag indicating whether to randomize the model of the objects. Default is False.
+    random_spawn_position_segments : list
+        List of segments within which the object can be randomly spawned. Default is an empty list.
+    random_spawn_position_update_workspace_centre : bool
+        Flag indicating whether to update the workspace center during random spawning. Default is False.
+    random_spawn_volume : tuple[float, float, float]
+        The volume within which objects can be randomly spawned, defined by (x, y, z). Default is (0.5, 0.5, 0.5).
+    models_rollouts_num : int
+        The number of rollouts for randomizing models. Default is 0.
+    """
+
+    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)
+    random_color: bool = field(default=False)
+
+
+@dataclass
+class ObjectData:
+    """
+    ObjectData stores the base properties for any object in the simulation environment.
+
+    Attributes:
+    ----------
+    name : str
+        The name of the object. Default is "object".
+    type : str
+        The type of the object (e.g., "sphere", "box"). Default is an empty string.
+    relative_to : str
+        The reference frame relative to which the object is positioned. Default is "world".
+    position : tuple[float, float, float]
+        The position of the object in (x, y, z) coordinates. Default is (0.0, 0.0, 0.0).
+    orientation : tuple[float, float, float, float]
+        The orientation of the object as a quaternion (x, y, z, w). Default is (1.0, 0.0, 0.0, 0.0).
+    static : bool
+        Flag indicating if the object is static in the simulation (immovable). Default is False.
+    randomize : ObjectRandomizerData
+        Object randomizer settings for generating randomized object properties. Default is an empty ObjectRandomizerData instance.
+    """
+
+    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))
+    color: tuple[float, float, float, float] | None = field(default=None)
+    static: bool = field(default=False)
+    randomize: ObjectRandomizerData = field(default_factory=ObjectRandomizerData)
+
+
+@dataclass
+class PrimitiveObjectData(ObjectData):
+    """
+    PrimitiveObjectData defines the base properties for primitive objects (e.g., spheres, boxes) in the simulation.
+
+    Attributes:
+    ----------
+    collision : bool
+        Flag indicating whether the object participates in collision detection. Default is True.
+    visual : bool
+        Flag indicating whether the object has a visual representation in the simulation. Default is True.
+    color : tuple[float, float, float, float]
+        The color of the object, represented in RGBA format. Default is (0.8, 0.8, 0.8, 1.0).
+    mass : float
+        The mass of the object. Default is 0.1.
+    """
+
+    collision: bool = field(default=True)
+    visual: bool = field(default=True)
+    mass: float = field(default=0.1)
+
+
+@dataclass
+class SphereObjectData(PrimitiveObjectData):
+    """
+    SphereObjectData defines the specific properties for a spherical object in the simulation.
+
+    Attributes:
+    ----------
+    radius : float
+        The radius of the sphere. Default is 0.025.
+    friction : float
+        The friction coefficient of the sphere when in contact with surfaces. Default is 1.0.
+    """
+
+    radius: float = field(default=0.025)
+    friction: float = field(default=1.0)
+
+
+@dataclass
+class PlaneObjectData(PrimitiveObjectData):
+    """
+    PlaneObjectData defines the specific properties for a planar object in the simulation.
+
+    Attributes:
+    ----------
+    size : tuple[float, float]
+        The size of the plane, defined by its width and length. Default is (1.0, 1.0).
+    direction : tuple[float, float, float]
+        The normal vector representing the direction the plane faces. Default is (0.0, 0.0, 1.0).
+    friction : float
+        The friction coefficient of the plane when in contact with other objects. Default is 1.0.
+    """
+
+    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):
+    """
+    CylinderObjectData defines the specific properties for a cylindrical object in the simulation.
+
+    Attributes:
+    ----------
+    radius : float
+        The radius of the cylinder. Default is 0.025.
+    length : float
+        The length of the cylinder. Default is 0.1.
+    friction : float
+        The friction coefficient of the cylinder when in contact with surfaces. Default is 1.0.
+    """
+
+    radius: float = field(default=0.025)
+    length: float = field(default=0.1)
+    friction: float = field(default=1.0)
+
+
+@dataclass
+class BoxObjectData(PrimitiveObjectData):
+    """
+    BoxObjectData defines the specific properties for a box-shaped object in the simulation.
+
+    Attributes:
+    ----------
+    size : tuple[float, float, float]
+        The dimensions of the box in (width, height, depth). Default is (0.05, 0.05, 0.05).
+    friction : float
+        The friction coefficient of the box when in contact with surfaces. Default is 1.0.
+    """
+
+    size: tuple = field(default=(0.05, 0.05, 0.05))
+    friction: float = field(default=1.0)
+
+
+@dataclass
+class ModelData(ObjectData):
+    """
+    MeshObjectData defines the specific properties for a mesh-based object in the simulation.
+
+    Attributes:
+    ----------
+    texture : list[float]
+        A list of texture coordinates or texture properties applied to the mesh. Default is an empty list.
+    """
+
+    texture: list[float] = field(default_factory=list)
+
+
+@dataclass
+class MeshData(ModelData):
+    mass: float = field(default_factory=float)
+    inertia: tuple[float, float, float, float, float, float] = field(
+        default_factory=tuple
+    )
+    cm: Pose = field(default_factory=Pose)
+    collision: str = field(default_factory=str)
+    visual: str = field(default_factory=str)
+    friction: float = field(default_factory=float)
+    density: float = field(default_factory=float)

env_manager/env_manager/env_manager/models/configs/robot.py

@@ -0,0 +1,103 @@
+from dataclasses import dataclass, field
+from enum import Enum
+
+
+@dataclass
+class ToolData:
+    name: str = field(default_factory=str)
+    type: str = field(default_factory=str)
+
+
+@dataclass
+class GripperData(ToolData):
+    pass
+
+
+@dataclass
+class ParallelGripperData(GripperData):
+    pass
+
+
+@dataclass
+class MultifingerGripperData(GripperData):
+    pass
+
+
+@dataclass
+class VacuumGripperData(GripperData):
+    pass
+
+
+class GripperEnum(Enum):
+    parallel = ParallelGripperData
+    mulrifinger = MultifingerGripperData
+    vacuum = VacuumGripperData
+
+
+@dataclass
+class RobotRandomizerData:
+    """
+    RobotRandomizerData stores configuration parameters for randomizing robot properties during simulation.
+
+    Attributes:
+    ----------
+    pose : bool
+        Flag indicating whether to randomize the robot's pose (position and orientation). Default is False.
+    spawn_volume : tuple[float, float, float]
+        The volume within which the robot can be spawned, defined by (x, y, z) dimensions. Default is (1.0, 1.0, 0.0).
+    joint_positions : bool
+        Flag indicating whether to randomize the robot's joint positions. Default is False.
+    joint_positions_std : float
+        The standard deviation for randomizing the robot's joint positions. Default is 0.1.
+    joint_positions_above_object_spawn : bool
+        Flag indicating whether the robot's joint positions should be randomized to place the robot above the object's spawn position. Default is False.
+    joint_positions_above_object_spawn_elevation : float
+        The elevation above the object's spawn position when placing the robot's joints. Default is 0.2.
+    joint_positions_above_object_spawn_xy_randomness : float
+        The randomness in the x and y coordinates when placing the robot's joints above the object's spawn position. Default is 0.2.
+    """
+
+    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:
+    """
+    RobotData stores the base properties and configurations for a robot in the simulation.
+
+    Attributes:
+    ----------
+    name : str
+        The name of the robot. Default is an empty string.
+    urdf_string : str
+        Optional parameter that can store a URDF. This parameter is overridden by the node parameters if set in the node configuration.
+    spawn_position : tuple[float, float, float]
+        The position where the robot will be spawned in (x, y, z) coordinates. Default is (0.0, 0.0, 0.0).
+    spawn_quat_xyzw : tuple[float, float, float, float]
+        The orientation of the robot in quaternion format (x, y, z, w) at spawn. Default is (0.0, 0.0, 0.0, 1.0).
+    joint_positions : list[float]
+        A list of the robot's joint positions. Default is an empty list.
+    with_gripper : bool
+        Flag indicating whether the robot is equipped with a gripper. Default is False.
+    gripper_joint_positions : list[float] | float
+        The joint positions for the gripper. Can be a list of floats or a single float. Default is an empty list.
+    randomizer : RobotRandomizerData
+        The randomization settings for the robot, allowing various properties to be randomized in simulation. Default is an instance of RobotRandomizerData.
+    """
+
+    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_positions: list[float] = field(default_factory=list)
+    with_gripper: bool = field(default=False)
+    gripper_joint_positions: list[float] | float = field(default_factory=list)
+    randomizer: RobotRandomizerData = field(default_factory=RobotRandomizerData)

env_manager/env_manager/env_manager/models/configs/scene.py

@@ -0,0 +1,68 @@
+from dataclasses import dataclass, field
+
+from .camera import CameraData
+from .light import LightData
+from .objects import ObjectData
+from .robot import RobotData
+from .terrain import TerrainData
+
+
+@dataclass
+class PluginsData:
+    """
+    PluginsData stores the configuration for various plugins used in the simulation environment.
+
+    Attributes:
+    ----------
+    scene_broadcaster : bool
+        Flag indicating whether the scene broadcaster plugin is enabled. Default is False.
+    user_commands : bool
+        Flag indicating whether the user commands plugin is enabled. Default is False.
+    fts_broadcaster : bool
+        Flag indicating whether the force torque sensor (FTS) broadcaster plugin is enabled. Default is False.
+    sensors_render_engine : str
+        The rendering engine used for sensors. Default is "ogre2".
+    """
+
+    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:
+    """
+    SceneData contains the configuration and settings for the simulation scene.
+
+    Attributes:
+    ----------
+    physics_rollouts_num : int
+        The number of physics rollouts to perform in the simulation. Default is 0.
+    gravity : tuple[float, float, float]
+        The gravitational acceleration vector applied in the scene, represented as (x, y, z). Default is (0.0, 0.0, -9.80665).
+    gravity_std : tuple[float, float, float]
+        The standard deviation for the gravitational acceleration, represented as (x, y, z). Default is (0.0, 0.0, 0.0232).
+    robot : RobotData
+        The configuration data for the robot present in the scene. Default is an instance of RobotData.
+    terrain : TerrainData
+        The configuration data for the terrain in the scene. Default is an instance of TerrainData.
+    light : LightData
+        The configuration data for the lighting in the scene. Default is an instance of LightData.
+    objects : list[ObjectData]
+        A list of objects present in the scene, represented by their ObjectData configurations. Default is an empty list.
+    camera : list[CameraData]
+        A list of cameras in the scene, represented by their CameraData configurations. Default is an empty list.
+    plugins : PluginsData
+        The configuration data for various plugins utilized in the simulation environment. Default is an instance of PluginsData.
+    """
+
+    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)

env_manager/env_manager/env_manager/models/configs/terrain.py

@@ -0,0 +1,33 @@
+from dataclasses import dataclass, field
+
+
+@dataclass
+class TerrainData:
+    """
+    TerrainData stores the configuration for the terrain in the simulation environment.
+
+    Attributes:
+    ----------
+    name : str
+        The name of the terrain. Default is "ground".
+    type : str
+        The type of terrain (e.g., "flat", "hilly", "uneven"). Default is "flat".
+    spawn_position : tuple[float, float, float]
+        The position where the terrain will be spawned in the simulation, represented as (x, y, z). Default is (0, 0, 0).
+    spawn_quat_xyzw : tuple[float, float, float, float]
+        The orientation of the terrain at spawn, represented as a quaternion (x, y, z, w). Default is (0, 0, 0, 1).
+    size : tuple[float, float]
+        The size of the terrain, represented as (width, length). Default is (1.5, 1.5).
+    model_rollouts_num : int
+        The number of rollouts for randomizing terrain models. Default is 1.
+    """
+
+    name: str = field(default="ground")
+    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))
+    ambient: tuple[float, float, float, float] = field(default=(0.8, 0.8, 0.8, 1.0))
+    specular: tuple[float, float, float, float] = field(default=(0.8, 0.8, 0.8, 1.0))
+    diffuse: tuple[float, float, float, float] = field(default=(0.8, 0.8, 0.8, 1.0))
+    model_rollouts_num: int = field(default=1)

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
+

env_manager/env_manager/env_manager/models/lights/random_sun.py

@@ -0,0 +1,240 @@
+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
+from scenario import core as scenario_core
+from scenario import gazebo as scenario_gazebo
+
+
+class RandomSun(model_wrapper.ModelWrapper):
+    """
+    RandomSun is a class that creates a randomly positioned directional light (like the Sun) in a simulation world.
+
+    Attributes:
+    ----------
+    world : scenario_gazebo.World
+        The simulation world where the model is inserted.
+    name : str
+        The name of the sun model. Default is "sun".
+    minmax_elevation : tuple[float, float]
+        Minimum and maximum values for the random elevation angle of the sun. Default is (-0.15, -0.65).
+    distance : float
+        Distance from the origin to the sun in the simulation. Default is 800.0.
+    visual : bool
+        Flag indicating whether the sun should have a visual sphere in the simulation. Default is True.
+    radius : float
+        Radius of the visual sphere representing the sun. Default is 20.0.
+    color_minmax_r : tuple[float, float]
+        Range for the red component of the sun's light color. Default is (1.0, 1.0).
+    color_minmax_g : tuple[float, float]
+        Range for the green component of the sun's light color. Default is (1.0, 1.0).
+    color_minmax_b : tuple[float, float]
+        Range for the blue component of the sun's light color. Default is (1.0, 1.0).
+    specular : float
+        The specular factor for the sun's light. Default is 1.0.
+    attenuation_minmax_range : tuple[float, float]
+        Range for the light attenuation (falloff) distance. Default is (750.0, 15000.0).
+    attenuation_minmax_constant : tuple[float, float]
+        Range for the constant component of the light attenuation. Default is (0.5, 1.0).
+    attenuation_minmax_linear : tuple[float, float]
+        Range for the linear component of the light attenuation. Default is (0.001, 0.1).
+    attenuation_minmax_quadratic : tuple[float, float]
+        Range for the quadratic component of the light attenuation. Default is (0.0001, 0.01).
+    np_random : RandomState | None
+        Random state for generating random values. Default is None, in which case a new random generator is created.
+
+    Raises:
+    -------
+    RuntimeError:
+        Raised if the sun model fails to be inserted into the world.
+
+    Methods:
+    --------
+    get_sdf():
+        Generates the SDF string for the sun model.
+    """
+
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str = "sun",
+        minmax_elevation: tuple[float, float] = (-0.15, -0.65),
+        distance: float = 800.0,
+        visual: bool = True,
+        radius: float = 20.0,
+        color_minmax_r: tuple[float, float] = (1.0, 1.0),
+        color_minmax_g: tuple[float, float] = (1.0, 1.0),
+        color_minmax_b: tuple[float, float] = (1.0, 1.0),
+        specular: float = 1.0,
+        attenuation_minmax_range: tuple[float, float] = (750.0, 15000.0),
+        attenuation_minmax_constant: tuple[float, float] = (0.5, 1.0),
+        attenuation_minmax_linear: tuple[float, float] = (0.001, 0.1),
+        attenuation_minmax_quadratic: tuple[float, float] = (0.0001, 0.01),
+        np_random: RandomState | None = 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)
+
+        # Get random yaw direction
+        direction = np_random.uniform(-1.0, 1.0, (2,))
+        # Normalize yaw direction
+        direction = direction / np.linalg.norm(direction)
+
+        # Get random elevation
+        direction = np.append(
+            direction,
+            np_random.uniform(minmax_elevation[0], minmax_elevation[1]),
+        )
+        # Normalize again
+        direction = direction / np.linalg.norm(direction)
+
+        # Initial pose
+        initial_pose = scenario_core.Pose(
+            (
+                -direction[0] * distance,
+                -direction[1] * distance,
+                -direction[2] * distance,
+            ),
+            (1, 0, 0, 0),
+        )
+
+        # Create SDF string for the model
+        sdf = self.get_sdf(
+            model_name=model_name,
+            direction=tuple(direction),
+            visual=visual,
+            radius=radius,
+            color_minmax_r=color_minmax_r,
+            color_minmax_g=color_minmax_g,
+            color_minmax_b=color_minmax_b,
+            attenuation_minmax_range=attenuation_minmax_range,
+            attenuation_minmax_constant=attenuation_minmax_constant,
+            attenuation_minmax_linear=attenuation_minmax_linear,
+            attenuation_minmax_quadratic=attenuation_minmax_quadratic,
+            specular=specular,
+            np_random=np_random,
+        )
+
+        # 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)
+
+    @classmethod
+    def get_sdf(
+        self,
+        model_name: str,
+        direction: tuple[float, float, float],
+        visual: bool,
+        radius: float,
+        color_minmax_r: tuple[float, float],
+        color_minmax_g: tuple[float, float],
+        color_minmax_b: tuple[float, float],
+        attenuation_minmax_range: tuple[float, float],
+        attenuation_minmax_constant: tuple[float, float],
+        attenuation_minmax_linear: tuple[float, float],
+        attenuation_minmax_quadratic: tuple[float, float],
+        specular: float,
+        np_random: RandomState,
+    ) -> str:
+        """
+        Generates the SDF string for the sun model.
+
+        Args:
+        -----
+        model_name : str
+            The name of the model.
+        direction : Tuple[float, float, float]
+            The direction of the sun's light.
+        visual : bool
+            If True, a visual representation of the sun will be created.
+        radius : float
+            The radius of the visual representation.
+        color_minmax_r : Tuple[float, float]
+            Range for the red component of the light color.
+        color_minmax_g : Tuple[float, float]
+            Range for the green component of the light color.
+        color_minmax_b : Tuple[float, float]
+            Range for the blue component of the light color.
+        attenuation_minmax_range : Tuple[float, float]
+            Range for light attenuation distance.
+        attenuation_minmax_constant : Tuple[float, float]
+            Range for the constant attenuation factor.
+        attenuation_minmax_linear : Tuple[float, float]
+            Range for the linear attenuation factor.
+        attenuation_minmax_quadratic : Tuple[float, float]
+            Range for the quadratic attenuation factor.
+        specular : float
+            The specular reflection factor for the light.
+        np_random : RandomState
+            The random number generator used to sample random values for the parameters.
+
+        Returns:
+        --------
+        str:
+            The SDF string for the sun model.
+        """
+        # Sample random values for parameters
+        color_r = np_random.uniform(color_minmax_r[0], color_minmax_r[1])
+        color_g = np_random.uniform(color_minmax_g[0], color_minmax_g[1])
+        color_b = np_random.uniform(color_minmax_b[0], color_minmax_b[1])
+        attenuation_range = np_random.uniform(
+            attenuation_minmax_range[0], attenuation_minmax_range[1]
+        )
+        attenuation_constant = np_random.uniform(
+            attenuation_minmax_constant[0], attenuation_minmax_constant[1]
+        )
+        attenuation_linear = np_random.uniform(
+            attenuation_minmax_linear[0], attenuation_minmax_linear[1]
+        )
+        attenuation_quadratic = np_random.uniform(
+            attenuation_minmax_quadratic[0], attenuation_minmax_quadratic[1]
+        )
+
+        return f'''<sdf version="1.9">
+                <model name="{model_name}">
+                    <static>true</static>
+                    <link name="{model_name}_link">
+                        <light type="directional" name="{model_name}_light">
+                            <direction>{direction[0]} {direction[1]} {direction[2]}</direction>
+                            <attenuation>
+                                <range>{attenuation_range}</range>
+                                <constant>{attenuation_constant}</constant>
+                                <linear>{attenuation_linear}</linear>
+                                <quadratic>{attenuation_quadratic}</quadratic>
+                            </attenuation>
+                            <diffuse>{color_r} {color_g} {color_b} 1</diffuse>
+                            <specular>{specular*color_r} {specular*color_g} {specular*color_b} 1</specular>
+                            <cast_shadows>true</cast_shadows>
+                        </light>
+                        {
+                        f"""
+                        <visual name="{model_name}_visual">
+                            <geometry>
+                                <sphere>
+                                    <radius>{radius}</radius>
+                                </sphere>
+                            </geometry>
+                            <material>
+                                <emissive>{color_r} {color_g} {color_b} 1</emissive>
+                            </material>
+                            <cast_shadows>false</cast_shadows>
+                        </visual>
+                        """ if visual else ""
+                        }
+                    </link>
+                </model>
+            </sdf>'''

env_manager/env_manager/env_manager/models/lights/sun.py

@@ -0,0 +1,191 @@
+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_core
+from scenario import gazebo as scenario_gazebo
+
+
+class Sun(model_wrapper.ModelWrapper):
+    """
+    Sun is a class that represents a directional light source in the simulation, similar to the Sun.
+
+    It can have both visual and light properties, with customizable parameters such as color, direction, and attenuation.
+
+    Attributes:
+    ----------
+    world : scenario_gazebo.World
+        The Gazebo world where the sun model will be inserted.
+    name : str
+        The name of the sun model. Default is "sun".
+    direction : tuple[float, float, float]
+        The direction of the sunlight, normalized. Default is (0.5, -0.25, -0.75).
+    color : tuple[float, float, float, float]
+        The RGBA color values for the light's diffuse color. Default is (1.0, 1.0, 1.0, 1.0).
+    distance : float
+        The distance from the origin to the sun. Default is 800.0.
+    visual : bool
+        If True, a visual representation of the sun will be added. Default is True.
+    radius : float
+        The radius of the visual representation of the sun. Default is 20.0.
+    specular : float
+        The intensity of the specular reflection. Default is 1.0.
+    attenuation_range : float
+        The maximum range for the light attenuation. Default is 10000.0.
+    attenuation_constant : float
+        The constant attenuation factor. Default is 0.9.
+    attenuation_linear : float
+        The linear attenuation factor. Default is 0.01.
+    attenuation_quadratic : float
+        The quadratic attenuation factor. Default is 0.001.
+
+    Raises:
+    -------
+    RuntimeError:
+        If the sun model fails to be inserted into the Gazebo world.
+
+    Methods:
+    --------
+    get_sdf() -> str:
+        Generates the SDF string used to describe the sun model in the Gazebo simulation.
+    """
+
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str = "sun",
+        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,
+        specular: float = 1.0,
+        attenuation_range: float = 10000.0,
+        attenuation_constant: float = 0.9,
+        attenuation_linear: float = 0.01,
+        attenuation_quadratic: float = 0.001,
+        **kwargs,
+    ):
+        # Get a unique model name
+        model_name = get_unique_model_name(world, name)
+
+        # Normalize direction
+        np_direction = np.array(direction)
+        np_direction = np_direction / np.linalg.norm(np_direction)
+
+        # Initial pose
+        initial_pose = scenario_core.Pose(
+            (
+                -np_direction[0] * distance,
+                -np_direction[1] * distance,
+                -np_direction[2] * distance,
+            ),
+            (1, 0, 0, 0),
+        )
+
+        # Create SDF string for the model
+        sdf = self.get_sdf(
+            model_name=model_name,
+            direction=tuple(np_direction),
+            color=color,
+            visual=visual,
+            radius=radius,
+            specular=specular,
+            attenuation_range=attenuation_range,
+            attenuation_constant=attenuation_constant,
+            attenuation_linear=attenuation_linear,
+            attenuation_quadratic=attenuation_quadratic,
+        )
+
+        # 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)
+
+    @classmethod
+    def get_sdf(
+        cls,
+        model_name: str,
+        direction: tuple[float, float, float],
+        color: tuple[float, float, float, float],
+        visual: bool,
+        radius: float,
+        specular: float,
+        attenuation_range: float,
+        attenuation_constant: float,
+        attenuation_linear: float,
+        attenuation_quadratic: float,
+    ) -> str:
+        """
+        Generates the SDF string for the sun model.
+
+        Args:
+        -----
+        model_name : str
+            The name of the sun model.
+        direction : tuple[float, float, float]
+            The direction vector for the sunlight.
+        color : tuple[float, float, float, float]
+            The RGBA color values for the sunlight.
+        visual : bool
+            Whether to include a visual representation of the sun (a sphere).
+        radius : float
+            The radius of the visual sphere.
+        specular : float
+            The specular reflection intensity.
+        attenuation_range : float
+            The range of the light attenuation.
+        attenuation_constant : float
+            The constant factor for the light attenuation.
+        attenuation_linear : float
+            The linear factor for the light attenuation.
+        attenuation_quadratic : float
+            The quadratic factor for the light attenuation.
+
+        Returns:
+        --------
+        str:
+            The SDF string for the sun model.
+        """
+
+        return f'''<sdf version="1.9">
+                <model name="{model_name}">
+                    <static>true</static>
+                    <link name="{model_name}_link">
+                        <light type="directional" name="{model_name}_light">
+                            <direction>{direction[0]} {direction[1]} {direction[2]}</direction>
+                            <attenuation>
+                                <range>{attenuation_range}</range>
+                                <constant>{attenuation_constant}</constant>
+                                <linear>{attenuation_linear}</linear>
+                                <quadratic>{attenuation_quadratic}</quadratic>
+                            </attenuation>
+                            <diffuse>{color[0]} {color[1]} {color[2]} 1</diffuse>
+                            <specular>{specular*color[0]} {specular*color[1]} {specular*color[2]} 1</specular>
+                            <cast_shadows>true</cast_shadows>
+                        </light>
+                        {
+                        f"""
+                        <visual name="{model_name}_visual">
+                            <geometry>
+                                <sphere>
+                                    <radius>{radius}</radius>
+                                </sphere>
+                            </geometry>
+                            <material>
+                                <emissive>{color[0]} {color[1]} {color[2]} 1</emissive>
+                            </material>
+                            <cast_shadows>false</cast_shadows>
+                        </visual>
+                        """ if visual else ""
+                        }
+                    </link>
+                </model>
+            </sdf>'''

env_manager/env_manager/env_manager/models/objects/__init__.py

@@ -0,0 +1,53 @@
+from enum import Enum
+
+from gym_gz.scenario.model_wrapper import ModelWrapper
+
+from .model import Model
+from .mesh import Mesh
+from .primitives import Box, Cylinder, Plane, Sphere
+from .random_object import RandomObject
+from .random_primitive import RandomPrimitive
+
+
+class ObjectType(Enum):
+    BOX = "box"
+    SPHERE = "sphere"
+    CYLINDER = "cylinder"
+    PLANE = "plane"
+    RANDOM_PRIMITIVE = "random_primitive"
+    RANDOM_MESH = "random_mesh"
+    MODEL = "model"
+    MESH = "mesh"
+
+OBJECT_MODEL_CLASSES = {
+    ObjectType.BOX: Box,
+    ObjectType.SPHERE: Sphere,
+    ObjectType.CYLINDER: Cylinder,
+    ObjectType.PLANE: Plane,
+    ObjectType.RANDOM_PRIMITIVE: RandomPrimitive,
+    ObjectType.RANDOM_MESH: RandomObject,
+    ObjectType.MODEL: Model,
+    ObjectType.MESH: Mesh
+}
+
+
+RANDOMIZABLE_TYPES = {
+    ObjectType.RANDOM_PRIMITIVE,
+    ObjectType.RANDOM_MESH,
+}
+
+
+def get_object_model_class(object_type: str) -> type[ModelWrapper]:
+    try:
+        object_enum = ObjectType(object_type)
+        return OBJECT_MODEL_CLASSES[object_enum]
+    except KeyError:
+        raise ValueError(f"Unsupported object type: {object_type}")
+
+
+def is_object_type_randomizable(object_type: str) -> bool:
+    try:
+        object_enum = ObjectType(object_type)
+        return object_enum in RANDOMIZABLE_TYPES
+    except ValueError:
+        return False

env_manager/env_manager/env_manager/models/objects/mesh.py

@@ -0,0 +1,183 @@
+import numpy as np
+import trimesh
+from gym_gz.scenario import model_wrapper
+from gym_gz.utils.scenario import get_unique_model_name
+from scenario import core as scenario_core
+from scenario import gazebo as scenario_gazebo
+from trimesh import Trimesh
+
+from env_manager.utils.types import Point
+
+InertiaTensor = tuple[float, float, float, float, float, float]
+
+
+class Mesh(model_wrapper.ModelWrapper):
+    """ """
+
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str = "object",
+        type: str = "mesh",
+        relative_to: str = "world",
+        position: tuple[float, float, float] = (0, 0, 0),
+        orientation: tuple[float, float, float, float] = (1, 0, 0, 0),
+        color: tuple[float, float, float, float] | None = None,
+        static: bool = False,
+        texture: list[float] = [],
+        mass: float = 0.0,
+        inertia: InertiaTensor | None = None,
+        cm: Point | None = None,
+        collision: str = "",
+        visual: str = "",
+        friction: float = 0.0,
+        density: float = 0.0,
+        **kwargs,
+    ):
+        # Get a unique model name
+        model_name = get_unique_model_name(world, name)
+
+        # Initial pose
+        initial_pose = scenario_core.Pose(position, orientation)
+
+        # Calculate inertia parameters for provided mesh file if not exist
+        if not inertia and collision and density:
+            mass, cm, inertia = self.calculate_inertia(collision, density)
+        else:
+            raise RuntimeError(
+                f"Please provide collision mesh filepath for model {name} to calculate inertia"
+            )
+
+        if not color:
+            color = tuple(np.random.uniform(0.0, 1.0, (3,)))
+            color = (color[0], color[1], color[2], 1.0)
+
+        # Create SDF string for the model
+        sdf = self.get_sdf(
+            name=name,
+            static=static,
+            collision=collision,
+            visual=visual,
+            mass=mass,
+            inertia=inertia,
+            friction=friction,
+            color=color,
+            center_of_mass=cm,
+            gui_only=True,
+        )
+
+        # Insert the model
+        ok_model = world.to_gazebo().insert_model_from_string(
+            sdf, initial_pose, model_name
+        )
+        if not ok_model:
+            raise RuntimeError(f"Failed to insert {model_name}")
+        # Get the model
+        model = world.get_model(model_name)
+
+        # Initialize base class
+        super().__init__(model=model)
+
+    def calculate_inertia(self, file_path, density):
+        mesh = trimesh.load(file_path)
+
+        if not isinstance(mesh, Trimesh):
+            raise RuntimeError("Please provide correct stl mesh filepath")
+
+        volume = mesh.volume
+        mass: float = volume * density
+        center_of_mass: Point = tuple(mesh.center_mass)
+        inertia = mesh.moment_inertia
+        eigenvalues = np.linalg.eigvals(inertia)
+        inertia_tensor: InertiaTensor = (
+            inertia[0, 0],
+            inertia[0, 1],
+            inertia[0, 2],
+            inertia[1, 1],
+            inertia[1, 2],
+            inertia[2, 2],
+        )
+        return mass, center_of_mass, inertia_tensor
+
+    @classmethod
+    def get_sdf(
+        cls,
+        name: str,
+        static: bool,
+        collision: str,
+        visual: str,
+        mass: float,
+        inertia: InertiaTensor,
+        friction: float,
+        color: tuple[float, float, float, float],
+        center_of_mass: Point,
+        gui_only: bool,
+    ) -> str:
+        """
+        Generates the SDF string for the box model.
+
+        Args:
+            - mesh_args (MeshPureData): Object that contain data of provided mesh data
+
+        Returns:
+            The SDF string that defines the box model in Gazebo.
+        """
+        return f'''<sdf version="1.7">
+                <model name="{name}">
+                    <static>{"true" if static else "false"}</static>
+                    <link name="{name}_link">
+                        {
+                        f"""
+                        <collision name="{name}_collision">
+                            <geometry>
+                                <mesh>
+                                    <uri>{collision}</uri>
+                                </mesh>
+                            </geometry>
+                            <surface>
+                                <friction>
+                                    <ode>
+                                        <mu>{friction}</mu>
+                                        <mu2>{friction}</mu2>
+                                        <fdir1>0 0 0</fdir1>
+                                        <slip1>0.0</slip1>
+                                        <slip2>0.0</slip2>
+                                    </ode>
+                                </friction>
+                            </surface>
+                        </collision>
+                        """ if collision else ""
+                        }
+                        {
+                        f"""
+                        <visual name="{name}_visual">
+                            <geometry>
+                                <mesh>
+                                    <uri>{visual}</uri>
+                                </mesh>
+                            </geometry>
+                            <material>
+                                <ambient>{color[0]} {color[1]} {color[2]} {color[3]}</ambient>
+                                <diffuse>{color[0]} {color[1]} {color[2]} {color[3]}</diffuse>
+                                <specular>{color[0]} {color[1]} {color[2]} {color[3]}</specular>
+                            </material>
+                            <transparency>{1.0 - color[3]}</transparency>
+                            {'<visibility_flags>1</visibility_flags> <cast_shadows>false</cast_shadows>' if gui_only else ''}
+                        </visual>
+                        """ if visual else ""
+                        }
+                        <inertial>
+                            <mass>{mass}</mass>
+                                <pose>{center_of_mass[0]} {center_of_mass[1]} {center_of_mass[2]} 0 0 0</pose>
+                            <inertia>
+                                <ixx>{inertia[0]}</ixx>
+                                <ixy>{inertia[1]}</ixy>
+                                <ixz>{inertia[2]}</ixz>
+                                <iyy>{inertia[3]}</iyy>
+                                <iyz>{inertia[4]}</iyz>
+                                <izz>{inertia[5]}</izz>
+                            </inertia>
+                        </inertial>
+                    </link>
+                </model>
+            </sdf>'''

env_manager/env_manager/env_manager/models/objects/model.py

@@ -0,0 +1,75 @@
+from gym_gz.scenario import model_wrapper
+from gym_gz.utils.scenario import get_unique_model_name
+from rbs_assets_library import get_model_file
+from scenario import core as scenario_core
+from scenario import gazebo as scenario_gazebo
+
+
+class Model(model_wrapper.ModelWrapper):
+    """
+    Represents a 3D mesh model in the Gazebo simulation environment.
+    This class is responsible for loading and inserting a mesh model
+    into the Gazebo world with specified attributes.
+
+    Args:
+        world (scenario_gazebo.World): The Gazebo world where the mesh model will be inserted.
+        name (str, optional): The name of the mesh model. Defaults to "object".
+        position (tuple[float, float, float], optional): The position of the mesh in the world. Defaults to (0, 0, 0).
+        orientation (tuple[float, float, float, float], optional): The orientation of the mesh in quaternion format. Defaults to (1, 0, 0, 0).
+        gui_only (bool, optional): If True, the visual representation of the mesh will only appear in the GUI. Defaults to False.
+        **kwargs: Additional keyword arguments.
+
+    Raises:
+        RuntimeError: If the mesh model fails to be inserted into the Gazebo world.
+
+    Methods:
+        get_sdf(model_name: str) -> str:
+            Generates the SDF string used to describe the mesh model in the Gazebo simulation.
+    """
+
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str = "object",
+        position: tuple[float, float, float] = (0, 0, 0),
+        orientation: tuple[float, float, float, float] = (1, 0, 0, 0),
+        gui_only: bool = False,
+        **kwargs,
+    ):
+        # Get a unique model name
+        model_name = get_unique_model_name(world, name)
+
+        # Initial pose
+        initial_pose = scenario_core.Pose(position, orientation)
+
+        # Create SDF string for the model
+        sdf = self.get_sdf(
+            model_name=name,
+        )
+
+        # Insert the model
+        ok_model = world.to_gazebo().insert_model(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
+        super().__init__(model=model)
+
+    @classmethod
+    def get_sdf(
+        cls,
+        model_name: str,
+    ) -> str:
+        """
+        Generates the SDF string for the specified mesh model.
+
+        Args:
+            model_name (str): The name of the mesh model to generate the SDF for.
+
+        Returns:
+            str: The SDF string that defines the mesh model in Gazebo.
+        """
+        return get_model_file(model_name)

env_manager/env_manager/env_manager/models/objects/primitives/__init__.py

@@ -0,0 +1,4 @@
+from .box import Box
+from .cylinder import Cylinder
+from .plane import Plane
+from .sphere import Sphere

env_manager/env_manager/env_manager/models/objects/primitives/box.py

@@ -0,0 +1,176 @@
+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_core
+from scenario import gazebo as scenario_gazebo
+
+
+class Box(model_wrapper.ModelWrapper):
+    """
+    The Box class represents a 3D box model in the Gazebo simulation environment.
+    It includes physical and visual properties such as size, mass, color, and collision properties.
+
+    Attributes:
+        world (scenario_gazebo.World): The Gazebo world where the box model will be inserted.
+        name (str): The name of the box model. Default is "box".
+        position (tuple[float, float, float]): The position of the box in the world. Default is (0, 0, 0).
+        orientation (tuple[float, float, float, float]): The orientation of the box in quaternion format. Default is (1, 0, 0, 0).
+        size (tuple[float, float, float]): The size of the box (width, length, height). Default is (0.05, 0.05, 0.05).
+        mass (float): The mass of the box in kilograms. Default is 0.1.
+        static (bool): If True, the box will be immovable and static. Default is False.
+        collision (bool): If True, the box will have collision properties. Default is True.
+        friction (float): The friction coefficient for the box’s surface. Default is 1.0.
+        visual (bool): If True, the box will have a visual representation. Default is True.
+        gui_only (bool): If True, the visual representation of the box will only appear in the GUI, not in the simulation physics. Default is False.
+        color (tuple[float, float, float, float]): The RGBA color of the box. Default is (0.8, 0.8, 0.8, 1.0).
+
+    Raises:
+    RuntimeError:
+        If the box model fails to be inserted into the Gazebo world.
+
+    Methods:
+    --------
+    get_sdf() -> str:
+        Generates the SDF string used to describe the box model in the Gazebo simulation.
+    """
+
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str = "box",
+        position: tuple[float, float, float] = (0, 0, 0),
+        orientation: tuple[float, float, float, float] = (1, 0, 0, 0),
+        size: tuple[float, float, float] = (0.05, 0.05, 0.05),
+        mass: float = 0.1,
+        static: bool = False,
+        collision: bool = True,
+        friction: float = 1.0,
+        visual: bool = True,
+        gui_only: bool = False,
+        color: tuple[float, float, float, float] | None = None,
+        **kwargs,
+    ):
+        # Get a unique model name
+        model_name = get_unique_model_name(world, name)
+
+        # Initial pose
+        initial_pose = scenario_core.Pose(position, orientation)
+
+        if not color:
+            color = tuple(np.random.uniform(0.0, 1.0, (3,)))
+            color = (color[0], color[1], color[2], 1.0)
+
+        # Create SDF string for the model
+        sdf = self.get_sdf(
+            model_name=model_name,
+            size=size,
+            mass=mass,
+            static=static,
+            collision=collision,
+            friction=friction,
+            visual=visual,
+            gui_only=gui_only,
+            color=color,
+        )
+
+        # Insert the model
+        ok_model = world.to_gazebo().insert_model_from_string(
+            sdf, initial_pose, model_name
+        )
+        if not ok_model:
+            raise RuntimeError(f"Failed to insert {model_name}")
+
+        # Get the model
+        model = world.get_model(model_name)
+
+        # Initialize base class
+        super().__init__(model=model)
+
+    @classmethod
+    def get_sdf(
+        cls,
+        model_name: str,
+        size: tuple[float, float, float],
+        mass: float,
+        static: bool,
+        collision: bool,
+        friction: float,
+        visual: bool,
+        gui_only: bool,
+        color: tuple[float, float, float, float],
+    ) -> str:
+        """
+        Generates the SDF string for the box model.
+
+        Args:
+            model_name (str): The name of the box model.
+            size (tuple[float, float, float]): The dimensions of the box (width, length, height).
+            mass (float): The mass of the box.
+            static (bool): If True, the box will be static and immovable.
+            collision (bool): If True, the box will have collision properties.
+            friction (float): The friction coefficient for the box.
+            visual (bool): If True, the box will have a visual representation.
+            gui_only (bool): If True, the box's visual representation will only appear in the GUI and will not affect physics.
+            color (tuple[float, float, float, float]): The RGBA color of the box.
+
+        Returns:
+            The SDF string that defines the box model in Gazebo.
+        """
+        return f'''<sdf version="1.7">
+                <model name="{model_name}">
+                    <static>{"true" if static else "false"}</static>
+                    <link name="{model_name}_link">
+                        {
+                        f"""
+                        <collision name="{model_name}_collision">
+                            <geometry>
+                                <box>
+                                    <size>{size[0]} {size[1]} {size[2]}</size>
+                                </box>
+                            </geometry>
+                            <surface>
+                                <friction>
+                                    <ode>
+                                        <mu>{friction}</mu>
+                                        <mu2>{friction}</mu2>
+                                        <fdir1>0 0 0</fdir1>
+                                        <slip1>0.0</slip1>
+                                        <slip2>0.0</slip2>
+                                    </ode>
+                                </friction>
+                            </surface>
+                        </collision>
+                        """ if collision else ""
+                        }
+                        {
+                        f"""
+                        <visual name="{model_name}_visual">
+                            <geometry>
+                                <box>
+                                    <size>{size[0]} {size[1]} {size[2]}</size>
+                                </box>
+                            </geometry>
+                            <material>
+                                <ambient>{color[0]} {color[1]} {color[2]} {color[3]}</ambient>
+                                <diffuse>{color[0]} {color[1]} {color[2]} {color[3]}</diffuse>
+                                <specular>{color[0]} {color[1]} {color[2]} {color[3]}</specular>
+                            </material>
+                            <transparency>{1.0 - color[3]}</transparency>
+                            {'<visibility_flags>1</visibility_flags> <cast_shadows>false</cast_shadows>' if gui_only else ''}
+                        </visual>
+                        """ if visual else ""
+                        }
+                        <inertial>
+                            <mass>{mass}</mass>
+                            <inertia>
+                                <ixx>{(size[1]**2 + size[2]**2) * mass / 12}</ixx>
+                                <iyy>{(size[0]**2 + size[2]**2) * mass / 12}</iyy>
+                                <izz>{(size[0]**2 + size[1]**2) * mass / 12}</izz>
+                                <ixy>0.0</ixy>
+                                <ixz>0.0</ixz>
+                                <iyz>0.0</iyz>
+                            </inertia>
+                        </inertial>
+                    </link>
+                </model>
+            </sdf>'''

env_manager/env_manager/env_manager/models/objects/primitives/cylinder.py

@@ -0,0 +1,173 @@
+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_core
+from scenario import gazebo as scenario_gazebo
+
+
+class Cylinder(model_wrapper.ModelWrapper):
+    """
+    The Cylinder class represents a cylindrical model in the Gazebo simulation environment.
+
+    Attributes:
+        world (scenario_gazebo.World): The Gazebo world where the cylinder model will be inserted.
+        name (str): The name of the cylinder model. Default is "cylinder".
+        position (tuple[float, float, float]): The position of the cylinder in the world. Default is (0, 0, 0).
+        orientation (tuple[float, float, float, float]): The orientation of the cylinder in quaternion format. Default is (1, 0, 0, 0).
+        radius (float): The radius of the cylinder. Default is 0.025.
+        length (float): The length/height of the cylinder. Default is 0.1.
+        mass (float): The mass of the cylinder in kilograms. Default is 0.1.
+        static (bool): If True, the cylinder will be immovable. Default is False.
+        collision (bool): If True, the cylinder will have collision properties. Default is True.
+        friction (float): The friction coefficient for the cylinder's surface. Default is 1.0.
+        visual (bool): If True, the cylinder will have a visual representation. Default is True.
+        gui_only (bool): If True, the visual representation of the cylinder will only appear in the GUI, not in the simulation physics. Default is False.
+        color (tuple[float, float, float, float]): The RGBA color of the cylinder. Default is (0.8, 0.8, 0.8, 1.0).
+
+    Raises:
+        RuntimeError: If the cylinder model fails to be inserted into the Gazebo world.
+
+    """
+
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str = "cylinder",
+        position: tuple[float, float, float] = (0, 0, 0),
+        orientation: tuple[float, float, float, float] = (1, 0, 0, 0),
+        radius: float = 0.025,
+        length: float = 0.1,
+        mass: float = 0.1,
+        static: bool = False,
+        collision: bool = True,
+        friction: float = 1.0,
+        visual: bool = True,
+        gui_only: bool = False,
+        color: tuple[float, float, float, float] | None = None,
+        **kwargs,
+    ):
+        model_name = get_unique_model_name(world, name)
+
+        initial_pose = scenario_core.Pose(position, orientation)
+
+        if not color:
+            color = tuple(np.random.uniform(0.0, 1.0, (3,)))
+            color = (color[0], color[1], color[2], 1.0)
+
+        sdf = self.get_sdf(
+            model_name=model_name,
+            radius=radius,
+            length=length,
+            mass=mass,
+            static=static,
+            collision=collision,
+            friction=friction,
+            visual=visual,
+            gui_only=gui_only,
+            color=color,
+        )
+
+        ok_model = world.to_gazebo().insert_model_from_string(
+            sdf, initial_pose, model_name
+        )
+        if not ok_model:
+            raise RuntimeError(f"Failed to insert {model_name}")
+
+        model = world.get_model(model_name)
+
+        super().__init__(model=model)
+
+    @classmethod
+    def get_sdf(
+        cls,
+        model_name: str,
+        radius: float,
+        length: float,
+        mass: float,
+        static: bool,
+        collision: bool,
+        friction: float,
+        visual: bool,
+        gui_only: bool,
+        color: tuple[float, float, float, float],
+    ) -> str:
+        """
+        Generates the SDF string for the cylinder model.
+
+        Args:
+            model_name (str): The name of the model.
+            radius (float): The radius of the cylinder.
+            length (float): The length or height of the cylinder.
+            mass (float): The mass of the cylinder in kilograms.
+            static (bool): If True, the cylinder will remain immovable in the simulation.
+            collision (bool): If True, adds collision properties to the cylinder.
+            friction (float): The friction coefficient for the cylinder's surface.
+            visual (bool): If True, a visual representation of the cylinder will be created.
+            gui_only (bool): If True, the visual representation will only appear in the GUI, without impacting the simulation's physics.
+            color (tuple[float, float, float, float]): The RGBA color of the cylinder, where each value is between 0 and 1.
+
+        Returns:
+            str: The SDF string representing the cylinder model
+        """
+        inertia_xx_yy = (3 * radius**2 + length**2) * mass / 12
+
+        return f'''<sdf version="1.7">
+                <model name="{model_name}">
+                    <static>{"true" if static else "false"}</static>
+                    <link name="{model_name}_link">
+                        {
+                        f"""
+                        <collision name="{model_name}_collision">
+                            <geometry>
+                                <cylinder>
+                                    <radius>{radius}</radius>
+                                    <length>{length}</length>
+                                </cylinder>
+                            </geometry>
+                            <surface>
+                                <friction>
+                                    <ode>
+                                        <mu>{friction}</mu>
+                                        <mu2>{friction}</mu2>
+                                        <fdir1>0 0 0</fdir1>
+                                        <slip1>0.0</slip1>
+                                        <slip2>0.0</slip2>
+                                    </ode>
+                                </friction>
+                            </surface>
+                        </collision>
+                        """ if collision else ""
+                        }
+                        {
+                        f"""
+                        <visual name="{model_name}_visual">
+                            <geometry>
+                                <cylinder>
+                                    <radius>{radius}</radius>
+                                    <length>{length}</length>
+                                </cylinder>
+                            </geometry>
+                            <material>
+                                <ambient>{color[0]} {color[1]} {color[2]} {color[3]}</ambient>
+                                <diffuse>{color[0]} {color[1]} {color[2]} {color[3]}</diffuse>
+                                <specular>{color[0]} {color[1]} {color[2]} {color[3]}</specular>
+                            </material>
+                            <transparency>{1.0-color[3]}</transparency>
+                            {'<visibility_flags>1</visibility_flags> <cast_shadows>false</cast_shadows>' if gui_only else ''}
+                        </visual>
+                        """ if visual else ""
+                        }
+                        <inertial>
+                            <mass>{mass}</mass>
+                            <inertia>
+                                <ixx>{inertia_xx_yy}</ixx>
+                                <iyy>{inertia_xx_yy}</iyy>
+                                <izz>{(mass*radius**2)/2}</izz>
+                                <ixy>0.0</ixy>
+                                <ixz>0.0</ixz>
+                                <iyz>0.0</iyz>
+                            </inertia>
+                        </inertial>
+                    </link>
+                </model>
+            </sdf>'''

env_manager/env_manager/env_manager/models/objects/primitives/plane.py

@@ -0,0 +1,100 @@
+from gym_gz.scenario import model_wrapper
+from gym_gz.utils.scenario import get_unique_model_name
+from scenario import core as scenario_core
+from scenario import gazebo as scenario_gazebo
+
+
+class Plane(model_wrapper.ModelWrapper):
+    """
+    The Plane class represents a plane model in the Gazebo simulation environment.
+    It allows for defining a flat surface with collision and visual properties, as well as its orientation and friction settings.
+
+    Attributes:
+        world (scenario_gazebo.World): The Gazebo world where the plane model will be inserted.
+        name (str): The name of the plane model. Default is "plane".
+        position (tuple[float, float, float]): The position of the plane in the world. Default is (0, 0, 0).
+        orientation (tuple[float, float, float, float]): The orientation of the plane in quaternion format. Default is (1, 0, 0, 0).
+        size (tuple[float, float]): The size of the plane along the x and y axes. Default is (1.0, 1.0).
+        direction (tuple[float, float, float]): The normal vector representing the plane's direction. Default is (0.0, 0.0, 1.0), representing a horizontal plane.
+        collision (bool): If True, the plane will have collision properties. Default is True.
+        friction (float): The friction coefficient for the plane's surface. Default is 1.0.
+        visual (bool): If True, the plane will have a visual representation. Default is True.
+
+    Raises:
+        RuntimeError: If the plane model fails to be inserted into the Gazebo world.
+    """
+
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str = "plane",
+        position: tuple[float, float, float] = (0, 0, 0),
+        orientation: tuple[float, float, float, float] = (1, 0, 0, 0),
+        size: tuple[float, float] = (1.0, 1.0),
+        direction: tuple[float, float, float] = (0.0, 0.0, 1.0),
+        collision: bool = True,
+        friction: float = 1.0,
+        visual: bool = True,
+        **kwargs,
+    ):
+        model_name = get_unique_model_name(world, name)
+
+        initial_pose = scenario_core.Pose(position, orientation)
+
+        sdf = f'''<sdf version="1.7">
+            <model name="{model_name}">
+                <static>true</static>
+                <link name="{model_name}_link">
+                    {
+                    f"""
+                    <collision name="{model_name}_collision">
+                        <geometry>
+                            <plane>
+                                <normal>{direction[0]} {direction[1]} {direction[2]}</normal>
+                                <size>{size[0]} {size[1]}</size>
+                            </plane>
+                        </geometry>
+                        <surface>
+                            <friction>
+                                <ode>
+                                    <mu>{friction}</mu>
+                                    <mu2>{friction}</mu2>
+                                    <fdir1>0 0 0</fdir1>
+                                    <slip1>0.0</slip1>
+                                    <slip2>0.0</slip2>
+                                </ode>
+                            </friction>
+                        </surface>
+                    </collision>
+                    """ if collision else ""
+                    }
+                    {
+                    f"""
+                    <visual name="{model_name}_visual">
+                        <geometry>
+                            <plane>
+                                <normal>{direction[0]} {direction[1]} {direction[2]}</normal>
+                                <size>{size[0]} {size[1]}</size>
+                            </plane>
+                        </geometry>
+                        <material>
+                            <ambient>0.8 0.8 0.8 1</ambient>
+                            <diffuse>0.8 0.8 0.8 1</diffuse>
+                            <specular>0.8 0.8 0.8 1</specular>
+                        </material>
+                    </visual>
+                    """ if visual else ""
+                    }
+                </link>
+            </model>
+        </sdf>'''
+
+        ok_model = world.to_gazebo().insert_model_from_string(
+            sdf, initial_pose, model_name
+        )
+        if not ok_model:
+            raise RuntimeError(f"Failed to insert {model_name}")
+
+        model = world.get_model(model_name)
+
+        super().__init__(model=model)

env_manager/env_manager/env_manager/models/objects/primitives/sphere.py

@@ -0,0 +1,176 @@
+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_core
+from scenario import gazebo as scenario_gazebo
+
+
+class Sphere(model_wrapper.ModelWrapper):
+    """
+    A Sphere model for Gazebo simulation.
+
+    This class represents a spherical object that can be inserted into a Gazebo world.
+    The sphere can be customized by specifying its physical properties, such as radius, mass,
+    collision parameters, friction, and visual attributes.
+
+    Attributes:
+        world (scenario_gazebo.World): The Gazebo world where the sphere model will be inserted.
+        name (str): The name of the sphere model. Defaults to "sphere".
+        position (tuple[float, float, float]): The position of the sphere in the world. Defaults to (0, 0, 0).
+        orientation (tuple[float, float, float, float]): The orientation of the sphere in quaternion format. Defaults to (1, 0, 0, 0).
+        radius (float): The radius of the sphere. Defaults to 0.025.
+        mass (float): The mass of the sphere. Defaults to 0.1.
+        static (bool): If True, the sphere will be static in the simulation. Defaults to False.
+        collision (bool): If True, the sphere will have collision properties. Defaults to True.
+        friction (float): The friction coefficient of the sphere's surface. Defaults to 1.0.
+        visual (bool): If True, the sphere will have a visual representation. Defaults to True.
+        gui_only (bool): If True, the visual will only appear in the GUI. Defaults to False.
+        color (tuple[float, float, float, float]): The RGBA color of the sphere. Defaults to (0.8, 0.8, 0.8, 1.0).
+
+    Raises:
+        RuntimeError: If the sphere fails to be inserted into the Gazebo world.
+    """
+
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str = "sphere",
+        position: tuple[float, float, float] = (0, 0, 0),
+        orientation: tuple[float, float, float, float] = (1, 0, 0, 0),
+        radius: float = 0.025,
+        mass: float = 0.1,
+        static: bool = False,
+        collision: bool = True,
+        friction: float = 1.0,
+        visual: bool = True,
+        gui_only: bool = False,
+        color: tuple[float, float, float, float] | None = None,
+        **kwargs,
+    ):
+        # Get a unique model name
+        model_name = get_unique_model_name(world, name)
+
+        # Initial pose
+        initial_pose = scenario_core.Pose(position, orientation)
+
+        if not color:
+            color = tuple(np.random.uniform(0.0, 1.0, (3,)))
+            color = (color[0], color[1], color[2], 1.0)
+
+        # Create SDF string for the model
+        sdf = self.get_sdf(
+            model_name=model_name,
+            radius=radius,
+            mass=mass,
+            static=static,
+            collision=collision,
+            friction=friction,
+            visual=visual,
+            gui_only=gui_only,
+            color=color,
+        )
+
+        # Insert the model
+        ok_model = world.to_gazebo().insert_model_from_string(
+            sdf, initial_pose, model_name
+        )
+        if not ok_model:
+            raise RuntimeError(f"Failed to insert {model_name}")
+
+        # Get the model
+        model = world.get_model(model_name)
+
+        # Initialize base class
+        super().__init__(model=model)
+
+    @classmethod
+    def get_sdf(
+        cls,
+        model_name: str,
+        radius: float,
+        mass: float,
+        static: bool,
+        collision: bool,
+        friction: float,
+        visual: bool,
+        gui_only: bool,
+        color: tuple[float, float, float, float],
+    ) -> str:
+        """
+        Generates the SDF (Simulation Description Format) string for the sphere model.
+
+        Args:
+            model_name (str): The name of the model.
+            radius (float): The radius of the sphere.
+            mass (float): The mass of the sphere.
+            static (bool): Whether the sphere is static in the simulation.
+            collision (bool): Whether the sphere should have collision properties.
+            friction (float): The friction coefficient for the sphere.
+            visual (bool): Whether the sphere should have a visual representation.
+            gui_only (bool): Whether the visual representation is only visible in the GUI.
+            color (tuple[float, float, float, float]): The RGBA color of the sphere.
+
+        Returns:
+            str: The SDF string representing the sphere.
+        """
+        # Inertia is identical for all axes
+        inertia_xx_yy_zz = (mass * radius**2) * 2 / 5
+
+        return f'''<sdf version="1.7">
+                <model name="{model_name}">
+                    <static>{"true" if static else "false"}</static>
+                    <link name="{model_name}_link">
+                        {
+                        f"""
+                        <collision name="{model_name}_collision">
+                            <geometry>
+                                <sphere>
+                                    <radius>{radius}</radius>
+                                </sphere>
+                            </geometry>
+                            <surface>
+                                <friction>
+                                    <ode>
+                                        <mu>{friction}</mu>
+                                        <mu2>{friction}</mu2>
+                                        <fdir1>0 0 0</fdir1>
+                                        <slip1>0.0</slip1>
+                                        <slip2>0.0</slip2>
+                                    </ode>
+                                </friction>
+                            </surface>
+                        </collision>
+                        """ if collision else ""
+                        }
+                        {
+                        f"""
+                        <visual name="{model_name}_visual">
+                            <geometry>
+                                <sphere>
+                                    <radius>{radius}</radius>
+                                </sphere>
+                            </geometry>
+                            <material>
+                                <ambient>{color[0]} {color[1]} {color[2]} {color[3]}</ambient>
+                                <diffuse>{color[0]} {color[1]} {color[2]} {color[3]}</diffuse>
+                                <specular>{color[0]} {color[1]} {color[2]} {color[3]}</specular>
+                            </material>
+                            <transparency>{1.0 - color[3]}</transparency>
+                            {'<visibility_flags>1</visibility_flags> <cast_shadows>false</cast_shadows>' if gui_only else ''}
+                        </visual>
+                        """ if visual else ""
+                        }
+                        <inertial>
+                            <mass>{mass}</mass>
+                            <inertia>
+                                <ixx>{inertia_xx_yy_zz}</ixx>
+                                <iyy>{inertia_xx_yy_zz}</iyy>
+                                <izz>{inertia_xx_yy_zz}</izz>
+                                <ixy>0.0</ixy>
+                                <ixz>0.0</ixz>
+                                <iyz>0.0</iyz>
+                            </inertia>
+                        </inertial>
+                    </link>
+                </model>
+            </sdf>'''

env_manager/env_manager/env_manager/models/objects/random_object.py

@@ -0,0 +1,84 @@
+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_core
+from scenario import gazebo as scenario_gazebo
+
+from env_manager.models.utils import ModelCollectionRandomizer
+
+
+class RandomObject(model_wrapper.ModelWrapper):
+    """
+    Represents a randomly selected 3D object model in the Gazebo simulation environment.
+    This class allows for the insertion of various models based on a collection of model paths,
+    utilizing a randomization strategy for the chosen model.
+
+    Args:
+        world (scenario_gazebo.World): The Gazebo world where the random object model will be inserted.
+        name (str, optional): The name of the random object model. Defaults to "object".
+        position (tuple[float, float, float], optional): The position of the object in the world. Defaults to (0, 0, 0).
+        orientation (tuple[float, float, float, float], optional): The orientation of the object in quaternion format. Defaults to (1, 0, 0, 0).
+        model_paths (str | None, optional): Paths to the model files. Must be set; raises an error if None.
+        owner (str, optional): The owner of the model collection. Defaults to "GoogleResearch".
+        collection (str, optional): The collection of models to choose from. Defaults to "Google Scanned Objects".
+        server (str, optional): The server URL for the model collection. Defaults to "https://fuel.ignitionrobotics.org".
+        server_version (str, optional): The version of the server to use. Defaults to "1.0".
+        unique_cache (bool, optional): If True, enables caching of unique models. Defaults to False.
+        reset_collection (bool, optional): If True, resets the model collection for new selections. Defaults to False.
+        np_random (RandomState | None, optional): An instance of RandomState for random number generation. Defaults to None.
+        **kwargs: Additional keyword arguments.
+
+    Raises:
+        RuntimeError: If the model path is not set or if the random object model fails to be inserted into the Gazebo world.
+    """
+
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str = "object",
+        position: tuple[float, float, float] = (0, 0, 0),
+        orientation: tuple[float, float, float, float] = (1, 0, 0, 0),
+        model_paths: str | None = None,
+        owner: str = "GoogleResearch",
+        collection: str = "Google Scanned Objects",
+        server: str = "https://fuel.ignitionrobotics.org",
+        server_version: str = "1.0",
+        unique_cache: bool = False,
+        reset_collection: bool = False,
+        np_random: RandomState | None = None,
+        **kwargs,
+    ):
+        # Get a unique model name
+        if model_paths is None:
+            raise RuntimeError("Set model path for continue")
+        model_name = get_unique_model_name(world, name)
+
+        # Initial pose
+        initial_pose = scenario_core.Pose(position, orientation)
+
+        model_collection_randomizer = ModelCollectionRandomizer(
+            model_paths=model_paths,
+            owner=owner,
+            collection=collection,
+            server=server,
+            server_version=server_version,
+            unique_cache=unique_cache,
+            reset_collection=reset_collection,
+            np_random=np_random,
+        )
+
+        # Note: using default arguments here
+        modified_sdf_file = model_collection_randomizer.random_model()
+
+        # Insert the model
+        ok_model = world.to_gazebo().insert_model(
+            modified_sdf_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
+        model_wrapper.ModelWrapper.__init__(self, model=model)

env_manager/env_manager/env_manager/models/objects/random_primitive.py

@@ -0,0 +1,164 @@
+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_core
+from scenario import gazebo as scenario_gazebo
+
+from . import Box, Cylinder, Sphere
+
+
+class RandomPrimitive(model_wrapper.ModelWrapper):
+    """
+    Represents a randomly generated primitive shape (box, cylinder, or sphere) in the Gazebo simulation environment.
+    This class allows for the insertion of various primitive models based on the user's specifications or randomly chosen.
+
+    Args:
+        world (scenario_gazebo.World): The Gazebo world where the primitive model will be inserted.
+        name (str, optional): The name of the primitive model. Defaults to "primitive".
+        use_specific_primitive ((str | None), optional): If specified, the exact type of primitive to create ('box', 'cylinder', or 'sphere'). Defaults to None, which will randomly select a primitive type.
+        position (tuple[float, float, float], optional): The position of the primitive in the world. Defaults to (0, 0, 0).
+        orientation (tuple[float, float, float, float], optional): The orientation of the primitive in quaternion format. Defaults to (1, 0, 0, 0).
+        static (bool, optional): If True, the primitive will be static and immovable. Defaults to False.
+        collision (bool, optional): If True, the primitive will have collision properties. Defaults to True.
+        visual (bool, optional): If True, the primitive will have a visual representation. Defaults to True.
+        gui_only (bool, optional): If True, the visual representation will only appear in the GUI and not in the simulation physics. Defaults to False.
+        np_random (RandomState | None, optional): An instance of RandomState for random number generation. If None, a default random generator will be used. Defaults to None.
+        **kwargs: Additional keyword arguments.
+
+    Raises:
+        RuntimeError: If the primitive model fails to be inserted into the Gazebo world.
+        TypeError: If the specified primitive type is not supported.
+    """
+
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str = "primitive",
+        use_specific_primitive: (str | None) = None,
+        position: tuple[float, float, float] = (0, 0, 0),
+        orientation: tuple[float, float, float, float] = (1, 0, 0, 0),
+        static: bool = False,
+        collision: bool = True,
+        visual: bool = True,
+        gui_only: bool = False,
+        np_random: RandomState | None = 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_core.Pose(position, orientation)
+
+        # Create SDF string for the model
+        sdf = self.get_sdf(
+            model_name=model_name,
+            use_specific_primitive=use_specific_primitive,
+            static=static,
+            collision=collision,
+            visual=visual,
+            gui_only=gui_only,
+            np_random=np_random,
+        )
+
+        # 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)
+
+    @classmethod
+    def get_sdf(
+        cls,
+        model_name: str,
+        use_specific_primitive: (str | None),
+        static: bool,
+        collision: bool,
+        visual: bool,
+        gui_only: bool,
+        np_random: RandomState,
+    ) -> str:
+        """
+        Generates the SDF (Simulation Description Format) string for the specified primitive model.
+
+        This method can create the SDF representation for a box, cylinder, or sphere based on the provided parameters.
+        If a specific primitive type is not provided, one will be randomly selected.
+
+        Args:
+            model_name (str): The name of the model being generated.
+            use_specific_primitive ((str | None)): The specific type of primitive to create ('box', 'cylinder', or 'sphere'). If None, a random primitive will be chosen.
+            static (bool): If True, the primitive will be static and immovable.
+            collision (bool): If True, the primitive will have collision properties.
+            visual (bool): If True, the primitive will have a visual representation.
+            gui_only (bool): If True, the visual representation will only appear in the GUI and will not affect physics.
+            np_random (RandomState): An instance of RandomState for random number generation.
+
+        Returns:
+            str: The SDF string that defines the specified primitive model, including its physical and visual properties.
+
+        Raises:
+            TypeError: If the specified primitive type is not supported.
+
+        """
+        if use_specific_primitive is not None:
+            primitive = use_specific_primitive
+        else:
+            primitive = np_random.choice(["box", "cylinder", "sphere"])
+
+        mass = np_random.uniform(0.05, 0.25)
+        friction = np_random.uniform(0.75, 1.5)
+        color = tuple(np_random.uniform(0.0, 1.0, (3,)))
+        color: tuple[float, float, float, float] = (color[0], color[1], color[2], 1.0)
+
+        if "box" == primitive:
+            return Box.get_sdf(
+                model_name=model_name,
+                size=tuple(np_random.uniform(0.04, 0.06, (3,))),
+                mass=mass,
+                static=static,
+                collision=collision,
+                friction=friction,
+                visual=visual,
+                gui_only=gui_only,
+                color=color,
+            )
+        elif "cylinder" == primitive:
+            return Cylinder.get_sdf(
+                model_name=model_name,
+                radius=np_random.uniform(0.01, 0.0375),
+                length=np_random.uniform(0.025, 0.05),
+                mass=mass,
+                static=static,
+                collision=collision,
+                friction=friction,
+                visual=visual,
+                gui_only=gui_only,
+                color=color,
+            )
+        elif "sphere" == primitive:
+            return Sphere.get_sdf(
+                model_name=model_name,
+                radius=np_random.uniform(0.01, 0.0375),
+                mass=mass,
+                static=static,
+                collision=collision,
+                friction=friction,
+                visual=visual,
+                gui_only=gui_only,
+                color=color,
+            )
+        else:
+            raise TypeError(
+                f"Type '{use_specific_primitive}' in not a supported primitive. Pleasure use 'box', 'cylinder' or 'sphere."
+            )

env_manager/env_manager/env_manager/models/robots/__init__.py

@@ -0,0 +1,16 @@
+from enum import Enum
+
+from .rbs_arm import RbsArm
+from .robot import RobotWrapper
+
+
+class RobotEnum(Enum):
+    RBS_ARM = "rbs_arm"
+
+
+def get_robot_model_class(robot_model: str) -> type[RobotWrapper]:
+    model_mapping = {
+        RobotEnum.RBS_ARM.value: RbsArm,
+    }
+
+    return model_mapping.get(robot_model, RbsArm)

env_manager/env_manager/env_manager/models/robots/rbs_arm.py

@@ -0,0 +1,153 @@
+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):
+    """
+    A class representing a robotic arm built using the `robot_builder` library.
+
+    This class is responsible for creating a robotic arm model within a Gazebo simulation environment.
+    It allows for the manipulation of joint positions for both the arm and the gripper.
+
+    Attributes:
+        - DEFAULT_ARM_JOINT_POSITIONS (list[float]): The default joint positions for the arm.
+        - OPEN_GRIPPER_JOINT_POSITIONS (list[float]): The joint positions for the gripper in the open state.
+        - CLOSED_GRIPPER_JOINT_POSITIONS (list[float]): The joint positions for the gripper in the closed state.
+        - DEFAULT_GRIPPER_JOINT_POSITIONS (list[float]): The default joint positions for the gripper.
+    
+    Args:
+        - world (scenario_gazebo.World): The Gazebo world where the robot model will be inserted.
+        - node (Node): The ROS2 node associated with the robotic arm.
+        - urdf_string (str): The URDF string defining the robot.
+        - name (str, optional): The name of the robotic arm. Defaults to "rbs_arm".
+        - position (tuple[float, float, float], optional): The initial position of the robot in the world. Defaults to (0.0, 0.0, 0.0).
+        - orientation (tuple[float, float, float, float], optional): The initial orientation of the robot in quaternion format. Defaults to (1.0, 0, 0, 0).
+        - initial_arm_joint_positions (list[float] | float, optional): The initial joint positions for the arm. Defaults to `DEFAULT_ARM_JOINT_POSITIONS`.
+        - initial_gripper_joint_positions (list[float] | float, optional): The initial joint positions for the gripper. Defaults to `DEFAULT_GRIPPER_JOINT_POSITIONS`.
+
+    Raises:
+        - RuntimeError: If the robot model fails to be inserted into the Gazebo world.
+    """
+
+    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] | float = DEFAULT_ARM_JOINT_POSITIONS,
+        initial_gripper_joint_positions: list[float]
+        | float = DEFAULT_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)
+
+        self.__initial_gripper_joint_positions = (
+            [float(initial_gripper_joint_positions)]
+            * len(self._robot.gripper_actuated_joint_names)
+            if isinstance(initial_gripper_joint_positions, (int, float))
+            else initial_gripper_joint_positions
+        )
+
+        self.__initial_arm_joint_positions = (
+            [float(initial_arm_joint_positions)] * len(self._robot.actuated_joint_names)
+            if isinstance(initial_arm_joint_positions, (int, float))
+            else initial_arm_joint_positions
+        )
+
+        # Set initial joint configuration
+        self.set_initial_joint_positions(model)
+        super().__init__(model=model)
+
+    @property
+    def robot(self) -> Robot:
+        """Returns the robot metadata parsed from the URDF string.
+
+        Returns:
+            Robot: The robot instance containing metadata.
+        """
+        return self._robot
+
+    @property
+    def initial_arm_joint_positions(self) -> list[float]:
+        """Returns the initial joint positions for the arm.
+
+        Returns:
+            list[float]: The initial joint positions for the arm.
+        """
+        return self.__initial_arm_joint_positions
+
+    @property
+    def initial_gripper_joint_positions(self) -> list[float]:
+        """Returns the initial joint positions for the gripper.
+
+        Returns:
+            list[float]: The initial joint positions for the gripper.
+        """
+        return self.__initial_gripper_joint_positions
+
+    def set_initial_joint_positions(self, model):
+        """Sets the initial positions for the robot's joints.
+
+        This method resets the joint positions of both the arm and gripper to their specified initial values.
+
+        Args:
+            model: The model representation of the robot within the Gazebo environment.
+
+        Raises:
+            RuntimeError: If resetting the joint positions fails for any of the joints.
+        """
+        model = model.to_gazebo()
+
+        joint_position_data = [
+            (self.__initial_arm_joint_positions, self._robot.actuated_joint_names),
+            (self.__initial_gripper_joint_positions, self._robot.gripper_actuated_joint_names),
+        ]
+
+        for positions, joint_names in joint_position_data:
+            print(f"Setting joint positions for {joint_names}: {positions}")
+            if not model.reset_joint_positions(positions, joint_names):
+                raise RuntimeError(f"Failed to set initial positions of {joint_names[0].split('_')[0]}'s joints")

env_manager/env_manager/env_manager/models/robots/robot.py

@@ -0,0 +1,95 @@
+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):
+    """
+    An abstract base class for robot models in a Gazebo simulation.
+
+    This class extends the ModelWrapper from gym_gz and provides a structure for creating
+    robot models with specific configurations, including joint positions for arms and grippers.
+
+    Args:
+        world (scenario_gazebo.World, optional): The Gazebo world where the robot model will be inserted.
+        urdf_string (str, optional): The URDF (Unified Robot Description Format) string defining the robot.
+        name (str, optional): The name of the robot model. Defaults to None.
+        position (tuple[float, float, float], optional): The initial position of the robot in the world. Defaults to None.
+        orientation (tuple[float, float, float, float], optional): The initial orientation of the robot in quaternion format. Defaults to None.
+        initial_arm_joint_positions (list[float] | float, optional): The initial joint positions for the arm. Defaults to an empty list.
+        initial_gripper_joint_positions (list[float] | float, optional): The initial joint positions for the gripper. Defaults to an empty list.
+        model (model_wrapper.ModelWrapper, optional): An existing model instance to initialize the wrapper. Must be provided.
+        **kwargs: Additional keyword arguments.
+
+    Raises:
+        ValueError: If the model parameter is not provided.
+    """
+    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] | float = [],
+        initial_gripper_joint_positions: list[float] | 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:
+        """Returns the robot instance containing metadata.
+
+        This property must be implemented by subclasses to return the specific robot metadata.
+
+        Returns:
+            Robot: The robot instance.
+        """
+        pass
+
+
+    @property
+    @abstractmethod
+    def initial_gripper_joint_positions(self) -> list[float]:
+        """Returns the initial joint positions for the gripper.
+
+        This property must be implemented by subclasses to provide the initial positions of the gripper joints.
+
+        Returns:
+            list[float]: The initial joint positions for the gripper.
+        """
+        pass
+
+    @property
+    @abstractmethod
+    def initial_arm_joint_positions(self) -> list[float]:
+        """Returns the initial joint positions for the arm.
+
+        This property must be implemented by subclasses to provide the initial positions of the arm joints.
+
+        Returns:
+            list[float]: The initial joint positions for the arm.
+        """
+        pass
+
+    @abstractmethod
+    def set_initial_joint_positions(self, model):
+        """Sets the initial positions for the robot's joints.
+
+        This method must be implemented by subclasses to reset the joint positions of the robot
+        to their specified initial values.
+
+        Args:
+            model: The model representation of the robot within the Gazebo environment.
+
+        Raises:
+            RuntimeError: If resetting the joint positions fails.
+        """
+        pass

env_manager/env_manager/env_manager/models/sensors/__init__.py

@@ -0,0 +1 @@
+from .camera import Camera

env_manager/env_manager/env_manager/models/sensors/camera.py

@@ -0,0 +1,436 @@
+import os
+from threading import Thread
+
+from gym_gz.scenario import model_wrapper
+from gym_gz.utils.scenario import get_unique_model_name
+from scenario import core as scenario_core
+from scenario import gazebo as scenario_gazebo
+
+from env_manager.models.utils import ModelCollectionRandomizer
+
+
+class Camera(model_wrapper.ModelWrapper):
+    """
+    Represents a camera model in a Gazebo simulation.
+
+    This class extends the ModelWrapper from gym_gz to define a camera model that can be inserted
+    into the Gazebo world. It supports different types of cameras and can bridge ROS2 topics for 
+    camera data.
+
+    Args:
+        world (scenario_gazebo.World): The Gazebo world where the camera will be inserted.
+        name (str, optional): The name of the camera model. If None, a unique name is generated.
+        position (tuple[float, float, float], optional): The initial position of the camera. Defaults to (0, 0, 0).
+        orientation (tuple[float, float, float, float], optional): The initial orientation of the camera in quaternion. Defaults to (1, 0, 0, 0).
+        static (bool, optional): If True, the camera is a static model. Defaults to True.
+        camera_type (str, optional): The type of camera to create. Defaults to "rgbd_camera".
+        width (int, optional): The width of the camera image. Defaults to 212.
+        height (int, optional): The height of the camera image. Defaults to 120.
+        image_format (str, optional): The format of the camera image. Defaults to "R8G8B8".
+        update_rate (int, optional): The update rate for the camera sensor. Defaults to 15.
+        horizontal_fov (float, optional): The horizontal field of view for the camera. Defaults to 1.567821.
+        vertical_fov (float, optional): The vertical field of view for the camera. Defaults to 1.022238.
+        clip_color (tuple[float, float], optional): The near and far clipping distances for color. Defaults to (0.02, 1000.0).
+        clip_depth (tuple[float, float], optional): The near and far clipping distances for depth. Defaults to (0.02, 10.0).
+        noise_mean (float, optional): The mean of the noise added to the camera images. Defaults to None.
+        noise_stddev (float, optional): The standard deviation of the noise added to the camera images. Defaults to None.
+        ros2_bridge_color (bool, optional): If True, a ROS2 bridge for color images is created. Defaults to False.
+        ros2_bridge_depth (bool, optional): If True, a ROS2 bridge for depth images is created. Defaults to False.
+        ros2_bridge_points (bool, optional): If True, a ROS2 bridge for point cloud data is created. Defaults to False.
+        visibility_mask (int, optional): The visibility mask for the camera sensor. Defaults to 0.
+        visual (str, optional): The type of visual representation for the camera. Defaults to None.
+    
+    Raises:
+        ValueError: If the visual mesh or textures cannot be found.
+        RuntimeError: If the camera model fails to insert into the Gazebo world.
+    """
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str | None = None,
+        position: tuple[float, float, float] = (0, 0, 0),
+        orientation: tuple[float, float, float, float] = (1, 0, 0, 0),
+        static: bool = True,
+        camera_type: str = "rgbd_camera",
+        width: int = 212,
+        height: int = 120,
+        image_format: str = "R8G8B8",
+        update_rate: int = 15,
+        horizontal_fov: float = 1.567821,
+        vertical_fov: float = 1.022238,
+        clip_color: tuple[float, float] = (0.02, 1000.0),
+        clip_depth: tuple[float, float] = (0.02, 10.0),
+        noise_mean: float | None = None,
+        noise_stddev: float | None = None,
+        ros2_bridge_color: bool = False,
+        ros2_bridge_depth: bool = False,
+        ros2_bridge_points: bool = False,
+        visibility_mask: int = 0,
+        visual: str | None = None,
+    ):
+        # Get a unique model name
+        if name is not None:
+            model_name = get_unique_model_name(world, name)
+        else:
+            model_name = get_unique_model_name(world, camera_type)
+        self._model_name = model_name
+        self._camera_type = camera_type
+
+        # Initial pose
+        initial_pose = scenario_core.Pose(position, orientation)
+
+        use_mesh: bool = False
+        mesh_path_visual: str = ""
+
+        albedo_map = None
+        normal_map = None
+        roughness_map = None
+        metalness_map = None
+        # Get resources for visual (if enabled)
+        if visual:
+            if "intel_realsense_d435" == visual:
+                use_mesh = True
+
+                # Get path to the model and the important directories
+                model_path = ModelCollectionRandomizer.get_collection_paths(
+                    owner="OpenRobotics",
+                    collection="",
+                    model_name="Intel RealSense D435",
+                )[0]
+
+                mesh_dir = os.path.join(model_path, "meshes")
+                texture_dir = os.path.join(model_path, "materials", "textures")
+
+                # Get path to the mesh
+                mesh_path_visual = os.path.join(mesh_dir, "realsense.dae")
+                # Make sure that it exists
+                if not os.path.exists(mesh_path_visual):
+                    raise ValueError(
+                        f"Visual mesh '{mesh_path_visual}' for Camera model is not a valid file."
+                    )
+
+                # Find PBR textures
+                if texture_dir:
+                    # List all files
+                    texture_files = os.listdir(texture_dir)
+
+                    # Extract the appropriate files
+                    for texture in texture_files:
+                        texture_lower = texture.lower()
+                        if "basecolor" in texture_lower or "albedo" in texture_lower:
+                            albedo_map = os.path.join(texture_dir, texture)
+                        elif "normal" in texture_lower:
+                            normal_map = os.path.join(texture_dir, texture)
+                        elif "roughness" in texture_lower:
+                            roughness_map = os.path.join(texture_dir, texture)
+                        elif (
+                            "specular" in texture_lower or "metalness" in texture_lower
+                        ):
+                            metalness_map = os.path.join(texture_dir, texture)
+
+                if not (albedo_map and normal_map and roughness_map and metalness_map):
+                    raise ValueError("Not all textures for Camera model were found.")
+
+        # Create SDF string for the model
+        sdf = f'''<sdf version="1.9">
+            <model name="{model_name}">
+                <static>{static}</static>
+                <link name="{self.link_name}">
+                    <sensor name="camera" type="{camera_type}">
+                        <topic>{model_name}</topic>
+                        <always_on>true</always_on>
+                        <update_rate>{update_rate}</update_rate>
+                        <camera name="{model_name}_camera">
+                            <image>
+                                <width>{width}</width>
+                                <height>{height}</height>
+                                <format>{image_format}</format>
+                            </image>
+                            <horizontal_fov>{horizontal_fov}</horizontal_fov>
+                            <vertical_fov>{vertical_fov}</vertical_fov>
+                            <clip>
+                                <near>{clip_color[0]}</near>
+                                <far>{clip_color[1]}</far>
+                            </clip>
+                            {
+                            f"""<depth_camera>
+                                <clip>
+                                    <near>{clip_depth[0]}</near>
+                                    <far>{clip_depth[1]}</far>
+                                </clip>
+                            </depth_camera>""" if "rgbd" in model_name else ""
+                            }
+                            {
+                            f"""<noise>
+                                <type>gaussian</type>
+                                <mean>{noise_mean}</mean>
+                                <stddev>{noise_stddev}</stddev>
+                            </noise>""" if noise_mean is not None and noise_stddev is not None else ""
+                            }
+                            <visibility_mask>{visibility_mask}</visibility_mask>
+                        </camera>
+                        <visualize>true</visualize>
+                    </sensor>
+                    {
+                        f"""
+                        <visual name="{model_name}_visual_lens">
+                            <pose>-0.01 0 0 0 1.5707963 0</pose>
+                            <geometry>
+                                <cylinder>
+                                    <radius>0.02</radius>
+                                    <length>0.02</length>
+                                </cylinder>
+                            </geometry>
+                            <material>
+                                <ambient>0.0 0.8 0.0</ambient>
+                                <diffuse>0.0 0.8 0.0</diffuse>
+                                <specular>0.0 0.8 0.0</specular>
+                            </material>
+                        </visual>
+                        <visual name="{model_name}_visual_body">
+                            <pose>-0.05 0 0 0 0 0</pose>
+                            <geometry>
+                                <box>
+                                    <size>0.06 0.05 0.05</size>
+                                </box>
+                            </geometry>
+                            <material>
+                                <ambient>0.0 0.8 0.0</ambient>
+                                <diffuse>0.0 0.8 0.0</diffuse>
+                                <specular>0.0 0.8 0.0</specular>
+                            </material>
+                        </visual>
+                        """ if visual and not use_mesh else ""
+                        }
+                        {
+                        f"""
+                        <inertial>
+                            <mass>0.0615752</mass>
+                            <inertia>
+                                <ixx>9.108e-05</ixx>
+                                <ixy>0.0</ixy>
+                                <ixz>0.0</ixz>
+                                <iyy>2.51e-06</iyy>
+                                <iyz>0.0</iyz>
+                                <izz>8.931e-05</izz>
+                            </inertia>
+                        </inertial>
+                        <visual name="{model_name}_visual">
+                            <pose>0 0 0 0 0 1.5707963</pose>
+                            <geometry>
+                                <mesh>
+                                    <uri>{mesh_path_visual}</uri>
+                                    <submesh>
+                                        <name>RealSense</name>
+                                        <center>false</center>
+                                    </submesh>
+                                </mesh>
+                            </geometry>
+                            <material>
+                                <diffuse>1 1 1 1</diffuse>
+                                <specular>1 1 1 1</specular>
+                                <pbr>
+                                    <metal>
+                                        <albedo_map>{albedo_map}</albedo_map>
+                                        <normal_map>{normal_map}</normal_map>
+                                        <roughness_map>{roughness_map}</roughness_map>
+                                        <metalness_map>{metalness_map}</metalness_map>
+                                    </metal>
+                                </pbr>
+                            </material>
+                        </visual>
+                        """ if visual and use_mesh else ""
+                        }
+                </link>
+            </model>
+        </sdf>'''
+
+        # 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)
+
+        if ros2_bridge_color or ros2_bridge_depth or ros2_bridge_points:
+            self.__threads = []
+            self.__threads.append(
+                Thread(
+                    target=self.construct_ros2_bridge,
+                    args=(
+                        self.info_topic,
+                        "sensor_msgs/msg/CameraInfo",
+                        "gz.msgs.CameraInfo",
+                    ),
+                    daemon=True,
+                )
+            )
+            if ros2_bridge_color:
+                self.__threads.append(
+                    Thread(
+                        target=self.construct_ros2_bridge,
+                        args=(
+                            self.color_topic,
+                            "sensor_msgs/msg/Image",
+                            "gz.msgs.Image",
+                        ),
+                        daemon=True,
+                    )
+                )
+
+            if ros2_bridge_depth:
+                self.__threads.append(
+                    Thread(
+                        target=self.construct_ros2_bridge,
+                        args=(
+                            self.depth_topic,
+                            "sensor_msgs/msg/Image",
+                            "gz.msgs.Image",
+                        ),
+                        daemon=True,
+                    )
+                )
+
+            if ros2_bridge_points:
+                self.__threads.append(
+                    Thread(
+                        target=self.construct_ros2_bridge,
+                        args=(
+                            self.points_topic,
+                            "sensor_msgs/msg/PointCloud2",
+                            "gz.msgs.PointCloudPacked",
+                        ),
+                        daemon=True,
+                    )
+                )
+
+            for thread in self.__threads:
+                thread.start()
+
+    def __del__(self):
+        """Cleans up threads when the Camera object is deleted."""
+        if hasattr(self, "__threads"):
+            for thread in self.__threads:
+                thread.join()
+
+    @classmethod
+    def construct_ros2_bridge(cls, topic: str, ros_msg: str, ign_msg: str):
+        """
+        Constructs and runs a ROS2 bridge command for a given topic.
+
+        Args:
+            topic (str): The topic to bridge.
+            ros_msg (str): The ROS2 message type to use.
+            ign_msg (str): The Ignition message type to use.
+        """
+        node_name = "parameter_bridge" + topic.replace("/", "_")
+        command = (
+            f"ros2 run ros_gz_bridge parameter_bridge {topic}@{ros_msg}[{ign_msg} "
+            + f"--ros-args --remap __node:={node_name} --ros-args -p use_sim_time:=true"
+        )
+        os.system(command)
+
+    @classmethod
+    def get_frame_id(cls, model_name: str) -> str:
+        """
+        Gets the frame ID for the camera model.
+
+        Args:
+            model_name (str): The name of the camera model.
+
+        Returns:
+            str: The frame ID.
+        """
+        return f"{model_name}/{model_name}_link/camera"
+
+    @property
+    def frame_id(self) -> str:
+        """Returns the frame ID of the camera."""
+        return self.get_frame_id(self._model_name)
+
+    @classmethod
+    def get_color_topic(cls, model_name: str, camera_type: str) -> str:
+        """
+        Gets the color topic for the camera.
+
+        Args:
+            model_name (str): The name of the camera model.
+            camera_type (str): The type of the camera.
+
+        Returns:
+            str: The color topic.
+        """
+        return f"/{model_name}/image" if "rgbd" in camera_type else f"/{model_name}"
+
+    @property
+    def color_topic(self) -> str:
+        """Returns the color topic for the camera."""
+        return self.get_color_topic(self._model_name, self._camera_type)
+
+    @classmethod
+    def get_depth_topic(cls, model_name: str, camera_type: str) -> str:
+        """
+        Gets the depth topic for the camera.
+
+        Args:
+            model_name (str): The name of the camera model.
+            camera_type (str): The type of the camera.
+
+        Returns:
+            str: The depth topic.
+        """
+        return (
+            f"/{model_name}/depth_image" if "rgbd" in camera_type else f"/{model_name}"
+        )
+
+    @property
+    def depth_topic(self) -> str:
+        """Returns the depth topic for the camera."""
+        return self.get_depth_topic(self._model_name, self._camera_type)
+
+    @classmethod
+    def get_points_topic(cls, model_name: str) -> str:
+        """
+        Gets the points topic for the camera.
+
+        Args:
+            model_name (str): The name of the camera model.
+
+        Returns:
+            str: /{model_name}/points.
+        """
+        return f"/{model_name}/points"
+
+    @property
+    def points_topic(self) -> str:
+        """Returns the points topic for the camera."""
+        return self.get_points_topic(self._model_name)
+
+    @property
+    def info_topic(self):
+        """Returns the camera info topic."""
+        return f"/{self._model_name}/camera_info"
+
+    @classmethod
+    def get_link_name(cls, model_name: str) -> str:
+        """
+        Gets the link name for the camera model.
+
+        Args:
+            model_name (str): The name of the camera model.
+
+        Returns:
+            str: {model_name}_link.
+        """
+        return f"{model_name}_link"
+
+    @property
+    def link_name(self) -> str:
+        """Returns the link name for the camera."""
+        return self.get_link_name(self._model_name)

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

env_manager/env_manager/env_manager/models/terrains/ground.py

@@ -0,0 +1,104 @@
+from gym_gz.scenario import model_wrapper
+from gym_gz.utils.scenario import get_unique_model_name
+from scenario import core as scenario_core
+from scenario import gazebo as scenario_gazebo
+
+
+class Ground(model_wrapper.ModelWrapper):
+    """
+    Represents a ground model in a Gazebo simulation.
+
+    This class extends the ModelWrapper from gym_gz to define a ground model that can be
+    inserted into the Gazebo world. The ground is defined by its size, position, orientation,
+    and friction properties.
+
+    Args:
+        world (scenario_gazebo.World): The Gazebo world where the ground will be inserted.
+        name (str, optional): The name of the ground model. Defaults to "ground".
+        position (tuple[float, float, float], optional): The initial position of the ground. Defaults to (0, 0, 0).
+        orientation (tuple[float, float, float, float], optional): The initial orientation of the ground in quaternion. Defaults to (1, 0, 0, 0).
+        size (tuple[float, float], optional): The size of the ground plane. Defaults to (1.5, 1.5).
+        collision_thickness (float, optional): The thickness of the collision surface. Defaults to 0.05.
+        friction (float, optional): The friction coefficient for the ground surface. Defaults to 5.0.
+        ambient (tuple[float, float, float, float], optional): The ambient color of the material. Defaults to (0.8, 0.8, 0.8, 1.0).
+        specular (tuple[float, float, float, float], optional): The specular color of the material. Defaults to (0.8, 0.8, 0.8, 1.0).
+        diffuse (tuple[float, float, float, float], optional): The diffuse color of the material. Defaults to (0.8, 0.8, 0.8, 1.0).
+        **kwargs: Additional keyword arguments for future extensions.
+
+    Raises:
+        RuntimeError: If the ground model fails to insert into the Gazebo world.
+    """
+
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str = "ground",
+        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,
+        ambient: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0),
+        specular: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0),
+        diffuse: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0),
+        **kwargs,
+    ):
+        # Get a unique model name
+        model_name = get_unique_model_name(world, name)
+
+        # Initial pose
+        initial_pose = scenario_core.Pose(position, orientation)
+
+        # Create SDF string for the model with the provided material properties
+        sdf = f"""<sdf version="1.9">
+            <model name="{model_name}">
+                <static>true</static>
+                <link name="{model_name}_link">
+                    <collision name="{model_name}_collision">
+                        <geometry>
+                            <plane>
+                                <normal>0 0 1</normal>
+                                <size>{size[0]} {size[1]}</size>
+                            </plane>
+                        </geometry>
+                        <surface>
+                            <friction>
+                                <ode>
+                                    <mu>{friction}</mu>
+                                    <mu2>{friction}</mu2>
+                                    <fdir1>0 0 0</fdir1>
+                                    <slip1>0.0</slip1>
+                                    <slip2>0.0</slip2>
+                                </ode>
+                            </friction>
+                        </surface>
+                    </collision>
+                    <visual name="{model_name}_visual">
+                        <geometry>
+                            <plane>
+                                <normal>0 0 1</normal>
+                                <size>{size[0]} {size[1]}</size>
+                            </plane>
+                        </geometry>
+                        <material>
+                            <ambient>{' '.join(map(str, ambient))}</ambient>
+                            <diffuse>{' '.join(map(str, diffuse))}</diffuse>
+                            <specular>{' '.join(map(str, specular))}</specular>
+                        </material>
+                    </visual>
+                </link>
+            </model>
+        </sdf>"""
+
+        # 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)

env_manager/env_manager/env_manager/models/terrains/random_ground.py

@@ -0,0 +1,137 @@
+import os
+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_core
+from scenario import gazebo as scenario_gazebo
+
+from rbs_assets_library import get_textures_path
+
+
+class RandomGround(model_wrapper.ModelWrapper):
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str = "random_ground",
+        position: tuple[float, float, float] = (0, 0, 0),
+        orientation: tuple[float, float, float, float] = (1, 0, 0, 0),
+        size: tuple[float, float] = (1.0, 1.0),
+        friction: float = 5.0,
+        texture_dir: str | None = None,
+        **kwargs,
+    ):
+
+        np_random = np.random.default_rng()
+
+        # Get a unique model name
+        model_name = get_unique_model_name(world, name)
+
+        # Initial pose
+        initial_pose = scenario_core.Pose(position, orientation)
+
+        # Get textures from ENV variable if not directly specified
+        if not texture_dir:
+            texture_dir = os.environ.get("TEXTURE_DIRS", default="")
+            if not texture_dir:
+                texture_dir = get_textures_path()
+
+        # 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))
+
+            random_texture_dir = texture_dir + random_texture_dir
+
+            # 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"""<sdf version="1.9">
+            <model name="{model_name}">
+                <static>true</static>
+                <link name="{model_name}_link">
+                    <collision name="{model_name}_collision">
+                        <geometry>
+                            <plane>
+                                <normal>0 0 1</normal>
+                                <size>{size[0]} {size[1]}</size>
+                            </plane>
+                        </geometry>
+                        <surface>
+                            <friction>
+                                <ode>
+                                    <mu>{friction}</mu>
+                                    <mu2>{friction}</mu2>
+                                    <fdir1>0 0 0</fdir1>
+                                    <slip1>0.0</slip1>
+                                    <slip2>0.0</slip2>
+                                </ode>
+                            </friction>
+                        </surface>
+                    </collision>
+                    <visual name="{model_name}_visual">
+                        <geometry>
+                            <plane>
+                                <normal>0 0 1</normal>
+                                <size>{size[0]} {size[1]}</size>
+                            </plane>
+                        </geometry>
+                        <material>
+                            <ambient>1 1 1 1</ambient>
+                            <diffuse>1 1 1 1</diffuse>
+                            <specular>1 1 1 1</specular>
+                            <pbr>
+                                <metal>
+                                    {"<albedo_map>%s</albedo_map>"
+                                        % albedo_map if albedo_map is not None else ""}
+                                    {"<normal_map>%s</normal_map>"
+                                        % normal_map if normal_map is not None else ""}
+                                    {"<roughness_map>%s</roughness_map>"
+                                        % roughness_map if roughness_map is not None else ""}
+                                    {"<metalness_map>%s</metalness_map>"
+                                        % metalness_map if metalness_map is not None else ""}
+                                </metal>
+                            </pbr>
+                        </material>
+                    </visual>
+                </link>
+            </model>
+        </sdf>"""
+
+        # 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)

env_manager/env_manager/env_manager/models/utils/__init__.py

@@ -0,0 +1,2 @@
+from .model_collection_randomizer import ModelCollectionRandomizer
+from .xacro2sdf import xacro2sdf

env_manager/env_manager/env_manager/models/utils/xacro2sdf.py

@@ -0,0 +1,37 @@
+
+import subprocess
+import tempfile
+from typing import Dict, Optional, Tuple
+
+import xacro
+
+
+def xacro2sdf(
+    input_file_path: str, mappings: Dict, model_path_remap: Optional[Tuple[str, str]]
+) -> str:
+    """Convert xacro (URDF variant) with given arguments to SDF and return as a string."""
+
+    # Convert all values in mappings to strings
+    for keys, values in mappings.items():
+        mappings[keys] = str(values)
+
+    # Convert xacro to URDF
+    urdf_xml = xacro.process(input_file_name=input_file_path, mappings=mappings)
+
+    # Create temporary file for URDF (`ign sdf -p` accepts only files)
+    with tempfile.NamedTemporaryFile() as tmp_urdf:
+        with open(tmp_urdf.name, "w") as urdf_file:
+            urdf_file.write(urdf_xml)
+
+        # Convert to SDF
+        result = subprocess.run(
+            ["ign", "sdf", "-p", tmp_urdf.name], stdout=subprocess.PIPE
+        )
+        sdf_xml = result.stdout.decode("utf-8")
+
+        # Remap package name to model name, such that meshes can be located by Ignition
+        if model_path_remap is not None:
+            sdf_xml = sdf_xml.replace(model_path_remap[0], model_path_remap[1])
+
+        # Return as string
+        return sdf_xml

env_manager/env_manager/env_manager/scene/__init__.py

@@ -0,0 +1 @@
+from .scene import Scene

env_manager/env_manager/env_manager/utils/__init__.py

@@ -0,0 +1,4 @@
+from . import conversions, gazebo, logging, math, types
+from .tf2_broadcaster import Tf2Broadcaster, Tf2BroadcasterStandalone
+from .tf2_dynamic_broadcaster import Tf2DynamicBroadcaster
+from .tf2_listener import Tf2Listener, Tf2ListenerStandalone

env_manager/env_manager/env_manager/utils/conversions.py

@@ -0,0 +1,182 @@
+from typing import Tuple, Union
+
+import numpy
+import open3d
+# import pyoctree
+import sensor_msgs
+from scipy.spatial.transform import Rotation
+
+from sensor_msgs.msg import PointCloud2
+from open3d.geometry import PointCloud
+from geometry_msgs.msg import Transform
+
+
+def pointcloud2_to_open3d(
+    ros_point_cloud2: PointCloud2,
+    include_color: bool = False,
+    include_intensity: bool = False,
+    # Note: Order does not matter for DL, that's why channel swapping is disabled by default
+    fix_rgb_channel_order: bool = False,
+) -> PointCloud:
+
+    # Create output Open3D PointCloud
+    open3d_pc = PointCloud()
+
+    size = ros_point_cloud2.width * ros_point_cloud2.height
+    xyz_dtype = ">f4" if ros_point_cloud2.is_bigendian else "<f4"
+    xyz = numpy.ndarray(
+        shape=(size, 3),
+        dtype=xyz_dtype,
+        buffer=ros_point_cloud2.data,
+        offset=0,
+        strides=(ros_point_cloud2.point_step, 4),
+    )
+
+    valid_points = numpy.isfinite(xyz).any(axis=1)
+    open3d_pc.points = open3d.utility.Vector3dVector(
+        xyz[valid_points].astype(numpy.float64)
+    )
+
+    if include_color or include_intensity:
+        if len(ros_point_cloud2.fields) > 3:
+            bgr = numpy.ndarray(
+                shape=(size, 3),
+                dtype=numpy.uint8,
+                buffer=ros_point_cloud2.data,
+                offset=ros_point_cloud2.fields[3].offset,
+                strides=(ros_point_cloud2.point_step, 1),
+            )
+            if fix_rgb_channel_order:
+                # Swap channels to gain rgb (faster than `bgr[:, [2, 1, 0]]`)
+                bgr[:, 0], bgr[:, 2] = bgr[:, 2], bgr[:, 0].copy()
+            open3d_pc.colors = open3d.utility.Vector3dVector(
+                (bgr[valid_points] / 255).astype(numpy.float64)
+            )
+        else:
+            open3d_pc.colors = open3d.utility.Vector3dVector(
+                numpy.zeros((len(valid_points), 3), dtype=numpy.float64)
+            )
+    # TODO: Update octree creator once L8 image format is supported in Ignition Gazebo
+    # elif include_intensity:
+    #     # Faster approach, but only the first channel gets the intensity value (rest is 0)
+    #     intensities = numpy.zeros((len(valid_points), 3), dtype=numpy.float64)
+    #     intensities[:, [0]] = (
+    #         numpy.ndarray(
+    #             shape=(size, 1),
+    #             dtype=numpy.uint8,
+    #             buffer=ros_point_cloud2.data,
+    #             offset=ros_point_cloud2.fields[3].offset,
+    #             strides=(ros_point_cloud2.point_step, 1),
+    #         )[valid_points]
+    #         / 255
+    #     ).astype(numpy.float64)
+    #     open3d_pc.colors = open3d.utility.Vector3dVector(intensities)
+    #     # # Slower approach, but all channels get the intensity value
+    #     # intensities = numpy.ndarray(
+    #     #     shape=(size, 1),
+    #     #     dtype=numpy.uint8,
+    #     #     buffer=ros_point_cloud2.data,
+    #     #     offset=ros_point_cloud2.fields[3].offset,
+    #     #     strides=(ros_point_cloud2.point_step, 1),
+    #     # )
+    #     # open3d_pc.colors = open3d.utility.Vector3dVector(
+    #     #     numpy.tile(intensities[valid_points] / 255, (1, 3)).astype(numpy.float64)
+    #     # )
+
+    # Return the converted Open3D PointCloud
+    return open3d_pc
+
+
+def transform_to_matrix(transform: Transform) -> numpy.ndarray:
+
+    transform_matrix = numpy.zeros((4, 4))
+    transform_matrix[3, 3] = 1.0
+
+    transform_matrix[0:3, 0:3] = open3d.geometry.get_rotation_matrix_from_quaternion(
+        [
+            transform.rotation.w,
+            transform.rotation.x,
+            transform.rotation.y,
+            transform.rotation.z,
+        ]
+    )
+    transform_matrix[0, 3] = transform.translation.x
+    transform_matrix[1, 3] = transform.translation.y
+    transform_matrix[2, 3] = transform.translation.z
+
+    return transform_matrix
+
+
+# def open3d_point_cloud_to_octree_points(
+#     open3d_point_cloud: PointCloud,
+#     include_color: bool = False,
+#     include_intensity: bool = False,
+# ) -> pyoctree.Points:
+#
+#     octree_points = pyoctree.Points()
+#
+#     if include_color:
+#         features = numpy.reshape(numpy.asarray(open3d_point_cloud.colors), -1)
+#     elif include_intensity:
+#         features = numpy.asarray(open3d_point_cloud.colors)[:, 0]
+#     else:
+#         features = []
+#
+#     octree_points.set_points(
+#         # XYZ points
+#         numpy.reshape(numpy.asarray(open3d_point_cloud.points), -1),
+#         # Normals
+#         numpy.reshape(numpy.asarray(open3d_point_cloud.normals), -1),
+#         # Other features, e.g. color
+#         features,
+#         # Labels - not used
+#         [],
+#     )
+#
+#     return octree_points
+
+
+def orientation_6d_to_quat(
+    v1: Tuple[float, float, float], v2: Tuple[float, float, float]
+) -> Tuple[float, float, float, float]:
+
+    # Normalize vectors
+    col1 = v1 / numpy.linalg.norm(v1)
+    col2 = v2 / numpy.linalg.norm(v2)
+
+    # Find their orthogonal vector via cross product
+    col3 = numpy.cross(col1, col2)
+
+    # Stack into rotation matrix as columns, convert to quaternion and return
+    quat_xyzw = Rotation.from_matrix(numpy.array([col1, col2, col3]).T).as_quat()
+    return quat_xyzw
+
+
+def orientation_quat_to_6d(
+    quat_xyzw: Tuple[float, float, float, float]
+) -> Tuple[Tuple[float, float, float], Tuple[float, float, float]]:
+
+    # Convert quaternion into rotation matrix
+    rot_mat = Rotation.from_quat(quat_xyzw).as_matrix()
+
+    # Return first two columns (already normalised)
+    return (tuple(rot_mat[:, 0]), tuple(rot_mat[:, 1]))
+
+
+def quat_to_wxyz(
+    xyzw:  tuple[float, float, float, float]
+) -> tuple[float, float, float, float]:
+
+    return (xyzw[3], xyzw[0], xyzw[1], xyzw[2])
+
+    # return xyzw[[3, 0, 1, 2]]
+
+
+def quat_to_xyzw(
+    wxyz: Union[numpy.ndarray, Tuple[float, float, float, float]]
+) -> numpy.ndarray:
+
+    if isinstance(wxyz, tuple):
+        return (wxyz[1], wxyz[2], wxyz[3], wxyz[0])
+
+    return wxyz[[1, 2, 3, 0]]

env_manager/env_manager/env_manager/utils/gazebo.py

@@ -0,0 +1,262 @@
+from typing import Tuple, Union
+
+from gym_gz.scenario.model_wrapper import ModelWrapper
+from numpy import exp
+from scenario.bindings.core import Link, World
+from scipy.spatial.transform import Rotation
+
+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: ModelWrapper | str,
+    link: Link | str | None = None,
+    xyzw: bool = False,
+) -> Pose:
+    """
+    Return pose of model's link. Orientation is represented as wxyz quaternion or xyzw based on the passed argument `xyzw`.
+    """
+
+    if isinstance(model, str):
+        # Get reference to the model from its name if string is passed
+        model = world.to_gazebo().get_model(model).to_gazebo()
+
+    if link is None:
+        # Use the first link if not specified
+        link = model.get_link(link_name=model.link_names()[0])
+    elif isinstance(link, str):
+        # Get reference to the link from its name if string
+        link = model.get_link(link_name=link)
+
+    # Get position and orientation
+    position = link.position()
+    quat = link.orientation()
+
+    # Convert to xyzw order if desired
+    if xyzw:
+        quat = quat_to_xyzw(quat)
+
+    # Return pose of the model's link
+    return (
+        position,
+        quat,
+    )
+
+
+def get_model_position(
+    world: World,
+    model: ModelWrapper | str,
+    link: Link | str | None = None,
+) -> Point:
+    """
+    Return position of model's link.
+    """
+
+    if isinstance(model, str):
+        # Get reference to the model from its name if string is passed
+        model = world.to_gazebo().get_model(model).to_gazebo()
+
+    if link is None:
+        # Use the first link if not specified
+        link = model.get_link(link_name=model.link_names()[0])
+    elif isinstance(link, str):
+        # Get reference to the link from its name if string
+        link = model.get_link(link_name=link)
+
+    # Return position of the model's link
+    return link.position()
+
+
+def get_model_orientation(
+    world: World,
+    model: ModelWrapper | str,
+    link: Link | str | None = None,
+    xyzw: bool = False,
+) -> Quat:
+    """
+    Return orientation of model's link that is represented as wxyz quaternion or xyzw based on the passed argument `xyzw`.
+    """
+
+    if isinstance(model, str):
+        # Get reference to the model from its name if string is passed
+        model = world.to_gazebo().get_model(model).to_gazebo()
+
+    if link is None:
+        # Use the first link if not specified
+        link = model.get_link(link_name=model.link_names()[0])
+    elif isinstance(link, str):
+        # Get reference to the link from its name if string
+        link = model.get_link(link_name=link)
+
+    # Get orientation
+    quat = link.orientation()
+
+    # Convert to xyzw order if desired
+    if xyzw:
+        quat = quat_to_xyzw(quat)
+
+    # Return orientation of the model's link
+    return quat
+
+
+def transform_move_to_model_pose(
+    world: World,
+    position: Point,
+    quat: Quat,
+    target_model: ModelWrapper | str,
+    target_link: Link | str | None = None,
+    xyzw: bool = False,
+) -> 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.
+    """
+
+    target_frame_position, target_frame_quat = get_model_pose(
+        world,
+        model=target_model,
+        link=target_link,
+        xyzw=True,
+    )
+
+    transformed_position = Rotation.from_quat(target_frame_quat).apply(position)
+    transformed_position = (
+        transformed_position[0] + target_frame_position[0],
+        transformed_position[1] + target_frame_position[1],
+        transformed_position[2] + target_frame_position[2],
+    )
+
+    if not xyzw:
+        target_frame_quat = quat_to_wxyz(target_frame_quat)
+    transformed_quat = quat_mul(quat, target_frame_quat, xyzw=xyzw)
+
+    return (transformed_position, transformed_quat)
+
+
+def transform_move_to_model_position(
+    world: World,
+    position: Point,
+    target_model: ModelWrapper | str,
+    target_link: Link | str | None = None,
+) -> Point:
+
+    target_frame_position, target_frame_quat_xyzw = get_model_pose(
+        world,
+        model=target_model,
+        link=target_link,
+        xyzw=True,
+    )
+
+    transformed_position = Rotation.from_quat(target_frame_quat_xyzw).apply(position)
+    transformed_position = (
+        target_frame_position[0] + transformed_position[0],
+        target_frame_position[1] + transformed_position[1],
+        target_frame_position[2] + transformed_position[2],
+    )
+
+    return transformed_position
+
+
+def transform_move_to_model_orientation(
+    world: World,
+    quat: Quat,
+    target_model: ModelWrapper | str,
+    target_link: Link | str | None = None,
+    xyzw: bool = False,
+) -> Quat:
+
+    target_frame_quat = get_model_orientation(
+        world,
+        model=target_model,
+        link=target_link,
+        xyzw=xyzw,
+    )
+
+    transformed_quat = quat_mul(quat, target_frame_quat, xyzw=xyzw)
+
+    return transformed_quat
+
+
+def transform_change_reference_frame_pose(
+    world: World,
+    position: Point,
+    quat: Quat,
+    target_model: ModelWrapper | str,
+    target_link: Link | str | None,
+    xyzw: bool = False,
+) -> Pose:
+    """
+    Change reference frame of original `position` and `quat` from world coordinate system to `target_model::target_link` coordinate system.
+    """
+
+    target_frame_position, target_frame_quat = get_model_pose(
+        world,
+        model=target_model,
+        link=target_link,
+        xyzw=True,
+    )
+
+    transformed_position = (
+        position[0] - target_frame_position[0],
+        position[1] - target_frame_position[1],
+        position[2] - target_frame_position[2],
+    )
+    transformed_position = Rotation.from_quat(target_frame_quat).apply(
+        transformed_position, inverse=True
+    )
+
+    if not xyzw:
+        target_frame_quat = quat_to_wxyz(target_frame_quat)
+    transformed_quat = quat_mul(target_frame_quat, quat, xyzw=xyzw)
+
+    return (tuple(transformed_position), transformed_quat)
+
+
+def transform_change_reference_frame_position(
+    world: World,
+    position: Point,
+    target_model: ModelWrapper | str,
+    target_link: Link | str | None = None,
+) -> Point:
+
+    target_frame_position, target_frame_quat_xyzw = get_model_pose(
+        world,
+        model=target_model,
+        link=target_link,
+        xyzw=True,
+    )
+
+    transformed_position = (
+        position[0] - target_frame_position[0],
+        position[1] - target_frame_position[1],
+        position[2] - target_frame_position[2],
+    )
+    transformed_position = Rotation.from_quat(target_frame_quat_xyzw).apply(
+        transformed_position, inverse=True
+    )
+
+    return tuple(transformed_position)
+
+
+def transform_change_reference_frame_orientation(
+    world: World,
+    quat: Quat,
+    target_model: ModelWrapper | str,
+    target_link: Link | str | None = None,
+    xyzw: bool = False,
+) -> Quat:
+
+    target_frame_quat = get_model_orientation(
+        world,
+        model=target_model,
+        link=target_link,
+        xyzw=xyzw,
+    )
+
+    transformed_quat = quat_mul(target_frame_quat, quat, xyzw=xyzw)
+
+    return transformed_quat

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()

env_manager/env_manager/env_manager/utils/logging.py

@@ -0,0 +1,25 @@
+from typing import Union
+
+from gymnasium import logger as gym_logger
+from gym_gz.utils import logger as gym_ign_logger
+
+
+def set_log_level(log_level: Union[int, str]):
+    """
+    Set log level for (Gym) Ignition.
+    """
+
+    if not isinstance(log_level, int):
+        log_level = str(log_level).upper()
+
+        if "WARNING" == log_level:
+            log_level = "WARN"
+        elif not log_level in ["DEBUG", "INFO", "WARN", "ERROR", "DISABLED"]:
+            log_level = "DISABLED"
+
+        log_level = getattr(gym_logger, log_level)
+
+    gym_ign_logger.set_level(
+        level=log_level,
+        scenario_level=log_level,
+    )

env_manager/env_manager/env_manager/utils/math.py

@@ -0,0 +1,44 @@
+import numpy as np
+
+
+def quat_mul(
+    quat_0: tuple[float, float, float, float],
+    quat_1: tuple[float, float, float, float],
+    xyzw: bool = True,
+) -> tuple[float, float, float, float]:
+    """
+    Multiply two quaternions
+    """
+    if xyzw:
+        x0, y0, z0, w0 = quat_0
+        x1, y1, z1, w1 = quat_1
+        return (
+            x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
+            -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
+            x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0,
+            -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
+        )
+    else:
+        w0, x0, y0, z0 = quat_0
+        w1, x1, y1, z1 = quat_1
+        return (
+            -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
+            x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
+            -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
+            x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0,
+        )
+
+
+def distance_to_nearest_point(
+    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]:
+
+    target_distances = np.linalg.norm(np.array(points) - np.array(origin), axis=1)
+    return points[target_distances.argmin()]

env_manager/env_manager/env_manager/utils/tf2_broadcaster.py

@@ -0,0 +1,74 @@
+import sys
+from typing import Tuple
+
+import rclpy
+from geometry_msgs.msg import TransformStamped
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from tf2_ros import StaticTransformBroadcaster
+
+
+class Tf2Broadcaster:
+    def __init__(
+        self,
+        node: Node,
+    ):
+
+        self._node = node
+        self.__tf2_broadcaster = StaticTransformBroadcaster(node=self._node)
+        self._transform_stamped = TransformStamped()
+
+    def broadcast_tf(
+        self,
+        parent_frame_id: str,
+        child_frame_id: str,
+        translation: Tuple[float, float, float],
+        rotation: Tuple[float, float, float, float],
+        xyzw: bool = True,
+    ):
+        """
+        Broadcast transformation of the camera
+        """
+
+        self._transform_stamped.header.frame_id = parent_frame_id
+        self._transform_stamped.child_frame_id = child_frame_id
+
+        self._transform_stamped.header.stamp = self._node.get_clock().now().to_msg()
+
+        self._transform_stamped.transform.translation.x = float(translation[0])
+        self._transform_stamped.transform.translation.y = float(translation[1])
+        self._transform_stamped.transform.translation.z = float(translation[2])
+
+        if xyzw:
+            self._transform_stamped.transform.rotation.x = float(rotation[0])
+            self._transform_stamped.transform.rotation.y = float(rotation[1])
+            self._transform_stamped.transform.rotation.z = float(rotation[2])
+            self._transform_stamped.transform.rotation.w = float(rotation[3])
+        else:
+            self._transform_stamped.transform.rotation.w = float(rotation[0])
+            self._transform_stamped.transform.rotation.x = float(rotation[1])
+            self._transform_stamped.transform.rotation.y = float(rotation[2])
+            self._transform_stamped.transform.rotation.z = float(rotation[3])
+
+        self.__tf2_broadcaster.sendTransform(self._transform_stamped)
+
+
+class Tf2BroadcasterStandalone(Node, Tf2Broadcaster):
+    def __init__(
+        self,
+        node_name: str = "env_manager_tf_broadcaster",
+        use_sim_time: bool = True,
+    ):
+
+        try:
+            rclpy.init()
+        except Exception as e:
+            if not rclpy.ok():
+                sys.exit(f"ROS 2 context could not be initialised: {e}")
+
+        Node.__init__(self, node_name)
+        self.set_parameters(
+            [Parameter("use_sim_time", type_=Parameter.Type.BOOL, value=use_sim_time)]
+        )
+
+        Tf2Broadcaster.__init__(self, node=self)

env_manager/env_manager/env_manager/utils/tf2_dynamic_broadcaster.py

@@ -0,0 +1,74 @@
+import sys
+from typing import Tuple
+
+import rclpy
+from geometry_msgs.msg import TransformStamped
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from tf2_ros import TransformBroadcaster
+
+
+class Tf2DynamicBroadcaster:
+    def __init__(
+        self,
+        node: Node,
+    ):
+
+        self._node = node
+        self.__tf2_broadcaster = TransformBroadcaster(node=self._node)
+        self._transform_stamped = TransformStamped()
+
+    def broadcast_tf(
+        self,
+        parent_frame_id: str,
+        child_frame_id: str,
+        translation: Tuple[float, float, float],
+        rotation: Tuple[float, float, float, float],
+        xyzw: bool = True,
+    ):
+        """
+        Broadcast transformation of the camera
+        """
+
+        self._transform_stamped.header.frame_id = parent_frame_id
+        self._transform_stamped.child_frame_id = child_frame_id
+
+        self._transform_stamped.header.stamp = self._node.get_clock().now().to_msg()
+
+        self._transform_stamped.transform.translation.x = float(translation[0])
+        self._transform_stamped.transform.translation.y = float(translation[1])
+        self._transform_stamped.transform.translation.z = float(translation[2])
+
+        if xyzw:
+            self._transform_stamped.transform.rotation.x = float(rotation[0])
+            self._transform_stamped.transform.rotation.y = float(rotation[1])
+            self._transform_stamped.transform.rotation.z = float(rotation[2])
+            self._transform_stamped.transform.rotation.w = float(rotation[3])
+        else:
+            self._transform_stamped.transform.rotation.w = float(rotation[0])
+            self._transform_stamped.transform.rotation.x = float(rotation[1])
+            self._transform_stamped.transform.rotation.y = float(rotation[2])
+            self._transform_stamped.transform.rotation.z = float(rotation[3])
+
+        self.__tf2_broadcaster.sendTransform(self._transform_stamped)
+
+
+class Tf2BroadcasterStandalone(Node, Tf2DynamicBroadcaster):
+    def __init__(
+        self,
+        node_name: str = "env_manager_tf_broadcaster",
+        use_sim_time: bool = True,
+    ):
+
+        try:
+            rclpy.init()
+        except Exception as e:
+            if not rclpy.ok():
+                sys.exit(f"ROS 2 context could not be initialised: {e}")
+
+        Node.__init__(self, node_name)
+        self.set_parameters(
+            [Parameter("use_sim_time", type_=Parameter.Type.BOOL, value=use_sim_time)]
+        )
+
+        Tf2DynamicBroadcaster.__init__(self, node=self)

env_manager/env_manager/env_manager/utils/tf2_listener.py

@@ -0,0 +1,74 @@
+import sys
+from typing import Optional
+
+import rclpy
+from geometry_msgs.msg import Transform
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from tf2_ros import Buffer, TransformListener
+
+
+class Tf2Listener:
+    def __init__(
+        self,
+        node: Node,
+    ):
+
+        self._node = node
+
+        # Create tf2 buffer and listener for transform lookup
+        self.__tf2_buffer = Buffer()
+        TransformListener(buffer=self.__tf2_buffer, node=node)
+
+    def lookup_transform_sync(
+        self, target_frame: str, source_frame: str, retry: bool = True
+    ) -> Optional[Transform]:
+
+        try:
+            return self.__tf2_buffer.lookup_transform(
+                target_frame=target_frame,
+                source_frame=source_frame,
+                time=rclpy.time.Time(),
+            ).transform
+        except:
+            if retry:
+                while rclpy.ok():
+                    if self.__tf2_buffer.can_transform(
+                        target_frame=target_frame,
+                        source_frame=source_frame,
+                        time=rclpy.time.Time(),
+                        timeout=rclpy.time.Duration(seconds=1, nanoseconds=0),
+                    ):
+                        return self.__tf2_buffer.lookup_transform(
+                            target_frame=target_frame,
+                            source_frame=source_frame,
+                            time=rclpy.time.Time(),
+                        ).transform
+
+                    self._node.get_logger().warn(
+                        f'Lookup of transform from "{source_frame}"'
+                        f' to "{target_frame}" failed, retrying...'
+                    )
+            else:
+                return None
+
+
+class Tf2ListenerStandalone(Node, Tf2Listener):
+    def __init__(
+        self,
+        node_name: str = "rbs_gym_tf_listener",
+        use_sim_time: bool = True,
+    ):
+
+        try:
+            rclpy.init()
+        except Exception as e:
+            if not rclpy.ok():
+                sys.exit(f"ROS 2 context could not be initialised: {e}")
+
+        Node.__init__(self, node_name)
+        self.set_parameters(
+            [Parameter("use_sim_time", type_=Parameter.Type.BOOL, value=use_sim_time)]
+        )
+
+        Tf2Listener.__init__(self, node=self)

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]

env_manager/env_manager/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>env_manager</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="ur.narmak@gmail.com">narmak</maintainer>
+  <license>Apache-2.0</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

env_manager/env_manager/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/env_manager
+[install]
+install_scripts=$base/lib/env_manager

env_manager/env_manager/setup.py

@@ -0,0 +1,25 @@
+from setuptools import find_packages, setup
+
+package_name = 'env_manager'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools', 'trimesh'],
+    zip_safe=True,
+    maintainer='narmak',
+    maintainer_email='ur.narmak@gmail.com',
+    description='TODO: Package description',
+    license='Apache-2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
+)

env_manager/env_manager/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

env_manager/env_manager/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

env_manager/env_manager/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

env_manager/env_manager_interfaces/CMakeLists.txt

@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 3.8)
+project(env_manager_interfaces)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+    srv/StartEnv.srv
+    srv/ConfigureEnv.srv
+    srv/LoadEnv.srv
+    srv/UnloadEnv.srv
+    srv/ResetEnv.srv
+    msg/EnvState.msg
+    DEPENDENCIES lifecycle_msgs
+)
+
+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()
+
+ament_package()

env_manager/env_manager_interfaces/LICENSE

@@ -0,0 +1,202 @@
+
+                                 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.

env_manager/env_manager_interfaces/msg/EnvState.msg

@@ -0,0 +1,4 @@
+string name
+string type
+string plugin_name
+lifecycle_msgs/State state

env_manager/env_manager_interfaces/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>env_manager_interfaces</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="ur.narmak@gmail.com">solid-sinusoid</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <build_depend>builtin_interfaces</build_depend>
+  <build_depend>lifecycle_msgs</build_depend>
+
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>lifecycle_msgs</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

env_manager/env_manager_interfaces/srv/ConfigureEnv.srv

@@ -0,0 +1,3 @@
+string name
+---
+bool ok

env_manager/env_manager_interfaces/srv/LoadEnv.srv

@@ -0,0 +1,4 @@
+string name
+string type
+---
+bool ok

env_manager/env_manager_interfaces/srv/ResetEnv.srv

@@ -0,0 +1,3 @@
+
+---
+bool ok

env_manager/env_manager_interfaces/srv/StartEnv.srv

@@ -0,0 +1,4 @@
+string name
+string type
+---
+bool ok

env_manager/env_manager_interfaces/srv/UnloadEnv.srv

@@ -0,0 +1,3 @@
+string name
+---
+bool ok

env_manager/rbs_gym/LICENSE

@@ -0,0 +1,202 @@
+
+                                 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.

env_manager/rbs_gym/hyperparams/sac.yml

@@ -0,0 +1,42 @@
+# Reach
+Reach-Gazebo-v0:
+  policy: "MlpPolicy"
+  policy_kwargs:
+    n_critics: 2
+    net_arch: [128, 64]
+  n_timesteps: 200000
+  buffer_size: 25000
+  learning_starts: 5000
+  batch_size: 512
+  learning_rate: lin_0.0002
+  gamma: 0.95
+  tau: 0.001
+  ent_coef: "auto_0.1"
+  target_entropy: "auto"
+  train_freq: [1, "episode"]
+  gradient_steps: 100
+  noise_type: "normal"
+  noise_std: 0.025
+  use_sde: False
+  optimize_memory_usage: False
+
+Reach-ColorImage-Gazebo-v0:
+  policy: "CnnPolicy"
+  policy_kwargs:
+    n_critics: 2
+    net_arch: [128, 128]
+  n_timesteps: 50000
+  buffer_size: 25000
+  learning_starts: 5000
+  batch_size: 32
+  learning_rate: lin_0.0002
+  gamma: 0.95
+  tau: 0.0005
+  ent_coef: "auto_0.1"
+  target_entropy: "auto"
+  train_freq: [1, "episode"]
+  gradient_steps: 100
+  noise_type: "normal"
+  noise_std: 0.025
+  use_sde: False
+  optimize_memory_usage: False

env_manager/rbs_gym/hyperparams/td3.yml

@@ -0,0 +1,39 @@
+Reach-Gazebo-v0:
+  policy: "MlpPolicy"
+  policy_kwargs:
+    n_critics: 2
+    net_arch: [128, 64]
+  n_timesteps: 200000
+  buffer_size: 25000
+  learning_starts: 5000
+  batch_size: 512
+  learning_rate: lin_0.0002
+  gamma: 0.95
+  tau: 0.001
+  train_freq: [1, "episode"]
+  gradient_steps: 100
+  target_policy_noise: 0.1
+  target_noise_clip: 0.2
+  noise_type: "normal"
+  noise_std: 0.025
+  optimize_memory_usage: False
+
+Reach-ColorImage-Gazebo-v0:
+  policy: "CnnPolicy"
+  policy_kwargs:
+    n_critics: 2
+    net_arch: [128, 128]
+  n_timesteps: 50000
+  buffer_size: 25000
+  learning_starts: 5000
+  batch_size: 32
+  learning_rate: lin_0.0002
+  gamma: 0.95
+  tau: 0.0005
+  train_freq: [1, "episode"]
+  gradient_steps: 100
+  target_policy_noise: 0.1
+  target_noise_clip: 0.2
+  noise_type: "normal"
+  noise_std: 0.025
+  optimize_memory_usage: True

env_manager/rbs_gym/hyperparams/tqc.yml

@@ -0,0 +1,46 @@
+# Reach
+Reach-Gazebo-v0:
+  policy: "MlpPolicy"
+  policy_kwargs:
+    n_quantiles: 25
+    n_critics: 2
+    net_arch: [128, 64]
+  n_timesteps: 200000
+  buffer_size: 25000
+  learning_starts: 5000
+  batch_size: 512
+  learning_rate: lin_0.0002
+  gamma: 0.95
+  tau: 0.001
+  ent_coef: "auto_0.1"
+  target_entropy: "auto"
+  top_quantiles_to_drop_per_net: 2
+  train_freq: [1, "episode"]
+  gradient_steps: 100
+  noise_type: "normal"
+  noise_std: 0.025
+  use_sde: False
+  optimize_memory_usage: False
+
+Reach-ColorImage-Gazebo-v0:
+  policy: "CnnPolicy"
+  policy_kwargs:
+    n_quantiles: 25
+    n_critics: 2
+    net_arch: [128, 128]
+  n_timesteps: 50000
+  buffer_size: 25000
+  learning_starts: 5000
+  batch_size: 32
+  learning_rate: lin_0.0002
+  gamma: 0.95
+  tau: 0.0005
+  ent_coef: "auto_0.1"
+  target_entropy: "auto"
+  top_quantiles_to_drop_per_net: 2
+  train_freq: [1, "episode"]
+  gradient_steps: 100
+  noise_type: "normal"
+  noise_std: 0.025
+  use_sde: False
+  optimize_memory_usage: True

env_manager/rbs_gym/launch/evaluate.launch.py

@@ -0,0 +1,426 @@
+from launch import LaunchContext, LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+    SetEnvironmentVariable,
+    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
+
+def launch_setup(context, *args, **kwargs):
+    # Initialize Arguments
+    robot_type = LaunchConfiguration("robot_type")
+    # General arguments
+    with_gripper_condition = LaunchConfiguration("with_gripper")
+    controllers_file = LaunchConfiguration("controllers_file")
+    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")
+    launch_controllers = LaunchConfiguration("launch_controllers")
+    gazebo_gui = LaunchConfiguration("gazebo_gui")
+    gripper_name = LaunchConfiguration("gripper_name")
+
+    # training arguments
+    env = LaunchConfiguration("env")
+    algo = LaunchConfiguration("algo")
+    num_threads = LaunchConfiguration("num_threads")
+    seed = LaunchConfiguration("seed")
+    log_folder = LaunchConfiguration("log_folder")
+    verbose = LaunchConfiguration("verbose")
+    # use_sim_time = LaunchConfiguration("use_sim_time")
+    log_level = LaunchConfiguration("log_level")
+    env_kwargs = LaunchConfiguration("env_kwargs")
+
+    n_episodes = LaunchConfiguration("n_episodes")
+    exp_id = LaunchConfiguration("exp_id")
+    load_best = LaunchConfiguration("load_best")
+    load_checkpoint = LaunchConfiguration("load_checkpoint")
+    stochastic = LaunchConfiguration("stochastic")
+    reward_log = LaunchConfiguration("reward_log")
+    norm_reward = LaunchConfiguration("norm_reward")
+    no_render = LaunchConfiguration("no_render")
+
+
+    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', 'controllers.yaml'
+    )
+
+    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": controllers_file,
+            "robot_type": robot_type,
+            "controllers_file": initial_joint_controllers_file_path,
+            "cartesian_controllers": cartesian_controllers,
+            "description_package": description_package,
+            "description_file": description_file,
+            "robot_name": robot_name,
+            "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": launch_controllers,
+            # "gazebo_gui": gazebo_gui
+        }.items()
+    )
+
+    args = [
+            "--env",
+            env,
+            "--env-kwargs",
+            env_kwargs,
+            "--algo",
+            algo,
+            "--seed",
+            seed,
+            "--num-threads",
+            num_threads,
+            "--n-episodes",
+            n_episodes,
+            "--log-folder",
+            log_folder,
+            "--exp-id",
+            exp_id,
+            "--load-best",
+            load_best,
+            "--load-checkpoint",
+            load_checkpoint,
+            "--stochastic",
+            stochastic,
+            "--reward-log",
+            reward_log,
+            "--norm-reward",
+            norm_reward,
+            "--no-render",
+            no_render,
+            "--verbose",
+            verbose,
+            "--ros-args",
+            "--log-level",
+            log_level,
+            ]
+
+    rl_task = Node(
+        package="rbs_gym",
+        executable="evaluate",
+        output="log",
+        arguments=args,
+        parameters=[{"use_sim_time": use_sim_time}],
+    )
+
+
+
+    delay_robot_control_stack = TimerAction(
+        period=10.0,
+        actions=[single_robot_setup]
+    )
+
+    nodes_to_start = [
+        # env,
+        rl_task,
+        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="controllers_gazebosim.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="false", 
+                              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?")
+    )
+    # training arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "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
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "algo",
+            default_value="sac",
+            description="RL algorithm to use during the training.",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "num_threads",
+            default_value="-1",
+            description="Number of threads for PyTorch (-1 to use default).",
+        ))
+        # Random seed
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "seed",
+            default_value="84",
+            description="Random generator seed.",
+        ))
+        # Logging
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "log_folder",
+            default_value="logs",
+            description="Path to the log directory.",
+        ))
+        # Verbosity
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "verbose",
+            default_value="1",
+            description="Verbose mode (0: no output, 1: INFO).",
+        ))
+        # HER specifics
+    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.",
+        ))
+
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "n_episodes",
+            default_value="200",
+            description="Number of evaluation episodes.",
+        ))
+    declared_arguments.append(
+    DeclareLaunchArgument(
+            "exp_id",
+            default_value="0",
+            description="Experiment ID (default: 0: latest, -1: no exp folder).",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "load_best",
+            default_value="false",
+            description="Load best model instead of last model if available.",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "load_checkpoint",
+            default_value="0",
+            description="Load checkpoint instead of last model if available, you must pass the number of timesteps corresponding to it.",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "stochastic",
+            default_value="false",
+            description="Use stochastic actions instead of deterministic.",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "reward_log",
+            default_value="reward_logs",
+            description="Where to log reward.",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "norm_reward",
+            default_value="false",
+            description="Normalize reward if applicable (trained with VecNormalize)",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "no_render",
+            default_value="true",
+            description="Do not render the environment (useful for tests).",
+        ))
+
+
+
+    env_variables = [
+        SetEnvironmentVariable(name="OMP_DYNAMIC", value="TRUE"),
+        SetEnvironmentVariable(name="OMP_NUM_THREADS", value=str(cpu_count() // 2))
+    ]
+
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)] + env_variables)

env_manager/rbs_gym/launch/optimize.launch.py

@@ -0,0 +1,519 @@
+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.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
+
+def launch_setup(context, *args, **kwargs):
+    # Initialize Arguments
+    robot_type = LaunchConfiguration("robot_type")
+    # General arguments
+    with_gripper_condition = LaunchConfiguration("with_gripper")
+    controllers_file = LaunchConfiguration("controllers_file")
+    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")
+    launch_controllers = LaunchConfiguration("launch_controllers")
+    gripper_name = LaunchConfiguration("gripper_name")
+
+    # training arguments
+    env = LaunchConfiguration("env")
+    env_kwargs = LaunchConfiguration("env_kwargs")
+    algo = LaunchConfiguration("algo")
+    hyperparams = LaunchConfiguration("hyperparams")
+    n_timesteps = LaunchConfiguration("n_timesteps")
+    num_threads = LaunchConfiguration("num_threads")
+    seed = LaunchConfiguration("seed")
+    preload_replay_buffer = LaunchConfiguration("preload_replay_buffer")
+    log_folder = LaunchConfiguration("log_folder")
+    tensorboard_log = LaunchConfiguration("tensorboard_log")
+    log_interval = LaunchConfiguration("log_interval")
+    uuid = LaunchConfiguration("uuid")
+    eval_episodes = LaunchConfiguration("eval_episodes")
+    verbose = LaunchConfiguration("verbose")
+    truncate_last_trajectory = LaunchConfiguration("truncate_last_trajectory")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    log_level = LaunchConfiguration("log_level")
+    sampler = LaunchConfiguration("sampler")
+    pruner = LaunchConfiguration("pruner")
+    n_trials = LaunchConfiguration("n_trials")
+    n_startup_trials = LaunchConfiguration("n_startup_trials")
+    n_evaluations = LaunchConfiguration("n_evaluations")
+    n_jobs = LaunchConfiguration("n_jobs")
+    storage = LaunchConfiguration("storage")
+    study_name = LaunchConfiguration("study_name")
+
+
+    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', 'controllers.yaml'
+    )
+
+    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": controllers_file,
+            "robot_type": robot_type,
+            "controllers_file": initial_joint_controllers_file_path,
+            "cartesian_controllers": cartesian_controllers,
+            "description_package": description_package,
+            "description_file": description_file,
+            "robot_name": robot_name,
+            "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": launch_controllers,
+            # "gazebo_gui": gazebo_gui
+        }.items()
+    )
+
+    args = [
+        "--env",
+        env,
+        "--env-kwargs",
+        env_kwargs,
+        "--algo",
+        algo,
+        "--seed",
+        seed,
+        "--num-threads",
+        num_threads,
+        "--n-timesteps",
+        n_timesteps,
+        "--preload-replay-buffer",
+        preload_replay_buffer,
+        "--log-folder",
+        log_folder,
+        "--tensorboard-log",
+        tensorboard_log,
+        "--log-interval",
+        log_interval,
+        "--uuid",
+        uuid,
+        "--optimize-hyperparameters",
+        "True",
+        "--sampler",
+        sampler,
+        "--pruner",
+        pruner,
+        "--n-trials",
+        n_trials,
+        "--n-startup-trials",
+        n_startup_trials,
+        "--n-evaluations",
+        n_evaluations,
+        "--n-jobs",
+        n_jobs,
+        "--storage",
+        storage,
+        "--study-name",
+        study_name,
+        "--eval-episodes",
+        eval_episodes,
+        "--verbose",
+        verbose,
+        "--truncate-last-trajectory",
+        truncate_last_trajectory,
+       "--ros-args",
+       "--log-level",
+       log_level,
+    ]
+
+    rl_task = Node(
+        package="rbs_gym",
+        executable="train",
+        output="log",
+        arguments = args,
+        parameters=[{"use_sim_time": True}]
+    )
+
+
+    delay_robot_control_stack = TimerAction(
+        period=10.0,
+        actions=[single_robot_setup]
+    )
+    
+    nodes_to_start = [
+        rl_task,
+        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="controllers_gazebosim.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="false", 
+                              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?")
+    )
+    # training arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "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
+    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
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "trained_agent",
+            default_value="",
+            description="Path to a pretrained agent to continue training.",
+        ))
+        # Random seed
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "seed",
+            default_value="84",
+            description="Random generator seed.",
+        ))
+        # 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
+    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
+    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.",
+        ))
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "sampler",
+            default_value="tpe",
+            description="Sampler to use when optimizing hyperparameters (random, tpe or skopt).",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "pruner",
+            default_value="median",
+            description="Pruner to use when optimizing hyperparameters (halving, median or none).",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "n_trials",
+            default_value="10",
+            description="Number of trials for optimizing hyperparameters.",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "n_startup_trials",
+            default_value="5",
+            description="Number of trials before using optuna sampler.",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "n_evaluations",
+            default_value="2",
+            description="Number of evaluations for hyperparameter optimization.",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "n_jobs",
+            default_value="1",
+            description="Number of parallel jobs when optimizing hyperparameters.",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "storage",
+            default_value="",
+            description="Database storage path if distributed optimization should be used.",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "study_name",
+            default_value="",
+            description="Study name for distributed optimization.",
+        ))
+
+        # 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
+    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."
+        )),
+    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))
+#   ]
+
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

env_manager/rbs_gym/launch/test_env.launch.py

@@ -0,0 +1,487 @@
+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,
+)
+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
+import yaml
+
+
+def launch_setup(context, *args, **kwargs):
+    # Initialize Arguments
+    robot_type = LaunchConfiguration("robot_type")
+    # General arguments
+    with_gripper_condition = LaunchConfiguration("with_gripper")
+    description_package = LaunchConfiguration("description_package")
+    description_file = LaunchConfiguration("description_file")
+    use_moveit = LaunchConfiguration("use_moveit")
+    moveit_config_package = LaunchConfiguration("moveit_config_package")
+    moveit_config_file = LaunchConfiguration("moveit_config_file")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    scene_config_file = LaunchConfiguration("scene_config_file").perform(context)
+
+    ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
+    base_link_name = LaunchConfiguration("base_link_name").perform(context)
+
+    control_space = LaunchConfiguration("control_space").perform(context)
+    control_strategy = LaunchConfiguration("control_strategy").perform(context)
+
+    interactive = LaunchConfiguration("interactive").perform(context)
+
+    # training arguments
+    env = LaunchConfiguration("env")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    log_level = LaunchConfiguration("log_level")
+    env_kwargs = LaunchConfiguration("env_kwargs")
+
+    description_package_abs_path = get_package_share_directory(
+        description_package.perform(context)
+    )
+
+    simulation_controllers = os.path.join(
+        description_package_abs_path, "config", "controllers.yaml"
+    )
+
+
+    xacro_file = os.path.join(
+        description_package_abs_path,
+        "urdf",
+        description_file.perform(context),
+    )
+
+    xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml"
+
+
+    # TODO: hide this to another place
+    # Load xacro_args
+    def param_constructor(loader, node, local_vars):
+        value = loader.construct_scalar(node)
+        return LaunchConfiguration(value).perform(
+            local_vars.get("context", "Launch context if not defined")
+        )
+
+    def variable_constructor(loader, node, local_vars):
+        value = loader.construct_scalar(node)
+        return local_vars.get(value, f"Variable '{value}' not found")
+
+    def load_xacro_args(yaml_file, local_vars):
+        # Get valut from ros2 argument
+        yaml.add_constructor(
+            "!param", lambda loader, node: param_constructor(loader, node, local_vars)
+        )
+
+        # Get value from local variable in this code
+        # The local variable should be initialized before the loader was called
+        yaml.add_constructor(
+            "!variable",
+            lambda loader, node: variable_constructor(loader, node, local_vars),
+        )
+
+        with open(yaml_file, "r") as file:
+            return yaml.load(file, Loader=yaml.FullLoader)
+
+    mappings_data = load_xacro_args(xacro_config_file, locals())
+
+    robot_description_doc = xacro.process_file(xacro_file, mappings=mappings_data)
+
+    robot_description_semantic_content = ""
+
+    if use_moveit.perform(context) == "true":
+        srdf_config_file = f"{description_package_abs_path}/config/srdf_xacro_args.yaml"
+        srdf_file = os.path.join(
+            get_package_share_directory(moveit_config_package.perform(context)),
+            "srdf",
+            moveit_config_file.perform(context),
+        )
+        srdf_mappings = load_xacro_args(srdf_config_file, locals())
+        robot_description_semantic_content = xacro.process_file(srdf_file, mappings=srdf_mappings)
+        robot_description_semantic_content = robot_description_semantic_content.toprettyxml(indent=" ")
+        control_space = "joint"
+        control_strategy = "position"
+        interactive = "false"
+
+
+    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={
+            "with_gripper": with_gripper_condition,
+            "controllers_file": simulation_controllers,
+            "robot_type": robot_type,
+            "description_package": description_package,
+            "description_file": description_file,
+            "robot_name": robot_type,
+            "use_moveit": "false",
+            "moveit_config_package": moveit_config_package,
+            "moveit_config_file": moveit_config_file,
+            "use_sim_time": "true",
+            "use_skills": "false",
+            "use_controllers": "true",
+            "robot_description": robot_description_content,
+            "robot_description_semantic": robot_description_semantic_content,
+            "base_link_name": base_link_name,
+            "ee_link_name": ee_link_name,
+            "control_space": control_space,
+            "control_strategy": "effort",
+            "interactive_control": "false",
+        }.items(),
+    )
+
+    args = [
+        "--env",
+        env,
+        "--env-kwargs",
+        env_kwargs,
+        "--ros-args",
+        "--log-level",
+        log_level,
+    ]
+
+    rl_task = Node(
+        package="rbs_gym",
+        executable="test_agent",
+        output="log",
+        arguments=args,
+        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])
+
+    nodes_to_start = [
+        # env,
+        # rl_task,
+        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",
+                "ar4",
+                "ur3",
+                "ur3e",
+                "ur5",
+                "ur5e",
+                "ur10",
+                "ur10e",
+                "ur16e",
+            ],
+            default_value="rbs_arm",
+        )
+    )
+    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(
+            "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(
+            "with_gripper", default_value="true", description="With gripper or not?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_moveit", default_value="false", description="Launch moveit?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "launch_perception", default_value="false", description="Launch perception?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_controllers",
+            default_value="true",
+            description="Launch controllers?",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "scene_config_file",
+            default_value="",
+            description="Path to a scene configuration file",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ee_link_name",
+            default_value="",
+            description="End effector name of robot arm",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "base_link_name",
+            default_value="",
+            description="Base link name if robot arm",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_space",
+            default_value="task",
+            choices=["task", "joint"],
+            description="Specify the control space for the robot (e.g., task space).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_strategy",
+            default_value="position",
+            choices=["position", "velocity", "effort"],
+            description="Specify the control strategy (e.g., position control).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "interactive",
+            default_value="true",
+            description="Wheter to run the motion_control_handle controller",
+        ),
+    )
+    # training arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "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
+    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
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "trained_agent",
+            default_value="",
+            description="Path to a pretrained agent to continue training.",
+        )
+    )
+    # Random seed
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "seed",
+            default_value="-1",
+            description="Random generator seed.",
+        )
+    )
+    # 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
+    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
+    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
+    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
+    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.",
+        )
+    )
+    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.",
+        )
+    )
+    cpu_c = cpu_count()
+    if cpu_c is not None:
+        env_variables = [
+            SetEnvironmentVariable(name="OMP_DYNAMIC", value="TRUE"),
+            SetEnvironmentVariable(name="OMP_NUM_THREADS", value=str(cpu_c // 2)),
+        ]
+    else:
+        env_variables = [
+            SetEnvironmentVariable(name="OMP_DYNAMIC", value="TRUE"),
+        ]
+
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)] + env_variables
+    )

env_manager/rbs_gym/launch/train.launch.py

@@ -0,0 +1,518 @@
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+    SetEnvironmentVariable,
+    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
+import yaml
+import xacro
+
+def launch_setup(context, *args, **kwargs):
+    # Initialize Arguments
+    robot_type = LaunchConfiguration("robot_type")
+    # General arguments
+    with_gripper_condition = LaunchConfiguration("with_gripper")
+    description_package = LaunchConfiguration("description_package")
+    description_file = LaunchConfiguration("description_file")
+    use_moveit = LaunchConfiguration("use_moveit")
+    moveit_config_package = LaunchConfiguration("moveit_config_package")
+    moveit_config_file = LaunchConfiguration("moveit_config_file")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    scene_config_file = LaunchConfiguration("scene_config_file").perform(context)
+
+    ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
+    base_link_name = LaunchConfiguration("base_link_name").perform(context)
+
+    control_space = LaunchConfiguration("control_space").perform(context)
+    control_strategy = LaunchConfiguration("control_strategy").perform(context)
+
+    interactive = LaunchConfiguration("interactive").perform(context)
+
+    # training arguments
+    env = LaunchConfiguration("env")
+    algo = LaunchConfiguration("algo")
+    hyperparams = LaunchConfiguration("hyperparams")
+    n_timesteps = LaunchConfiguration("n_timesteps")
+    num_threads = LaunchConfiguration("num_threads")
+    seed = LaunchConfiguration("seed")
+    trained_agent = LaunchConfiguration("trained_agent")
+    save_freq = LaunchConfiguration("save_freq")
+    save_replay_buffer = LaunchConfiguration("save_replay_buffer")
+    preload_replay_buffer = LaunchConfiguration("preload_replay_buffer")
+    log_folder = LaunchConfiguration("log_folder")
+    tensorboard_log = LaunchConfiguration("tensorboard_log")
+    log_interval = LaunchConfiguration("log_interval")
+    uuid = LaunchConfiguration("uuid")
+    eval_freq = LaunchConfiguration("eval_freq")
+    eval_episodes = LaunchConfiguration("eval_episodes")
+    verbose = LaunchConfiguration("verbose")
+    truncate_last_trajectory = LaunchConfiguration("truncate_last_trajectory")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    log_level = LaunchConfiguration("log_level")
+    env_kwargs = LaunchConfiguration("env_kwargs")
+
+    track = LaunchConfiguration("track")
+
+    description_package_abs_path = get_package_share_directory(
+        description_package.perform(context)
+    )
+
+    simulation_controllers = os.path.join(
+        description_package_abs_path, "config", "controllers.yaml"
+    )
+
+
+    xacro_file = os.path.join(
+        description_package_abs_path,
+        "urdf",
+        description_file.perform(context),
+    )
+
+    xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml"
+
+
+    # TODO: hide this to another place
+    # Load xacro_args
+    def param_constructor(loader, node, local_vars):
+        value = loader.construct_scalar(node)
+        return LaunchConfiguration(value).perform(
+            local_vars.get("context", "Launch context if not defined")
+        )
+
+    def variable_constructor(loader, node, local_vars):
+        value = loader.construct_scalar(node)
+        return local_vars.get(value, f"Variable '{value}' not found")
+
+    def load_xacro_args(yaml_file, local_vars):
+        # Get valut from ros2 argument
+        yaml.add_constructor(
+            "!param", lambda loader, node: param_constructor(loader, node, local_vars)
+        )
+
+        # Get value from local variable in this code
+        # The local variable should be initialized before the loader was called
+        yaml.add_constructor(
+            "!variable",
+            lambda loader, node: variable_constructor(loader, node, local_vars),
+        )
+
+        with open(yaml_file, "r") as file:
+            return yaml.load(file, Loader=yaml.FullLoader)
+
+    mappings_data = load_xacro_args(xacro_config_file, locals())
+
+    robot_description_doc = xacro.process_file(xacro_file, mappings=mappings_data)
+
+    robot_description_semantic_content = ""
+
+    if use_moveit.perform(context) == "true":
+        srdf_config_file = f"{description_package_abs_path}/config/srdf_xacro_args.yaml"
+        srdf_file = os.path.join(
+            get_package_share_directory(moveit_config_package.perform(context)),
+            "srdf",
+            moveit_config_file.perform(context),
+        )
+        srdf_mappings = load_xacro_args(srdf_config_file, locals())
+        robot_description_semantic_content = xacro.process_file(srdf_file, mappings=srdf_mappings)
+        robot_description_semantic_content = robot_description_semantic_content.toprettyxml(indent=" ")
+        control_space = "joint"
+        control_strategy = "position"
+        interactive = "false"
+
+
+    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={
+            "with_gripper": with_gripper_condition,
+            "controllers_file": simulation_controllers,
+            "robot_type": robot_type,
+            "description_package": description_package,
+            "description_file": description_file,
+            "robot_name": robot_type,
+            "use_moveit": use_moveit,
+            "moveit_config_package": moveit_config_package,
+            "moveit_config_file": moveit_config_file,
+            "use_sim_time": use_sim_time,
+            "use_controllers": "true",
+            "robot_description": robot_description_content,
+            "robot_description_semantic": robot_description_semantic_content,
+            "base_link_name": base_link_name,
+            "ee_link_name": ee_link_name,
+            "control_space": control_space,
+            "control_strategy": control_strategy,
+            "interactive_control": interactive,
+        }.items(),
+    )
+
+    args = [
+        "--env",
+        env,
+        "--env-kwargs",
+        env_kwargs,
+        "--algo",
+        algo,
+        "--hyperparams",
+        hyperparams,
+        "--n-timesteps",
+        n_timesteps,
+        "--num-threads",
+        num_threads,
+        "--seed",
+        seed,
+        "--trained-agent",
+        trained_agent,
+        "--save-freq",
+        save_freq,
+        "--save-replay-buffer",
+        save_replay_buffer,
+        "--preload-replay-buffer",
+        preload_replay_buffer,
+        "--log-folder",
+        log_folder,
+        "--tensorboard-log",
+        tensorboard_log,
+        "--log-interval",
+        log_interval,
+        "--uuid",
+        uuid,
+        "--eval-freq",
+        eval_freq,
+        "--eval-episodes",
+        eval_episodes,
+        "--verbose",
+        verbose,
+        "--track",
+        track,
+        "--truncate-last-trajectory",
+        truncate_last_trajectory,
+        "--ros-args",
+        "--log-level",
+        log_level,
+    ]
+
+    clock_bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'],
+        output='screen')
+
+    rl_task = Node(
+        package="rbs_gym",
+        executable="train",
+        output="log",
+        arguments=args,
+        parameters=[{"use_sim_time": True}]
+    )
+
+
+    delay_robot_control_stack = TimerAction(
+        period=20.0,
+        actions=[single_robot_setup]
+    )
+    
+    nodes_to_start = [
+        # env,
+        rl_task,
+        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",
+                "ar4",
+                "ur3",
+                "ur3e",
+                "ur5",
+                "ur5e",
+                "ur10",
+                "ur10e",
+                "ur16e",
+            ],
+            default_value="rbs_arm",
+        )
+    )
+    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(
+            "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(
+            "with_gripper", default_value="true", description="With gripper or not?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_moveit", default_value="false", description="Launch moveit?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "launch_perception", default_value="false", description="Launch perception?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_controllers",
+            default_value="true",
+            description="Launch controllers?",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "scene_config_file",
+            default_value="",
+            description="Path to a scene configuration file",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ee_link_name",
+            default_value="",
+            description="End effector name of robot arm",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "base_link_name",
+            default_value="",
+            description="Base link name if robot arm",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_space",
+            default_value="task",
+            choices=["task", "joint"],
+            description="Specify the control space for the robot (e.g., task space).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_strategy",
+            default_value="position",
+            choices=["position", "velocity", "effort"],
+            description="Specify the control strategy (e.g., position control).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "interactive",
+            default_value="true",
+            description="Wheter to run the motion_control_handle controller",
+        ),
+    )
+    # training arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "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
+    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
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "trained_agent",
+            default_value="",
+            description="Path to a pretrained agent to continue training.",
+        ))
+        # Random seed
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "seed",
+            default_value="-1",
+            description="Random generator seed.",
+        ))
+        # 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
+    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
+    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
+    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
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "verbose",
+            default_value="1",
+            description="Verbose mode (0: no output, 1: INFO).",
+        ))
+    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.",
+        ))
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "track",
+            default_value="true",
+            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))
+    ]
+
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)] + env_variables)

env_manager/rbs_gym/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rbs_gym</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="ur.narmak@gmail.com">narmak</maintainer>
+  <license>Apache-2.0</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>