refactor env_manager and rbs_gym to work with rbs_runtime

env_manager/env_interface/CMakeLists.txt

@@ -1,59 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(env_interface)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-set(INCLUDE_DEPENDS
-  rclcpp_lifecycle
-  rbs_utils
-  tf2_ros
-  tf2_eigen
-)
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-foreach(Dependency IN LISTS INCLUDE_DEPENDS)
-  find_package(${Dependency} REQUIRED)
-endforeach()
-
-add_library(${PROJECT_NAME} SHARED
-  src/env_interface_base.cpp)
-
-target_include_directories(${PROJECT_NAME} PUBLIC
-  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include/env_interface>
-)
-
-ament_target_dependencies(${PROJECT_NAME} PUBLIC ${INCLUDE_DEPENDS})
-
-target_compile_definitions(${PROJECT_NAME} PRIVATE "ENVIROMENT_INTERFACE_BUILDING_DLL")
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-install(
-  DIRECTORY include/
-  DESTINATION include/env_interface
-)
-install(TARGETS ${PROJECT_NAME}
-  EXPORT export_${PROJECT_NAME}
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-)
-
-ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-ament_export_dependencies(${INCLUDE_DEPENDS})
-
-ament_package()

env_manager/env_interface/include/env_interface/env_interface.hpp

@@ -1,16 +0,0 @@
-#include "env_interface/env_interface_base.hpp"
-#include "rbs_utils/rbs_utils.hpp"
-#include <memory>
-#include <vector>
-
-namespace env_interface
-{
-class EnvInterface : public EnvInterfaceBase
-{
-public:
-  EnvInterface() = default;
-  virtual ~EnvInterface() = default;
-protected:
-  std::shared_ptr<std::vector<rbs_utils::EnvModel>> m_env_models;
-};
-}  // namespace env_interface

env_manager/env_interface/include/env_interface/env_interface_base.hpp

@@ -1,40 +0,0 @@
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_lifecycle/lifecycle_node.hpp"
-#include <cstddef>
-#include <cstdint>
-
-namespace env_interface
-{
-
-using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
-
-// template <typename A_space, typename S_space>
-class EnvInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
-{
-public:
-  EnvInterfaceBase() = default;
-
-  virtual ~EnvInterfaceBase() = default;
-
-  virtual CallbackReturn init(const std::string& t_env_name, const std::string& t_namespace = "",
-                              const rclcpp::NodeOptions& t_node_options = rclcpp::NodeOptions());
-
-  const rclcpp_lifecycle::State& configure();
-
-  virtual CallbackReturn on_init() = 0;
-
-  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> getNode();
-  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> getNode() const;
-
-  const rclcpp_lifecycle::State& getState() const;
-
-  // virtual void setActionSpace(const A_space& action, const std::size_t& size);
-
-protected:
-  // A_space action_space;
-  // S_space observation_space;
-
-private:
-  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> m_node;
-};
-}  // namespace env_interface

env_manager/env_interface/package.xml

@@ -1,19 +0,0 @@
-<?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_interface</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
-  <license>Apache-2.0</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>rbs_utils</depend>
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

env_manager/env_interface/src/env_interface.cpp

@@ -1,6 +0,0 @@
-#include "env_interface/env_interface.hpp"
-
-namespace env_interface {
-
-
-}

env_manager/env_interface/src/env_interface_base.cpp

@@ -1,45 +0,0 @@
-#include "env_interface/env_interface_base.hpp"
-
-namespace env_interface
-{
-
-CallbackReturn EnvInterfaceBase::init(const std::string& t_env_name, const std::string& t_namespace,
-                                      const rclcpp::NodeOptions& t_node_options)
-{
-  m_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(t_env_name, t_namespace, t_node_options, false);
-
-  if (on_init() == CallbackReturn::FAILURE)
-  {
-    return CallbackReturn::FAILURE;
-  }
-
-  m_node->register_on_configure(std::bind(&EnvInterfaceBase::on_configure, this, std::placeholders::_1));
-  m_node->register_on_activate(std::bind(&EnvInterfaceBase::on_activate, this, std::placeholders::_1));
-  m_node->register_on_cleanup(std::bind(&EnvInterfaceBase::on_cleanup, this, std::placeholders::_1));
-  m_node->register_on_deactivate(std::bind(&EnvInterfaceBase::on_deactivate, this, std::placeholders::_1));
-  m_node->register_on_error(std::bind(&EnvInterfaceBase::on_error, this, std::placeholders::_1));
-  m_node->register_on_shutdown(std::bind(&EnvInterfaceBase::on_shutdown, this, std::placeholders::_1));
-
-  return CallbackReturn::SUCCESS;
-}
-
-const rclcpp_lifecycle::State& EnvInterfaceBase::configure()
-{
-  return getNode()->configure();
-}
-
-std::shared_ptr<rclcpp_lifecycle::LifecycleNode> EnvInterfaceBase::getNode()
-{
-  if (!m_node.get())
-  {
-    throw std::runtime_error("Lifecycle node hasn't been initialized yet");
-  }
-  return m_node;
-}
-
-const rclcpp_lifecycle::State& EnvInterfaceBase::getState() const
-{
-  return m_node->get_current_state();
-}
-
-}  // namespace env_interface

env_manager/env_manager/CMakeLists.txt

@@ -5,52 +5,13 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-set(INCLUDE_DEPENDS
-    rclcpp
-    rclcpp_lifecycle
-    env_manager_interfaces
-    pluginlib
-    env_interface
-)
-
 # find dependencies
 find_package(ament_cmake REQUIRED)
-foreach(Dep IN ITEMS ${INCLUDE_DEPENDS})
-    find_package(${Dep} REQUIRED)    
-endforeach()
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
-
+ament_python_install_package(${PROJECT_NAME})
-
-add_library(${PROJECT_NAME} SHARED
-    src/env_manager.cpp
-)
-target_include_directories(${PROJECT_NAME} PRIVATE include)
-ament_target_dependencies(${PROJECT_NAME} ${INCLUDE_DEPENDS})
-target_compile_definitions(${PROJECT_NAME} PRIVATE "ENVIRONMENT_MANAGER_BUILDING_DLL")
-
-add_executable(run_env_manager src/run_env_manager.cpp)
-target_include_directories(run_env_manager PRIVATE include)
-target_link_libraries(run_env_manager ${PROJECT_NAME})
-ament_target_dependencies(run_env_manager ${INCLUDE_DEPENDS})
-
-install(TARGETS ${PROJECT_NAME}
-    RUNTIME DESTINATION bin
-    LIBRARY DESTINATION lib
-    ARCHIVE DESTINATION lib
-)
-
-install(TARGETS run_env_manager
-    RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
-
-install(DIRECTORY include/
-    DESTINATION include
-)
-
-
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -64,14 +25,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_export_libraries(
+
-    ${PROJECT_NAME}
+install(DIRECTORY env_manager/worlds DESTINATION share/${PROJECT_NAME})
-)
-ament_export_include_directories(
-    include
-)
-ament_export_dependencies(
-    ${INCLUDE_DEPENDS}
-)
 
 ament_package()

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

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

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

@@ -0,0 +1,42 @@
+from dataclasses import dataclass, field
+import numpy as np
+
+@dataclass
+class CameraData:
+    name: str  = field(default_factory=str)
+    enable: bool = field(default=True)
+    type: str = field(default="rgbd_camera")
+    relative_to: str = field(default="base_link")
+
+    width: int = field(default=128)
+    height: int = field(default=128)
+    image_format: str = field(default="R8G8B8")
+    update_rate: int = field(default=10)
+    horizontal_fov: float = field(default=np.pi / 3.0)
+    vertical_fov: float = field(default=np.pi / 3.0)
+
+    clip_color: tuple[float, float] = field(default=(0.01, 1000.0))
+    clip_depth: tuple[float, float] = field(default=(0.05, 10.0))
+
+    noise_mean: float | None = None
+    noise_stddev: float | None = None
+
+    publish_color: bool = field(default=False)
+    publish_depth: bool = field(default=False)
+    publish_points: bool = field(default=False)
+
+    spawn_position: tuple[float, float, float] = field(default=(0, 0, 1))
+    spawn_quat_xyzw: tuple[float, float, float, float] = field(
+        default=(0, 0.70710678118, 0, 0.70710678118)
+    )
+
+    random_pose_rollouts_num: int = field(default=1)
+    random_pose_mode: str = field(default="orbit")
+    random_pose_orbit_distance: float = field(default=1.0)
+    random_pose_orbit_height_range: tuple[float, float] = field(default=(0.1, 0.7))
+    random_pose_orbit_ignore_arc_behind_robot: float = field(default=np.pi / 8)
+    random_pose_select_position_options: list[tuple[float, float, float]] = field(
+        default_factory=list
+    )
+    random_pose_focal_point_z_offset: float = field(default=0.0)
+    random_pose_rollout_counter: float = field(default=0.0)

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

@@ -0,0 +1,12 @@
+from dataclasses import dataclass, field
+
+@dataclass
+class LightData:
+    type: str = field(default="sun")
+    direction: tuple[float, float, float] = field(default=(0.5, -0.25, -0.75))
+    random_minmax_elevation: tuple[float, float] = field(default=(-0.15, -0.65))
+    color: tuple[float, float, float, float] = field(default=(1.0, 1.0, 1.0, 1.0))
+    distance: float = field(default=1000.0)
+    visual: bool = field(default=True)
+    radius: float = field(default=25.0)
+    model_rollouts_num: int = field(default=1)

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

@@ -0,0 +1,64 @@
+from dataclasses import dataclass, field
+
+
+@dataclass
+class ObjectRandomizerData:
+    count: int = field(default=0)
+    random_pose: bool = field(default=False)
+    random_position: bool = field(default=False)
+    random_orientation: bool = field(default=False)
+    random_model: bool = field(default=False)
+    random_spawn_position_segments: list = field(default_factory=list)
+    random_spawn_position_update_workspace_centre: bool = field(default=False)
+    random_spawn_volume: tuple[float, float, float] = field(default=(0.5, 0.5, 0.5))
+    models_rollouts_num: int = field(default=0)
+
+
+@dataclass
+class ObjectData:
+    name: str = field(default="object")
+    type: str = field(default_factory=str)
+    relative_to: str = field(default="world")
+    position: tuple[float, float, float] = field(default=(0.0, 0.0, 0.0))
+    orientation: tuple[float, float, float, float] = field(default=(1.0, 0.0, 0.0, 0.0))
+    static: bool = field(default=False)
+    randomize: ObjectRandomizerData = field(default_factory=ObjectRandomizerData)
+
+
+@dataclass
+class PrimitiveObjectData(ObjectData):
+    collision: bool = field(default=True)
+    visual: bool = field(default=True)
+    color: tuple = field(default=(0.8, 0.8, 0.8, 1.0))
+    mass: float = field(default=0.1)
+
+
+@dataclass
+class SphereObjectData(PrimitiveObjectData):
+    radius: float = field(default=0.025)
+    friction: float = field(default=1.0)
+
+
+@dataclass
+class PlaneObjectData(PrimitiveObjectData):
+    size: tuple = field(default=(1.0, 1.0))
+    direction: tuple = field(default=(0.0, 0.0, 1.0))
+    friction: float = field(default=1.0)
+
+
+@dataclass
+class CylinderObjectData(PrimitiveObjectData):
+    radius: float = field(default=0.025)
+    length: float = field(default=0.1)
+    friction: float = field(default=1.0)
+
+
+@dataclass
+class BoxObjectData(PrimitiveObjectData):
+    size: tuple = field(default=(0.05, 0.05, 0.05))
+    friction: float = field(default=1.0)
+
+
+@dataclass
+class MeshObjectData(ObjectData):
+    texture: list[float] = field(default_factory=list)

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

@@ -0,0 +1,25 @@
+from dataclasses import dataclass, field
+
+@dataclass
+class RobotRandomizerData:
+    pose: bool = field(default=False)
+    spawn_volume: tuple[float, float, float] = field(default=(1.0, 1.0, 0.0))
+    joint_positions: bool = field(default=False)
+    joint_positions_std: float = field(default=0.1)
+    joint_positions_above_object_spawn: bool = field(default=False)
+    joint_positions_above_object_spawn_elevation: float = field(default=0.2)
+    joint_positions_above_object_spawn_xy_randomness: float = field(default=0.2)
+
+
+@dataclass
+class RobotData:
+    name: str = field(default_factory=str)
+    urdf_string: str = field(default_factory=str)
+    spawn_position: tuple[float, float, float] = field(default=(0.0, 0.0, 0.0))
+    spawn_quat_xyzw: tuple[float, float, float, float] = field(
+        default=(0.0, 0.0, 0.0, 1.0)
+    )
+    joint_positioins: list[float] = field(default_factory=list)
+    with_gripper: bool = field(default=False)
+    gripper_jont_positions: list[float] = field(default_factory=list)
+    randomizer: RobotRandomizerData = field(default_factory=RobotRandomizerData)

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

@@ -0,0 +1,30 @@
+
+from dataclasses import dataclass, field
+from .objects import ObjectData
+from .robot import RobotData
+from .terrain import TerrainData
+from .light import LightData
+from .camera import CameraData
+
+@dataclass
+class PluginsData:
+    scene_broadcaster: bool = field(default_factory=bool)
+    user_commands: bool = field(default_factory=bool)
+    fts_broadcaster: bool = field(default_factory=bool)
+    sensors_render_engine: str = field(default="ogre2")
+
+@dataclass
+class SceneData:
+    physics_rollouts_num: int = field(default=0)
+    gravity: tuple[float, float, float] = field(default=(0.0, 0.0, -9.80665))
+    gravity_std: tuple[float, float, float] = field(default=(0.0, 0.0, 0.0232))
+    robot: RobotData = field(default_factory=RobotData)
+    terrain: TerrainData = field(default_factory=TerrainData)
+    light: LightData = field(default_factory=LightData)
+    objects: list[ObjectData] = field(default_factory=list)
+    camera: list[CameraData] = field(default_factory=list)
+    plugins: PluginsData = field(default_factory=PluginsData)
+
+    
+    
+

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

@@ -0,0 +1,11 @@
+
+from dataclasses import dataclass, field
+
+
+@dataclass
+class TerrainData:
+    type: str = field(default="flat")
+    spawn_position: tuple[float, float, float] = field(default=(0, 0, 0))
+    spawn_quat_xyzw: tuple[float, float, float, float] = field(default=(0, 0, 0, 1))
+    size: tuple[float, float] = field(default=(1.5, 1.5))
+    model_rollouts_num: int = field(default=1)

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/sun.py

@@ -1,18 +1,17 @@
-from typing import List, Tuple
-
 import numpy as np
 from gym_gz.scenario import model_wrapper
 from gym_gz.utils.scenario import get_unique_model_name
-from scenario import core as scenario
+from scenario import core as scenario_core
+from scenario import gazebo as scenario_gazebo
 
 
 class Sun(model_wrapper.ModelWrapper):
     def __init__(
         self,
-        world: scenario.World,
+        world: scenario_gazebo.World,
         name: str = "sun",
-        direction: Tuple[float, float, float] = (0.5, -0.25, -0.75),
+        direction: tuple[float, float, float] = (0.5, -0.25, -0.75),
-        color: List[float] = (1.0, 1.0, 1.0, 1.0),
+        color: tuple[float, float, float, float] = (1.0, 1.0, 1.0, 1.0),
         distance: float = 800.0,
         visual: bool = True,
         radius: float = 20.0,
@@ -23,20 +22,19 @@ class Sun(model_wrapper.ModelWrapper):
         attenuation_quadratic: float = 0.001,
         **kwargs,
     ):
-
         # Get a unique model name
         model_name = get_unique_model_name(world, name)
 
         # Normalize direction
-        direction = np.array(direction)
+        np_direction = np.array(direction)
-        direction = direction / np.linalg.norm(direction)
+        np_direction = np_direction / np.linalg.norm(np_direction)
 
         # Initial pose
-        initial_pose = scenario.Pose(
+        initial_pose = scenario_core.Pose(
             (
-                -direction[0] * distance,
+                -np_direction[0] * distance,
-                -direction[1] * distance,
+                -np_direction[1] * distance,
-                -direction[2] * distance,
+                -np_direction[2] * distance,
             ),
             (1, 0, 0, 0),
         )
@@ -44,7 +42,7 @@ class Sun(model_wrapper.ModelWrapper):
         # Create SDF string for the model
         sdf = self.get_sdf(
             model_name=model_name,
-            direction=direction,
+            direction=tuple(np_direction),
             color=color,
             visual=visual,
             radius=radius,
@@ -70,10 +68,10 @@ class Sun(model_wrapper.ModelWrapper):
 
     @classmethod
     def get_sdf(
-        self,
+        cls,
         model_name: str,
-        direction: Tuple[float, float, float],
+        direction: tuple[float, float, float],
-        color: Tuple[float, float, float, float],
+        color: tuple[float, float, float, float],
         visual: bool,
         radius: float,
         specular: float,
@@ -82,7 +80,6 @@ class Sun(model_wrapper.ModelWrapper):
         attenuation_linear: float,
         attenuation_quadratic: float,
     ) -> str:
-
         return f'''<sdf version="1.9">
                 <model name="{model_name}">
                     <static>true</static>

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

@@ -5,7 +5,7 @@ from gym_gz.utils.scenario import get_unique_model_name
 from numpy.random import RandomState
 from scenario import core as scenario
 
-from rbs_gym.envs.models.utils import ModelCollectionRandomizer
+from env_manager.models.utils import ModelCollectionRandomizer
 
 
 class RandomObject(model_wrapper.ModelWrapper):

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

@@ -0,0 +1,21 @@
+from .robot import RobotWrapper
+# from .rbs_arm import RbsArm
+from .rbs_arm_b import RbsArm as RbsArmB
+
+# from .panda import Panda
+from enum import Enum
+
+
+class RobotEnum(Enum):
+    RBS_ARM = "rbs_arm"
+    # PANDA = "panda"  # Uncomment when Panda is implemented
+
+
+def get_robot_model_class(robot_model: str) -> type[RobotWrapper]:
+    # Используем enum для проверки модели робота
+    model_mapping = {
+        RobotEnum.RBS_ARM.value: RbsArmB,
+        # RobotModel.PANDA.value: Panda,  # Uncomment when Panda is implemented
+    }
+
+    return model_mapping.get(robot_model, RbsArmB)

env_manager/env_manager/env_manager/models/robots/panda.py

@@ -7,7 +7,7 @@ from gym_gz.utils.scenario import get_unique_model_name
 from scenario import core as scenario
 from scenario import gazebo as scenario_gazebo
 
-from rbs_gym.envs.models.utils import xacro2sdf
+from env_manager.models.utils import xacro2sdf
 
 
 class Panda(model_wrapper.ModelWrapper, model_with_file.ModelWithFile):

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

@@ -8,11 +8,13 @@ import numpy as np
 from scenario import core as scenario
 from scenario import gazebo as scenario_gazebo
 
+from .robot import RobotWrapper
+
 # from robot_builder.elements.robot import Robot
 # from robot_builder.writer.urdf import URDF_writer
 # from robot_builder.parser.urdf import URDF_parser
 
-class RbsArm(model_wrapper.ModelWrapper, model_with_file.ModelWithFile):
+class RbsArm(RobotWrapper):
 
     DEFAULT_ARM_JOINT_POSITIONS: List[float] = (
         0.0, 0.5, 3.14159, 1.5, 0.0, 1.4, 0.0,
@@ -261,5 +263,5 @@ class RbsArm(model_wrapper.ModelWrapper, model_with_file.ModelWithFile):
 
     @classmethod
     def get_model_file(cls) -> str:
-        return "/home/bill-finger/rbs_ws/current.urdf"
+        return "/home/narmak/rbs_ws/current.urdf"
 

env_manager/env_manager/env_manager/models/robots/rbs_arm_b.py

@@ -0,0 +1,90 @@
+from gym_gz.utils.scenario import get_unique_model_name
+from rclpy.node import Node
+from robot_builder.parser.urdf import URDF_parser
+from robot_builder.elements.robot import Robot
+
+from scenario import core as scenario_core
+from scenario import gazebo as scenario_gazebo
+
+from .robot import RobotWrapper
+
+
+class RbsArm(RobotWrapper):
+    """
+    Experimental class with `robot_builder`
+    """
+    DEFAULT_ARM_JOINT_POSITIONS: list[float] = [
+        0.0,
+        0.5,
+        3.14159,
+        1.5,
+        0.0,
+        1.4,
+        0.0,
+    ]
+
+    OPEN_GRIPPER_JOINT_POSITIONS: list[float] = [
+        0.064,
+    ]
+
+    CLOSED_GRIPPER_JOINT_POSITIONS: list[float] = [
+        0.0,
+    ]
+
+    DEFAULT_GRIPPER_JOINT_POSITIONS: list[float] = CLOSED_GRIPPER_JOINT_POSITIONS
+
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        node: Node,
+        urdf_string: str,
+        name: str = "rbs_arm",
+        position: tuple[float, float, float] = (0.0, 0.0, 0.0),
+        orientation: tuple[float, float, float, float] = (1.0, 0, 0, 0),
+        initial_arm_joint_positions: list[float] = DEFAULT_ARM_JOINT_POSITIONS,
+        initial_gripper_joint_positions: list[float] = DEFAULT_GRIPPER_JOINT_POSITIONS,
+    ):
+        self.__initial_arm_joint_positions = initial_arm_joint_positions
+        self.__initial_gripper_joint_positions = initial_gripper_joint_positions
+
+        # Get a unique model name
+        model_name = get_unique_model_name(world, name)
+
+        # Setup initial pose
+        initial_pose = scenario_core.Pose(position, orientation)
+
+        # Insert the model
+        ok_model = world.insert_model_from_string(urdf_string, initial_pose, model_name)
+        if not ok_model:
+            raise RuntimeError("Failed to insert " + model_name)
+
+        # Get the model
+        model = world.get_model(model_name)
+
+        # Parse robot to get metadata
+        self._robot: Robot = URDF_parser.load_string(urdf_string)
+        node.get_logger().info("{}".format(self._robot.actuated_joint_names))
+        node.get_logger().info("{}".format(self._robot.get_gripper_mimic_joint_name))
+        node.get_logger().info("{}".format(self._robot.gripper_actuated_joint_names))
+
+        node.get_logger().info("{}".format(model.joint_names()))
+
+        # Set initial joint configuration
+        self.set_initial_joint_positions(model)
+        super().__init__(model=model)
+
+    @property
+    def robot(self) -> Robot:
+        return self._robot
+
+    def set_initial_joint_positions(self, model):
+        model = model.to_gazebo()
+        if not model.reset_joint_positions(
+            self.__initial_arm_joint_positions, self._robot.actuated_joint_names
+        ):
+            raise RuntimeError("Failed to set initial positions of arm's joints")
+        if not model.reset_joint_positions(
+            self.__initial_gripper_joint_positions, [self._robot.get_gripper_mimic_joint_name]
+        ):
+            raise RuntimeError("Failed to set initial positions of gripper's joints")
+

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

@@ -0,0 +1,32 @@
+from gym_gz.scenario import model_wrapper
+from robot_builder.elements.robot import Robot
+from scenario import gazebo as scenario_gazebo
+from abc import abstractmethod
+
+
+class RobotWrapper(model_wrapper.ModelWrapper):
+    def __init__(
+        self,
+        world: scenario_gazebo.World | None = None,
+        urdf_string: str | None = None,
+        name: str | None = None,
+        position: tuple[float, float, float] | None = None,
+        orientation: tuple[float, float, float, float] | None = None,
+        initial_arm_joint_positions: list[float] = [],
+        initial_gripper_joint_positions: list[float] = [],
+        model: model_wrapper.ModelWrapper | None = None,
+        **kwargs,
+    ):
+        if model is not None:
+            super().__init__(model=model)
+        else:
+            raise ValueError("Model should be defined for the parent class")
+
+    @property
+    @abstractmethod
+    def robot(self) -> Robot:
+        pass
+
+    @abstractmethod
+    def set_initial_joint_positions(self, model):
+        pass

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

@@ -6,7 +6,7 @@ from gym_gz.utils.scenario import get_unique_model_name
 from scenario import core as scenario_core
 from scenario import gazebo as scenario_gz
 
-from rbs_gym.envs.models.utils import ModelCollectionRandomizer
+from env_manager.models.utils import ModelCollectionRandomizer
 
 
 class Camera(model_wrapper.ModelWrapper):

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

@@ -3,17 +3,17 @@ from typing import List
 from gym_gz.scenario import model_wrapper
 from gym_gz.utils import misc
 from gym_gz.utils.scenario import get_unique_model_name
-from scenario import core as scenario
+from scenario import core as scenario_core
-
+from scenario import gazebo as scenario_gazebo
 
 class Ground(model_wrapper.ModelWrapper):
     def __init__(
         self,
-        world: scenario.World,
+        world: scenario_gazebo.World,
         name: str = "ground",
-        position: List[float] = (0, 0, 0),
+        position: tuple[float, float, float] = (0, 0, 0),
-        orientation: List[float] = (1, 0, 0, 0),
+        orientation: tuple[float, float, float, float] = (1, 0, 0, 0),
-        size: List[float] = (1.5, 1.5),
+        size: tuple[float, float] = (1.5, 1.5),
         collision_thickness=0.05,
         friction: float = 5.0,
         **kwargs,
@@ -23,7 +23,7 @@ class Ground(model_wrapper.ModelWrapper):
         model_name = get_unique_model_name(world, name)
 
         # Initial pose
-        initial_pose = scenario.Pose(position, orientation)
+        initial_pose = scenario_core.Pose(position, orientation)
 
         # Create SDF string for the model
         sdf = f"""<sdf version="1.9">

env_manager/env_manager/env_manager/scene/__init__.py

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

env_manager/env_manager/env_manager/scene/scene.py

@@ -0,0 +1,805 @@
+from dataclasses import asdict
+
+import numpy as np
+from rclpy.node import Node
+from scenario.bindings.gazebo import GazeboSimulator, World, PhysicsEngine_dart
+from scipy.spatial.transform import Rotation
+from scipy.spatial import distance
+
+from ..models import Box, Camera, Cylinder, Ground, Mesh, Plane, Sphere, Sun
+from ..models.configs import CameraData, ObjectData, SceneData
+from ..models.robots import get_robot_model_class
+from ..utils import Tf2Broadcaster
+from ..utils.conversions import quat_to_wxyz
+from ..utils.gazebo import (
+    transform_move_to_model_pose,
+    transform_move_to_model_position,
+)
+from ..utils.types import Point, Pose
+
+
+# TODO: Split scene randomizer and scene
+class Scene:
+    """
+    Manager scene during runtime, including randomization.
+    """
+
+    def __init__(
+        self,
+        node: Node,
+        gazebo: GazeboSimulator,
+        scene: SceneData,
+        robot_urdf_string: str,
+    ) -> None:
+        self.gazebo = gazebo
+        self.robot = scene.robot
+        self.cameras = scene.camera
+        self.objects = scene.objects
+        self.node = node
+        self.light = scene.light
+        self.terrain = scene.terrain
+        self.__scene_initialized = False
+        self.__plugin_scene_broadcaster = True
+        self.__plugin_user_commands = True
+        self.__fts_enable = True
+        self.__plugint_sensor_render_engine = "ogre2"
+        self.world: World = gazebo.get_world()
+
+        # if not node.has_parameter("robot_description"):
+        #     node.declare_parameter("robot_description")
+
+        print(node._parameters)
+        self.__objects_unique_names = {}
+        self.__object_positions: dict[str, Point] = {}
+        self.__objects_workspace_centre: Point = (0.0, 0.0, 0.0)
+        scene.robot.urdf_string = robot_urdf_string
+
+        self.tf2_broadcaster = Tf2Broadcaster(node=node)
+
+        self.__workspace_center = (0.0, 0.0, 0.0)
+        self.__workspace_dimensions = (1.5, 1.5, 1.5)
+
+        half_width = self.__workspace_dimensions[0] / 2
+        half_height = self.__workspace_dimensions[1] / 2
+        half_depth = self.__workspace_dimensions[2] / 2
+
+        self.__workspace_min_bound = (
+            self.__workspace_center[0] - half_width,
+            self.__workspace_center[1] - half_height,
+            self.__workspace_center[2] - half_depth,
+        )
+        self.__workspace_max_bound = (
+            self.__workspace_center[0] + half_width,
+            self.__workspace_center[1] + half_height,
+            self.__workspace_center[2] + half_depth,
+        )
+
+    def init_scene(self):
+        self.gazebo_paused_run()
+
+        self.init_world_plugins()
+
+        self.world.set_physics_engine(PhysicsEngine_dart)
+
+        self.add_terrain()
+        self.add_light()
+        self.add_robot()
+        for obj in self.objects:
+            self.add_object(obj)
+        for camera in self.cameras:
+            self.add_camera(camera)
+
+        self.__scene_initialized = True
+
+    def reset_scene(self):
+        self.reset_robot_joint_positions()
+
+        object_randomized = set()
+        self.__object_positions.clear()
+        for object in self.objects:
+            if object.name in object_randomized:
+                continue
+            if object.randomize.random_pose:
+                self.randomize_object_position(object)
+            else:
+                self.reset_objects_pose(object)
+            object_randomized.add(object.name)
+
+        for camera in self.cameras:
+            if camera.enable and self.is_camera_pose_expired(camera):
+                self.randomize_camera_pose(camera=camera)
+
+    def init_world_plugins(self):
+        # SceneBroadcaster
+        if self.__plugin_scene_broadcaster:
+            if not self.gazebo.scene_broadcaster_active(self.world.to_gazebo().name()):
+                self.node.get_logger().info(
+                    "Inserting world plugins for broadcasting scene to GUI clients..."
+                )
+                self.world.to_gazebo().insert_world_plugin(
+                    "ignition-gazebo-scene-broadcaster-system",
+                    "ignition::gazebo::systems::SceneBroadcaster",
+                )
+
+                self.gazebo_paused_run()
+
+        # UserCommands
+        if self.__plugin_user_commands:
+            self.node.get_logger().info(
+                "Inserting world plugins to enable user commands..."
+            )
+            self.world.to_gazebo().insert_world_plugin(
+                "ignition-gazebo-user-commands-system",
+                "ignition::gazebo::systems::UserCommands",
+            )
+
+            self.gazebo_paused_run()
+
+        # Sensors
+        self._camera_enable = False
+        for camera in self.cameras:
+            if camera.enable:
+                self._camera_enable = True
+
+        if self._camera_enable:
+            self.node.get_logger().info(
+                f"Inserting world plugins for sensors with {self.__plugint_sensor_render_engine} rendering engine..."
+            )
+            self.world.to_gazebo().insert_world_plugin(
+                "libignition-gazebo-sensors-system.so",
+                "ignition::gazebo::systems::Sensors",
+                "<sdf version='1.9'>"
+                f"<render_engine>{self.__plugint_sensor_render_engine}</render_engine>"
+                "</sdf>",
+            )
+
+        if self.__fts_enable:
+            self.world.to_gazebo().insert_world_plugin(
+                "ignition-gazebo-forcetorque-system",
+                "ignition::gazebo::systems::ForceTorque",
+            )
+
+            self.gazebo_paused_run()
+
+    def add_robot(self):
+        """
+        Configure and insert robot into the simulation
+        """
+
+        robot_model_class = get_robot_model_class(self.robot.name)
+
+        # Instantiate robot class based on the selected model
+        self.robot_wrapper = robot_model_class(
+            world=self.world,
+            name=self.robot.name,
+            node=self.node,
+            position=self.robot.spawn_position,
+            orientation=quat_to_wxyz(self.robot.spawn_quat_xyzw),
+            urdf_string=self.robot.urdf_string,
+            initial_arm_joint_positions=self.robot.joint_positioins,
+            initial_gripper_joint_positions=self.robot.gripper_jont_positions,
+        )
+
+        # The desired name is passed as arg on creation, however, a different name might be selected to be unique
+        # Therefore, return the name back to the task
+        self.robot_name = self.robot_wrapper.name()
+
+        # Enable contact detection for all gripper links (fingers)
+        robot_gazebo = self.robot_wrapper.to_gazebo()
+        # for gripper_link_name in self.robot.gripper_link_names:
+        #     finger = robot_gazebo.get_link(link_name=gripper_link_name)
+        #     finger.enable_contact_detection(True)
+        robot_gazebo.enable_contacts()
+
+        # for arm_link_name in self.robot_wrapper.arm_link_names:
+        #     link = robot_gazebo.get_link(link_name=arm_link_name)
+        #     link.enable_contact_detection(True)
+
+        # # If mobile, enable contact detection also for the wheels
+        # if self.robot.is_mobile:
+        #     for wheel_link_name in self.robot.wheel_link_names:
+        #         wheel = robot_gazebo.get_link(link_name=wheel_link_name)
+        #         wheel.enable_contact_detection(True)
+
+        self.gazebo_paused_run()
+
+        # Reset robot joints to the defaults
+        self.reset_robot_joint_positions()
+
+    def add_camera(self, camera: CameraData):
+        """
+        Configure and insert camera into the simulation. Camera is places with respect to the robot
+        """
+
+        if self.world.to_gazebo().name() == camera.relative_to:
+            camera_position = camera.spawn_position
+            camera_quat_wxyz = quat_to_wxyz(camera.spawn_quat_xyzw)
+        else:
+            # Transform the pose of camera to be with respect to robot - but still represented in world reference frame for insertion into the world
+            camera_position, camera_quat_wxyz = transform_move_to_model_pose(
+                world=self.world,
+                position=camera.spawn_position,
+                quat=quat_to_wxyz(camera.spawn_quat_xyzw),
+                target_model=self.robot_wrapper,
+                target_link=camera.relative_to,
+                xyzw=False,
+            )
+
+        # Create camera
+        self.camera_wrapper = Camera(
+            name=camera.name,
+            world=self.world,
+            position=camera_position,
+            orientation=camera_quat_wxyz,
+            camera_type=camera.type,
+            width=camera.width,
+            height=camera.height,
+            image_format=camera.image_format,
+            update_rate=camera.update_rate,
+            horizontal_fov=camera.horizontal_fov,
+            vertical_fov=camera.vertical_fov,
+            clip_color=camera.clip_color,
+            clip_depth=camera.clip_depth,
+            noise_mean=camera.noise_mean,
+            noise_stddev=camera.noise_stddev,
+            ros2_bridge_color=camera.publish_color,
+            ros2_bridge_depth=camera.publish_depth,
+            ros2_bridge_points=camera.publish_points,
+        )
+
+        self.gazebo_paused_run()
+
+        # Attach to robot if needed
+        if self.world.to_gazebo().name() != camera.relative_to:
+            if not self.robot_wrapper.to_gazebo().attach_link(
+                camera.relative_to,
+                self.camera_wrapper.name(),
+                self.camera_wrapper.link_name,
+            ):
+                raise Exception("Cannot attach camera link to robot")
+
+        self.__is_camera_attached = True
+
+        self.gazebo_paused_run()
+
+        # Broadcast tf
+        self.tf2_broadcaster.broadcast_tf(
+            parent_frame_id=camera.relative_to,
+            child_frame_id=self.camera_wrapper.frame_id,
+            translation=camera.spawn_position,
+            rotation=camera.spawn_quat_xyzw,
+            xyzw=True,
+        )
+
+    def add_object(self, obj: ObjectData):
+        obj_dict = asdict(obj)
+        obj_dict["world"] = self.world
+        try:
+            if obj.type:
+                match obj.type:
+                    case "box":
+                        object_wrapper = Box(**obj_dict)
+                    case "plane":
+                        object_wrapper = Plane(**obj_dict)
+                    case "sphere":
+                        object_wrapper = Sphere(**obj_dict)
+                    case "cylinder":
+                        object_wrapper = Cylinder(**obj_dict)
+                    case "mesh":
+                        object_wrapper = Mesh(**obj_dict)
+                    case _:
+                        raise NotImplementedError("Type is not supported")
+            else:
+                from ..models.configs import (
+                    BoxObjectData,
+                    CylinderObjectData,
+                    MeshObjectData,
+                    PlaneObjectData,
+                    SphereObjectData,
+                )
+
+                match obj:
+                    case BoxObjectData():
+                        object_wrapper = Box(**obj_dict)
+                    case PlaneObjectData():
+                        object_wrapper = Plane(**obj_dict)
+                    case SphereObjectData():
+                        object_wrapper = Sphere(**obj_dict)
+                    case CylinderObjectData():
+                        object_wrapper = Cylinder(**obj_dict)
+                    case MeshObjectData():
+                        object_wrapper = Mesh(**obj_dict)
+                    case _:
+                        raise NotImplementedError("Type is not supported")
+
+            model_name = object_wrapper.name()
+            if obj.name not in self.__objects_unique_names:
+                self.__objects_unique_names[obj.name] = []
+            self.__objects_unique_names[obj.name].append(model_name)
+            self.__object_positions[model_name] = obj.position
+
+            # Enable contact detection
+            for link_name in object_wrapper.link_names():
+                link = object_wrapper.to_gazebo().get_link(link_name=link_name)
+                link.enable_contact_detection(True)
+
+        except Exception as ex:
+            self.node.get_logger().warn(f"Model could not be inserted. Reason: {ex}")
+
+        self.gazebo_paused_run
+
+    def add_light(self):
+        """
+        Configure and insert default light into the simulation
+        """
+
+        # Create light
+        match self.light.type:
+            case "sun":
+                self.light = Sun(
+                    self.world,
+                    direction=self.light.direction,
+                    color=self.light.color,
+                    distance=self.light.distance,
+                    visual=self.light.visual,
+                    radius=self.light.radius,
+                )
+            case "random_sun":
+                raise RuntimeError("random_sun is not supported yet")
+            case _:
+                raise RuntimeError(
+                    f"Type of light [{self.light.type}] currently not supported"
+                )
+
+        self.gazebo_paused_run()
+
+    def add_terrain(self):
+        """
+        Configure and insert default terrain into the simulation
+        """
+
+        # TODO: add another terrains
+        self.terrain_wrapper = Ground(
+            self.world,
+            position=self.terrain.spawn_position,
+            orientation=quat_to_wxyz(self.terrain.spawn_quat_xyzw),
+            size=self.terrain.size,
+        )
+
+        # Enable contact detection
+        for link_name in self.terrain_wrapper.link_names():
+            link = self.terrain_wrapper.to_gazebo().get_link(link_name=link_name)
+            link.enable_contact_detection(True)
+
+        self.gazebo_paused_run()
+
+    def randomize_object_position(self, object: ObjectData):
+        task_object_names = self.__objects_unique_names[object.name]
+        for object_name in task_object_names:
+            position, quat_random = self.get_random_object_pose(object)
+            obj = self.world.to_gazebo().get_model(object_name).to_gazebo()
+            if object.randomize.random_pose:
+                obj.reset_base_pose(position, quat_random)
+            elif object.randomize.random_orientation:
+                obj.reset_base_pose(object.position, quat_random)
+            elif object.randomize.random_position:
+                obj.reset_base_pose(position, object.orientation)
+
+            self.__object_positions[object_name] = position
+
+        self.gazebo_paused_run()
+
+    def randomize_camera_pose(self, camera: CameraData):
+        # Get random camera pose, centred at object position (or centre of object spawn box)
+        if "orbit" == camera.random_pose_mode:
+            camera_position, camera_quat_xyzw = self.get_random_camera_pose_orbit(
+                camera
+            )
+        elif "select_random" == camera.random_pose_mode:
+            (
+                camera_position,
+                camera_quat_xyzw,
+            ) = self.get_random_camera_pose_sample_random(camera)
+        elif "select_nearest" == camera.random_pose_mode:
+            (
+                camera_position,
+                camera_quat_xyzw,
+            ) = self.get_random_camera_pose_sample_nearest(camera)
+        else:
+            raise TypeError("Invalid mode for camera pose randomization.")
+
+        if self.world.to_gazebo().name() == camera.relative_to:
+            transformed_camera_position = camera_position
+            transformed_camera_quat_wxyz = quat_to_wxyz(camera_quat_xyzw)
+        else:
+            # Transform the pose of camera to be with respect to robot - but represented in world reference frame for insertion into the world
+            (
+                transformed_camera_position,
+                transformed_camera_quat_wxyz,
+            ) = transform_move_to_model_pose(
+                world=self.world,
+                position=camera_position,
+                quat=quat_to_wxyz(camera_quat_xyzw),
+                target_model=self.robot_wrapper,
+                target_link=camera.relative_to,
+                xyzw=False,
+            )
+
+        # Detach camera if needed
+        if self.__is_camera_attached:
+            if not self.robot_wrapper.to_gazebo().detach_link(
+                camera.relative_to,
+                self.camera_wrapper.name(),
+                self.camera_wrapper.link_name,
+            ):
+                raise Exception("Cannot detach camera link from robot")
+
+        self.gazebo_paused_run()
+
+        # Move pose of the camera
+        camera_gazebo = self.camera_wrapper.to_gazebo()
+        camera_gazebo.reset_base_pose(
+            transformed_camera_position, transformed_camera_quat_wxyz
+        )
+
+        self.gazebo_paused_run()
+
+        # Attach to robot again if needed
+        if self.__is_camera_attached:
+            if not self.robot_wrapper.to_gazebo().attach_link(
+                camera.relative_to,
+                self.camera_wrapper.name(),
+                self.camera_wrapper.link_name,
+            ):
+                raise Exception("Cannot attach camera link to robot")
+
+        self.gazebo_paused_run()
+
+        # Broadcast tf
+        self.tf2_broadcaster.broadcast_tf(
+            parent_frame_id=camera.relative_to,
+            child_frame_id=self.camera_wrapper.frame_id,
+            translation=camera_position,
+            rotation=camera_quat_xyzw,
+            xyzw=True,
+        )
+
+    def get_random_camera_pose_orbit(self, camera: CameraData) -> Pose:
+        rng = np.random.default_rng()
+
+        while True:
+            position = rng.uniform(
+                low=(-1.0, -1.0, camera.random_pose_orbit_height_range[0]),
+                high=(1.0, 1.0, camera.random_pose_orbit_height_range[1]),
+            )
+
+            norm = np.linalg.norm(position)
+            position /= norm
+
+            if (
+                abs(np.arctan2(position[0], position[1]) + np.pi / 2)
+                > camera.random_pose_orbit_ignore_arc_behind_robot
+            ):
+                break
+
+        rpy = np.array(
+            [
+                0.0,
+                np.arctan2(
+                    position[2] - camera.random_pose_focal_point_z_offset,
+                    np.linalg.norm(position[:2]),
+                ),
+                np.arctan2(position[1], position[0]) + np.pi,
+            ]
+        )
+        quat_xyzw = Rotation.from_euler("xyz", rpy).as_quat()
+
+        position *= camera.random_pose_orbit_distance
+        position[:2] += self.__objects_workspace_centre[:2]
+
+        return tuple(position), tuple(quat_xyzw)
+
+    def get_random_camera_pose_sample_random(self, camera: CameraData) -> Pose:
+        # Select a random entry from the options
+        camera.random_pose_select_position_options
+
+        selection = camera.random_pose_select_position_options[
+            np.random.randint(len(camera.random_pose_select_position_options))
+        ]
+
+        return self.get_random_camera_pose_sample_process(camera, selection)
+
+    def get_random_camera_pose_sample_nearest(self, camera: CameraData) -> Pose:
+        # Select the nearest entry
+        dist_sqr = np.sum(
+            (
+                np.array(camera.random_pose_select_position_options)
+                - np.array(camera.random_pose_select_position_options)
+            )
+            ** 2,
+            axis=1,
+        )
+        nearest = camera.random_pose_select_position_options[np.argmin(dist_sqr)]
+
+        return self.get_random_camera_pose_sample_process(camera, nearest)
+
+    def get_random_camera_pose_sample_process(
+        self, camera: CameraData, position: Point
+    ) -> Pose:
+        # Determine orientation such that camera faces the centre
+        rpy = [
+            0.0,
+            np.arctan2(
+                position[2] - camera.random_pose_focal_point_z_offset,
+                np.linalg.norm(
+                    (
+                        position[0] - self.__objects_workspace_centre[0],
+                        position[1] - self.__objects_workspace_centre[1],
+                    ),
+                    2,
+                ),
+            ),
+            np.arctan2(
+                position[1] - self.__objects_workspace_centre[1],
+                position[0] - self.__objects_workspace_centre[0],
+            )
+            + np.pi,
+        ]
+        quat_xyzw = Rotation.from_euler("xyz", rpy).as_quat()
+
+        return camera.spawn_position, tuple(quat_xyzw)
+
+    def reset_robot_pose(self):
+        if self.robot.randomizer.pose:
+            position = [
+                self.robot.spawn_position[0]
+                + np.random.RandomState().uniform(
+                    -self.robot.randomizer.spawn_volume[0] / 2,
+                    self.robot.randomizer.spawn_volume[0] / 2,
+                ),
+                self.robot.spawn_position[1]
+                + np.random.RandomState().uniform(
+                    -self.robot.randomizer.spawn_volume[1] / 2,
+                    self.robot.randomizer.spawn_volume[1] / 2,
+                ),
+                self.robot.spawn_position[2]
+                + np.random.RandomState().uniform(
+                    -self.robot.randomizer.spawn_volume[2] / 2,
+                    self.robot.randomizer.spawn_volume[2] / 2,
+                ),
+            ]
+            quat_xyzw = Rotation.from_euler(
+                "xyz", (0, 0, np.random.RandomState().uniform(-np.pi, np.pi))
+            ).as_quat()
+            quat_xyzw = tuple(quat_xyzw)
+        else:
+            position = self.robot.spawn_position
+            quat_xyzw = self.robot.spawn_quat_xyzw
+
+        gazebo_robot = self.robot_wrapper.to_gazebo()
+        gazebo_robot.reset_base_pose(position, quat_to_wxyz(quat_xyzw))
+
+        self.gazebo_paused_run()
+
+    def reset_robot_joint_positions(self):
+        gazebo_robot = self.robot_wrapper.to_gazebo()
+
+        arm_joint_positions = self.robot.joint_positioins
+
+        # Add normal noise if desired
+        if self.robot.randomizer.joint_positions:
+            for joint_position in arm_joint_positions:
+                joint_position += np.random.RandomState().normal(
+                    loc=0.0, scale=self.robot.randomizer.joint_positions_std
+                )
+
+        # Arm joints - apply joint positions zero out velocities
+        if not gazebo_robot.reset_joint_positions(
+            arm_joint_positions, self.robot_wrapper.robot.actuated_joint_names
+        ):
+            raise RuntimeError("Failed to reset robot joint positions")
+        if not gazebo_robot.reset_joint_velocities(
+            [0.0] * len(self.robot_wrapper.robot.actuated_joint_names),
+            self.robot_wrapper.robot.actuated_joint_names,
+        ):
+            raise RuntimeError("Failed to reset robot joint velocities")
+
+        # Gripper joints - apply joint positions zero out velocities
+        if (
+            self.robot.with_gripper
+            and self.robot_wrapper.robot.gripper_actuated_joint_names
+        ):
+            if not gazebo_robot.reset_joint_positions(
+                self.robot.gripper_jont_positions,
+                self.robot_wrapper.robot.gripper_actuated_joint_names,
+            ):
+                raise RuntimeError("Failed to reset gripper joint positions")
+            if not gazebo_robot.reset_joint_velocities(
+                [0.0] * len(self.robot_wrapper.robot.gripper_actuated_joint_names),
+                self.robot_wrapper.robot.gripper_actuated_joint_names,
+            ):
+                raise RuntimeError("Failed to reset gripper joint velocities")
+
+        # Execute an unpaused run to process model modification and get new JointStates
+        if not self.gazebo.step():
+            raise RuntimeError("Failed to execute an unpaused Gazebo step")
+
+        # if self.robot.with_gripper:
+        #     if (
+        #         self.robot_wrapper.CLOSED_GRIPPER_JOINT_POSITIONS
+        #         == self.robot.gripper_jont_positions
+        #     ):
+        #         self.gripper_controller.close()
+        #     else:
+        #         self.gripper.open()
+
+    def reset_objects_pose(self, object: ObjectData):
+        task_object_names = self.__objects_unique_names[object.name]
+        for object_name in task_object_names:
+            obj = self.world.to_gazebo().get_model(object_name).to_gazebo()
+            obj.reset_base_pose(object.position, object.orientation)
+
+        self.gazebo_paused_run()
+
+    def get_random_object_pose(
+        self,
+        obj: ObjectData,
+        name: str = "",
+        min_distance_to_other_objects: float = 0.2,
+        min_distance_decay_factor: float = 0.95,
+    ) -> Pose:
+        is_too_close = True
+        # The default value for randomization
+        object_position = obj.position
+        while is_too_close:
+            object_position = (
+                obj.position[0]
+                + np.random.uniform(
+                    -obj.randomize.random_spawn_volume[0] / 2,
+                    obj.randomize.random_spawn_volume[0] / 2,
+                ),
+                obj.position[1]
+                + np.random.uniform(
+                    -obj.randomize.random_spawn_volume[1] / 2,
+                    obj.randomize.random_spawn_volume[1] / 2,
+                ),
+                obj.position[2]
+                + np.random.uniform(
+                    -obj.randomize.random_spawn_volume[2] / 2,
+                    obj.randomize.random_spawn_volume[2] / 2,
+                ),
+            )
+
+            if self.world.to_gazebo().name() != obj.relative_to:
+                # Transform the pose of camera to be with respect to robot - but represented in world reference frame for insertion into the world
+                object_position = transform_move_to_model_position(
+                    world=self.world,
+                    position=object_position,
+                    target_model=self.robot_wrapper,
+                    target_link=obj.relative_to,
+                )
+
+            # Check if position is far enough from other
+            is_too_close = False
+            for (
+                existing_object_name,
+                existing_object_position,
+            ) in self.__object_positions.items():
+                if existing_object_name == name:
+                    # Do not compare to itself
+                    continue
+                if (
+                    distance.euclidean(object_position, existing_object_position)
+                    < min_distance_to_other_objects
+                ):
+                    min_distance_to_other_objects *= min_distance_decay_factor
+                    is_too_close = True
+                    break
+
+        quat = np.random.uniform(-1, 1, 4)
+        quat /= np.linalg.norm(quat)
+
+        return object_position, tuple(quat)
+
+    def is_camera_pose_expired(self, camera: CameraData) -> bool:
+        """
+        Checks if camera pose needs to be randomized.
+
+        Return:
+            True if expired, false otherwise
+        """
+
+        if camera.random_pose_rollouts_num == 0:
+            return False
+
+        camera.random_pose_rollout_counter += 1
+
+        if camera.random_pose_rollout_counter >= camera.random_pose_rollouts_num:
+            camera.random_pose_rollout_counter = 0
+            return True
+
+        return False
+
+    def gazebo_paused_run(self):
+        if not self.gazebo.run(paused=True):
+            raise RuntimeError("Failed to execute a paused Gazebo run")
+
+    def check_object_outside_workspace(
+        self,
+        object_position: Point,
+    ) -> bool:
+        """
+        Returns true if the object is outside the workspace (parallelepiped defined by center and dimensions).
+        """
+
+        return (
+            object_position[0] < self.__workspace_min_bound[0]
+            or object_position[1] < self.__workspace_min_bound[1]
+            or object_position[2] < self.__workspace_min_bound[2]
+            or object_position[0] > self.__workspace_max_bound[0]
+            or object_position[1] > self.__workspace_max_bound[1]
+            or object_position[2] > self.__workspace_max_bound[2]
+        )
+
+    def check_object_overlapping(
+        self,
+        object: ObjectData,
+        allowed_penetration_depth: float = 0.001,
+        terrain_allowed_penetration_depth: float = 0.002,
+    ) -> bool:
+        """
+        Go through all objects and make sure that none of them are overlapping.
+        If an object is overlapping, reset its position.
+        Positions are reset also if object is in collision with robot right after reset.
+        Collisions/overlaps with terrain are ignored.
+        Returns True if all objects are okay, false if they had to be reset
+        """
+
+        # Update object positions
+        for object_name in self.__objects_unique_names[object.name]:
+            model = self.world.get_model(object_name).to_gazebo()
+            self.__object_positions[object_name] = model.get_link(
+                link_name=model.link_names()[0]
+            ).position()
+
+        for object_name in self.__objects_unique_names[object.name]:
+            obj = self.world.get_model(object_name).to_gazebo()
+
+            # Make sure the object is inside workspace
+            if self.check_object_outside_workspace(
+                self.__object_positions[object_name]
+            ):
+                position, quat_random = self.get_random_object_pose(
+                    object,
+                    name=object_name,
+                )
+                obj.reset_base_pose(position, quat_random)
+                obj.reset_base_world_velocity([0.0, 0.0, 0.0], [0.0, 0.0, 0.0])
+                return False
+
+            # Make sure the object is not intersecting other objects
+            try:
+                for contact in obj.contacts():
+                    depth = np.mean([point.depth for point in contact.points])
+                    if (
+                        self.terrain_wrapper.name() in contact.body_b
+                        and depth < terrain_allowed_penetration_depth
+                    ):
+                        continue
+                    if (
+                        self.robot_wrapper.name() in contact.body_b
+                        or depth > allowed_penetration_depth
+                    ):
+                        position, quat_random = self.get_random_object_pose(
+                            object,
+                            name=object_name,
+                        )
+                        obj.reset_base_pose(position, quat_random)
+                        obj.reset_base_world_velocity([0.0, 0.0, 0.0], [0.0, 0.0, 0.0])
+                        return False
+            except Exception as e:
+                self.node.get_logger().error(
+                    f"Runtime error encountered while checking objects intersections: {e}"
+                )
+
+        return True

env_manager/env_manager/env_manager/scene/scene_randomizer.py

@@ -0,0 +1,4 @@
+
+class SceneRandomizer:
+    def __init__(self) -> None:
+        pass

env_manager/env_manager/env_manager/utils/__init__.py

@@ -1,3 +1,3 @@
-from . import conversions, gazebo, logging, math
+from . import conversions, gazebo, logging, math, types
 from .tf2_broadcaster import Tf2Broadcaster, Tf2BroadcasterStandalone
 from .tf2_listener import Tf2Listener, Tf2ListenerStandalone

env_manager/env_manager/env_manager/utils/gazebo.py

@@ -2,19 +2,21 @@ from typing import Tuple, Union
 
 from gym_gz.scenario.model_wrapper import ModelWrapper
 from numpy import exp
-from scenario.bindings.gazebo import Link, World
+from scenario.bindings.core import Link, World
 from scipy.spatial.transform import Rotation
 
-from rbs_gym.envs.utils.conversions import quat_to_wxyz, quat_to_xyzw
+from .conversions import quat_to_wxyz, quat_to_xyzw
-from rbs_gym.envs.utils.math import quat_mul
+from .math import quat_mul
+from .types import Pose, Point, Quat
 
 
+#NOTE: Errors in pyright will be fixed only with pybind11 in the scenario module
 def get_model_pose(
     world: World,
-    model: Union[ModelWrapper, str],
+    model: ModelWrapper | str,
-    link: Union[Link, str, None] = None,
+    link: Link | str | None = None,
     xyzw: bool = False,
-) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]:
+) -> Pose:
     """
     Return pose of model's link. Orientation is represented as wxyz quaternion or xyzw based on the passed argument `xyzw`.
     """
@@ -47,9 +49,9 @@ def get_model_pose(
 
 def get_model_position(
     world: World,
-    model: Union[ModelWrapper, str],
+    model: ModelWrapper | str,
-    link: Union[Link, str, None] = None,
+    link: Link | str | None = None,
-) -> Tuple[float, float, float]:
+) -> Point:
     """
     Return position of model's link.
     """
@@ -71,10 +73,10 @@ def get_model_position(
 
 def get_model_orientation(
     world: World,
-    model: Union[ModelWrapper, str],
+    model: ModelWrapper | str,
-    link: Union[Link, str, None] = None,
+    link: Link | str | None = None,
     xyzw: bool = False,
-) -> Tuple[float, float, float, float]:
+) -> Quat:
     """
     Return orientation of model's link that is represented as wxyz quaternion or xyzw based on the passed argument `xyzw`.
     """
@@ -103,12 +105,12 @@ def get_model_orientation(
 
 def transform_move_to_model_pose(
     world: World,
-    position: tuple[float, float, float],
+    position: Point,
-    quat: tuple[float, float, float, float],
+    quat: Quat,
     target_model: ModelWrapper | str,
     target_link: Link | str | None = None,
     xyzw: bool = False,
-) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]:
+) -> Pose:
     """
     Transform such that original `position` and `quat` are represented with respect to `target_model::target_link`.
     The resulting pose is still represented in world coordinate system.
@@ -137,10 +139,10 @@ def transform_move_to_model_pose(
 
 def transform_move_to_model_position(
     world: World,
-    position: Tuple[float, float, float],
+    position: Point,
-    target_model: Union[ModelWrapper, str],
+    target_model: ModelWrapper | str,
-    target_link: Union[Link, str, None] = None,
+    target_link: Link | str | None = None,
-) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]:
+) -> Point:
 
     target_frame_position, target_frame_quat_xyzw = get_model_pose(
         world,
@@ -161,11 +163,11 @@ def transform_move_to_model_position(
 
 def transform_move_to_model_orientation(
     world: World,
-    quat: Tuple[float, float, float, float],
+    quat: Quat,
-    target_model: Union[ModelWrapper, str],
+    target_model: ModelWrapper | str,
-    target_link: Union[Link, str, None] = None,
+    target_link: Link | str | None = None,
     xyzw: bool = False,
-) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]:
+) -> Quat:
 
     target_frame_quat = get_model_orientation(
         world,
@@ -181,12 +183,12 @@ def transform_move_to_model_orientation(
 
 def transform_change_reference_frame_pose(
     world: World,
-    position: Tuple[float, float, float],
+    position: Point,
-    quat: Tuple[float, float, float, float],
+    quat: Quat,
-    target_model: Union[ModelWrapper, str],
+    target_model: ModelWrapper | str,
-    target_link: Union[Link, str, None] = None,
+    target_link: Link | str | None,
     xyzw: bool = False,
-) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]:
+) -> Pose:
     """
     Change reference frame of original `position` and `quat` from world coordinate system to `target_model::target_link` coordinate system.
     """
@@ -216,10 +218,10 @@ def transform_change_reference_frame_pose(
 
 def transform_change_reference_frame_position(
     world: World,
-    position: Tuple[float, float, float],
+    position: Point,
-    target_model: Union[ModelWrapper, str],
+    target_model: ModelWrapper | str,
-    target_link: Union[Link, str, None] = None,
+    target_link: Link | str | None = None,
-) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]:
+) -> Point:
 
     target_frame_position, target_frame_quat_xyzw = get_model_pose(
         world,
@@ -242,11 +244,11 @@ def transform_change_reference_frame_position(
 
 def transform_change_reference_frame_orientation(
     world: World,
-    quat: Tuple[float, float, float, float],
+    quat: Quat,
-    target_model: Union[ModelWrapper, str],
+    target_model: ModelWrapper | str,
-    target_link: Union[Link, str, None] = None,
+    target_link: Link | str | None = None,
     xyzw: bool = False,
-) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]:
+) -> Quat:
 
     target_frame_quat = get_model_orientation(
         world,

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/math.py

@@ -1,13 +1,11 @@
-from typing import List, Tuple
-
 import numpy as np
 
 
 def quat_mul(
-    quat_0: Tuple[float, float, float, float],
+    quat_0: tuple[float, float, float, float],
-    quat_1: Tuple[float, float, float, float],
+    quat_1: tuple[float, float, float, float],
     xyzw: bool = True,
-) -> Tuple[float, float, float, float]:
+) -> tuple[float, float, float, float]:
     """
     Multiply two quaternions
     """
@@ -32,15 +30,15 @@ def quat_mul(
 
 
 def distance_to_nearest_point(
-    origin: Tuple[float, float, float], points: List[Tuple[float, float, float]]
+    origin: tuple[float, float, float], points: list[tuple[float, float, float]]
 ) -> float:
 
     return np.linalg.norm(np.array(points) - np.array(origin), axis=1).min()
 
 
 def get_nearest_point(
-    origin: Tuple[float, float, float], points: List[Tuple[float, float, float]]
+    origin: tuple[float, float, float], points: list[tuple[float, float, float]]
-) -> 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

@@ -56,7 +56,7 @@ class Tf2Broadcaster:
 class Tf2BroadcasterStandalone(Node, Tf2Broadcaster):
     def __init__(
         self,
-        node_name: str = "rbs_gym_tf_broadcaster",
+        node_name: str = "env_manager_tf_broadcaster",
         use_sim_time: bool = True,
     ):
 

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/include/env_manager/env_manager.hpp

@@ -1,100 +0,0 @@
-#ifndef __ENV_MANAGER_HPP__
-#define __ENV_MANAGER_HPP__
-
-#include "env_manager_interfaces/srv/configure_env.hpp"
-#include "env_manager_interfaces/srv/load_env.hpp"
-#include "env_manager_interfaces/srv/unload_env.hpp"
-#include "env_manager_interfaces/srv/start_env.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_lifecycle/lifecycle_node.hpp"
-
-#include "env_interface/env_interface.hpp"
-
-#include "pluginlib/class_loader.hpp"
-
-namespace env_manager {
-using EnvInterface = env_interface::EnvInterface;
-using EnvInterfaceSharedPtr = std::shared_ptr<env_interface::EnvInterface>;
-using EnvStateReturnType =
-    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
-
-struct EnvSpec {
-  std::string name;
-  std::string type;
-  EnvInterfaceSharedPtr env_ptr;
-};
-
-class EnvManager : public rclcpp::Node {
-public:
-  // EnvManager(rclcpp::Executor::SharedPtr executor,
-  //            const std::string &asm_config,
-  //            const std::string &node_name = "env_manager",
-  //            const std::string &node_namespace = "",
-  //            rclcpp::NodeOptions &node_options =
-  //                rclcpp::NodeOptions()
-  //                    .allow_undeclared_parameters(true)
-  //                    .automatically_declare_parameters_from_overrides(true));
-  EnvManager(rclcpp::Executor::SharedPtr executor,
-             const std::string &node_name = "env_manager",
-             const std::string &node_namespace = "",
-             rclcpp::NodeOptions &node_options =
-                 rclcpp::NodeOptions()
-                     .allow_undeclared_parameters(true)
-                     .automatically_declare_parameters_from_overrides(true));
-  virtual ~EnvManager() = default;
-
-  // EnvInterfaceSharedPtr loadEnv(const std::string& env_name);
-  EnvInterfaceSharedPtr loadEnv(const std::string &env_name,
-                                const std::string &env_class_type);
-  EnvStateReturnType unloadEnv(const std::string &env_name);
-  EnvStateReturnType configureEnv(const std::string &env_name);
-  EnvInterfaceSharedPtr addEnv(const EnvSpec &enviment);
-
-  // TODO: Define the input data format
-  //  Set Target for RL enviroment
-  EnvInterfaceSharedPtr setTarget();
-  EnvInterfaceSharedPtr switchTarget();
-  EnvInterfaceSharedPtr unsetTarget();
-  // Load Constraints for RL enviroment
-  EnvInterfaceSharedPtr loadConstraints();
-  EnvInterfaceSharedPtr switchConstraints();
-  EnvInterfaceSharedPtr unloadConstraints();
-
-protected:
-  void initServices();
-  rclcpp::Node::SharedPtr getNode();
-  void startEnv_cb(
-      const env_manager_interfaces::srv::StartEnv::Request::SharedPtr request,
-      env_manager_interfaces::srv::StartEnv::Response::SharedPtr response);
-  void loadEnv_cb(
-      const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
-      env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response);
-
-  void configureEnv_cb(
-      const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr
-          request,
-      env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response);
-
-  void unloadEnv_cb(
-      const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
-      env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response);
-
-private:
-  rclcpp::Node::SharedPtr m_node;
-  std::mutex m_env_mutex;
-  std::vector<EnvSpec> m_active_envs;
-  rclcpp::Service<env_manager_interfaces::srv::LoadEnv>::SharedPtr
-      m_load_env_srv;
-  rclcpp::Service<env_manager_interfaces::srv::ConfigureEnv>::SharedPtr
-      m_configure_env_srv;
-  rclcpp::Service<env_manager_interfaces::srv::UnloadEnv>::SharedPtr
-      m_unload_env_srv;
-  rclcpp::Service<env_manager_interfaces::srv::StartEnv>::SharedPtr m_start_env_srv;
-
-  rclcpp::CallbackGroup::SharedPtr m_callback_group;
-  rclcpp::Executor::SharedPtr m_executor;
-
-  std::shared_ptr<pluginlib::ClassLoader<env_interface::EnvInterface>> m_loader;
-};
-} // namespace env_manager
-#endif // __ENV_MANAGER_HPP__

env_manager/env_manager/package.xml

@@ -4,15 +4,10 @@
   <name>env_manager</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
-  <maintainer email="ur.narmak@gmail.com">banana</maintainer>
+  <maintainer email="ur.narmak@gmail.com">narmak</maintainer>
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  
-  <depend>env_interface</depend>
-  <depend>env_manager_interfaces</depend>
-  <depend>rclcpp</depend>
-  <depend>rclcpp_lifecycle</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

env_manager/env_manager/src/env_manager.cpp

@@ -1,190 +0,0 @@
-#include "env_manager/env_manager.hpp"
-#include <string>
-
-static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE =
-    "env_interface";
-static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAME =
-    "env_interface::EnvInterface";
-
-namespace env_manager {
-EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor,
-                       const std::string &node_name,
-                       const std::string &node_namespace,
-                       rclcpp::NodeOptions &options)
-    : rclcpp::Node(node_name, node_namespace, options), m_executor(executor),
-      m_loader(
-          std::make_shared<pluginlib::ClassLoader<env_interface::EnvInterface>>(
-              ENV_INTERFACE_BASE_CLASS_NAMESPACE,
-              ENV_INTERFACE_BASE_CLASS_NAME)) {
-  initServices();
-}
-
-// EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor,
-//                        const std::string &asm_config,
-//                        const std::string &node_name,
-//                        const std::string &node_namespace,
-//                        rclcpp::NodeOptions &options)
-//     : rclcpp::Node(node_name, node_namespace, options), m_executor(executor),
-//       m_loader(
-//           std::make_shared<pluginlib::ClassLoader<env_interface::EnvInterface>>(
-//               ENV_INTERFACE_BASE_CLASS_NAMESPACE,
-//               ENV_INTERFACE_BASE_CLASS_NAME)) {
-//   initServices();
-// }
-
-void EnvManager::initServices() {
-  using namespace std::placeholders;
-
-  m_load_env_srv = create_service<env_manager_interfaces::srv::LoadEnv>(
-      "~/load_env", std::bind(&EnvManager::loadEnv_cb, this, _1, _2));
-  m_configure_env_srv =
-      create_service<env_manager_interfaces::srv::ConfigureEnv>(
-          "~/configure_env",
-          std::bind(&EnvManager::configureEnv_cb, this, _1, _2));
-  m_unload_env_srv = create_service<env_manager_interfaces::srv::UnloadEnv>(
-      "~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2));
-  m_start_env_srv = create_service<env_manager_interfaces::srv::StartEnv>(
-      "~/start_env", std::bind(&EnvManager::startEnv_cb, this, _1, _2));
-}
-
-rclcpp::Node::SharedPtr EnvManager::getNode() { return m_node; }
-
-void EnvManager::startEnv_cb(
-    const env_manager_interfaces::srv::StartEnv::Request::SharedPtr request,
-    env_manager_interfaces::srv::StartEnv::Response::SharedPtr response) {
-
-  auto it = std::find_if(
-      m_active_envs.begin(), m_active_envs.end(),
-      [&request](const EnvSpec &env) { return env.name == request->name; });
-
-  if (it != m_active_envs.end()) {
-    RCLCPP_INFO(get_logger(), "Unloading existing environment '%s'",
-                request->name.c_str());
-    unloadEnv(request->name);
-  }
-
-  auto loadedEnv = loadEnv(request->name, request->type);
-  auto configuredEnv = configureEnv(request->name);
-  response->ok = (loadedEnv.get() != nullptr &&
-                  configuredEnv == EnvStateReturnType::SUCCESS)
-                     ? true
-                     : false;
-}
-void EnvManager::loadEnv_cb(
-    const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
-    env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) {
-  response->ok = loadEnv(request->name, request->type).get() != nullptr;
-}
-
-void EnvManager::configureEnv_cb(
-    const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request,
-    env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) {
-  response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS;
-}
-
-void EnvManager::unloadEnv_cb(
-    const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
-    env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) {
-  response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS;
-}
-
-EnvInterfaceSharedPtr EnvManager::addEnv(const EnvSpec &enviroment) {
-  // generate list of active enviroments
-  // std::string unique_env_name = enviroment.name;// + "_" +
-  // std::to_string(m_active_envs.size()); m_active_envs[unique_env_name] =
-  // enviroment.env_ptr;
-  if (enviroment.env_ptr->init(enviroment.name, get_namespace()) ==
-      rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
-          CallbackReturn::ERROR) {
-    RCLCPP_ERROR(get_logger(),
-                 "Enviroment with name %s cannot been initialized",
-                 enviroment.name.c_str());
-    return nullptr;
-  }
-
-  m_active_envs.push_back(enviroment);
-  m_executor->add_node(
-      enviroment.env_ptr->getNode()->get_node_base_interface());
-  return enviroment.env_ptr;
-}
-
-EnvInterfaceSharedPtr EnvManager::loadEnv(const std::string &env_name,
-                                          const std::string &env_class_type) {
-  EnvInterfaceSharedPtr loaded_env = nullptr;
-  RCLCPP_INFO(this->get_logger(), "Loading enviroment '%s'", env_name.c_str());
-  try {
-    if (m_loader->isClassAvailable(env_class_type)) {
-      loaded_env = m_loader->createSharedInstance(env_class_type);
-    } else {
-      RCLCPP_ERROR(
-          this->get_logger(),
-          "Loading enviroment is not available '%s' with class type '%s'",
-          env_name.c_str(), env_class_type.c_str());
-      RCLCPP_INFO(this->get_logger(), "Available classes");
-      for (const auto &available_class : m_loader->getDeclaredClasses()) {
-        RCLCPP_INFO(this->get_logger(), "\t%s", available_class.c_str());
-      }
-      return nullptr;
-    }
-  } catch (const pluginlib::PluginlibException &e) {
-    RCLCPP_ERROR(
-        get_logger(),
-        "Error happened during creation of enviroment '%s' with type '%s':\n%s",
-        env_name.c_str(), env_class_type.c_str(), e.what());
-    return nullptr;
-  }
-  EnvSpec enviroment;
-  enviroment.name = env_name;
-  enviroment.type = env_class_type;
-  enviroment.env_ptr = loaded_env;
-  return addEnv(enviroment);
-}
-
-EnvStateReturnType EnvManager::unloadEnv(const std::string &env_name) {
-  auto it = std::find_if(
-      m_active_envs.begin(), m_active_envs.end(),
-      [&env_name](const EnvSpec &env) { return env.name == env_name; });
-
-  if (it != m_active_envs.end()) {
-    it->env_ptr->getNode()->deactivate();
-    it->env_ptr->getNode()->cleanup();
-    m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface());
-    m_active_envs.erase(it);
-    return EnvStateReturnType::SUCCESS;
-  } else {
-    RCLCPP_ERROR(this->get_logger(),
-                 "Environment '%s' not found in the active environments list.",
-                 env_name.c_str());
-    return EnvStateReturnType::ERROR;
-  }
-  return EnvStateReturnType::FAILURE;
-}
-
-// EnvInterfaceSharedPtr
-// EnvManager::loadEnv(const std::string env_name)
-// {
-//     std::string env_class_type = "some_default_namespace::" + env_name;
-
-//     return loadEnv(env_name, env_class_type);
-// }
-
-EnvStateReturnType EnvManager::configureEnv(const std::string &env_name) {
-  RCLCPP_INFO(this->get_logger(), "Configuring enviroment '%s'", env_name.c_str());
-
-  auto it = std::find_if(
-      m_active_envs.begin(), m_active_envs.end(),
-      [&env_name](const EnvSpec &env) { return env.name == env_name; });
-
-  if (it == m_active_envs.end()) {
-    RCLCPP_ERROR(this->get_logger(),
-                 "Environment '%s' not found in the active environments list.",
-                 env_name.c_str());
-    return EnvStateReturnType::FAILURE;
-  }
-  it->env_ptr->configure();
-  it->env_ptr->getNode()->activate();
-
-  return EnvStateReturnType::SUCCESS;
-}
-
-} // namespace env_manager

env_manager/env_manager/src/run_env_manager.cpp

@@ -1,21 +0,0 @@
-#include "rclcpp/rclcpp.hpp"
-#include "env_manager/env_manager.hpp"
-
-int main(int argc, char** argv)
-{
-  rclcpp::init(argc, argv);
-  std::shared_ptr<rclcpp::Executor> executor = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
-  std::string manager_node_name = "env_manager";
-
-  auto em = std::make_shared<env_manager::EnvManager>(executor, manager_node_name);
-
-  RCLCPP_INFO(em->get_logger(), "Env manager is starting");
-
-  em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment");
-  em->configureEnv("gz_enviroment");
-
-  executor->add_node(em);
-  executor->spin();
-  rclcpp::shutdown();
-  return 0;
-}

env_manager/env_manager_interfaces/CMakeLists.txt

@@ -18,6 +18,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     srv/ConfigureEnv.srv
     srv/LoadEnv.srv
     srv/UnloadEnv.srv
+    srv/ResetEnv.srv
     msg/EnvState.msg
     DEPENDENCIES lifecycle_msgs
 )

env_manager/env_manager_interfaces/srv/ResetEnv.srv

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

env_manager/gz_enviroment/CMakeLists.txt

@@ -1,78 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(gz_enviroment)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-set(THIS_PACKAGE_INCLUDE_DEPENDS
-    rclcpp
-    rclcpp_lifecycle
-    tf2_ros
-    tf2_msgs
-    tf2_eigen
-    ros_gz
-    pluginlib
-    ignition-transport11
-    ignition-msgs8
-    ros_gz_bridge
-    env_interface
-    ignition-gazebo6
-)
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
-    find_package(${Dependency} REQUIRED)    
-endforeach()
-
-# TODO: generate_parameter_library
-
-
-
-add_library(${PROJECT_NAME} SHARED src/gz_enviroment.cpp)
-target_include_directories(${PROJECT_NAME} PUBLIC
-    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include/gz_enviroment>
-)
-ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
-target_compile_definitions(${PROJECT_NAME} PRIVATE "GZENVIROMENT_BUILDING_DLL")
-#TODO: replace rclcpp_lifecycle with custom interface class
-pluginlib_export_plugin_description_file(env_interface gz_enviroment.xml)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-install(
-    DIRECTORY include/
-    DESTINATION include/${PROJECT_NAME}
-)
-
-install(TARGETS ${PROJECT_NAME}
-    EXPORT export_${PROJECT_NAME}
-    RUNTIME DESTINATION bin
-    ARCHIVE DESTINATION lib
-    LIBRARY DESTINATION lib
-)
-
-install(PROGRAMS
-  scripts/publish_asm_config.py
-  scripts/test_tf.py
-  DESTINATION lib/${PROJECT_NAME}
-)
-
-ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-# ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
-ament_package()

env_manager/gz_enviroment/LICENSE

@@ -1,202 +0,0 @@
-
-                                 Apache License
-                           Version 2.0, January 2004
-                        http://www.apache.org/licenses/
-
-   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
-
-   1. Definitions.
-
-      "License" shall mean the terms and conditions for use, reproduction,
-      and distribution as defined by Sections 1 through 9 of this document.
-
-      "Licensor" shall mean the copyright owner or entity authorized by
-      the copyright owner that is granting the License.
-
-      "Legal Entity" shall mean the union of the acting entity and all
-      other entities that control, are controlled by, or are under common
-      control with that entity. For the purposes of this definition,
-      "control" means (i) the power, direct or indirect, to cause the
-      direction or management of such entity, whether by contract or
-      otherwise, or (ii) ownership of fifty percent (50%) or more of the
-      outstanding shares, or (iii) beneficial ownership of such entity.
-
-      "You" (or "Your") shall mean an individual or Legal Entity
-      exercising permissions granted by this License.
-
-      "Source" form shall mean the preferred form for making modifications,
-      including but not limited to software source code, documentation
-      source, and configuration files.
-
-      "Object" form shall mean any form resulting from mechanical
-      transformation or translation of a Source form, including but
-      not limited to compiled object code, generated documentation,
-      and conversions to other media types.
-
-      "Work" shall mean the work of authorship, whether in Source or
-      Object form, made available under the License, as indicated by a
-      copyright notice that is included in or attached to the work
-      (an example is provided in the Appendix below).
-
-      "Derivative Works" shall mean any work, whether in Source or Object
-      form, that is based on (or derived from) the Work and for which the
-      editorial revisions, annotations, elaborations, or other modifications
-      represent, as a whole, an original work of authorship. For the purposes
-      of this License, Derivative Works shall not include works that remain
-      separable from, or merely link (or bind by name) to the interfaces of,
-      the Work and Derivative Works thereof.
-
-      "Contribution" shall mean any work of authorship, including
-      the original version of the Work and any modifications or additions
-      to that Work or Derivative Works thereof, that is intentionally
-      submitted to Licensor for inclusion in the Work by the copyright owner
-      or by an individual or Legal Entity authorized to submit on behalf of
-      the copyright owner. For the purposes of this definition, "submitted"
-      means any form of electronic, verbal, or written communication sent
-      to the Licensor or its representatives, including but not limited to
-      communication on electronic mailing lists, source code control systems,
-      and issue tracking systems that are managed by, or on behalf of, the
-      Licensor for the purpose of discussing and improving the Work, but
-      excluding communication that is conspicuously marked or otherwise
-      designated in writing by the copyright owner as "Not a Contribution."
-
-      "Contributor" shall mean Licensor and any individual or Legal Entity
-      on behalf of whom a Contribution has been received by Licensor and
-      subsequently incorporated within the Work.
-
-   2. Grant of Copyright License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      copyright license to reproduce, prepare Derivative Works of,
-      publicly display, publicly perform, sublicense, and distribute the
-      Work and such Derivative Works in Source or Object form.
-
-   3. Grant of Patent License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      (except as stated in this section) patent license to make, have made,
-      use, offer to sell, sell, import, and otherwise transfer the Work,
-      where such license applies only to those patent claims licensable
-      by such Contributor that are necessarily infringed by their
-      Contribution(s) alone or by combination of their Contribution(s)
-      with the Work to which such Contribution(s) was submitted. If You
-      institute patent litigation against any entity (including a
-      cross-claim or counterclaim in a lawsuit) alleging that the Work
-      or a Contribution incorporated within the Work constitutes direct
-      or contributory patent infringement, then any patent licenses
-      granted to You under this License for that Work shall terminate
-      as of the date such litigation is filed.
-
-   4. Redistribution. You may reproduce and distribute copies of the
-      Work or Derivative Works thereof in any medium, with or without
-      modifications, and in Source or Object form, provided that You
-      meet the following conditions:
-
-      (a) You must give any other recipients of the Work or
-          Derivative Works a copy of this License; and
-
-      (b) You must cause any modified files to carry prominent notices
-          stating that You changed the files; and
-
-      (c) You must retain, in the Source form of any Derivative Works
-          that You distribute, all copyright, patent, trademark, and
-          attribution notices from the Source form of the Work,
-          excluding those notices that do not pertain to any part of
-          the Derivative Works; and
-
-      (d) If the Work includes a "NOTICE" text file as part of its
-          distribution, then any Derivative Works that You distribute must
-          include a readable copy of the attribution notices contained
-          within such NOTICE file, excluding those notices that do not
-          pertain to any part of the Derivative Works, in at least one
-          of the following places: within a NOTICE text file distributed
-          as part of the Derivative Works; within the Source form or
-          documentation, if provided along with the Derivative Works; or,
-          within a display generated by the Derivative Works, if and
-          wherever such third-party notices normally appear. The contents
-          of the NOTICE file are for informational purposes only and
-          do not modify the License. You may add Your own attribution
-          notices within Derivative Works that You distribute, alongside
-          or as an addendum to the NOTICE text from the Work, provided
-          that such additional attribution notices cannot be construed
-          as modifying the License.
-
-      You may add Your own copyright statement to Your modifications and
-      may provide additional or different license terms and conditions
-      for use, reproduction, or distribution of Your modifications, or
-      for any such Derivative Works as a whole, provided Your use,
-      reproduction, and distribution of the Work otherwise complies with
-      the conditions stated in this License.
-
-   5. Submission of Contributions. Unless You explicitly state otherwise,
-      any Contribution intentionally submitted for inclusion in the Work
-      by You to the Licensor shall be under the terms and conditions of
-      this License, without any additional terms or conditions.
-      Notwithstanding the above, nothing herein shall supersede or modify
-      the terms of any separate license agreement you may have executed
-      with Licensor regarding such Contributions.
-
-   6. Trademarks. This License does not grant permission to use the trade
-      names, trademarks, service marks, or product names of the Licensor,
-      except as required for reasonable and customary use in describing the
-      origin of the Work and reproducing the content of the NOTICE file.
-
-   7. Disclaimer of Warranty. Unless required by applicable law or
-      agreed to in writing, Licensor provides the Work (and each
-      Contributor provides its Contributions) on an "AS IS" BASIS,
-      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
-      implied, including, without limitation, any warranties or conditions
-      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
-      PARTICULAR PURPOSE. You are solely responsible for determining the
-      appropriateness of using or redistributing the Work and assume any
-      risks associated with Your exercise of permissions under this License.
-
-   8. Limitation of Liability. In no event and under no legal theory,
-      whether in tort (including negligence), contract, or otherwise,
-      unless required by applicable law (such as deliberate and grossly
-      negligent acts) or agreed to in writing, shall any Contributor be
-      liable to You for damages, including any direct, indirect, special,
-      incidental, or consequential damages of any character arising as a
-      result of this License or out of the use or inability to use the
-      Work (including but not limited to damages for loss of goodwill,
-      work stoppage, computer failure or malfunction, or any and all
-      other commercial damages or losses), even if such Contributor
-      has been advised of the possibility of such damages.
-
-   9. Accepting Warranty or Additional Liability. While redistributing
-      the Work or Derivative Works thereof, You may choose to offer,
-      and charge a fee for, acceptance of support, warranty, indemnity,
-      or other liability obligations and/or rights consistent with this
-      License. However, in accepting such obligations, You may act only
-      on Your own behalf and on Your sole responsibility, not on behalf
-      of any other Contributor, and only if You agree to indemnify,
-      defend, and hold each Contributor harmless for any liability
-      incurred by, or claims asserted against, such Contributor by reason
-      of your accepting any such warranty or additional liability.
-
-   END OF TERMS AND CONDITIONS
-
-   APPENDIX: How to apply the Apache License to your work.
-
-      To apply the Apache License to your work, attach the following
-      boilerplate notice, with the fields enclosed by brackets "[]"
-      replaced with your own identifying information. (Don't include
-      the brackets!)  The text should be enclosed in the appropriate
-      comment syntax for the file format. We also recommend that a
-      file or class name and description of purpose be included on the
-      same "printed page" as the copyright notice for easier
-      identification within third-party archives.
-
-   Copyright [yyyy] [name of copyright owner]
-
-   Licensed under the Apache License, Version 2.0 (the "License");
-   you may not use this file except in compliance with the License.
-   You may obtain a copy of the License at
-
-       http://www.apache.org/licenses/LICENSE-2.0
-
-   Unless required by applicable law or agreed to in writing, software
-   distributed under the License is distributed on an "AS IS" BASIS,
-   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-   See the License for the specific language governing permissions and
-   limitations under the License.

env_manager/gz_enviroment/gz_enviroment.xml

@@ -1,7 +0,0 @@
-<library path="gz_enviroment">
-    <class
-        type="gz_enviroment::GzEnviroment" 
-        base_class_type="env_interface::EnvInterface">
-      <description>Gazebo enviroment for env_manager</description>
-    </class>
-  </library>

env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp

@@ -1,70 +0,0 @@
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_lifecycle/lifecycle_node.hpp"
-// #include "ros_gz_bridge/convert.hpp"
-#include "env_interface/env_interface.hpp"
-#include "geometry_msgs/msg/pose_array.hpp"
-#include "gz/msgs/pose_v.pb.h"
-#include <geometry_msgs/msg/transform_stamped.hpp>
-#include <gz/transport/Node.hh>
-#include <memory>
-#include <mutex>
-#include <rclcpp/timer.hpp>
-#include <string>
-#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
-#include <tf2_msgs/msg/tf_message.hpp>
-#include <tf2_ros/transform_broadcaster.h>
-
-#include <gz/sim/Entity.hh>
-#include <gz/sim/EntityComponentManager.hh>
-#include <gz/sim/components/Model.hh>
-#include <thread>
-
-namespace gz_enviroment {
-using CallbackReturn =
-    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
-
-class GzEnviroment : public env_interface::EnvInterface {
-public:
-  GzEnviroment();
-  CallbackReturn on_init() override;
-  CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
-  CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
-  CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;
-  CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override;
-
-protected:
-  void onGzPoseSub(const gz::msgs::Pose_V &pose_v);
-
-  // bool doGzSpawn();
-
-private:
-  std::unique_ptr<tf2_ros::TransformBroadcaster> m_tf2_broadcaster;
-  std::shared_ptr<gz::transport::Node> m_gz_node;
-  std::vector<std::string> m_follow_frames;
-  std::string m_topic_name;
-  std::string m_service_spawn;
-  std::string m_world_name;
-  std::shared_ptr<rbs_utils::AssemblyConfigLoader> m_config_loader;
-  tf2_msgs::msg::TFMessage m_transforms;
-  tf2_msgs::msg::TFMessage m_target_places;
-  rclcpp::TimerBase::SharedPtr m_timer;
-
-  std::vector<geometry_msgs::msg::TransformStamped> m_all_transforms{};
-
-  std::string getGzWorldName();
-  std::string createGzModelString(const std::vector<double> &pose,
-                                  const std::vector<double> &inertia,
-                                  const double &mass,
-                                  const std::string &mesh_filepath,
-                                  const std::string &name);
-
-  void broadcast_timer_callback() {
-    if (!m_transforms.transforms.empty()) {
-      for (auto &transform : m_transforms.transforms) {
-        m_tf2_broadcaster->sendTransform(std::move(transform));
-      }
-      // m_transforms.transforms.clear();
-    }
-  }
-};
-} // namespace gz_enviroment

env_manager/gz_enviroment/package.xml

@@ -1,25 +0,0 @@
-<?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>gz_enviroment</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
-  <license>Apache-2.0</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>rclcpp</depend>
-  <depend>rclcpp_lifecycle</depend>
-  <depend>tf2_ros</depend>
-  <depend>tf2_msgs</depend>
-  <depend>ros_gz</depend>
-  <depend>env_interface</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

env_manager/gz_enviroment/scripts/publish_asm_config.py

@@ -1,114 +0,0 @@
-#! /usr/bin/env python3
-
-import yaml
-import rclpy
-from rclpy.node import Node
-from rbs_utils_interfaces.msg import AssemblyConfig, NamedPose, RelativeNamedPose
-from geometry_msgs.msg import Point
-import os
-
-
-class ConfigPublisherNode(Node):
-
-    def __init__(self):
-        super().__init__('config_publisher_node')
-        self.publisher_ = self.create_publisher(AssemblyConfig, '/assembly_config', 10)
-        timer_period = 1  # seconds
-        self.timer = self.create_timer(timer_period, self.timer_callback)
-        if 'RBS_ASSEMBLY_DIR' in os.environ:
-            assembly_dir = os.environ['RBS_ASSEMBLY_DIR']
-        else:
-            assembly_dir = '~/assembly/robossembler-stuff'
-        self.assembly_dir = os.path.expanduser(assembly_dir)
-        datafolder = "asp-example"
-        # Read data from YAML file
-        with open(f"{self.assembly_dir}/{datafolder}/rbs_db.yaml") as f:
-            self.data = yaml.safe_load(f)
-
-    def timer_callback(self):
-
-        # Create AssemblyConfig message
-        msg = AssemblyConfig()
-        # Fill in the data from the YAML file
-        msg.assets_db = self.data['assets_db']
-
-        # Populate workspace
-        for wp in self.data['workspace']:
-            point = Point()
-            point.x = wp["x"]
-            point.y = wp["y"]
-            point.z = wp["z"]
-            msg.workspace.append(point)
-
-        # Populate absolute_part
-        for part in self.data['absolute_part']:
-            named_pose = NamedPose()
-            named_pose.name = part['name']
-            pose = part['pose']
-            named_pose.pose.position.x = pose['position']['x']
-            named_pose.pose.position.y = pose['position']['y']
-            named_pose.pose.position.z = pose['position']['z']
-            named_pose.pose.orientation.x = pose['orientation']['x']
-            named_pose.pose.orientation.y = pose['orientation']['y']
-            named_pose.pose.orientation.z = pose['orientation']['z']
-            named_pose.pose.orientation.w = pose['orientation']['w']
-            msg.absolute_part.append(named_pose)
-
-        # Populate relative_part
-        for part in self.data['relative_part']:
-            relative_named_pose = RelativeNamedPose()
-            relative_named_pose.name = part['name']
-            relative_named_pose.relative_at = part['relative_at']
-            pose = part['pose']
-            relative_named_pose.pose.position.x = pose['position']['x']
-            relative_named_pose.pose.position.y = pose['position']['y']
-            relative_named_pose.pose.position.z = pose['position']['z']
-            relative_named_pose.pose.orientation.x = pose['orientation']['x']
-            relative_named_pose.pose.orientation.y = pose['orientation']['y']
-            relative_named_pose.pose.orientation.z = pose['orientation']['z']
-            relative_named_pose.pose.orientation.w = pose['orientation']['w']
-            msg.relative_part.append(relative_named_pose)
-
-        # Populate grasp_pose
-        for part in self.data['grasp_pose']:
-            relative_named_pose = RelativeNamedPose()
-            relative_named_pose.name = part['name']
-            relative_named_pose.relative_at = part['relative_at']
-            pose = part['pose']
-            relative_named_pose.pose.position.x = pose['position']['x']
-            relative_named_pose.pose.position.y = pose['position']['y']
-            relative_named_pose.pose.position.z = pose['position']['z']
-            relative_named_pose.pose.orientation.x = pose['orientation']['x']
-            relative_named_pose.pose.orientation.y = pose['orientation']['y']
-            relative_named_pose.pose.orientation.z = pose['orientation']['z']
-            relative_named_pose.pose.orientation.w = pose['orientation']['w']
-            msg.grasp_pose.append(relative_named_pose)
-
-        # Populate extra_poses
-        for part in self.data['extra_poses']:
-            named_pose = NamedPose()
-            named_pose.name = part['name']
-            pose = part['pose']
-            named_pose.pose.position.x = pose['position']['x']
-            named_pose.pose.position.y = pose['position']['y']
-            named_pose.pose.position.z = pose['position']['z']
-            named_pose.pose.orientation.x = pose['orientation']['x']
-            named_pose.pose.orientation.y = pose['orientation']['y']
-            named_pose.pose.orientation.z = pose['orientation']['z']
-            named_pose.pose.orientation.w = pose['orientation']['w']
-            msg.extra_poses.append(named_pose)
-
-        # Publish the message
-        self.publisher_.publish(msg)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    config_publisher_node = ConfigPublisherNode()
-    rclpy.spin(config_publisher_node)
-    config_publisher_node.destroy_node()
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

env_manager/gz_enviroment/scripts/test_tf.py

@@ -1,75 +0,0 @@
-#! /usr/bin/env python3
-
-import rclpy
-from rclpy.node import Node
-from tf2_msgs.msg import TFMessage
-from geometry_msgs.msg import TransformStamped
-from rbs_utils_interfaces.msg import AssemblyConfig
-from tf2_ros.transform_broadcaster import TransformBroadcaster
-
-
-class TF2PublisherNode(Node):
-
-    def __init__(self):
-        super().__init__('tf2_publisher_node')
-        self.tfb_ = TransformBroadcaster(self)
-        self.subscription = self.create_subscription(
-            AssemblyConfig, '/assembly_config', self.config_callback, 10)
-
-    def config_callback(self, msg):
-        # Populate transforms for absolute_part
-        for part in msg.absolute_part:
-            ts = TransformStamped()
-            ts.header.stamp = self.get_clock().now().to_msg()
-            ts.header.frame_id = "world"
-            ts.child_frame_id = part.name
-            ts.transform.translation.x = part.pose.position.x
-            ts.transform.translation.y = part.pose.position.y
-            ts.transform.translation.z = part.pose.position.z
-            ts.transform.rotation.x = part.pose.orientation.x
-            ts.transform.rotation.y = part.pose.orientation.y
-            ts.transform.rotation.z = part.pose.orientation.z
-            ts.transform.rotation.w = part.pose.orientation.w
-            self.tfb_.sendTransform(ts)
-
-        # Populate transforms for relative_part
-        for part in msg.relative_part:
-            ts = TransformStamped()
-            ts.header.stamp = self.get_clock().now().to_msg()
-            ts.header.frame_id = part.relative_at
-            ts.child_frame_id = part.name
-            ts.transform.translation.x = part.pose.position.x
-            ts.transform.translation.y = part.pose.position.y
-            ts.transform.translation.z = part.pose.position.z
-            ts.transform.rotation.x = part.pose.orientation.x
-            ts.transform.rotation.y = part.pose.orientation.y
-            ts.transform.rotation.z = part.pose.orientation.z
-            ts.transform.rotation.w = part.pose.orientation.w
-            self.tfb_.sendTransform(ts)
-
-        # Populate transforms for grasp_pose
-        for part in msg.grasp_pose:
-            ts = TransformStamped()
-            ts.header.stamp = self.get_clock().now().to_msg()
-            ts.header.frame_id = part.relative_at
-            ts.child_frame_id = part.name
-            ts.transform.translation.x = part.pose.position.x
-            ts.transform.translation.y = part.pose.position.y
-            ts.transform.translation.z = part.pose.position.z
-            ts.transform.rotation.x = part.pose.orientation.x
-            ts.transform.rotation.y = part.pose.orientation.y
-            ts.transform.rotation.z = part.pose.orientation.z
-            ts.transform.rotation.w = part.pose.orientation.w
-            self.tfb_.sendTransform(ts)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    tf2_publisher_node = TF2PublisherNode()
-    rclpy.spin(tf2_publisher_node)
-    tf2_publisher_node.destroy_node()
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

env_manager/gz_enviroment/src/gz_enviroment.cpp

@@ -1,208 +0,0 @@
-#include "gz_enviroment/gz_enviroment.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
-#include "ros_gz_bridge/convert.hpp"
-#include <algorithm>
-#include <chrono>
-#include <functional>
-#include <iterator>
-#include <memory>
-#include <rclcpp/logging.hpp>
-#include <rclcpp/utilities.hpp>
-#include <rclcpp_lifecycle/lifecycle_node.hpp>
-#include <ros_gz_interfaces/msg/detail/entity__builder.hpp>
-
-#include <gz/sim/components.hh>
-#include <string>
-#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
-#include <tf2_ros/transform_broadcaster.h>
-
-namespace gz_enviroment {
-
-GzEnviroment::GzEnviroment() : env_interface::EnvInterface() {}
-
-CallbackReturn GzEnviroment::on_init() {
-  RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()");
-  gz::sim::EntityComponentManager ecm;
-
-  const auto modelEntities =
-      ecm.EntitiesByComponents(gz::sim::components::Model());
-
-  for (const auto entity : modelEntities) {
-    const auto modelName = ecm.Component<gz::sim::components::Name>(entity);
-    if (modelName) {
-      RCLCPP_INFO_STREAM(getNode()->get_logger(),
-                         "Model Name: " << modelName->Data());
-    }
-  }
-
-  // getNode()->declare_parameter("use_sim_time", true);
-
-  return CallbackReturn::SUCCESS;
-}
-
-CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
-  // Configuration of parameters
-  using namespace std::chrono_literals;
-  RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure");
-  m_gz_node = std::make_shared<gz::transport::Node>();
-  m_world_name = getGzWorldName();
-  m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info";
-  m_service_spawn = std::string("/world/") + m_world_name + "/create";
-  m_config_loader = std::make_shared<rbs_utils::AssemblyConfigLoader>(
-      "asp-example", getNode());
-  m_follow_frames = m_config_loader->getUniqueSceneModelNames();
-  m_transforms = m_config_loader->getGraspTfData("bishop");
-  // m_transforms = m_config_loader->getAllPossibleTfData();
-
-  return CallbackReturn::SUCCESS;
-}
-
-CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State &) {
-  // Setting up the subscribers and publishers
-  RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate");
-
-  // Initialize tf broadcaster before use in subscriber
-  m_tf2_broadcaster =
-      std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
-  m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
-
-  return CallbackReturn::SUCCESS;
-}
-
-CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) {
-  RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup");
-  // Clear variables on_cleanup
-  m_gz_node.reset();
-  m_follow_frames.clear();
-  m_topic_name.clear();
-  m_service_spawn.clear();
-  m_world_name.clear();
-  m_config_loader.reset();
-  m_transforms = tf2_msgs::msg::TFMessage();
-  m_target_places = tf2_msgs::msg::TFMessage();
-
-  return CallbackReturn::SUCCESS;
-}
-
-CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State &) {
-  RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate");
-  return CallbackReturn::SUCCESS;
-}
-
-// TODO: Check to do this via EntityComponentManager
-void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
-  std::vector<geometry_msgs::msg::TransformStamped> all_transforms{};
-
-  for (const auto &it : pose_v.pose()) {
-    if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) ==
-        m_follow_frames.end())
-      continue;
-
-    geometry_msgs::msg::PoseStamped rpose;
-    ros_gz_bridge::convert_gz_to_ros(it, rpose);
-
-    geometry_msgs::msg::TransformStamped t;
-    t.header.stamp = getNode()->get_clock()->now();
-    t.header.frame_id = "world";
-    t.child_frame_id = it.name();
-
-    t.transform.translation.x = rpose.pose.position.x;
-    t.transform.translation.y = rpose.pose.position.y;
-    t.transform.translation.z = rpose.pose.position.z;
-
-    t.transform.rotation.x = rpose.pose.orientation.x;
-    t.transform.rotation.y = rpose.pose.orientation.y;
-    t.transform.rotation.z = rpose.pose.orientation.z;
-    t.transform.rotation.w = rpose.pose.orientation.w;
-
-    all_transforms.push_back(t);
-  }
-
-  for (auto &place : m_transforms.transforms) {
-    place.header.stamp = getNode()->get_clock()->now();
-    all_transforms.push_back(place);
-  }
-
-  // m_all_transforms.swap(all_transforms);
-}
-
-std::string GzEnviroment::getGzWorldName() {
-  bool executed{false};
-  bool result{false};
-  unsigned int timeout{5000};
-  std::string service{"/gazebo/worlds"};
-  gz::msgs::StringMsg_V worlds_msg;
-
-  while (rclcpp::ok() && !executed) {
-    RCLCPP_INFO(getNode()->get_logger(), "Requesting list of world names.");
-    executed = m_gz_node->Request(service, timeout, worlds_msg, result);
-  }
-
-  if (!executed) {
-    RCLCPP_INFO(getNode()->get_logger(), "Timed out when getting world names.");
-    return "";
-  }
-
-  if (!result || worlds_msg.data().empty()) {
-    RCLCPP_INFO(getNode()->get_logger(), "Failed to get world names.");
-    return "";
-  }
-  RCLCPP_INFO(getNode()->get_logger(), "%s", worlds_msg.data(0).c_str());
-  return worlds_msg.data(0);
-}
-
-// bool GzEnviroment::doGzSpawn() {
-//   gz::msgs::EntityFactory req;
-//   gz::msgs::Boolean rep;
-//   bool result;
-//   unsigned int timeout = 5000;
-//   bool executed;
-//
-//   auto env_models = m_config_loader->getEnvModels();
-//   for (auto &model : *env_models) {
-//     std::string sdf_string =
-//         createGzModelString(model.model_pose, model.model_inertia, model.mass,
-//                             model.mesh_path, model.model_name);
-//     req.set_sdf(sdf_string);
-//     executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result);
-//   }
-//   return executed;
-// }
-
-// std::string GzEnviroment::createGzModelString(
-//     const std::vector<double> &pose, const std::vector<double> &inertia,
-//     const double &mass, const std::string &mesh_filepath,
-//     const std::string &name) {
-//   std::string model_template =
-//       std::string("<sdf version='1.7'>") + "<model name='" + name + "'>" +
-//       "<link name='link_" + name + "'>" + "<static>1</static>" + "<pose>" +
-//       std::to_string(pose[0]) + " " + std::to_string(pose[1]) + " " +
-//       std::to_string(pose[2]) + " " + std::to_string(pose[3]) + " " +
-//       std::to_string(pose[4]) + " " + std::to_string(pose[5]) +
-//       "</pose>"
-//       // + "<inertial>"
-//       // + "<inertia>"
-//       // + "<ixx>" + std::to_string(inertia[0]) + "</ixx>"
-//       // + "<ixy>" + std::to_string(inertia[1]) + "</ixy>"
-//       // + "<ixz>" + std::to_string(inertia[2]) + "</ixz>"
-//       // + "<iyy>" + std::to_string(inertia[3]) + "</iyy>"
-//       // + "<iyz>" + std::to_string(inertia[4]) + "</iyz>"
-//       // + "<izz>" + std::to_string(inertia[5]) + "</izz>"
-//       // + "</inertia>"
-//       // + "<mass>" + std::to_string(mass) + "</mass>"
-//       // + "</inertial>"
-//       + "<visual name='visual_" + name + "'>" + "<geometry><mesh><uri>file://" +
-//       mesh_filepath + "</uri></mesh></geometry>" + "</visual>" +
-//       "<collision name='collision_" + name + "'>" +
-//       "<geometry><mesh><uri>file://" + mesh_filepath +
-//       "</uri></mesh></geometry>" + "</collision>" + "</link>" + "</model>" +
-//       "</sdf>";
-//   return model_template;
-// }
-
-} // namespace gz_enviroment
-
-#include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment,
-                       env_interface::EnvInterface);

env_manager/planning_scene_manager/LICENSE

@@ -1,202 +0,0 @@
-
-                                 Apache License
-                           Version 2.0, January 2004
-                        http://www.apache.org/licenses/
-
-   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
-
-   1. Definitions.
-
-      "License" shall mean the terms and conditions for use, reproduction,
-      and distribution as defined by Sections 1 through 9 of this document.
-
-      "Licensor" shall mean the copyright owner or entity authorized by
-      the copyright owner that is granting the License.
-
-      "Legal Entity" shall mean the union of the acting entity and all
-      other entities that control, are controlled by, or are under common
-      control with that entity. For the purposes of this definition,
-      "control" means (i) the power, direct or indirect, to cause the
-      direction or management of such entity, whether by contract or
-      otherwise, or (ii) ownership of fifty percent (50%) or more of the
-      outstanding shares, or (iii) beneficial ownership of such entity.
-
-      "You" (or "Your") shall mean an individual or Legal Entity
-      exercising permissions granted by this License.
-
-      "Source" form shall mean the preferred form for making modifications,
-      including but not limited to software source code, documentation
-      source, and configuration files.
-
-      "Object" form shall mean any form resulting from mechanical
-      transformation or translation of a Source form, including but
-      not limited to compiled object code, generated documentation,
-      and conversions to other media types.
-
-      "Work" shall mean the work of authorship, whether in Source or
-      Object form, made available under the License, as indicated by a
-      copyright notice that is included in or attached to the work
-      (an example is provided in the Appendix below).
-
-      "Derivative Works" shall mean any work, whether in Source or Object
-      form, that is based on (or derived from) the Work and for which the
-      editorial revisions, annotations, elaborations, or other modifications
-      represent, as a whole, an original work of authorship. For the purposes
-      of this License, Derivative Works shall not include works that remain
-      separable from, or merely link (or bind by name) to the interfaces of,
-      the Work and Derivative Works thereof.
-
-      "Contribution" shall mean any work of authorship, including
-      the original version of the Work and any modifications or additions
-      to that Work or Derivative Works thereof, that is intentionally
-      submitted to Licensor for inclusion in the Work by the copyright owner
-      or by an individual or Legal Entity authorized to submit on behalf of
-      the copyright owner. For the purposes of this definition, "submitted"
-      means any form of electronic, verbal, or written communication sent
-      to the Licensor or its representatives, including but not limited to
-      communication on electronic mailing lists, source code control systems,
-      and issue tracking systems that are managed by, or on behalf of, the
-      Licensor for the purpose of discussing and improving the Work, but
-      excluding communication that is conspicuously marked or otherwise
-      designated in writing by the copyright owner as "Not a Contribution."
-
-      "Contributor" shall mean Licensor and any individual or Legal Entity
-      on behalf of whom a Contribution has been received by Licensor and
-      subsequently incorporated within the Work.
-
-   2. Grant of Copyright License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      copyright license to reproduce, prepare Derivative Works of,
-      publicly display, publicly perform, sublicense, and distribute the
-      Work and such Derivative Works in Source or Object form.
-
-   3. Grant of Patent License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      (except as stated in this section) patent license to make, have made,
-      use, offer to sell, sell, import, and otherwise transfer the Work,
-      where such license applies only to those patent claims licensable
-      by such Contributor that are necessarily infringed by their
-      Contribution(s) alone or by combination of their Contribution(s)
-      with the Work to which such Contribution(s) was submitted. If You
-      institute patent litigation against any entity (including a
-      cross-claim or counterclaim in a lawsuit) alleging that the Work
-      or a Contribution incorporated within the Work constitutes direct
-      or contributory patent infringement, then any patent licenses
-      granted to You under this License for that Work shall terminate
-      as of the date such litigation is filed.
-
-   4. Redistribution. You may reproduce and distribute copies of the
-      Work or Derivative Works thereof in any medium, with or without
-      modifications, and in Source or Object form, provided that You
-      meet the following conditions:
-
-      (a) You must give any other recipients of the Work or
-          Derivative Works a copy of this License; and
-
-      (b) You must cause any modified files to carry prominent notices
-          stating that You changed the files; and
-
-      (c) You must retain, in the Source form of any Derivative Works
-          that You distribute, all copyright, patent, trademark, and
-          attribution notices from the Source form of the Work,
-          excluding those notices that do not pertain to any part of
-          the Derivative Works; and
-
-      (d) If the Work includes a "NOTICE" text file as part of its
-          distribution, then any Derivative Works that You distribute must
-          include a readable copy of the attribution notices contained
-          within such NOTICE file, excluding those notices that do not
-          pertain to any part of the Derivative Works, in at least one
-          of the following places: within a NOTICE text file distributed
-          as part of the Derivative Works; within the Source form or
-          documentation, if provided along with the Derivative Works; or,
-          within a display generated by the Derivative Works, if and
-          wherever such third-party notices normally appear. The contents
-          of the NOTICE file are for informational purposes only and
-          do not modify the License. You may add Your own attribution
-          notices within Derivative Works that You distribute, alongside
-          or as an addendum to the NOTICE text from the Work, provided
-          that such additional attribution notices cannot be construed
-          as modifying the License.
-
-      You may add Your own copyright statement to Your modifications and
-      may provide additional or different license terms and conditions
-      for use, reproduction, or distribution of Your modifications, or
-      for any such Derivative Works as a whole, provided Your use,
-      reproduction, and distribution of the Work otherwise complies with
-      the conditions stated in this License.
-
-   5. Submission of Contributions. Unless You explicitly state otherwise,
-      any Contribution intentionally submitted for inclusion in the Work
-      by You to the Licensor shall be under the terms and conditions of
-      this License, without any additional terms or conditions.
-      Notwithstanding the above, nothing herein shall supersede or modify
-      the terms of any separate license agreement you may have executed
-      with Licensor regarding such Contributions.
-
-   6. Trademarks. This License does not grant permission to use the trade
-      names, trademarks, service marks, or product names of the Licensor,
-      except as required for reasonable and customary use in describing the
-      origin of the Work and reproducing the content of the NOTICE file.
-
-   7. Disclaimer of Warranty. Unless required by applicable law or
-      agreed to in writing, Licensor provides the Work (and each
-      Contributor provides its Contributions) on an "AS IS" BASIS,
-      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
-      implied, including, without limitation, any warranties or conditions
-      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
-      PARTICULAR PURPOSE. You are solely responsible for determining the
-      appropriateness of using or redistributing the Work and assume any
-      risks associated with Your exercise of permissions under this License.
-
-   8. Limitation of Liability. In no event and under no legal theory,
-      whether in tort (including negligence), contract, or otherwise,
-      unless required by applicable law (such as deliberate and grossly
-      negligent acts) or agreed to in writing, shall any Contributor be
-      liable to You for damages, including any direct, indirect, special,
-      incidental, or consequential damages of any character arising as a
-      result of this License or out of the use or inability to use the
-      Work (including but not limited to damages for loss of goodwill,
-      work stoppage, computer failure or malfunction, or any and all
-      other commercial damages or losses), even if such Contributor
-      has been advised of the possibility of such damages.
-
-   9. Accepting Warranty or Additional Liability. While redistributing
-      the Work or Derivative Works thereof, You may choose to offer,
-      and charge a fee for, acceptance of support, warranty, indemnity,
-      or other liability obligations and/or rights consistent with this
-      License. However, in accepting such obligations, You may act only
-      on Your own behalf and on Your sole responsibility, not on behalf
-      of any other Contributor, and only if You agree to indemnify,
-      defend, and hold each Contributor harmless for any liability
-      incurred by, or claims asserted against, such Contributor by reason
-      of your accepting any such warranty or additional liability.
-
-   END OF TERMS AND CONDITIONS
-
-   APPENDIX: How to apply the Apache License to your work.
-
-      To apply the Apache License to your work, attach the following
-      boilerplate notice, with the fields enclosed by brackets "[]"
-      replaced with your own identifying information. (Don't include
-      the brackets!)  The text should be enclosed in the appropriate
-      comment syntax for the file format. We also recommend that a
-      file or class name and description of purpose be included on the
-      same "printed page" as the copyright notice for easier
-      identification within third-party archives.
-
-   Copyright [yyyy] [name of copyright owner]
-
-   Licensed under the Apache License, Version 2.0 (the "License");
-   you may not use this file except in compliance with the License.
-   You may obtain a copy of the License at
-
-       http://www.apache.org/licenses/LICENSE-2.0
-
-   Unless required by applicable law or agreed to in writing, software
-   distributed under the License is distributed on an "AS IS" BASIS,
-   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-   See the License for the specific language governing permissions and
-   limitations under the License.

env_manager/planning_scene_manager/include/planning_scene_manager/planning_scene_manager.hpp

@@ -1,29 +0,0 @@
-#ifndef __PLANNING_SCENE_MANAGER_H__
-#define __PLANNING_SCENE_MANAGER_H__
-
-#include "rclcpp/rclcpp.hpp"
-#include "moveit/planning_scene_monitor/planning_scene_monitor.h"
-
-namespace planning_scene_manager
-{
-class PlanningSceneManager : public planning_scene_monitor::PlanningSceneMonitor
-{
-public:
-    PlanningSceneManager();
-
-    bool GetPlanningObject();
-    bool SetPlanningObject();
-
-    bool SetPlanningScene();
-    bool GetPlanningScene();
-
-    bool UpdatePlanningScene();
-
-private:
-
-};
-
-    
-} // namespace planning_scene_manager
-
-#endif // __PLANNING_SCENE_MANAGER_H__

env_manager/planning_scene_manager/src/planning_scene_manager.cpp

@@ -1,31 +0,0 @@
-#include "planning_scene_manager/planning_scene_manager.hpp"
-#include "moveit_msgs/msg/collision_object.hpp"
-#include "moveit/planning_scene_interface/planning_scene_interface.h"
-
-namespace planning_scene_manager
-{
-
-bool PlanningSceneManager::SetPlanningObject()
-{
-    moveit_msgs::msg::CollisionObject object;
-    std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> pcm_;
-    planning_scene_monitor::LockedPlanningSceneRW scene(pcm_);
-    scene->removeAllCollisionObjects();
-    RCLCPP_INFO(pnode_->get_logger(), "All object has been delete");
-    return true;
-}
-
-} // namespace planning_scene_manager
-
-int main(int argc, char** argv)
-{
-    rclcpp::init(argc, argv);
-    auto node = std::make_shared<planning_scene_manager::PlanningSceneManager>();
-    
-    // Добавляем объект в сцену коллизии
-    node->SetPlanningObject();
-
-    rclcpp::spin(node);
-    rclcpp::shutdown();
-    return 0;
-}

env_manager/rbs_gym/CMakeLists.txt

@@ -34,6 +34,6 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-install(DIRECTORY rbs_gym/envs/worlds launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 
 ament_package()

env_manager/rbs_gym/launch/test_env.launch.py

@@ -1,18 +1,21 @@
+import os
+from os import cpu_count
+
+import xacro
+from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
     IncludeLaunchDescription,
     OpaqueFunction,
     SetEnvironmentVariable,
-    TimerAction
+    TimerAction,
 )
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.substitutions import FindPackageShare
 from launch_ros.actions import Node
-import os
+from launch_ros.substitutions import FindPackageShare
-from os import cpu_count
+
-from ament_index_python.packages import get_package_share_directory
 
 def launch_setup(context, *args, **kwargs):
     # Initialize Arguments
@@ -45,33 +48,50 @@ def launch_setup(context, *args, **kwargs):
     log_level = LaunchConfiguration("log_level")
     env_kwargs = LaunchConfiguration("env_kwargs")
 
-
     sim_gazebo = LaunchConfiguration("sim_gazebo")
     launch_simulation = LaunchConfiguration("launch_sim")
 
     initial_joint_controllers_file_path = os.path.join(
-        get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml'
+        get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml"
     )
 
+    xacro_file = os.path.join(
+        get_package_share_directory(description_package.perform(context)),
+        "urdf",
+        description_file.perform(context),
+    )
+
+    robot_description_doc = xacro.process_file(
+        xacro_file,
+        mappings={
+            "gripper_name": gripper_name.perform(context),
+            "hardware": hardware.perform(context),
+            "simulation_controllers": initial_joint_controllers_file_path,
+            "namespace": "",
+        },
+    )
+
+    robot_description_content = robot_description_doc.toprettyxml(indent="  ")
+    robot_description = {"robot_description": robot_description_content}
+
     single_robot_setup = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([
+        PythonLaunchDescriptionSource(
-            PathJoinSubstitution([
+            [
-                FindPackageShare('rbs_bringup'),
+                PathJoinSubstitution(
-                "launch",
+                    [FindPackageShare("rbs_bringup"), "launch", "rbs_robot.launch.py"]
-                "rbs_robot.launch.py"
+                )
-            ])
+            ]
-        ]),
+        ),
         launch_arguments={
             "env_manager": env_manager,
             "with_gripper": with_gripper_condition,
             "gripper_name": gripper_name,
-            "controllers_file": controllers_file,
-            "robot_type": robot_type,
             "controllers_file": initial_joint_controllers_file_path,
+            "robot_type": robot_type,
             "cartesian_controllers": cartesian_controllers,
             "description_package": description_package,
             "description_file": description_file,
-            "robot_name": robot_name,
+            "robot_name": robot_type,
             "start_joint_controller": start_joint_controller,
             "initial_joint_controller": initial_joint_controller,
             "launch_simulation": launch_simulation,
@@ -83,9 +103,9 @@ def launch_setup(context, *args, **kwargs):
             "use_sim_time": use_sim_time,
             "sim_gazebo": sim_gazebo,
             "hardware": hardware,
-            "launch_controllers": launch_controllers,
+            "launch_controllers": "true",
-            # "gazebo_gui": gazebo_gui
+            "robot_description": robot_description_content,
-        }.items()
+        }.items(),
     )
 
     args = [
@@ -103,26 +123,23 @@ def launch_setup(context, *args, **kwargs):
         executable="test_agent.py",
         output="log",
         arguments=args,
-        parameters=[{"use_sim_time": True}]
+        parameters=[{"use_sim_time": True}, robot_description],
     )
 
     clock_bridge = Node(
-        package='ros_gz_bridge',
+        package="ros_gz_bridge",
-        executable='parameter_bridge',
+        executable="parameter_bridge",
-        arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'],
+        arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
-        output='screen')
+        output="screen",
-
-
-    delay_robot_control_stack = TimerAction(
-        period=10.0,
-        actions=[single_robot_setup]
     )
 
+    delay_robot_control_stack = TimerAction(period=10.0, actions=[single_robot_setup])
+
     nodes_to_start = [
         # env,
         rl_task,
         clock_bridge,
-        delay_robot_control_stack
+        delay_robot_control_stack,
     ]
     return nodes_to_start
 
@@ -133,7 +150,7 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "robot_type",
             description="Type of robot by name",
-            choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
+            choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
             default_value="rbs_arm",
         )
     )
@@ -213,62 +230,72 @@ def generate_launch_description():
         )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("with_gripper", 
+        DeclareLaunchArgument(
-                              default_value="true", 
+            "with_gripper", default_value="true", description="With gripper or not?"
-                              description="With gripper or not?")
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("sim_gazebo", 
+        DeclareLaunchArgument(
-                              default_value="true", 
+            "sim_gazebo", default_value="true", description="Gazebo Simulation"
-                              description="Gazebo Simulation")
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("env_manager", 
+        DeclareLaunchArgument(
-                              default_value="false", 
+            "env_manager", default_value="false", description="Launch env_manager?"
-                              description="Launch env_manager?")
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_sim", 
+        DeclareLaunchArgument(
-                              default_value="true", 
+            "launch_sim",
-                              description="Launch simulator (Gazebo)?\
+            default_value="true",
-                              Most general arg")
+            description="Launch simulator (Gazebo)?\
+                              Most general arg",
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_moveit", 
+        DeclareLaunchArgument(
-                              default_value="false", 
+            "launch_moveit", default_value="false", description="Launch moveit?"
-                              description="Launch moveit?")
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_perception", 
+        DeclareLaunchArgument(
-                              default_value="false", 
+            "launch_perception", default_value="false", description="Launch perception?"
-                              description="Launch perception?")
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_task_planner", 
+        DeclareLaunchArgument(
-                              default_value="false", 
+            "launch_task_planner",
-                              description="Launch task_planner?")
+            default_value="false",
+            description="Launch task_planner?",
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("cartesian_controllers", 
+        DeclareLaunchArgument(
-                              default_value="true", 
+            "cartesian_controllers",
-                              description="Load cartesian\
+            default_value="true",
-                              controllers?")
+            description="Load cartesian\
+                              controllers?",
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("hardware", 
+        DeclareLaunchArgument(
-                              choices=["gazebo", "mock"],
+            "hardware",
-                              default_value="gazebo", 
+            choices=["gazebo", "mock"],
-                              description="Choose your harware_interface")
+            default_value="gazebo",
+            description="Choose your harware_interface",
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_controllers", 
+        DeclareLaunchArgument(
-                              default_value="true", 
+            "launch_controllers",
-                              description="Launch controllers?")
+            default_value="true",
+            description="Launch controllers?",
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("gazebo_gui", 
+        DeclareLaunchArgument(
-                              default_value="true", 
+            "gazebo_gui", default_value="true", description="Launch gazebo with gui?"
-                              description="Launch gazebo with gui?")
+        )
     )
     # training arguments
     declared_arguments.append(
@@ -276,140 +303,165 @@ def generate_launch_description():
             "env",
             default_value="Reach-Gazebo-v0",
             description="Environment ID",
-        ))
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "env_kwargs",
             default_value="",
             description="Optional keyword argument to pass to the env constructor.",
-        ))
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "vec_env",
             default_value="dummy",
             description="Type of VecEnv to use (dummy or subproc).",
-        ))
+        )
-        # Algorithm and training
+    )
+    # Algorithm and training
     declared_arguments.append(
         DeclareLaunchArgument(
             "algo",
             default_value="sac",
             description="RL algorithm to use during the training.",
-        ))
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "n_timesteps",
             default_value="-1",
             description="Overwrite the number of timesteps.",
-        ))
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "hyperparams",
             default_value="",
             description="Optional RL hyperparameter overwrite (e.g. learning_rate:0.01 train_freq:10).",
-        ))
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "num_threads",
             default_value="-1",
             description="Number of threads for PyTorch (-1 to use default).",
-        ))
+        )
-        # Continue training an already trained agent
+    )
+    # Continue training an already trained agent
     declared_arguments.append(
         DeclareLaunchArgument(
             "trained_agent",
             default_value="",
             description="Path to a pretrained agent to continue training.",
-        ))
+        )
-        # Random seed
+    )
+    # Random seed
     declared_arguments.append(
         DeclareLaunchArgument(
             "seed",
             default_value="-1",
             description="Random generator seed.",
-        ))
+        )
-        # Saving of model
+    )
+    # Saving of model
     declared_arguments.append(
         DeclareLaunchArgument(
             "save_freq",
             default_value="10000",
             description="Save the model every n steps (if negative, no checkpoint).",
-        ))
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "save_replay_buffer",
             default_value="False",
             description="Save the replay buffer too (when applicable).",
-        ))
+        )
-        # Pre-load a replay buffer and start training on it
+    )
+    # Pre-load a replay buffer and start training on it
     declared_arguments.append(
         DeclareLaunchArgument(
             "preload_replay_buffer",
             default_value="",
             description="Path to a replay buffer that should be preloaded before starting the training process.",
-        ))
+        )
-        # Logging
+    )
+    # Logging
     declared_arguments.append(
         DeclareLaunchArgument(
             "log_folder",
             default_value="logs",
             description="Path to the log directory.",
-        ))
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "tensorboard_log",
             default_value="tensorboard_logs",
             description="Tensorboard log dir.",
-        ))
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "log_interval",
             default_value="-1",
             description="Override log interval (default: -1, no change).",
-        ))
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "uuid",
             default_value="False",
             description="Ensure that the run has a unique ID.",
-        ))
+        )
-        # Evaluation
+    )
+    # Evaluation
     declared_arguments.append(
         DeclareLaunchArgument(
             "eval_freq",
             default_value="-1",
             description="Evaluate the agent every n steps (if negative, no evaluation).",
-        ))
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "eval_episodes",
             default_value="5",
             description="Number of episodes to use for evaluation.",
-        ))
+        )
-        # Verbosity
+    )
+    # Verbosity
     declared_arguments.append(
         DeclareLaunchArgument(
             "verbose",
             default_value="1",
             description="Verbose mode (0: no output, 1: INFO).",
-        ))
+        )
-        # HER specifics
+    )
-    declared_arguments.append(
+    # HER specifics
-        DeclareLaunchArgument(
+    (
-            "truncate_last_trajectory",
+        declared_arguments.append(
-            default_value="True",
+            DeclareLaunchArgument(
-            description="When using HER with online sampling the last trajectory in the replay buffer will be truncated after) reloading the replay buffer."
+                "truncate_last_trajectory",
-        )),
+                default_value="True",
+                description="When using HER with online sampling the last trajectory in the replay buffer will be truncated after) reloading the replay buffer.",
+            )
+        ),
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "log_level",
             default_value="error",
             description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ))
+        )
+    )
 
     env_variables = [
         SetEnvironmentVariable(name="OMP_DYNAMIC", value="TRUE"),
-        SetEnvironmentVariable(name="OMP_NUM_THREADS", value=str(cpu_count() // 2))
+        SetEnvironmentVariable(name="OMP_NUM_THREADS", value=str(cpu_count() // 2)),
     ]
 
-    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)] + env_variables)
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)] + env_variables
+    )

env_manager/rbs_gym/rbs_gym/envs/__init__.py

@@ -1,4 +1,13 @@
 # Note: The `open3d` and `stable_baselines3` modules must be imported prior to `gym_gz`
+from env_manager.models.configs import (
+    RobotData,
+    SceneData,
+    SphereObjectData,
+    TerrainData,
+)
+from env_manager.models.configs.objects import ObjectRandomizerData
+from env_manager.models.configs.robot import RobotRandomizerData
+from env_manager.models.configs.scene import PluginsData
 import open3d  # isort:skip
 import stable_baselines3  # isort:skip
 
@@ -13,16 +22,16 @@ except:
     pass
 
 from os import environ, path
-from typing import Dict, Tuple, Any
+from typing import Any, Dict, Tuple
 
 import numpy as np
 from ament_index_python.packages import get_package_share_directory
 from gymnasium.envs.registration import register
+from rbs_assets_library import get_world_file
 
 from rbs_gym.utils.utils import str2bool
 
 from . import tasks
-from dataclasses import dataclass, field
 
 ######################
 # Runtime Entrypoint #
@@ -48,15 +57,15 @@ RBS_ENVS_WORLDS_DIR: str = path.join(get_package_share_directory("rbs_gym"), "wo
 # Presets #
 ###########
 # Gravity preset for Earth
-GRAVITY_EARTH: Tuple[float, float, float] = (0.0, 0.0, -9.80665)
+# GRAVITY_EARTH: Tuple[float, float, float] = (0.0, 0.0, -9.80665)
-GRAVITY_EARTH_STD: Tuple[float, float, float] = (0.0, 0.0, 0.0232)
+# GRAVITY_EARTH_STD: Tuple[float, float, float] = (0.0, 0.0, 0.0232)
 
 ############################
 # Additional Configuration #
 ############################
-BROADCAST_GUI: bool = str2bool(
+# BROADCAST_GUI: bool = str2bool(
-    environ.get("RBS_ENVS_BROADCAST_INTERACTIVE_GUI", default=True)
+#     environ.get("RBS_ENVS_BROADCAST_INTERACTIVE_GUI", default=True)
-)
+# )
 
 
 #########
@@ -84,224 +93,38 @@ REACH_KWARGS: Dict[str, Any] = {
 REACH_KWARGS_SIM: Dict[str, Any] = {
     "physics_rate": 1000.0,
     "real_time_factor": float(np.finfo(np.float32).max),
-    "world": path.join(RBS_ENVS_WORLDS_DIR, "default.sdf"),
+    "world": get_world_file("default"),
 }
 
 
-@dataclass
-class ObjectRandomizerData:
-    count: int = field(default=0)
-    random_pose: bool = field(default=False)
-    random_position: bool = field(default=False)
-    random_orientation: bool = field(default=False)
-    random_model: bool = field(default=False)
-    random_spawn_position_segments: list = field(default_factory=list)
-    random_spawn_position_update_workspace_centre: bool = field(default=False)
-    random_spawn_volume: tuple = field(default=(0.5, 0.5, 0.5))
-    models_rollouts_num: int = field(default=0)
-
-
-@dataclass
-class ObjectData:
-    name: str = field(default="object")
-    relative_to: str = field(default="world")
-    position: tuple = field(default=(0.0, 0.0, 0.0))
-    orientation: tuple = field(default=(1.0, 0.0, 0.0, 0.0))
-    static: bool = field(default=False)
-    randomize: ObjectRandomizerData = field(default_factory=ObjectRandomizerData)
-
-
-@dataclass
-class PrimitiveObjectData(ObjectData):
-    collision: bool = field(default=True)
-    visual: bool = field(default=True)
-    color: tuple = field(default=(0.8, 0.8, 0.8, 1.0))
-    mass: float = field(default=0.1)
-
-
-@dataclass
-class SphereObjectData(PrimitiveObjectData):
-    radius: float = field(default=0.025)
-    friction: float = field(default=1.0)
-
-
-@dataclass
-class PlaneObjectData(PrimitiveObjectData):
-    size: tuple = field(default=(1.0, 1.0))
-    direction: tuple = field(default=(0.0, 0.0, 1.0))
-    friction: float = field(default=1.0)
-
-
-@dataclass
-class CylinderObjectData(PrimitiveObjectData):
-    radius: float = field(default=0.025)
-    length: float = field(default=0.1)
-    friction: float = field(default=1.0)
-
-
-@dataclass
-class BoxObjectData(PrimitiveObjectData):
-    size: tuple = field(default=(0.05, 0.05, 0.05))
-    friction: float = field(default=1.0)
-
-
-@dataclass
-class MeshObjectData(ObjectData):
-    texture: list[float] = field(default_factory=list)
-
-
 REACH_RANDOMIZER: str = "rbs_gym.envs.randomizers:ManipulationGazeboEnvRandomizer"
-REACH_KWARGS_RANDOMIZER: Dict[str, Any] = {
+
-    "gravity": GRAVITY_EARTH,
+SCENE_CONFIGURATION: SceneData = SceneData(
-    "gravity_std": GRAVITY_EARTH_STD,
+    physics_rollouts_num=0,
-    "plugin_scene_broadcaster": BROADCAST_GUI,
+    robot=RobotData(
-    "plugin_user_commands": BROADCAST_GUI,
+        name="rbs_arm",
-    "plugin_sensors_render_engine": "ogre2",
+        joint_positioins=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-    "robot_random_pose": False,
+        with_gripper=True,
-    "robot_random_joint_positions": True,  # FIXME:
+        gripper_jont_positions=[0],
-    "robot_random_joint_positions_std": 0.2,
+        randomizer=RobotRandomizerData(joint_positions=True),
-    "robot_random_joint_positions_above_object_spawn": False,
-    "robot_random_joint_positions_above_object_spawn_elevation": 0.0,
-    "robot_random_joint_positions_above_object_spawn_xy_randomness": 0.2,
-    "terrain_enable": True,
-    "light_type": "sun",
-    "light_direction": (0.5, 0.4, -0.2),
-    "light_random_minmax_elevation": (-0.15, -0.5),
-    "light_distance": 1000.0,
-    "light_visual": False,
-    "light_radius": 25.0,
-    "light_model_rollouts_num": 1,
-    "underworld_collision_plane": False,
-}
-REACH_KWARGS_OBJECT_RANDOMIZER: list = [
-    MeshObjectData(
-        "bishop",
-        "base_link",
-        (-0.3, 0.0, 0.0),
-        randomize=ObjectRandomizerData(
-            random_position=True, random_spawn_volume=(0.2, 0.5, 0.0)
-        ),
     ),
-    SphereObjectData(
+    objects=[
-        "sphere",
+        SphereObjectData(
-        "base_link",
+            name="sphere",
-        (-0.3, 0.0, 0.3),
+            relative_to="base_link",
-        mass=1.0,
+            position=(0.0, 0.4, 1.0),
-        color=(0.0, 0.0, 1.0, 1.0),
+            static=True,
-        # randomize=ObjectRandomizerData(
+            collision=False,
-        #     random_pose=True, random_spawn_volume=(0.2, 0.2, 0.0)
+            color=(0.0, 1.0, 0.0, 0.8),
-        # ),
+            randomize=ObjectRandomizerData(random_pose=True, models_rollouts_num=2),
+        )
+    ],
+    plugins=PluginsData(
+        scene_broadcaster=True, user_commands=True, fts_broadcaster=True
     ),
-]
+)
-# REACH_KWARGS_RANDOMIZER_CAMERA: Dict[str, Any] = {
-#     "camera_enable": True,
-#     "camera_width": 64,
-#     "camera_height": 64,
-#     "camera_update_rate": 1.2 * REACH_KWARGS["agent_rate"],
-#     "camera_horizontal_fov": np.pi / 3.0,
-#     "camera_vertical_fov": np.pi / 3.0,
-#     "camera_noise_mean": 0.0,
-#     "camera_noise_stddev": 0.001,
-#     "camera_relative_to": "base_link",
-#     "camera_spawn_position": (0.85, -0.4, 0.45),
-#     "camera_spawn_quat_xyzw": (-0.0402991, -0.0166924, 0.9230002, 0.3823192),
-#     "camera_random_pose_rollouts_num": 0,
-#     "camera_random_pose_mode": "orbit",
-#     "camera_random_pose_orbit_distance": 1.0,
-#     "camera_random_pose_orbit_height_range": (0.1, 0.7),
-#     "camera_random_pose_orbit_ignore_arc_behind_robot": np.pi / 8,
-#     "camera_random_pose_select_position_options": [],
-#     "camera_random_pose_focal_point_z_offset": 0.0,
-# }
 
 
-@dataclass
-class CameraData:
-    name: str | None = None
-    enable: bool = field(default=True)
-    type: str = field(default="rgbd_camera")
-    relative_to: str = field(default="base_link")
-
-    width: int = field(default=128)
-    height: int = field(default=128)
-    image_format: str = field(default="R8G8B8")
-    update_rate: int = field(default=10)
-    horizontal_fov: float = field(default=np.pi / 3.0)
-    vertical_fov: float = field(default=np.pi / 3.0)
-
-    clip_color: tuple[float, float] = field(default=(0.01, 1000.0))
-    clip_depth: tuple[float, float] = field(default=(0.05, 10.0))
-
-    noise_mean: float | None = None
-    noise_stddev: float | None = None
-
-    publish_color: bool = field(default=False)
-    publish_depth: bool = field(default=False)
-    publish_points: bool = field(default=False)
-
-    spawn_position: tuple[float, float, float] = field(default=(0, 0, 1))
-    spawn_quat_xyzw: tuple[float, float, float, float] = field(
-        default=(0, 0.70710678118, 0, 0.70710678118)
-    )
-
-    random_pose_rollouts_num: int = field(default=1)
-    random_pose_mode: str = field(default="orbit")
-    random_pose_orbit_distance: float = field(default=1.0)
-    random_pose_orbit_height_range: tuple[float, float] = field(default=(0.1, 0.7))
-    random_pose_orbit_ignore_arc_behind_robot: float = field(default=np.pi / 8)
-    random_pose_select_position_options: list[tuple[float, float, float]] = field(
-        default_factory=list
-    )
-    random_pose_focal_point_z_offset: float = field(default=0.0)
-
-
-REACH_KWARGS_RANDOMIZER_CAMERAS: list[CameraData] = [
-    CameraData(
-        name="inner_robot_camera",
-        enable=True,
-        type="rgbd_camera",
-        relative_to="rbs_gripper_rot_base_link",
-        width=320,
-        height=240,
-        update_rate=1.2 * REACH_KWARGS["agent_rate"],
-        horizontal_fov=np.pi / 3.0,
-        vertical_fov=np.pi / 3.0,
-        noise_mean=0.0,
-        publish_color=True,
-        noise_stddev=0.001,
-        spawn_position=(-0.02, 0.0, 0.03),
-        spawn_quat_xyzw=(0.0, -0.707, 0.0, 0.707),
-        random_pose_mode="orbit",
-        random_pose_rollouts_num=0,
-        random_pose_orbit_distance=1.0,
-        random_pose_orbit_height_range=(0.1, 0.7),
-        random_pose_focal_point_z_offset=0.0,
-        random_pose_orbit_ignore_arc_behind_robot=np.pi / 8,
-    ),
-    CameraData(
-        name="outer_robot_camera",
-        enable=True,
-        type="rgbd_camera",
-        relative_to="base_link",
-        width=320,
-        height=240,
-        update_rate=1.2 * REACH_KWARGS["agent_rate"],
-        publish_color=True,
-        horizontal_fov=np.pi / 3.0,
-        vertical_fov=np.pi / 3.0,
-        noise_mean=0.0,
-        noise_stddev=0.001,
-        spawn_position=(-0.2, 1.0, 1.0),
-        spawn_quat_xyzw=(0.183, 0.183, -0.683, 0.683),
-        random_pose_mode="orbit",
-        random_pose_rollouts_num=0,
-        random_pose_orbit_distance=1.0,
-        random_pose_orbit_height_range=(0.1, 0.7),
-        random_pose_focal_point_z_offset=0.0,
-        random_pose_orbit_ignore_arc_behind_robot=np.pi / 8,
-    ),
-]
-
 # Task
 register(
     id="Reach-v0",
@@ -336,12 +159,7 @@ register(
     id="Reach-Gazebo-v0",
     entry_point=REACH_RANDOMIZER,
     max_episode_steps=REACH_MAX_EPISODE_STEPS,
-    kwargs={
+    kwargs={"env": "Reach-v0", **REACH_KWARGS_SIM, "scene_args": SCENE_CONFIGURATION},
-        "env": "Reach-v0",
-        **REACH_KWARGS_SIM,
-        **REACH_KWARGS_RANDOMIZER,
-        "object_args": REACH_KWARGS_OBJECT_RANDOMIZER,
-    },
 )
 register(
     id="Reach-ColorImage-Gazebo-v0",
@@ -350,9 +168,7 @@ register(
     kwargs={
         "env": "Reach-ColorImage-v0",
         **REACH_KWARGS_SIM,
-        **REACH_KWARGS_RANDOMIZER,
+        "scene_args": SCENE_CONFIGURATION,
-        "camera_args": REACH_KWARGS_RANDOMIZER_CAMERAS,
-        "object_args": REACH_KWARGS_OBJECT_RANDOMIZER,
     },
 )
 register(
@@ -362,8 +178,6 @@ register(
     kwargs={
         "env": "Reach-DepthImage-v0",
         **REACH_KWARGS_SIM,
-        **REACH_KWARGS_RANDOMIZER,
+        "scene_args": SCENE_CONFIGURATION,
-        "camera_args": REACH_KWARGS_RANDOMIZER_CAMERAS,
-        "object_args": REACH_KWARGS_OBJECT_RANDOMIZER,
     },
 )

env_manager/rbs_gym/rbs_gym/envs/control/grippper_controller.py

@@ -14,6 +14,7 @@ class GripperController:
         self._open_position = open_position
         self._close_position = close_position
         self._max_effort = max_effort
+        self.is_executed = False
 
     def open(self):
         self.send_goal(self._open_position)
@@ -44,3 +45,6 @@ class GripperController:
         result = future.result().result
         self.get_logger().info(f"Gripper position: {result.position}")
 
+    def wait_until_executed(self):
+        while not self.is_executed:
+            pass

env_manager/rbs_gym/rbs_gym/envs/models/lights/__init__.py

@@ -1,20 +0,0 @@
-from gym_gz.scenario.model_wrapper import ModelWrapper
-
-from .random_sun import RandomSun
-from .sun import Sun
-
-
-def get_light_model_class(light_type: str) -> ModelWrapper:
-    # TODO: Refactor into enum
-
-    if "sun" == light_type:
-        return Sun
-    elif "random_sun" == light_type:
-        return RandomSun
-
-
-def is_light_type_randomizable(light_type: str) -> bool:
-
-    if "random_sun" == light_type:
-        return True
-    return False

env_manager/rbs_gym/rbs_gym/envs/models/robots/__init__.py

@@ -1,17 +0,0 @@
-from gym_gz.scenario.model_wrapper import ModelWrapper
-
-from .rbs_arm import RbsArm
-# from .panda import Panda
-
-# TODO: When adding new a robot, create abstract classes to simplify such process
-
-
-def get_robot_model_class(robot_model: str) -> type[RbsArm]:
-    # TODO: Refactor into enum
-
-    if "rbs_arm" == robot_model:
-        return RbsArm
-    else:
-        return RbsArm
-    # elif "panda" == robot_model:
-    #     return Panda

env_manager/rbs_gym/rbs_gym/envs/models/terrains/__init__.py

@@ -1,29 +0,0 @@
-from gym_gz.scenario.model_wrapper import ModelWrapper
-
-from .ground import Ground
-from .lunar_heightmap import LunarHeightmap
-from .lunar_surface import LunarSurface
-from .random_ground import RandomGround
-from .random_lunar_surface import RandomLunarSurface
-
-
-def get_terrain_model_class(terrain_type: str) -> type[ModelWrapper]:
-    # TODO: Refactor into enum
-
-    if "flat" == terrain_type:
-        return Ground
-    elif "random_flat" == terrain_type:
-        return RandomGround
-    elif "lunar_heightmap" == terrain_type:
-        return LunarHeightmap
-    elif "lunar_surface" == terrain_type:
-        return LunarSurface
-    elif "random_lunar_surface" == terrain_type:
-        return RandomLunarSurface
-    else:
-        raise AttributeError(f"Unsupported terrain [{terrain_type}]")
-
-
-def is_terrain_type_randomizable(terrain_type: str) -> bool:
-
-    return "random_flat" == terrain_type or "random_lunar_surface" == terrain_type

env_manager/rbs_gym/rbs_gym/envs/models/terrains/lunar_heightmap.py

@@ -1,52 +0,0 @@
-from typing import List
-
-from gym_gz.scenario import model_with_file, model_wrapper
-from gym_gz.utils.scenario import get_unique_model_name
-from scenario import core as scenario
-from scenario import gazebo as scenario_gazebo
-
-
-class LunarHeightmap(model_wrapper.ModelWrapper, model_with_file.ModelWithFile):
-    def __init__(
-        self,
-        world: scenario.World,
-        name: str = "lunar_heightmap",
-        position: List[float] = (0, 0, 0),
-        orientation: List[float] = (1, 0, 0, 0),
-        model_file: str = None,
-        use_fuel: bool = False,
-        **kwargs,
-    ):
-
-        # Allow passing of custom model file as an argument
-        if model_file is None:
-            model_file = self.get_model_file(fuel=use_fuel)
-
-        # Get a unique model name
-        model_name = get_unique_model_name(world, name)
-
-        # Setup initial pose
-        initial_pose = scenario.Pose(position, orientation)
-
-        # Insert the model
-        ok_model = world.to_gazebo().insert_model_from_file(
-            model_file, initial_pose, model_name
-        )
-        if not ok_model:
-            raise RuntimeError("Failed to insert " + model_name)
-
-        # Get the model
-        model = world.get_model(model_name)
-
-        # Initialize base class
-        super().__init__(model=model)
-
-    @classmethod
-    def get_model_file(self, fuel: bool = False) -> str:
-        if fuel:
-            raise NotImplementedError
-            return scenario_gazebo.get_model_file_from_fuel(
-                "https://fuel.ignitionrobotics.org/1.0/AndrejOrsula/models/lunar_heightmap"
-            )
-        else:
-            return "lunar_heightmap"

env_manager/rbs_gym/rbs_gym/envs/models/terrains/lunar_surface.py

@@ -1,53 +0,0 @@
-from typing import List
-
-from gym_gz.scenario import model_with_file, model_wrapper
-from gym_gz.utils.scenario import get_unique_model_name
-from scenario import core as scenario
-from scenario import gazebo as scenario_gazebo
-
-
-class LunarSurface(model_wrapper.ModelWrapper, model_with_file.ModelWithFile):
-    def __init__(
-        self,
-        world: scenario.World,
-        name: str = "lunar_surface",
-        position: List[float] = (0, 0, 0),
-        orientation: List[float] = (1, 0, 0, 0),
-        model_file: str = None,
-        use_fuel: bool = False,
-        variant: str = "tycho",
-        **kwargs,
-    ):
-
-        # Allow passing of custom model file as an argument
-        if model_file is None:
-            model_file = self.get_model_file(fuel=use_fuel, variant=variant)
-
-        # Get a unique model name
-        model_name = get_unique_model_name(world, name)
-
-        # Setup initial pose
-        initial_pose = scenario.Pose(position, orientation)
-
-        # Insert the model
-        ok_model = world.to_gazebo().insert_model_from_file(
-            model_file, initial_pose, model_name
-        )
-        if not ok_model:
-            raise RuntimeError("Failed to insert " + model_name)
-
-        # Get the model
-        model = world.get_model(model_name)
-
-        # Initialize base class
-        super().__init__(model=model)
-
-    @classmethod
-    def get_model_file(self, fuel: bool = False, variant: str = "tycho") -> str:
-        if fuel:
-            raise NotImplementedError
-            return scenario_gazebo.get_model_file_from_fuel(
-                f"https://fuel.ignitionrobotics.org/1.0/AndrejOrsula/models/lunar_surface_{variant}"
-            )
-        else:
-            return f"lunar_surface_{variant}"

env_manager/rbs_gym/rbs_gym/envs/models/terrains/random_ground.py

@@ -1,135 +0,0 @@
-import os
-from typing import List, Optional
-
-import numpy as np
-from gym_gz.scenario import model_wrapper
-from gym_gz.utils.scenario import get_unique_model_name
-from numpy.random import RandomState
-from scenario import core as scenario
-
-
-class RandomGround(model_wrapper.ModelWrapper):
-    def __init__(
-        self,
-        world: scenario.World,
-        name: str = "random_ground",
-        position: List[float] = (0, 0, 0),
-        orientation: List[float] = (1, 0, 0, 0),
-        size: List[float] = (1.0, 1.0),
-        collision_thickness: float = 0.05,
-        friction: float = 5.0,
-        texture_dir: Optional[str] = None,
-        np_random: Optional[RandomState] = None,
-        **kwargs,
-    ):
-
-        if np_random is None:
-            np_random = np.random.default_rng()
-
-        # Get a unique model name
-        model_name = get_unique_model_name(world, name)
-
-        # Initial pose
-        initial_pose = scenario.Pose(position, orientation)
-
-        # Get textures from ENV variable if not directly specified
-        if not texture_dir:
-            texture_dir = os.environ.get("TEXTURE_DIRS", default="")
-
-        # Find random PBR texture
-        albedo_map = None
-        normal_map = None
-        roughness_map = None
-        metalness_map = None
-        if texture_dir:
-            if ":" in texture_dir:
-                textures = []
-                for d in texture_dir.split(":"):
-                    textures.extend([os.path.join(d, f) for f in os.listdir(d)])
-            else:
-                # Get list of the available textures
-                textures = os.listdir(texture_dir)
-
-            # Choose a random texture from these
-            random_texture_dir = str(np_random.choice(textures))
-
-            # List all files
-            texture_files = os.listdir(random_texture_dir)
-
-            # Extract the appropriate files
-            for texture in texture_files:
-                texture_lower = texture.lower()
-                if "color" in texture_lower or "albedo" in texture_lower:
-                    albedo_map = os.path.join(random_texture_dir, texture)
-                elif "normal" in texture_lower:
-                    normal_map = os.path.join(random_texture_dir, texture)
-                elif "roughness" in texture_lower:
-                    roughness_map = os.path.join(random_texture_dir, texture)
-                elif "specular" in texture_lower or "metalness" in texture_lower:
-                    metalness_map = os.path.join(random_texture_dir, texture)
-
-        # Create SDF string for the model
-        sdf = f"""<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/rbs_gym/rbs_gym/envs/models/terrains/random_lunar_surface.py

@@ -1,57 +0,0 @@
-import os
-from typing import List, Optional, Tuple
-
-import numpy as np
-from gym_gz.scenario import model_wrapper
-from gym_gz.utils.scenario import get_unique_model_name
-from numpy.random import RandomState
-from scenario import core as scenario
-
-
-class RandomLunarSurface(model_wrapper.ModelWrapper):
-    def __init__(
-        self,
-        world: scenario.World,
-        name: str = "lunar_surface",
-        position: List[float] = (0, 0, 0),
-        orientation: List[float] = (1, 0, 0, 0),
-        models_dir: Optional[str] = None,
-        np_random: Optional[RandomState] = None,
-        **kwargs,
-    ):
-
-        if np_random is None:
-            np_random = np.random.default_rng()
-
-        # Get a unique model name
-        model_name = get_unique_model_name(world, name)
-
-        # Setup initial pose
-        initial_pose = scenario.Pose(position, orientation)
-
-        # Get path to all lunar surface models
-        if not models_dir:
-            models_dir = os.environ.get("SDF_PATH_LUNAR_SURFACE", default="")
-
-        # Make sure the path exists
-        if not os.path.exists(models_dir):
-            raise ValueError(
-                f"Invalid path '{models_dir}' pointed by 'SDF_PATH_LUNAR_SURFACE' environment variable."
-            )
-
-        # Select a single model at random
-        model_dir = np_random.choice(os.listdir(models_dir))
-        sdf_filepath = os.path.join(model_dir, "model.sdf")
-
-        # Insert the model
-        ok_model = world.to_gazebo().insert_model_from_file(
-            sdf_filepath, initial_pose, model_name
-        )
-        if not ok_model:
-            raise RuntimeError("Failed to insert " + model_name)
-
-        # Get the model
-        model = world.get_model(model_name)
-
-        # Initialize base class
-        model_wrapper.ModelWrapper.__init__(self, model=model)

env_manager/rbs_gym/rbs_gym/envs/tasks/__init__.py

@@ -1,4 +1,4 @@
-from .curriculums import *
+# from .curriculums import *
 # from .grasp import *
 # from .grasp_planetary import *
 from .reach import *

env_manager/rbs_gym/rbs_gym/envs/tasks/manipulation.py

@@ -3,11 +3,29 @@ import multiprocessing
 import sys
 from itertools import count
 from threading import Thread
-from typing import Dict, Optional, Tuple, Union
+from typing import Any, Dict, Optional, Tuple, Union
 
 import numpy as np
 import rclpy
+from env_manager.models.configs import RobotData
+from env_manager.models.robots import RobotWrapper, get_robot_model_class
+from env_manager.scene import Scene
+from env_manager.utils import Tf2Broadcaster, Tf2Listener
+from env_manager.utils.conversions import orientation_6d_to_quat
+from env_manager.utils.gazebo import (
+    Point,
+    Pose,
+    Quat,
+    get_model_orientation,
+    get_model_pose,
+    get_model_position,
+    transform_change_reference_frame_orientation,
+    transform_change_reference_frame_pose,
+    transform_change_reference_frame_position,
+)
+from env_manager.utils.math import quat_mul
 from gym_gz.base.task import Task
+from gym_gz.scenario.model_wrapper import ModelWrapper
 from gym_gz.utils.typing import (
     Action,
     ActionSpace,
@@ -18,14 +36,14 @@ from gym_gz.utils.typing import (
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
 from rclpy.node import Node
+from robot_builder.elements.robot import Robot
 from scipy.spatial.transform import Rotation
 
-from rbs_gym.envs.control import *
+from rbs_gym.envs.control import (
-from rbs_gym.envs.models.robots import get_robot_model_class
+    CartesianForceController,
-from rbs_gym.envs.utils import Tf2Broadcaster, Tf2Listener
+    GripperController,
-from rbs_gym.envs.utils.conversions import orientation_6d_to_quat
+    JointEffortController,
-from rbs_gym.envs.utils.gazebo import *
+)
-from rbs_gym.envs.utils.math import quat_mul
 
 
 class Manipulation(Task, Node, abc.ABC):
@@ -47,7 +65,6 @@ class Manipulation(Task, Node, abc.ABC):
         num_threads: int,
         **kwargs,
     ):
-
         # Get next ID for this task instance
         self.id = next(self._ids)
 
@@ -64,6 +81,8 @@ class Manipulation(Task, Node, abc.ABC):
         # Initialize ROS 2 Node base class
         Node.__init__(self, f"rbs_gym_{self.id}")
 
+        self._allow_undeclared_parameters = True
+        self.declare_parameter("robot_description", "")
         # Create callback group that allows execution of callbacks in parallel without restrictions
         self._callback_group = ReentrantCallbackGroup()
 
@@ -85,92 +104,92 @@ class Manipulation(Task, Node, abc.ABC):
         self._executor_thread.start()
 
         # Get class of the robot model based on passed argument
-        self.robot_model_class = get_robot_model_class(robot_model)
+        # self.robot_model_class = get_robot_model_class(robot_model)
 
         # Store passed arguments for later use
-        self.workspace_centre = (
+        # self.workspace_centre = (
-            workspace_centre[0],
+        #     workspace_centre[0],
-            workspace_centre[1],
+        #     workspace_centre[1],
-            workspace_centre[2] + self.robot_model_class.BASE_LINK_Z_OFFSET,
+        #     workspace_centre[2] + self.robot_model_class.BASE_LINK_Z_OFFSET,
-        )
+        # )
-        self.workspace_volume = workspace_volume
+        # self.workspace_volume = workspace_volume
-        self._restrict_position_goal_to_workspace = restrict_position_goal_to_workspace
+        # self._restrict_position_goal_to_workspace = restrict_position_goal_to_workspace
-        self._use_servo = use_servo
+        # self._use_servo = use_servo
-        self.__scaling_factor_translation = scaling_factor_translation
+        # self.__scaling_factor_translation = scaling_factor_translation
-        self.__scaling_factor_rotation = scaling_factor_rotation
+        # self.__scaling_factor_rotation = scaling_factor_rotation
         self._enable_gripper = enable_gripper
-
+        self._scene: Scene | None = None
-        # Get workspace bounds, useful is many computations
+        #
-        workspace_volume_half = (
+        # # Get workspace bounds, useful is many computations
-            workspace_volume[0] / 2,
+        # workspace_volume_half = (
-            workspace_volume[1] / 2,
+        #     workspace_volume[0] / 2,
-            workspace_volume[2] / 2,
+        #     workspace_volume[1] / 2,
-        )
+        #     workspace_volume[2] / 2,
-        self.workspace_min_bound = (
+        # )
-            self.workspace_centre[0] - workspace_volume_half[0],
+        # self.workspace_min_bound = (
-            self.workspace_centre[1] - workspace_volume_half[1],
+        #     self.workspace_centre[0] - workspace_volume_half[0],
-            self.workspace_centre[2] - workspace_volume_half[2],
+        #     self.workspace_centre[1] - workspace_volume_half[1],
-        )
+        #     self.workspace_centre[2] - workspace_volume_half[2],
-        self.workspace_max_bound = (
+        # )
-            self.workspace_centre[0] + workspace_volume_half[0],
+        # self.workspace_max_bound = (
-            self.workspace_centre[1] + workspace_volume_half[1],
+        #     self.workspace_centre[0] + workspace_volume_half[0],
-            self.workspace_centre[2] + workspace_volume_half[2],
+        #     self.workspace_centre[1] + workspace_volume_half[1],
-        )
+        #     self.workspace_centre[2] + workspace_volume_half[2],
+        # )
 
         # Determine robot name and prefix based on current ID of the task
-        self.robot_prefix: str = self.robot_model_class.DEFAULT_PREFIX
+        # self.robot_prefix: str = self.robot_model_class.DEFAULT_PREFIX
-        if 0 == self.id:
+        # if 0 == self.id:
-            self.robot_name = self.robot_model_class.ROBOT_MODEL_NAME
+        #     self.robot_name = self.robot_model_class.ROBOT_MODEL_NAME
-        else:
+        # else:
-            self.robot_name = f"{self.robot_model_class.ROBOT_MODEL_NAME}{self.id}"
+        #     self.robot_name = f"{self.robot_model_class.ROBOT_MODEL_NAME}{self.id}"
-            if self.robot_prefix.endswith("_"):
+        #     if self.robot_prefix.endswith("_"):
-                self.robot_prefix = f"{self.robot_prefix[:-1]}{self.id}_"
+        #         self.robot_prefix = f"{self.robot_prefix[:-1]}{self.id}_"
-            elif self.robot_prefix == "":
+        #     elif self.robot_prefix == "":
-                self.robot_prefix = f"robot{self.id}_"
+        #         self.robot_prefix = f"robot{self.id}_"
 
         # Names of specific robot links, useful all around the code
-        self.robot_base_link_name = self.robot_model_class.get_robot_base_link_name(
+        # self.robot_base_link_name = self.robot_model_class.get_robot_base_link_name(
-            self.robot_prefix
+        #     self.robot_prefix
-        )
+        # )
-        self.robot_arm_base_link_name = self.robot_model_class.get_arm_base_link_name(
+        # self.robot_arm_base_link_name = self.robot_model_class.get_arm_base_link_name(
-            self.robot_prefix
+        #     self.robot_prefix
-        )
+        # )
-        self.robot_ee_link_name = self.robot_model_class.get_ee_link_name(
+        # self.robot_ee_link_name = self.robot_model_class.get_ee_link_name(
-            self.robot_prefix
+        #     self.robot_prefix
-        )
+        # )
-        self.robot_arm_link_names = self.robot_model_class.get_arm_link_names(
+        # self.robot_arm_link_names = self.robot_model_class.get_arm_link_names(
-            self.robot_prefix
+        #     self.robot_prefix
-        )
+        # )
-        self.robot_gripper_link_names = self.robot_model_class.get_gripper_link_names(
+        # self.robot_gripper_link_names = self.robot_model_class.get_gripper_link_names(
-            self.robot_prefix
+        #     self.robot_prefix
-        )
+        # )
-        self.robot_arm_joint_names = self.robot_model_class.get_arm_joint_names(
+        # self.robot_arm_joint_names = self.robot_model_class.get_arm_joint_names(
-            self.robot_prefix
+        #     self.robot_prefix
-        )
+        # )
-        self.robot_gripper_joint_names = self.robot_model_class.get_gripper_joint_names(
+        # self.robot_gripper_joint_names = self.robot_model_class.get_gripper_joint_names(
-            self.robot_prefix
+        #     self.robot_prefix
-        )
+        # )
 
         # Get exact name substitution of the frame for workspace
         self.workspace_frame_id = self.substitute_special_frame(workspace_frame_id)
 
         # Specify initial positions (default configuration is used here)
-        self.initial_arm_joint_positions = (
+        # self.initial_arm_joint_positions = (
-            self.robot_model_class.DEFAULT_ARM_JOINT_POSITIONS
+        #     self.robot_model_class.DEFAULT_ARM_JOINT_POSITIONS
-        )
+        # )
-        self.initial_gripper_joint_positions = (
+        # self.initial_gripper_joint_positions = (
-            self.robot_model_class.DEFAULT_GRIPPER_JOINT_POSITIONS
+        #     self.robot_model_class.DEFAULT_GRIPPER_JOINT_POSITIONS
-        )
+        # )
 
         # Names of important models (in addition to robot model)
         self.terrain_name = "terrain"
-        self.object_names = []
 
         # Setup listener and broadcaster of transforms via tf2
         self.tf2_listener = Tf2Listener(node=self)
         self.tf2_broadcaster = Tf2Broadcaster(node=self)
 
-        self.cartesian_control = True #TODO: make it as an external parameter
+        self.cartesian_control = True  # TODO: make it as an external parameter
 
         # Setup controllers
         self.controller = CartesianForceController(self)
@@ -181,195 +200,181 @@ class Manipulation(Task, Node, abc.ABC):
 
         # Initialize task and randomizer overrides (e.g. from curriculum)
         # Both of these are consumed at the beginning of reset
-        self.__task_parameter_overrides: Dict[str, any] = {}
+        self.__task_parameter_overrides: dict[str, Any] = {}
-        self._randomizer_parameter_overrides: Dict[str, any] = {}
+        self._randomizer_parameter_overrides: dict[str, Any] = {}
 
     def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]:
-
         action_space = self.create_action_space()
         observation_space = self.create_observation_space()
 
         return action_space, observation_space
 
     def create_action_space(self) -> ActionSpace:
-
         raise NotImplementedError()
 
     def create_observation_space(self) -> ObservationSpace:
-
         raise NotImplementedError()
 
     def set_action(self, action: Action):
-
         raise NotImplementedError()
 
     def get_observation(self) -> Observation:
-
         raise NotImplementedError()
 
     def get_reward(self) -> Reward:
-
         raise NotImplementedError()
 
     def is_done(self) -> bool:
-
         raise NotImplementedError()
 
     def reset_task(self):
-
         self.__consume_parameter_overrides()
 
-    # Helper functions #
+    # # Helper functions #
-    def get_relative_ee_position(
+    # def get_relative_ee_position(
-        self, translation: Tuple[float, float, float]
+    #     self, translation: Tuple[float, float, float]
-    ) -> Tuple[float, float, float]:
+    # ) -> Tuple[float, float, float]:
+    #     # Scale relative action to metric units
+    #     translation = self.scale_relative_translation(translation)
+    #     # Get current position
+    #     current_position = self.get_ee_position()
+    #     # Compute target position
+    #     target_position = (
+    #         current_position[0] + translation[0],
+    #         current_position[1] + translation[1],
+    #         current_position[2] + translation[2],
+    #     )
+    #
+    #     # Restrict target position to a limited workspace, if desired
+    #     if self._restrict_position_goal_to_workspace:
+    #         target_position = self.restrict_position_goal_to_workspace(target_position)
+    #
+    #     return target_position
+    #
+    # def get_relative_ee_orientation(
+    #     self,
+    #     rotation: Union[
+    #         float,
+    #         Tuple[float, float, float, float],
+    #         Tuple[float, float, float, float, float, float],
+    #     ],
+    #     representation: str = "quat",
+    # ) -> Tuple[float, float, float, float]:
+    #     # Get current orientation
+    #     current_quat_xyzw = self.get_ee_orientation()
+    #
+    #     # For 'z' representation, result should always point down
+    #     # Therefore, create a new quatertnion that contains only yaw component
+    #     if "z" == representation:
+    #         current_yaw = Rotation.from_quat(current_quat_xyzw).as_euler("xyz")[2]
+    #         current_quat_xyzw = Rotation.from_euler(
+    #             "xyz", [np.pi, 0, current_yaw]
+    #         ).as_quat()
+    #
+    #     # Convert relative orientation representation to quaternion
+    #     relative_quat_xyzw = None
+    #     if "quat" == representation:
+    #         relative_quat_xyzw = rotation
+    #     elif "6d" == representation:
+    #         vectors = tuple(
+    #             rotation[x : x + 3] for x, _ in enumerate(rotation) if x % 3 == 0
+    #         )
+    #         relative_quat_xyzw = orientation_6d_to_quat(vectors[0], vectors[1])
+    #     elif "z" == representation:
+    #         rotation = self.scale_relative_rotation(rotation)
+    #         relative_quat_xyzw = Rotation.from_euler("xyz", [0, 0, rotation]).as_quat()
+    #
+    #     # Compute target position (combine quaternions)
+    #     target_quat_xyzw = quat_mul(current_quat_xyzw, relative_quat_xyzw)
+    #
+    #     # Normalise quaternion (should not be needed, but just to be safe)
+    #     target_quat_xyzw /= np.linalg.norm(target_quat_xyzw)
+    #
+    #     return target_quat_xyzw
 
-        # Scale relative action to metric units
+    # def scale_relative_translation(
-        translation = self.scale_relative_translation(translation)
+    #     self, translation: Tuple[float, float, float]
-        # Get current position
+    # ) -> Tuple[float, float, float]:
-        current_position = self.get_ee_position()
+    #     return (
-        # Compute target position
+    #         self.__scaling_factor_translation * translation[0],
-        target_position = (
+    #         self.__scaling_factor_translation * translation[1],
-            current_position[0] + translation[0],
+    #         self.__scaling_factor_translation * translation[2],
-            current_position[1] + translation[1],
+    #     )
-            current_position[2] + translation[2],
-        )
 
-        # Restrict target position to a limited workspace, if desired
+    # def scale_relative_rotation(
-        if self._restrict_position_goal_to_workspace:
+    #     self,
-            target_position = self.restrict_position_goal_to_workspace(target_position)
+    #     rotation: Union[float, Tuple[float, float, float], np.floating, np.ndarray],
+    # ) -> float:
+    #     if not hasattr(rotation, "__len__"):
+    #         return self.__scaling_factor_rotation * rotation
+    #     else:
+    #         return (
+    #             self.__scaling_factor_rotation * rotation[0],
+    #             self.__scaling_factor_rotation * rotation[1],
+    #             self.__scaling_factor_rotation * rotation[2],
+    #         )
+    #
+    # def restrict_position_goal_to_workspace(
+    #     self, position: Tuple[float, float, float]
+    # ) -> Tuple[float, float, float]:
+    #     return (
+    #         min(
+    #             self.workspace_max_bound[0],
+    #             max(
+    #                 self.workspace_min_bound[0],
+    #                 position[0],
+    #             ),
+    #         ),
+    #         min(
+    #             self.workspace_max_bound[1],
+    #             max(
+    #                 self.workspace_min_bound[1],
+    #                 position[1],
+    #             ),
+    #         ),
+    #         min(
+    #             self.workspace_max_bound[2],
+    #             max(
+    #                 self.workspace_min_bound[2],
+    #                 position[2],
+    #             ),
+    #         ),
+    #     )
 
-        return target_position
+    # def restrict_servo_translation_to_workspace(
-
+    #     self, translation: tuple[float, float, float]
-    def get_relative_ee_orientation(
+    # ) -> tuple[float, float, float]:
-        self,
+    #     current_ee_position = self.get_ee_position()
-        rotation: Union[
+    #
-            float,
+    #     translation = tuple(
-            Tuple[float, float, float, float],
+    #         0.0
-            Tuple[float, float, float, float, float, float],
+    #         if (
-        ],
+    #             current_ee_position[i] > self.workspace_max_bound[i]
-        representation: str = "quat",
+    #             and translation[i] > 0.0
-    ) -> Tuple[float, float, float, float]:
+    #         )
-
+    #         or (
-        # Get current orientation
+    #             current_ee_position[i] < self.workspace_min_bound[i]
-        current_quat_xyzw = self.get_ee_orientation()
+    #             and translation[i] < 0.0
-
+    #         )
-        # For 'z' representation, result should always point down
+    #         else translation[i]
-        # Therefore, create a new quatertnion that contains only yaw component
+    #         for i in range(3)
-        if "z" == representation:
+    #     )
-            current_yaw = Rotation.from_quat(current_quat_xyzw).as_euler("xyz")[2]
+    #
-            current_quat_xyzw = Rotation.from_euler(
+    #     return translation
-                "xyz", [np.pi, 0, current_yaw]
-            ).as_quat()
-
-        # Convert relative orientation representation to quaternion
-        relative_quat_xyzw = None
-        if "quat" == representation:
-            relative_quat_xyzw = rotation
-        elif "6d" == representation:
-            vectors = tuple(
-                rotation[x : x + 3] for x, _ in enumerate(rotation) if x % 3 == 0
-            )
-            relative_quat_xyzw = orientation_6d_to_quat(vectors[0], vectors[1])
-        elif "z" == representation:
-            rotation = self.scale_relative_rotation(rotation)
-            relative_quat_xyzw = Rotation.from_euler("xyz", [0, 0, rotation]).as_quat()
-
-        # Compute target position (combine quaternions)
-        target_quat_xyzw = quat_mul(current_quat_xyzw, relative_quat_xyzw)
-
-        # Normalise quaternion (should not be needed, but just to be safe)
-        target_quat_xyzw /= np.linalg.norm(target_quat_xyzw)
-
-        return target_quat_xyzw
-
-    def scale_relative_translation(
-        self, translation: Tuple[float, float, float]
-    ) -> Tuple[float, float, float]:
-
-        return (
-            self.__scaling_factor_translation * translation[0],
-            self.__scaling_factor_translation * translation[1],
-            self.__scaling_factor_translation * translation[2],
-        )
-
-    def scale_relative_rotation(
-        self,
-        rotation: Union[float, Tuple[float, float, float], np.floating, np.ndarray],
-    ) -> float:
-
-        if not hasattr(rotation, "__len__"):
-            return self.__scaling_factor_rotation * rotation
-        else:
-            return (
-                self.__scaling_factor_rotation * rotation[0],
-                self.__scaling_factor_rotation * rotation[1],
-                self.__scaling_factor_rotation * rotation[2],
-            )
-
-    def restrict_position_goal_to_workspace(
-        self, position: Tuple[float, float, float]
-    ) -> Tuple[float, float, float]:
-
-        return (
-            min(
-                self.workspace_max_bound[0],
-                max(
-                    self.workspace_min_bound[0],
-                    position[0],
-                ),
-            ),
-            min(
-                self.workspace_max_bound[1],
-                max(
-                    self.workspace_min_bound[1],
-                    position[1],
-                ),
-            ),
-            min(
-                self.workspace_max_bound[2],
-                max(
-                    self.workspace_min_bound[2],
-                    position[2],
-                ),
-            ),
-        )
-
-    def restrict_servo_translation_to_workspace(
-        self, translation: Tuple[float, float, float]
-    ) -> Tuple[float, float, float]:
-
-        current_ee_position = self.get_ee_position()
-
-        translation = tuple(
-            0.0
-            if (
-                current_ee_position[i] > self.workspace_max_bound[i]
-                and translation[i] > 0.0
-            )
-            or (
-                current_ee_position[i] < self.workspace_min_bound[i]
-                and translation[i] < 0.0
-            )
-            else translation[i]
-            for i in range(3)
-        )
-
-        return translation
 
     def get_ee_pose(
         self,
-    ) -> Optional[Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]]:
+    ) -> Pose:
         """
         Return the current pose of the end effector with respect to arm base link.
         """
 
         try:
-            robot_model = self.world.to_gazebo().get_model(self.robot_name).to_gazebo()
+            robot_model = self.world.to_gazebo().get_model(self.robot.name).to_gazebo()
             ee_position, ee_quat_xyzw = get_model_pose(
                 world=self.world,
                 model=robot_model,
-                link=self.robot_ee_link_name,
+                link=self.robot.ee_link,
                 xyzw=True,
             )
             return transform_change_reference_frame_pose(
@@ -377,7 +382,7 @@ class Manipulation(Task, Node, abc.ABC):
                 position=ee_position,
                 quat=ee_quat_xyzw,
                 target_model=robot_model,
-                target_link=self.robot_arm_base_link_name,
+                target_link=self.robot.base_link,
                 xyzw=True,
             )
         except Exception as e:
@@ -385,8 +390,8 @@ class Manipulation(Task, Node, abc.ABC):
                 f"Cannot get end effector pose from Gazebo ({e}), using tf2..."
             )
             transform = self.tf2_listener.lookup_transform_sync(
-                source_frame=self.robot_ee_link_name,
+                source_frame=self.robot.ee_link,
-                target_frame=self.robot_arm_base_link_name,
+                target_frame=self.robot.base_link,
                 retry=False,
             )
             if transform is not None:
@@ -412,31 +417,31 @@ class Manipulation(Task, Node, abc.ABC):
                     (0.0, 0.0, 0.0, 1.0),
                 )
 
-    def get_ee_position(self) -> Tuple[float, float, float]:
+    def get_ee_position(self) -> Point:
         """
         Return the current position of the end effector with respect to arm base link.
         """
 
         try:
-            robot_model = self.world.to_gazebo().get_model(self.robot_name).to_gazebo()
+            robot_model = self.robot_wrapper
             ee_position = get_model_position(
                 world=self.world,
                 model=robot_model,
-                link=self.robot_ee_link_name,
+                link=self.robot.ee_link,
             )
             return transform_change_reference_frame_position(
                 world=self.world,
                 position=ee_position,
                 target_model=robot_model,
-                target_link=self.robot_arm_base_link_name,
+                target_link=self.robot.base_link,
             )
         except Exception as e:
             self.get_logger().debug(
                 f"Cannot get end effector position from Gazebo ({e}), using tf2..."
             )
             transform = self.tf2_listener.lookup_transform_sync(
-                source_frame=self.robot_ee_link_name,
+                source_frame=self.robot.ee_link,
-                target_frame=self.robot_arm_base_link_name,
+                target_frame=self.robot.base_link,
                 retry=False,
             )
             if transform is not None:
@@ -451,24 +456,24 @@ class Manipulation(Task, Node, abc.ABC):
                 )
                 return (0.0, 0.0, 0.0)
 
-    def get_ee_orientation(self) -> Tuple[float, float, float, float]:
+    def get_ee_orientation(self) -> Quat:
         """
         Return the current xyzw quaternion of the end effector with respect to arm base link.
         """
 
         try:
-            robot_model = self.world.to_gazebo().get_model(self.robot_name).to_gazebo()
+            robot_model = self.robot_wrapper
             ee_quat_xyzw = get_model_orientation(
                 world=self.world,
                 model=robot_model,
-                link=self.robot_ee_link_name,
+                link=self.robot.ee_link,
                 xyzw=True,
             )
             return transform_change_reference_frame_orientation(
                 world=self.world,
                 quat=ee_quat_xyzw,
                 target_model=robot_model,
-                target_link=self.robot_arm_base_link_name,
+                target_link=self.robot.base_link,
                 xyzw=True,
             )
         except Exception as e:
@@ -476,8 +481,8 @@ class Manipulation(Task, Node, abc.ABC):
                 f"Cannot get end effector orientation from Gazebo ({e}), using tf2..."
             )
             transform = self.tf2_listener.lookup_transform_sync(
-                source_frame=self.robot_ee_link_name,
+                source_frame=self.robot.ee_link,
-                target_frame=self.robot_arm_base_link_name,
+                target_frame=self.robot.base_link,
                 retry=False,
             )
             if transform is not None:
@@ -494,8 +499,8 @@ class Manipulation(Task, Node, abc.ABC):
                 return (0.0, 0.0, 0.0, 1.0)
 
     def get_object_position(
-        self, object_model: Union[ModelWrapper, str]
+        self, object_model: ModelWrapper | str
-    ) -> Tuple[float, float, float]:
+    ) -> Point:
         """
         Return the current position of an object with respect to arm base link.
         Note: Only simulated objects are currently supported.
@@ -509,8 +514,8 @@ class Manipulation(Task, Node, abc.ABC):
             return transform_change_reference_frame_position(
                 world=self.world,
                 position=object_position,
-                target_model=self.robot_name,
+                target_model=self.robot_wrapper.name(),
-                target_link=self.robot_arm_base_link_name,
+                target_link=self.robot.base_link,
             )
         except Exception as e:
             self.get_logger().error(
@@ -518,7 +523,7 @@ class Manipulation(Task, Node, abc.ABC):
             )
             return (0.0, 0.0, 0.0)
 
-    def get_object_positions(self) -> Dict[str, Tuple[float, float, float]]:
+    def get_object_positions(self) -> dict[str, Point]:
         """
         Return the current position of all objects with respect to arm base link.
         Note: Only simulated objects are currently supported.
@@ -527,22 +532,20 @@ class Manipulation(Task, Node, abc.ABC):
         object_positions = {}
 
         try:
-            robot_model = self.world.to_gazebo().get_model(self.robot_name).to_gazebo()
+            robot_model = self.robot_wrapper
-            robot_arm_base_link = robot_model.get_link(
+            robot_arm_base_link = robot_model.get_link(link_name=self.robot.base_link)
-                link_name=self.robot_arm_base_link_name
+            for object_name in self.scene.__objects_unique_names:
-            )
-            for object_name in self.object_names:
                 object_position = get_model_position(
                     world=self.world,
                     model=object_name,
                 )
-                object_positions[
+                object_positions[object_name] = (
-                    object_name
+                    transform_change_reference_frame_position(
-                ] = transform_change_reference_frame_position(
+                        world=self.world,
-                    world=self.world,
+                        position=object_position,
-                    position=object_position,
+                        target_model=robot_model,
-                    target_model=robot_model,
+                        target_link=robot_arm_base_link,
-                    target_link=robot_arm_base_link,
+                    )
                 )
         except Exception as e:
             self.get_logger().error(
@@ -552,30 +555,30 @@ class Manipulation(Task, Node, abc.ABC):
         return object_positions
 
     def substitute_special_frame(self, frame_id: str) -> str:
-
         if "arm_base_link" == frame_id:
-            return self.robot_arm_base_link_name
+            return self.robot.base_link
         elif "base_link" == frame_id:
-            return self.robot_base_link_name
+            return self.robot.base_link
         elif "end_effector" == frame_id:
-            return self.robot_ee_link_name
+            return self.robot.ee_link
         elif "world" == frame_id:
             try:
                 # In Gazebo, where multiple worlds are allowed
                 return self.world.to_gazebo().name()
             except Exception as e:
-                self.get_logger().warn(f"")
+                self.get_logger().warn(f"{e}")
                 # Otherwise (e.g. real world)
                 return "rbs_gym_world"
         else:
             return frame_id
 
     def wait_until_action_executed(self):
-
         if self._enable_gripper:
+            #FIXME: code is not tested
             self.gripper.wait_until_executed()
 
     def move_to_initial_joint_configuration(self):
+        #TODO: Write this code for cartesian_control
         pass
 
         # self.moveit2.move_to_configuration(self.initial_arm_joint_positions)
@@ -592,69 +595,66 @@ class Manipulation(Task, Node, abc.ABC):
         """
         Returns true if robot links are in collision with the ground.
         """
-        robot_name_len = len(self.robot_name)
+        robot_name_len = len(self.robot.name)
         terrain_model = self.world.get_model(self.terrain_name)
 
         for contact in terrain_model.contacts():
             body_b = contact.body_b
-            
-            if body_b.startswith(self.robot_name) and len(body_b) > robot_name_len:
-                link = body_b[robot_name_len + 2:]
 
-                if link != self.robot_base_link_name and (
+            if body_b.startswith(self.robot.name) and len(body_b) > robot_name_len:
-                    link in self.robot_arm_link_names or link in self.robot_gripper_link_names
+                link = body_b[robot_name_len + 2 :]
+
+                if link != self.robot.base_link and (
+                    link in self.robot.actuated_joint_names
+                    or link in self.robot.gripper_actuated_joint_names
                 ):
                     return True
 
         return False
 
-    def check_all_objects_outside_workspace(
+    # def check_all_objects_outside_workspace(
-        self,
+    #     self,
-        object_positions: Dict[str, Tuple[float, float, float]],
+    #     object_positions: Dict[str, Tuple[float, float, float]],
-    ) -> bool:
+    # ) -> bool:
-        """
+    #     """
-        Returns true if all objects are outside the workspace
+    #     Returns true if all objects are outside the workspace
-        """
+    #     """
-
+    #
-        return all(
+    #     return all(
-            [
+    #         [
-                self.check_object_outside_workspace(object_position)
+    #             self.check_object_outside_workspace(object_position)
-                for object_position in object_positions.values()
+    #             for object_position in object_positions.values()
-            ]
+    #         ]
-        )
+    #     )
-
+    #
-    def check_object_outside_workspace(
+    # def check_object_outside_workspace(
-        self,
+    #     self,
-        object_position: Tuple[float, float, float],
+    #     object_position: Tuple[float, float, float],
-    ) -> bool:
+    # ) -> bool:
-        """
+    #     """
-        Returns true if the object is outside the workspace
+    #     Returns true if the object is outside the workspace
-        """
+    #     """
-
+    #
-        return (
+    #     return (
-            object_position[0] < self.workspace_min_bound[0]
+    #         object_position[0] < self.workspace_min_bound[0]
-            or object_position[1] < self.workspace_min_bound[1]
+    #         or object_position[1] < self.workspace_min_bound[1]
-            or object_position[2] < self.workspace_min_bound[2]
+    #         or object_position[2] < self.workspace_min_bound[2]
-            or object_position[0] > self.workspace_max_bound[0]
+    #         or object_position[0] > self.workspace_max_bound[0]
-            or object_position[1] > self.workspace_max_bound[1]
+    #         or object_position[1] > self.workspace_max_bound[1]
-            or object_position[2] > self.workspace_max_bound[2]
+    #         or object_position[2] > self.workspace_max_bound[2]
-        )
+    #     )
-
-    def add_parameter_overrides(self, parameter_overrides: Dict[str, any]):
 
+    def add_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
         self.add_task_parameter_overrides(parameter_overrides)
         self.add_randomizer_parameter_overrides(parameter_overrides)
-
+    #
-    def add_task_parameter_overrides(self, parameter_overrides: Dict[str, any]):
+    def add_task_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
-
         self.__task_parameter_overrides.update(parameter_overrides)
-
+    #
-    def add_randomizer_parameter_overrides(self, parameter_overrides: Dict[str, any]):
+    def add_randomizer_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
-
         self._randomizer_parameter_overrides.update(parameter_overrides)
 
     def __consume_parameter_overrides(self):
-
         for key, value in self.__task_parameter_overrides.items():
             if hasattr(self, key):
                 setattr(self, key, value)
@@ -668,3 +668,39 @@ class Manipulation(Task, Node, abc.ABC):
                 )
 
         self.__task_parameter_overrides.clear()
+
+    @property
+    def robot(self) -> Robot:
+        """The robot property."""
+        if self._scene:
+            return self._scene.robot_wrapper.robot
+        else:
+            raise ValueError("R")
+
+    @property
+    def robot_data(self) -> RobotData:
+        """The robot_data property."""
+        if self._scene:
+            return self._scene.robot
+        else:
+            raise ValueError("RD")
+
+    @property
+    def robot_wrapper(self) -> RobotWrapper:
+        """The robot_wrapper property."""
+        if self._scene:
+            return self._scene.robot_wrapper
+        else:
+            raise ValueError("RW")
+
+    @property
+    def scene(self) -> Scene:
+        """The scene property."""
+        if self._scene:
+            return self._scene
+        else:
+            raise ValueError("S")
+
+    @scene.setter
+    def scene(self, value: Scene):
+        self._scene = value

env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach.py

@@ -13,9 +13,10 @@ from gym_gz.utils.typing import (
 )
 
 from rbs_gym.envs.tasks.manipulation import Manipulation
-from rbs_gym.envs.utils.math import distance_to_nearest_point
+from env_manager.utils.math import distance_to_nearest_point
 from std_msgs.msg import Float64MultiArray
 from rbs_gym.envs.observation import TwistSubscriber, JointStates
+from env_manager.utils.helper import get_object_position
 
 
 class Reach(Manipulation, abc.ABC):
@@ -27,7 +28,6 @@ class Reach(Manipulation, abc.ABC):
         required_accuracy: float,
         **kwargs,
     ):
-
         # Initialize the Task base class
         Manipulation.__init__(
             self,
@@ -50,30 +50,30 @@ class Reach(Manipulation, abc.ABC):
         self._is_terminated: bool = False
 
         # Distance to target in the previous step (or after reset)
-        self._previous_distance: float = None
+        self._previous_distance: float = 0.0
         # self._collision_reward: float = collision_reward
 
-        self.initial_gripper_joint_positions = (
+        # self.initial_gripper_joint_positions = self.robot_data.gripper_jont_positions
-            self.robot_model_class.CLOSED_GRIPPER_JOINT_POSITIONS
+        self.twist = TwistSubscriber(
+            self,
+            topic="/cartesian_force_controller/current_twist",
+            callback_group=self._callback_group,
         )
-        self.twist = TwistSubscriber(self,
-                                     topic="/cartesian_force_controller/current_twist", 
-                                     callback_group=self._callback_group)
 
-        self.joint_states = JointStates(self, topic="/joint_states", callback_group=self._callback_group)
+        self.joint_states = JointStates(
+            self, topic="/joint_states", callback_group=self._callback_group
+        )
 
         self._action = WrenchStamped()
-        self._action_array: Action = []
+        self._action_array: Action = np.array([])
-
+        self.followed_object_name = "sphere"
 
     def create_action_space(self) -> ActionSpace:
-
         # 0:3 - (x, y, z) force
         # 3:6 - (x, y, z) torque
         return gym.spaces.Box(low=-1.0, high=1.0, shape=(3,), dtype=np.float32)
 
     def create_observation_space(self) -> ObservationSpace:
-
         # 0:3 - (x, y, z) end effector position
         # 3:6 - (x, y, z) target position
         # 6:9 - (x, y, z) current twist
@@ -81,18 +81,19 @@ class Reach(Manipulation, abc.ABC):
         return gym.spaces.Box(low=-np.inf, high=np.inf, shape=(12,), dtype=np.float32)
 
     def set_action(self, action: Action):
-
         # self.get_logger().debug(f"action: {action}")
         # act = Float64MultiArray()
         # act.data = action
         # self.joint_controller.publisher.publish(act)
-    
+        if isinstance(action, np.number):
+            raise RuntimeError("For the task Reach the action space should be array")
+
         # Store action for reward function
         self._action_array = action
 
         # self._action.header.frame_id = self.robot_ee_link_name
         self._action.header.stamp = self.get_clock().now().to_msg()
-        self._action.header.frame_id = self.robot_ee_link_name
+        self._action.header.frame_id = self.robot.ee_link
         self._action.wrench.force.x = float(action[0]) * 30.0
         self._action.wrench.force.y = float(action[1]) * 30.0
         self._action.wrench.force.z = float(action[2]) * 30.0
@@ -102,21 +103,24 @@ class Reach(Manipulation, abc.ABC):
         self.controller.publisher.publish(self._action)
 
     def get_observation(self) -> Observation:
-
         # Get current end-effector and target positions
+        self.object_names = []
         ee_position = self.get_ee_position()
-        target_position = self.get_object_position(object_model=self.object_names[0])
+        target_position = self.get_object_position(self.followed_object_name)
+        # target_position = self.get_object_position(object_model=self.object_names[0])
         # joint_states = tuple(self.joint_states.get_positions())
 
         # self.get_logger().warn(f"joint_states: {joint_states[:7]}")
 
         twist = self.twist.get_observation()
-        twt = (twist.twist.linear.x,
+        twt: tuple[float, float, float, float, float, float] = (
-               twist.twist.linear.y,
+            twist.twist.linear.x,
-               twist.twist.linear.z,
+            twist.twist.linear.y,
-               twist.twist.angular.x,
+            twist.twist.linear.z,
-               twist.twist.angular.y,
+            twist.twist.angular.x,
-               twist.twist.angular.z)
+            twist.twist.angular.y,
+            twist.twist.angular.z,
+        )
 
         # Create the observation
         observation = Observation(
@@ -129,7 +133,6 @@ class Reach(Manipulation, abc.ABC):
         return observation
 
     def get_reward(self) -> Reward:
-
         reward = 0.0
 
         # Compute the current distance to the target
@@ -138,7 +141,7 @@ class Reach(Manipulation, abc.ABC):
         # Mark the episode done if target is reached
         if current_distance < self._required_accuracy:
             self._is_terminated = True
-            reward += 1.0 if self._sparse_reward else 0.0 # 100.0
+            reward += 1.0 if self._sparse_reward else 0.0  # 100.0
         self.get_logger().debug(f"reward_target: {reward}")
 
         # Give reward based on how much closer robot got relative to the target for dense reward
@@ -167,12 +170,12 @@ class Reach(Manipulation, abc.ABC):
     def is_terminated(self) -> bool:
         self.get_logger().debug(f"terminated: {self._is_terminated}")
         return self._is_terminated
+
     def is_truncated(self) -> bool:
         self.get_logger().debug(f"truncated: {self._is_truncated}")
         return self._is_truncated
 
     def reset_task(self):
-
         Manipulation.reset_task(self)
 
         self._is_done = False
@@ -183,12 +186,13 @@ class Reach(Manipulation, abc.ABC):
         if not self._sparse_reward:
             self._previous_distance = self.get_distance_to_target()
 
-        self.get_logger().debug(f"\ntask reset")
+        self.get_logger().debug("\ntask reset")
-
-    def get_distance_to_target(self) -> Tuple[float, float, float]:
 
+    def get_distance_to_target(self) -> float:
         ee_position = self.get_ee_position()
-        object_position = self.get_object_position(object_model=self.object_names[0])
+        object_position = self.get_object_position(object_model=self.followed_object_name)
-        self.tf2_broadcaster.broadcast_tf("world", "object", object_position, (0.0,0.0,0.0,1.0), xyzw=True)
+        self.tf2_broadcaster.broadcast_tf(
+            "world", "object", object_position, (0.0, 0.0, 0.0, 1.0), xyzw=True
+        )
 
         return distance_to_nearest_point(origin=ee_position, points=[object_position])

env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach_color_image.py

@@ -4,7 +4,7 @@ import gymnasium as gym
 import numpy as np
 from gym_gz.utils.typing import Observation, ObservationSpace
 
-from rbs_gym.envs.models.sensors import Camera
+from env_manager.models.sensors import Camera
 from rbs_gym.envs.observation import CameraSubscriber
 from rbs_gym.envs.tasks.reach import Reach
 

env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach_depth_image.py

@@ -4,7 +4,7 @@ import gymnasium as gym
 import numpy as np
 from gym_gz.utils.typing import Observation, ObservationSpace
 
-from rbs_gym.envs.models.sensors import Camera
+from env_manager.models.sensors import Camera
 from rbs_gym.envs.observation import CameraSubscriber
 from rbs_gym.envs.tasks.reach import Reach
 

env_manager/rbs_gym/scripts/test_agent.py

@@ -61,7 +61,7 @@ if __name__ == "__main__":
 
     # Environment and its parameters
     parser.add_argument(
-        "--env", type=str, default="Reach-ColorImage-Gazebo-v0", help="Environment ID"
+        "--env", type=str, default="Reach-Gazebo-v0", help="Environment ID"
     )
     parser.add_argument(
         "--env-kwargs",

env_manager/rbs_runtime/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(planning_scene_manager)
+project(rbs_runtime)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
@@ -11,6 +11,14 @@ find_package(ament_cmake REQUIRED)
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
+ament_python_install_package(${PROJECT_NAME})
+
+
+install(PROGRAMS
+  scripts/runtime.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
@@ -23,4 +31,6 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+
 ament_package()

env_manager/rbs_runtime/config/scene_config.yaml

@@ -0,0 +1,139 @@
+robot:
+  name: rbs_arm
+  urdf_string: ""
+  spawn_position:
+    - 0.0
+    - 0.0
+    - 0.0
+  spawn_quat_xyzw:
+    - 0.0
+    - 0.0
+    - 0.0
+    - 1.0
+  joint_positioins:
+    - 0
+    - 0
+    - 0
+    - 0
+    - 0
+    - 0
+  with_gripper: true
+  gripper_jont_positions:
+    - 0
+  randomizer:
+    pose: false
+    spawn_volume:
+      - 1.0
+      - 1.0
+      - 0.0
+    joint_positions: false
+    joint_positions_std: 0.1
+    joint_positions_above_object_spawn: false
+    joint_positions_above_object_spawn_elevation: 0.2
+    joint_positions_above_object_spawn_xy_randomness: 0.2
+
+terrain:
+  type: flat
+  spawn_position:
+    - 0
+    - 0
+    - 0
+  spawn_quat_xyzw:
+    - 0
+    - 0
+    - 0
+    - 1
+  size:
+    - 1.5
+    - 1.5
+  model_rollouts_num: 1
+
+light:
+  type: sun
+  direction:
+    - 0.5
+    - -0.25
+    - -0.75
+  random_minmax_elevation:
+    - -0.15
+    - -0.65
+  color:
+    - 1.0
+    - 1.0
+    - 1.0
+    - 1.0
+  distance: 1000.0
+  visual: true
+  radius: 25.0
+  model_rollouts_num: 1
+
+objects:
+  - name: bishop
+    type: ""
+    relative_to: world
+    position:
+      - 0.0
+      - 0.0
+      - 0.0
+    orientation:
+      - 1.0
+      - 0.0
+      - 0.0
+      - 0.0
+    static: false
+    randomize:
+      count: 0
+      random_pose: false
+      random_position: false
+      random_orientation: false
+      random_model: false
+      random_spawn_position_segments: []
+      random_spawn_position_update_workspace_centre: false
+      random_spawn_volume:
+        - 0.5
+        - 0.5
+        - 0.5
+      models_rollouts_num: 0
+    texture: []
+
+camera:
+  - name: robot_camera
+    enable: true
+    type: rgbd_camera
+    relative_to: base_link
+    width: 128
+    height: 128
+    image_format: R8G8B8
+    update_rate: 10
+    horizontal_fov: 1.0471975511965976
+    vertical_fov: 1.0471975511965976
+    clip_color:
+      - 0.01
+      - 1000.0
+    clip_depth:
+      - 0.05
+      - 10.0
+    noise_mean: null
+    noise_stddev: null
+    publish_color: false
+    publish_depth: false
+    publish_points: false
+    spawn_position:
+      - 0
+      - 0
+      - 1
+    spawn_quat_xyzw:
+      - 0
+      - 0.70710678118
+      - 0
+      - 0.70710678118
+    random_pose_rollouts_num: 1
+    random_pose_mode: orbit
+    random_pose_orbit_distance: 1.0
+    random_pose_orbit_height_range:
+      - 0.1
+      - 0.7
+    random_pose_orbit_ignore_arc_behind_robot: 0.39269908169872414
+    random_pose_select_position_options: []
+    random_pose_focal_point_z_offset: 0.0
+    random_pose_rollout_counter: 0.0

env_manager/rbs_runtime/launch/runtime.launch.py

@@ -0,0 +1,278 @@
+import os
+
+import xacro
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+    TimerAction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def launch_setup(context, *args, **kwargs):
+    # Initialize Arguments
+    robot_type = LaunchConfiguration("robot_type")
+    # General arguments
+    with_gripper_condition = LaunchConfiguration("with_gripper")
+    cartesian_controllers = LaunchConfiguration("cartesian_controllers")
+    description_package = LaunchConfiguration("description_package")
+    description_file = LaunchConfiguration("description_file")
+    robot_name = LaunchConfiguration("robot_name")
+    start_joint_controller = LaunchConfiguration("start_joint_controller")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    launch_simulation = LaunchConfiguration("launch_sim")
+    launch_moveit = LaunchConfiguration("launch_moveit")
+    launch_task_planner = LaunchConfiguration("launch_task_planner")
+    launch_perception = LaunchConfiguration("launch_perception")
+    moveit_config_package = LaunchConfiguration("moveit_config_package")
+    moveit_config_file = LaunchConfiguration("moveit_config_file")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    sim_gazebo = LaunchConfiguration("sim_gazebo")
+    hardware = LaunchConfiguration("hardware")
+    env_manager = LaunchConfiguration("env_manager")
+    gazebo_gui = LaunchConfiguration("gazebo_gui")
+    gripper_name = LaunchConfiguration("gripper_name")
+
+    initial_joint_controllers_file_path = os.path.join(
+        get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml"
+    )
+
+    xacro_file = os.path.join(
+        get_package_share_directory(description_package.perform(context)),
+        "urdf",
+        description_file.perform(context),
+    )
+
+    robot_description_doc = xacro.process_file(
+        xacro_file,
+        mappings={
+            "gripper_name": gripper_name.perform(context),
+            "hardware": hardware.perform(context),
+            "simulation_controllers": initial_joint_controllers_file_path,
+            "namespace": "",
+        },
+    )
+
+    robot_description_content = robot_description_doc.toprettyxml(indent="  ")
+    robot_description = {"robot_description": robot_description_content}
+
+    single_robot_setup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                PathJoinSubstitution(
+                    [FindPackageShare("rbs_bringup"), "launch", "rbs_robot.launch.py"]
+                )
+            ]
+        ),
+        launch_arguments={
+            "env_manager": env_manager,
+            "with_gripper": with_gripper_condition,
+            "gripper_name": gripper_name,
+            "controllers_file": initial_joint_controllers_file_path,
+            "robot_type": robot_type,
+            # "controllers_file": controller_paramfile,
+            "cartesian_controllers": cartesian_controllers,
+            "description_package": description_package,
+            "description_file": description_file,
+            "robot_name": robot_type,
+            "start_joint_controller": start_joint_controller,
+            "initial_joint_controller": initial_joint_controller,
+            "launch_simulation": launch_simulation,
+            "launch_moveit": launch_moveit,
+            "launch_task_planner": launch_task_planner,
+            "launch_perception": launch_perception,
+            "moveit_config_package": moveit_config_package,
+            "moveit_config_file": moveit_config_file,
+            "use_sim_time": use_sim_time,
+            "sim_gazebo": sim_gazebo,
+            "hardware": hardware,
+            "launch_controllers": "true",
+            "gazebo_gui": gazebo_gui,
+            "robot_description": robot_description_content,
+        }.items(),
+    )
+
+    rbs_runtime = Node(
+        package="rbs_runtime",
+        executable="runtime.py",
+        parameters=[robot_description, {"use_sim_time": True}],
+    )
+
+    clock_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
+        output="screen",
+    )
+
+    delay_robot_control_stack = TimerAction(period=10.0, actions=[single_robot_setup])
+
+    nodes_to_start = [rbs_runtime, clock_bridge, 
+                      delay_robot_control_stack
+                      ]
+    return nodes_to_start
+
+
+def generate_launch_description():
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_type",
+            description="Type of robot by name",
+            choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
+            default_value="rbs_arm",
+        )
+    )
+    # General arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controllers_file",
+            default_value="rbs_arm0_controllers.yaml",
+            description="YAML file with the controllers configuration.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_package",
+            default_value="rbs_arm",
+            description="Description package with robot URDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom description.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_file",
+            default_value="rbs_arm_modular.xacro",
+            description="URDF/XACRO description file with the robot.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_name",
+            default_value="arm0",
+            description="Name for robot, used to apply namespace for specific robot in multirobot setup",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "start_joint_controller",
+            default_value="false",
+            description="Enable headless mode for robot control",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="joint_trajectory_controller",
+            description="Robot controller to start.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_package",
+            default_value="rbs_arm",
+            description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom moveit config.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_file",
+            default_value="rbs_arm.srdf.xacro",
+            description="MoveIt SRDF/XACRO description file with the robot.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="true",
+            description="Make MoveIt to use simulation time.\
+            This is needed for the trajectory planing in simulation.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "gripper_name",
+            default_value="rbs_gripper",
+            choices=["rbs_gripper", ""],
+            description="choose gripper by name (leave empty if hasn't)",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "with_gripper", default_value="true", description="With gripper or not?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "sim_gazebo", default_value="true", description="Gazebo Simulation"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "env_manager", default_value="true", description="Launch env_manager?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "launch_sim",
+            default_value="true",
+            description="Launch simulator (Gazebo)?\
+                              Most general arg",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "launch_moveit", default_value="false", description="Launch moveit?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "launch_perception", default_value="false", description="Launch perception?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "launch_task_planner",
+            default_value="false",
+            description="Launch task_planner?",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "cartesian_controllers",
+            default_value="true",
+            description="Load cartesian\
+                              controllers?",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "hardware",
+            choices=["gazebo", "mock"],
+            default_value="gazebo",
+            description="Choose your harware_interface",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "launch_controllers",
+            default_value="true",
+            description="Launch controllers?",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "gazebo_gui", default_value="true", description="Launch gazebo with gui?"
+        )
+    )
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )

env_manager/rbs_runtime/package.xml

@@ -1,10 +1,10 @@
 <?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>planning_scene_manager</name>
+  <name>rbs_runtime</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
-  <maintainer email="ur.narmak@gmail.com">Bill Finger</maintainer>
+  <maintainer email="ur.narmak@gmail.com">narmak</maintainer>
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

env_manager/rbs_runtime/scripts/runtime.py

@@ -0,0 +1,81 @@
+#!/usr/bin/python3
+
+from rclpy.lifecycle import LifecycleNode, LifecycleState, TransitionCallbackReturn
+from rclpy.node import Node
+from env_manager.scene import Scene
+from env_manager.models.configs import (
+    CameraData,
+    SceneData,
+    RobotData,
+    ObjectData,
+    MeshObjectData,
+    TerrainData,
+    LightData,
+)
+import rclpy
+from scenario.bindings.gazebo import GazeboSimulator
+from env_manager_interfaces.srv import ResetEnv
+from rbs_assets_library import get_world_file
+
+
+DEFAULT_SCENE: SceneData = SceneData(
+    robot=RobotData(
+        name="rbs_arm",
+        with_gripper=True,
+        joint_positioins=[0, 0, 0, 0, 0, 0, 0],
+        gripper_jont_positions=[0],
+    ),
+    objects=[MeshObjectData("bishop", position=(0.0, 1.0, 0.3))],
+    camera=[CameraData("robot_camera")],
+)
+
+
+class GazeboRuntime(Node):
+    def __init__(self) -> None:
+        super().__init__(node_name="rbs_gz_runtime")
+        self.declare_parameter("robot_description", "")
+        self.gazebo = GazeboSimulator(step_size=0.001, rtf=1.0, steps_per_run=1)
+
+        self.gazebo.insert_world_from_sdf(get_world_file("default"))
+        if not self.gazebo.initialize():
+            raise RuntimeError("Gazebo cannot be initialized")
+
+        self.scene = Scene(
+            node=self,
+            gazebo=self.gazebo,
+            scene=DEFAULT_SCENE,
+            robot_urdf_string=self.get_parameter("robot_description")
+            .get_parameter_value()
+            .string_value,
+        )
+        self.scene.init_scene()
+
+        self.reset_env_srv = self.create_service(
+            ResetEnv, "/env_manager/reset_env", self.reset_env
+        )
+        self.is_env_reset = False
+        self.timer = self.create_timer(self.gazebo.step_size(), self.gazebo_run)
+
+    def gazebo_run(self):
+        if self.is_env_reset:
+            self.scene.reset_scene()
+            self.is_env_reset = False
+        self.gazebo.run()
+
+    def reset_env(self, req, res):
+        self.is_env_reset = True
+        res.ok = self.is_env_reset
+        return res
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    runtime_node = GazeboRuntime()
+    rclpy.spin(runtime_node)
+    runtime_node.gazebo.close()
+    runtime_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()