rbs_gym multi camera and multi object setup

env_manager/rbs_gym/rbs_gym/envs/__init__.py

@@ -13,7 +13,7 @@ except:
     pass
 
 from os import environ, path
-from typing import Dict, Tuple
+from typing import Dict, Tuple, Any
 
 import numpy as np
 from ament_index_python.packages import get_package_share_directory
@@ -22,15 +22,13 @@ from gymnasium.envs.registration import register
 from rbs_gym.utils.utils import str2bool
 
 from . import tasks
+from dataclasses import dataclass, field
 
 ######################
 # Runtime Entrypoint #
 ######################
-# Entrypoint for tasks (can be simulated or real)
 
-RBS_ENVS_TASK_ENTRYPOINT: str = (
+RBS_ENVS_TASK_ENTRYPOINT: str = "gym_gz.runtimes.gazebo_runtime:GazeboRuntime"
-    "gym_gz.runtimes.gazebo_runtime:GazeboRuntime"
-)
 
 
 ###################
@@ -43,9 +41,7 @@ RBS_ENVS_ROBOT_MODEL: str = "rbs_arm"
 # Datasets and paths #
 ######################
 # Path to directory containing base SDF worlds
-RBS_ENVS_WORLDS_DIR: str = path.join(
+RBS_ENVS_WORLDS_DIR: str = path.join(get_package_share_directory("rbs_gym"), "worlds")
-    get_package_share_directory("rbs_gym"), "worlds"
-)
 
 
 ###########
@@ -53,7 +49,7 @@ RBS_ENVS_WORLDS_DIR: str = path.join(
 ###########
 # Gravity preset for Earth
 GRAVITY_EARTH: Tuple[float, float, float] = (0.0, 0.0, -9.80665)
-GRAVITY_EARTH_STD: Tuple[float, float, float] =  (0.0, 0.0, 0.0232)
+GRAVITY_EARTH_STD: Tuple[float, float, float] = (0.0, 0.0, 0.0232)
 
 ############################
 # Additional Configuration #
@@ -67,7 +63,7 @@ BROADCAST_GUI: bool = str2bool(
 # Reach #
 #########
 REACH_MAX_EPISODE_STEPS: int = 100
-REACH_KWARGS: Dict[str, any] = {
+REACH_KWARGS: Dict[str, Any] = {
     "agent_rate": 4.0,
     "robot_model": RBS_ENVS_ROBOT_MODEL,
     "workspace_frame_id": "world",
@@ -85,20 +81,84 @@ REACH_KWARGS: Dict[str, any] = {
     "required_accuracy": 0.05,
     "num_threads": 3,
 }
-REACH_KWARGS_SIM: 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"),
 }
+
+
+@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] = {
+REACH_KWARGS_RANDOMIZER: Dict[str, Any] = {
     "gravity": GRAVITY_EARTH,
     "gravity_std": GRAVITY_EARTH_STD,
     "plugin_scene_broadcaster": BROADCAST_GUI,
     "plugin_user_commands": BROADCAST_GUI,
     "plugin_sensors_render_engine": "ogre2",
     "robot_random_pose": False,
-    "robot_random_joint_positions": True, # FIXME: 
+    "robot_random_joint_positions": True,  # FIXME:
     "robot_random_joint_positions_std": 0.2,
     "robot_random_joint_positions_above_object_spawn": False,
     "robot_random_joint_positions_above_object_spawn_elevation": 0.0,
@@ -111,42 +171,136 @@ REACH_KWARGS_RANDOMIZER: Dict[str, any] = {
     "light_visual": False,
     "light_radius": 25.0,
     "light_model_rollouts_num": 1,
-    "object_enable": True,
-    "object_type": "sphere",
-    "objects_relative_to": "base_link",
-    "object_static": True,
-    "object_collision": False,
-    "object_visual": True,
-    "object_color": (0.0, 0.0, 1.0, 1.0),
-    "object_dimensions": [0.025, 0.025, 0.025],
-    "object_count": 1,
-    "object_spawn_position": (-0.4, 0, 0.3),
-    "object_random_pose": True,
-    "object_random_spawn_position_segments": [],
-    "object_random_spawn_volume": (0.2, 0.2, 0.2),
-    "object_models_rollouts_num": 0,
     "underworld_collision_plane": False,
 }
-REACH_KWARGS_RANDOMIZER_CAMERA: Dict[str, any] = {
+REACH_KWARGS_OBJECT_RANDOMIZER: list = [
-    "camera_enable": True,
+    MeshObjectData(
-    "camera_width": 64,
+        "bishop",
-    "camera_height": 64,
+        "base_link",
-    "camera_update_rate": 1.2 * REACH_KWARGS["agent_rate"],
+        (-0.3, 0.0, 0.0),
-    "camera_horizontal_fov": np.pi / 3.0,
+        randomize=ObjectRandomizerData(
-    "camera_vertical_fov": np.pi / 3.0,
+            random_position=True, random_spawn_volume=(0.2, 0.5, 0.0)
-    "camera_noise_mean": 0.0,
+        ),
-    "camera_noise_stddev": 0.001,
+    ),
-    "camera_relative_to": "base_link",
+    SphereObjectData(
-    "camera_spawn_position": (0.85, -0.4, 0.45),
+        "sphere",
-    "camera_spawn_quat_xyzw": (-0.0402991, -0.0166924, 0.9230002, 0.3823192),
+        "base_link",
-    "camera_random_pose_rollouts_num": 0,
+        (-0.3, 0.0, 0.3),
-    "camera_random_pose_mode": "orbit",
+        mass=1.0,
-    "camera_random_pose_orbit_distance": 1.0,
+        color=(0.0, 0.0, 1.0, 1.0),
-    "camera_random_pose_orbit_height_range": (0.1, 0.7),
+        # randomize=ObjectRandomizerData(
-    "camera_random_pose_orbit_ignore_arc_behind_robot": np.pi / 8,
+        #     random_pose=True, random_spawn_volume=(0.2, 0.2, 0.0)
-    "camera_random_pose_select_position_options": [],
+        # ),
-    "camera_random_pose_focal_point_z_offset": 0.0,
+    ),
-}
+]
+# 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(
@@ -186,7 +340,7 @@ register(
         "env": "Reach-v0",
         **REACH_KWARGS_SIM,
         **REACH_KWARGS_RANDOMIZER,
-        "camera_enable": False,
+        "object_args": REACH_KWARGS_OBJECT_RANDOMIZER,
     },
 )
 register(
@@ -197,9 +351,8 @@ register(
         "env": "Reach-ColorImage-v0",
         **REACH_KWARGS_SIM,
         **REACH_KWARGS_RANDOMIZER,
-        **REACH_KWARGS_RANDOMIZER_CAMERA,
+        "camera_args": REACH_KWARGS_RANDOMIZER_CAMERAS,
-        "camera_type": "rgbd_camera",
+        "object_args": REACH_KWARGS_OBJECT_RANDOMIZER,
-        "camera_publish_color": True,
     },
 )
 register(
@@ -210,8 +363,7 @@ register(
         "env": "Reach-DepthImage-v0",
         **REACH_KWARGS_SIM,
         **REACH_KWARGS_RANDOMIZER,
-        **REACH_KWARGS_RANDOMIZER_CAMERA,
+        "camera_args": REACH_KWARGS_RANDOMIZER_CAMERAS,
-        "camera_type": "depth_camera",
+        "object_args": REACH_KWARGS_OBJECT_RANDOMIZER,
-        "camera_publish_depth": True,
     },
 )

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

@@ -1,35 +1,51 @@
+from enum import Enum
+
 from gym_gz.scenario.model_wrapper import ModelWrapper
 
+from .model import Mesh
 from .primitives import Box, Cylinder, Plane, Sphere
-from .random_lunar_rock import RandomLunarRock
 from .random_object import RandomObject
 from .random_primitive import RandomPrimitive
-from .rock import Rock
 
 
-def get_object_model_class(object_type: str) -> ModelWrapper:
+# Enum для типов объектов
-    # TODO: Refactor into enum
+class ObjectType(Enum):
+    BOX = "box"
+    SPHERE = "sphere"
+    CYLINDER = "cylinder"
+    PLANE = "plane"
+    RANDOM_PRIMITIVE = "random_primitive"
+    RANDOM_MESH = "random_mesh"
+    MESH = "mesh"
 
-    if "box" == object_type:
+OBJECT_MODEL_CLASSES = {
-        return Box
+    ObjectType.BOX: Box,
-    elif "sphere" == object_type:
+    ObjectType.SPHERE: Sphere,
-        return Sphere
+    ObjectType.CYLINDER: Cylinder,
-    elif "cylinder" == object_type:
+    ObjectType.PLANE: Plane,
-        return Cylinder
+    ObjectType.RANDOM_PRIMITIVE: RandomPrimitive,
-    elif "random_primitive" == object_type:
+    ObjectType.RANDOM_MESH: RandomObject,
-        return RandomPrimitive
+    ObjectType.MESH: Mesh,
-    elif "random_mesh" == object_type:
+}
-        return RandomObject
+
-    elif "rock" == object_type:
+
-        return Rock
+RANDOMIZABLE_TYPES = {
-    elif "random_lunar_rock" == object_type:
+    ObjectType.RANDOM_PRIMITIVE,
-        return RandomLunarRock
+    ObjectType.RANDOM_MESH,
+}
+
+
+def get_object_model_class(object_type: str) -> type[ModelWrapper]:
+    try:
+        object_enum = ObjectType(object_type)
+        return OBJECT_MODEL_CLASSES[object_enum]
+    except KeyError:
+        raise ValueError(f"Unsupported object type: {object_type}")
 
 
 def is_object_type_randomizable(object_type: str) -> bool:
-
+    try:
-    return (
+        object_enum = ObjectType(object_type)
-        "random_primitive" == object_type
+        return object_enum in RANDOMIZABLE_TYPES
-        or "random_mesh" == object_type
+    except ValueError:
-        or "random_lunar_rock" == object_type
+        return False
-    )

env_manager/rbs_gym/rbs_gym/envs/models/objects/model.py

@@ -0,0 +1,51 @@
+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_core
+from scenario import gazebo as scenario_gazebo
+from rbs_assets_library import get_model_file
+
+
+class Mesh(model_wrapper.ModelWrapper):
+    def __init__(
+        self,
+        world: scenario_gazebo.World,
+        name: str = "object",
+        position: tuple[float, float, float] = (0, 0, 0),
+        orientation: tuple[float, float, float, float] = (1, 0, 0, 0),
+        gui_only: bool = False,
+        **kwargs,
+    ):
+
+        # Get a unique model name
+        model_name = get_unique_model_name(world, name)
+
+        # Initial pose
+        initial_pose = scenario_core.Pose(position, orientation)
+
+        # Create SDF string for the model
+        sdf = self.get_sdf(
+            model_name=name,
+        )
+
+        # Insert the model
+        ok_model = world.to_gazebo().insert_model(
+            sdf, initial_pose, model_name
+        )
+        if not ok_model:
+            raise RuntimeError("Failed to insert " + model_name)
+
+        # Get the model
+        model = world.get_model(model_name)
+
+        # Initialize base class
+        super().__init__(model=model)
+
+    @classmethod
+    def get_sdf(
+        cls,
+        model_name: str,
+    ) -> str:
+        return get_model_file(model_name)

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

@@ -1,26 +1,24 @@
-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 Box(model_wrapper.ModelWrapper):
     def __init__(
         self,
-        world: scenario.World,
+        world: scenario_gazebo.World,
         name: str = "box",
-        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] = (0.05, 0.05, 0.05),
+        size: tuple[float, float, float] = (0.05, 0.05, 0.05),
         mass: float = 0.1,
         static: bool = False,
         collision: bool = True,
         friction: float = 1.0,
         visual: bool = True,
         gui_only: bool = False,
-        color: List[float] = (0.8, 0.8, 0.8, 1.0),
+        color: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0),
         **kwargs,
     ):
 
@@ -28,7 +26,7 @@ class Box(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 = self.get_sdf(
@@ -48,26 +46,26 @@ class Box(model_wrapper.ModelWrapper):
             sdf, initial_pose, model_name
         )
         if not ok_model:
-            raise RuntimeError("Failed to insert " + model_name)
+            raise RuntimeError(f"Failed to insert {model_name}")
 
         # Get the model
         model = world.get_model(model_name)
 
         # Initialize base class
-        model_wrapper.ModelWrapper.__init__(self, model=model)
+        super().__init__(model=model)
 
     @classmethod
     def get_sdf(
-        self,
+        cls,
         model_name: str,
-        size: List[float],
+        size: tuple[float, float, float],
         mass: float,
         static: bool,
         collision: bool,
         friction: float,
         visual: bool,
         gui_only: bool,
-        color: List[float],
+        color: tuple[float, float, float, float],
     ) -> str:
         return f'''<sdf version="1.7">
                 <model name="{model_name}">
@@ -108,7 +106,7 @@ class Box(model_wrapper.ModelWrapper):
                                 <diffuse>{color[0]} {color[1]} {color[2]} {color[3]}</diffuse>
                                 <specular>{color[0]} {color[1]} {color[2]} {color[3]}</specular>
                             </material>
-                            <transparency>{1.0-color[3]}</transparency>
+                            <transparency>{1.0 - color[3]}</transparency>
                             {'<visibility_flags>1</visibility_flags> <cast_shadows>false</cast_shadows>' if gui_only else ''}
                         </visual>
                         """ if visual else ""
@@ -116,9 +114,9 @@ class Box(model_wrapper.ModelWrapper):
                         <inertial>
                             <mass>{mass}</mass>
                             <inertia>
-                                <ixx>{(size[1]**2 + size[2]**2)*mass/12}</ixx>
+                                <ixx>{(size[1]**2 + size[2]**2) * mass / 12}</ixx>
-                                <iyy>{(size[0]**2 + size[2]**2)*mass/12}</iyy>
+                                <iyy>{(size[0]**2 + size[2]**2) * mass / 12}</iyy>
-                                <izz>{(size[0]**2 + size[1]**2)*mass/12}</izz>
+                                <izz>{(size[0]**2 + size[1]**2) * mass / 12}</izz>
                                 <ixy>0.0</ixy>
                                 <ixz>0.0</ixz>
                                 <iyz>0.0</iyz>

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

@@ -1,18 +1,16 @@
-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 Cylinder(model_wrapper.ModelWrapper):
     def __init__(
         self,
-        world: scenario.World,
+        world: scenario_gazebo.World,
         name: str = "cylinder",
-        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),
         radius: float = 0.025,
         length: float = 0.1,
         mass: float = 0.1,
@@ -21,17 +19,17 @@ class Cylinder(model_wrapper.ModelWrapper):
         friction: float = 1.0,
         visual: bool = True,
         gui_only: bool = False,
-        color: List[float] = (0.8, 0.8, 0.8, 1.0),
+        color: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0),
         **kwargs,
     ):
 
-        # Get a unique model name
+        # Получаем уникальное имя модели
         model_name = get_unique_model_name(world, name)
 
-        # Initial pose
+        # Инициализируем начальную позу
-        initial_pose = scenario.Pose(position, orientation)
+        initial_pose = scenario_core.Pose(position, orientation)
 
-        # Create SDF string for the model
+        # Создаем строку SDF для модели
         sdf = self.get_sdf(
             model_name=model_name,
             radius=radius,
@@ -45,22 +43,22 @@ class Cylinder(model_wrapper.ModelWrapper):
             color=color,
         )
 
-        # Insert the model
+        # Вставляем модель
         ok_model = world.to_gazebo().insert_model_from_string(
             sdf, initial_pose, model_name
         )
         if not ok_model:
-            raise RuntimeError("Failed to insert " + model_name)
+            raise RuntimeError(f"Failed to insert {model_name}")
 
-        # Get the model
+        # Получаем модель
         model = world.get_model(model_name)
 
-        # Initialize base class
+        # Вызов конструктора родительского класса
-        model_wrapper.ModelWrapper.__init__(self, model=model)
+        super().__init__(model=model)
 
     @classmethod
     def get_sdf(
-        self,
+        cls,
         model_name: str,
         radius: float,
         length: float,
@@ -70,9 +68,9 @@ class Cylinder(model_wrapper.ModelWrapper):
         friction: float,
         visual: bool,
         gui_only: bool,
-        color: List[float],
+        color: tuple[float, float, float, float],
     ) -> str:
-        # Inertia is identical for xx and yy components, compute only once
+        # Инерция одинакова для xx и yy, вычисляем ее один раз
         inertia_xx_yy = (3 * radius**2 + length**2) * mass / 12
 
         return f'''<sdf version="1.7">

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

@@ -1,33 +1,30 @@
-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 Plane(model_wrapper.ModelWrapper):
     def __init__(
         self,
-        world: scenario.World,
+        world: scenario_gazebo.World,
         name: str = "plane",
-        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.0, 1.0),
+        size: tuple[float, float] = (1.0, 1.0),
-        direction: List[float] = (0.0, 0.0, 1.0),
+        direction: tuple[float, float, float] = (0.0, 0.0, 1.0),
         collision: bool = True,
         friction: float = 1.0,
         visual: bool = True,
         **kwargs,
     ):
-
+        # Получаем уникальное имя модели
-        # Get a unique model name
         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 для модели
         sdf = f'''<sdf version="1.7">
             <model name="{model_name}">
                 <static>true</static>
@@ -76,15 +73,15 @@ class Plane(model_wrapper.ModelWrapper):
             </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)
+            raise RuntimeError(f"Failed to insert {model_name}")
 
-        # Get the model
+        # Получение модели
         model = world.get_model(model_name)
 
-        # Initialize base class
+        # Вызов конструктора родительского класса
-        model_wrapper.ModelWrapper.__init__(self, model=model)
+        super().__init__(model=model)

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

@@ -1,18 +1,16 @@
-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 Sphere(model_wrapper.ModelWrapper):
     def __init__(
         self,
-        world: scenario.World,
+        world: scenario_gazebo.World,
         name: str = "sphere",
-        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),
         radius: float = 0.025,
         mass: float = 0.1,
         static: bool = False,
@@ -20,7 +18,7 @@ class Sphere(model_wrapper.ModelWrapper):
         friction: float = 1.0,
         visual: bool = True,
         gui_only: bool = False,
-        color: List[float] = (0.8, 0.8, 0.8, 1.0),
+        color: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0),
         **kwargs,
     ):
 
@@ -28,7 +26,7 @@ class Sphere(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 = self.get_sdf(
@@ -48,17 +46,17 @@ class Sphere(model_wrapper.ModelWrapper):
             sdf, initial_pose, model_name
         )
         if not ok_model:
-            raise RuntimeError("Failed to insert " + model_name)
+            raise RuntimeError(f"Failed to insert {model_name}")
 
         # Get the model
         model = world.get_model(model_name)
 
         # Initialize base class
-        model_wrapper.ModelWrapper.__init__(self, model=model)
+        super().__init__(model=model)
 
     @classmethod
     def get_sdf(
-        self,
+        cls,
         model_name: str,
         radius: float,
         mass: float,
@@ -67,7 +65,7 @@ class Sphere(model_wrapper.ModelWrapper):
         friction: float,
         visual: bool,
         gui_only: bool,
-        color: List[float],
+        color: tuple[float, float, float, float],
     ) -> str:
         # Inertia is identical for all axes
         inertia_xx_yy_zz = (mass * radius**2) * 2 / 5
@@ -111,7 +109,7 @@ class Sphere(model_wrapper.ModelWrapper):
                                 <diffuse>{color[0]} {color[1]} {color[2]} {color[3]}</diffuse>
                                 <specular>{color[0]} {color[1]} {color[2]} {color[3]}</specular>
                             </material>
-                            <transparency>{1.0-color[3]}</transparency>
+                            <transparency>{1.0 - color[3]}</transparency>
                             {'<visibility_flags>1</visibility_flags> <cast_shadows>false</cast_shadows>' if gui_only else ''}
                         </visual>
                         """ if visual else ""

env_manager/rbs_gym/rbs_gym/envs/models/objects/random_lunar_rock.py

@@ -1,57 +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 RandomLunarRock(model_wrapper.ModelWrapper):
-    def __init__(
-        self,
-        world: scenario.World,
-        name: str = "rock",
-        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)
-
-        # Initial pose
-        initial_pose = scenario.Pose(position, orientation)
-
-        # Get path to all lunar rock models
-        if not models_dir:
-            models_dir = os.environ.get("SDF_PATH_LUNAR_ROCK", 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_ROCK' 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/models/objects/rock.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 Rock(model_wrapper.ModelWrapper, model_with_file.ModelWithFile):
-    def __init__(
-        self,
-        world: scenario.World,
-        name: str = "rock",
-        position: List[float] = (0, 0, 0),
-        orientation: List[float] = (1, 0, 0, 0),
-        model_file: str = None,
-        use_fuel: bool = True,
-        variant: int = 6,
-        **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: int = 6) -> str:
-        if fuel:
-            return scenario_gazebo.get_model_file_from_fuel(
-                f"https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Falling Rock {variant}"
-            )
-        else:
-            return "lunar_surface"

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

@@ -6,10 +6,12 @@ from .rbs_arm import RbsArm
 # TODO: When adding new a robot, create abstract classes to simplify such process
 
 
-def get_robot_model_class(robot_model: str) -> ModelWrapper:
+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/robots/rbs_arm.py

@@ -1,4 +1,4 @@
-from typing import Dict, List, Optional, Tuple
+from typing import Dict, List, Optional, Tuple, Any
 
 from gym_gz.scenario import model_with_file, model_wrapper
 from gym_gz.utils.scenario import get_unique_model_name
@@ -8,6 +8,10 @@ import numpy as np
 from scenario import core as scenario
 from scenario import gazebo as scenario_gazebo
 
+# 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):
 
     DEFAULT_ARM_JOINT_POSITIONS: List[float] = (
@@ -34,13 +38,13 @@ class RbsArm(model_wrapper.ModelWrapper, model_with_file.ModelWithFile):
         self,
         world: scenario.World,
         name: str = "rbs_arm",
-        position: List[float] = (0.0, 0.0, 0.0),
+        position: tuple[float, float, float] = (0.0, 0.0, 0.0),
-        orientation: List[float] = (1.0, 0, 0, 0),
+        orientation: tuple[float, float, float, float] = (1.0, 0, 0, 0),
-        model_file: Optional[str] = None,
+        model_file: str | None = None,
         use_fuel: bool = False,
         use_xacro: bool = False,
         xacro_file: str = "",
-        xacro_mappings: Dict[str, any] = {},
+        xacro_mappings: Dict[str, Any] = {},
         initial_arm_joint_positions: List[float] = DEFAULT_ARM_JOINT_POSITIONS,
         initial_gripper_joint_positions: List[float] = DEFAULT_GRIPPER_JOINT_POSITIONS,
         **kwargs

env_manager/rbs_gym/rbs_gym/envs/models/sensors/camera.py

@@ -1,10 +1,10 @@
 import os
 from threading import Thread
-from typing import List, Optional, Union
 
 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_gz
 
 from rbs_gym.envs.models.utils import ModelCollectionRandomizer
 
@@ -12,10 +12,10 @@ from rbs_gym.envs.models.utils import ModelCollectionRandomizer
 class Camera(model_wrapper.ModelWrapper):
     def __init__(
         self,
-        world: scenario.World,
+        world: scenario_gz.World,
-        name: Union[str, None] = None,
+        name: str | None = None,
-        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),
         static: bool = True,
         camera_type: str = "rgbd_camera",
         width: int = 212,
@@ -24,31 +24,37 @@ class Camera(model_wrapper.ModelWrapper):
         update_rate: int = 15,
         horizontal_fov: float = 1.567821,
         vertical_fov: float = 1.022238,
-        clip_color: List[float] = (0.02, 1000.0),
+        clip_color: tuple[float, float] = (0.02, 1000.0),
-        clip_depth: List[float] = (0.02, 10.0),
+        clip_depth: tuple[float, float] = (0.02, 10.0),
-        noise_mean: float = None,
+        noise_mean: float | None = None,
-        noise_stddev: float = None,
+        noise_stddev: float | None = None,
         ros2_bridge_color: bool = False,
         ros2_bridge_depth: bool = False,
         ros2_bridge_points: bool = False,
         visibility_mask: int = 0,
-        visual: Optional[str] = None,
+        visual: str | None = None,
         # visual: Optional[str] = "intel_realsense_d435",
     ):
-
         # Get a unique model name
         if name is not None:
             model_name = get_unique_model_name(world, name)
         else:
             model_name = get_unique_model_name(world, camera_type)
         self._model_name = model_name
+        self._camera_type = camera_type
 
         # Initial pose
-        initial_pose = scenario.Pose(position, orientation)
+        initial_pose = scenario_core.Pose(position, orientation)
 
+        use_mesh: bool = False
+        mesh_path_visual: str = ""
+
+        albedo_map = None
+        normal_map = None
+        roughness_map = None
+        metalness_map = None
         # Get resources for visual (if enabled)
         if visual:
-            use_mesh: bool = False
             if "intel_realsense_d435" == visual:
                 use_mesh = True
 
@@ -71,10 +77,6 @@ class Camera(model_wrapper.ModelWrapper):
                     )
 
                 # Find PBR textures
-                albedo_map = None
-                normal_map = None
-                roughness_map = None
-                metalness_map = None
                 if texture_dir:
                     # List all files
                     texture_files = os.listdir(texture_dir)
@@ -94,7 +96,7 @@ class Camera(model_wrapper.ModelWrapper):
                             metalness_map = os.path.join(texture_dir, texture)
 
                 if not (albedo_map and normal_map and roughness_map and metalness_map):
-                    raise ValueError(f"Not all textures for Camera model were found.")
+                    raise ValueError("Not all textures for Camera model were found.")
 
         # Create SDF string for the model
         sdf = f'''<sdf version="1.9">
@@ -225,6 +227,17 @@ class Camera(model_wrapper.ModelWrapper):
 
         if ros2_bridge_color or ros2_bridge_depth or ros2_bridge_points:
             self.__threads = []
+            self.__threads.append(
+                Thread(
+                    target=self.construct_ros2_bridge,
+                    args=(
+                        self.info_topic,
+                        "sensor_msgs/msg/CameraInfo",
+                        "gz.msgs.CameraInfo",
+                    ),
+                    daemon=True,
+                )
+            )
             if ros2_bridge_color:
                 self.__threads.append(
                     Thread(
@@ -232,7 +245,7 @@ class Camera(model_wrapper.ModelWrapper):
                         args=(
                             self.color_topic,
                             "sensor_msgs/msg/Image",
-                            "ignition.msgs.Image",
+                            "gz.msgs.Image",
                         ),
                         daemon=True,
                     )
@@ -245,7 +258,7 @@ class Camera(model_wrapper.ModelWrapper):
                         args=(
                             self.depth_topic,
                             "sensor_msgs/msg/Image",
-                            "ignition.msgs.Image",
+                            "gz.msgs.Image",
                         ),
                         daemon=True,
                     )
@@ -258,7 +271,7 @@ class Camera(model_wrapper.ModelWrapper):
                         args=(
                             self.points_topic,
                             "sensor_msgs/msg/PointCloud2",
-                            "ignition.msgs.PointCloudPacked",
+                            "gz.msgs.PointCloudPacked",
                         ),
                         daemon=True,
                     )
@@ -273,10 +286,10 @@ class Camera(model_wrapper.ModelWrapper):
                 thread.join()
 
     @classmethod
-    def construct_ros2_bridge(self, topic: str, ros_msg: str, ign_msg: str):
+    def construct_ros2_bridge(cls, topic: str, ros_msg: str, ign_msg: str):
         node_name = "parameter_bridge" + topic.replace("/", "_")
         command = (
-            f"ros2 run ros_ign_bridge parameter_bridge {topic}@{ros_msg}[{ign_msg} "
+            f"ros2 run ros_gz_bridge parameter_bridge {topic}@{ros_msg}[{ign_msg} "
             + f"--ros-args --remap __node:={node_name} --ros-args -p use_sim_time:=true"
         )
         os.system(command)
@@ -290,22 +303,22 @@ class Camera(model_wrapper.ModelWrapper):
         return self.get_frame_id(self._model_name)
 
     @classmethod
-    def get_color_topic(cls, model_name: str) -> str:
+    def get_color_topic(cls, model_name: str, camera_type: str) -> str:
-        return f"/{model_name}/image" if "rgbd" in model_name else f"/{model_name}"
+        return f"/{model_name}/image" if "rgbd" in camera_type else f"/{model_name}"
 
     @property
     def color_topic(self) -> str:
-        return self.get_color_topic(self._model_name)
+        return self.get_color_topic(self._model_name, self._camera_type)
 
     @classmethod
-    def get_depth_topic(cls, model_name: str) -> str:
+    def get_depth_topic(cls, model_name: str, camera_type: str) -> str:
         return (
-            f"/{model_name}/depth_image" if "rgbd" in model_name else f"/{model_name}"
+            f"/{model_name}/depth_image" if "rgbd" in camera_type else f"/{model_name}"
         )
 
     @property
     def depth_topic(self) -> str:
-        return self.get_depth_topic(self._model_name)
+        return self.get_depth_topic(self._model_name, self._camera_type)
 
     @classmethod
     def get_points_topic(cls, model_name: str) -> str:
@@ -315,6 +328,10 @@ class Camera(model_wrapper.ModelWrapper):
     def points_topic(self) -> str:
         return self.get_points_topic(self._model_name)
 
+    @property
+    def info_topic(self):
+        return f"/{self._model_name}/camera_info"
+
     @classmethod
     def get_link_name(cls, model_name: str) -> str:
         return f"{model_name}_link"

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

@@ -7,7 +7,7 @@ from .random_ground import RandomGround
 from .random_lunar_surface import RandomLunarSurface
 
 
-def get_terrain_model_class(terrain_type: str) -> ModelWrapper:
+def get_terrain_model_class(terrain_type: str) -> type[ModelWrapper]:
     # TODO: Refactor into enum
 
     if "flat" == terrain_type:
@@ -20,6 +20,8 @@ def get_terrain_model_class(terrain_type: str) -> ModelWrapper:
         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:

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

@@ -0,0 +1,120 @@
+import abc
+
+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 rbs_gym.envs.observation import CameraSubscriber
+from rbs_gym.envs.tasks.reach import Reach
+
+
+class ReachColorImage(Reach, abc.ABC):
+    def __init__(
+        self,
+        camera_info: list[dict] = [],
+        monochromatic: bool = False,
+        **kwargs,
+    ):
+        # Initialize the Task base class
+        Reach.__init__(
+            self,
+            **kwargs,
+        )
+
+        # Store parameters for later use
+        self._camera_width = sum(w.get("width", 0) for w in camera_info)
+        self._camera_height = camera_info[0]["height"]
+        self._monochromatic = monochromatic
+        self._saved = False
+
+        self.camera_subs: list[CameraSubscriber] = []
+        for camera in camera_info:
+            # Perception (RGB camera)
+            self.camera_subs.append(
+                CameraSubscriber(
+                    node=self,
+                    topic=Camera.get_color_topic(camera["name"], camera["type"]),
+                    is_point_cloud=False,
+                    callback_group=self._callback_group,
+                )
+            )
+
+    def create_observation_space(self) -> ObservationSpace:
+        """
+        Creates the observation space for the Imitation Learning algorithm.
+
+        This method returns a dictionary-based observation space that includes:
+
+        - **image**: A 2D image captured from the robot's camera. The image's shape is determined by the camera's
+          width and height, and the number of channels (either monochromatic or RGB).
+          - Dimensions: `(camera_height, camera_width, channels)` where `channels` is 1 for monochromatic images
+            and 3 for RGB images.
+          - Pixel values are in the range `[0, 255]` with data type `np.uint8`.
+
+        - **joint_states**: The joint positions of the robot arm, represented as continuous values within the
+          range `[-1.0, 1.0]`, where `nDoF` refers to the number of degrees of freedom of the robot arm.
+
+        Returns:
+            gym.spaces.Dict: A dictionary defining the observation space for the learning algorithm,
+            containing both image and joint_states information.
+        """
+        return gym.spaces.Dict(
+            {
+                "image": gym.spaces.Box(
+                    low=0,
+                    high=255,
+                    shape=(
+                        self._camera_height,
+                        self._camera_width,
+                        1 if self._monochromatic else 3,
+                    ),
+                    dtype=np.uint8,
+                ),
+                "joint_states": gym.spaces.Box(low=-1.0, high=1.0),
+            }
+        )
+
+    def get_observation(self) -> Observation:
+        # Get the latest images
+        image = []
+
+        for sub in self.camera_subs:
+            image.append(sub.get_observation())
+
+        image_width = sum(i.width for i in image)
+        image_height = image[0].height
+        # if image_width == self._camera_width and image_height == self._camera_height:
+
+        # image_data = np.concatenate([i.data for i in image], axis=1)
+
+        assert (
+            image_width == self._camera_width and image_height == self._camera_height
+        ), f"Error: Resolution of the input image does not match the configured observation space. ({image_width}x{image_height} instead of {self._camera_width}x{self._camera_height})"
+
+        # Reshape and create the observation
+        # color_image = np.array([i.data for i in image], dtype=np.uint8).reshape(
+        #     self._camera_height, self._camera_width, 3
+        # )
+        color_image = np.concatenate(
+            [
+                np.array(i.data, dtype=np.uint8).reshape(i.height, i.width, 3)
+                for i in image
+            ],
+            axis=1,
+        )
+
+        # # Debug save images
+        # from PIL import Image
+        # img_color = Image.fromarray(color_image)
+        # img_color.save("img_color.png")
+
+        if self._monochromatic:
+            observation = Observation(color_image[:, :, 0])
+        else:
+            observation = Observation(color_image)
+
+        self.get_logger().debug(f"\nobservation: {observation}")
+
+        # Return the observation
+        return observation

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

@@ -12,9 +12,7 @@ from rbs_gym.envs.tasks.reach import Reach
 class ReachColorImage(Reach, abc.ABC):
     def __init__(
         self,
-        camera_width: int,
+        camera_info: list[dict] = [],
-        camera_height: int,
-        camera_type: str = "camera",
         monochromatic: bool = False,
         **kwargs,
     ):
@@ -26,17 +24,20 @@ class ReachColorImage(Reach, abc.ABC):
         )
 
         # Store parameters for later use
-        self._camera_width = camera_width
+        self._camera_width = sum(w.get('width', 0) for w in camera_info)
-        self._camera_height = camera_height
+        self._camera_height = camera_info[0]["height"]
         self._monochromatic = monochromatic
+        self._saved = False
 
-        # Perception (RGB camera)
+        self.camera_subs: list[CameraSubscriber] = []
-        self.camera_sub = CameraSubscriber(
+        for camera in camera_info:
-            node=self,
+            # Perception (RGB camera)
-            topic=Camera.get_color_topic(camera_type),
+            self.camera_subs.append(CameraSubscriber(
-            is_point_cloud=False,
+                node=self,
-            callback_group=self._callback_group,
+                topic=Camera.get_color_topic(camera["name"], camera["type"]),
-        )
+                is_point_cloud=False,
+                callback_group=self._callback_group,
+            ))
 
     def create_observation_space(self) -> ObservationSpace:
 
@@ -54,17 +55,22 @@ class ReachColorImage(Reach, abc.ABC):
         )
 
     def get_observation(self) -> Observation:
+        # Get the latest images
+        image = []
 
-        # Get the latest image
+        for sub in self.camera_subs:
-        image = self.camera_sub.get_observation()
+            image.append(sub.get_observation())
+
+        image_width = sum(i.width for i in image)
+        image_height = image[0].height
 
         assert (
-            image.width == self._camera_width and image.height == self._camera_height
+            image_width == self._camera_width and image_height == self._camera_height
-        ), f"Error: Resolution of the input image does not match the configured observation space. ({image.width}x{image.height} instead of {self._camera_width}x{self._camera_height})"
+        ), f"Error: Resolution of the input image does not match the configured observation space. ({image_width}x{image_height} instead of {self._camera_width}x{self._camera_height})"
 
-        # Reshape and create the observation
+        color_image = np.concatenate(
-        color_image = np.array(image.data, dtype=np.uint8).reshape(
+            [np.array(i.data, dtype=np.uint8).reshape(i.height, i.width, 3) for i in image],
-            self._camera_height, self._camera_width, 3
+            axis=1
         )
 
         # # Debug save images

env_manager/rbs_gym/rbs_gym/envs/utils/conversions.py

@@ -164,13 +164,12 @@ def orientation_quat_to_6d(
 
 
 def quat_to_wxyz(
-    xyzw: Union[numpy.ndarray, Tuple[float, float, float, float]]
+    xyzw:  tuple[float, float, float, float]
-) -> numpy.ndarray:
+) -> tuple[float, float, float, float]:
 
-    if isinstance(xyzw, tuple):
+    return (xyzw[3], xyzw[0], xyzw[1], xyzw[2])
-        return (xyzw[3], xyzw[0], xyzw[1], xyzw[2])
 
-    return xyzw[[3, 0, 1, 2]]
+    # return xyzw[[3, 0, 1, 2]]
 
 
 def quat_to_xyzw(

env_manager/rbs_gym/rbs_gym/envs/utils/gazebo.py

@@ -103,12 +103,12 @@ def get_model_orientation(
 
 def transform_move_to_model_pose(
     world: World,
-    position: Tuple[float, float, float],
+    position: tuple[float, float, float],
-    quat: Tuple[float, float, float, float],
+    quat: tuple[float, float, float, float],
-    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]]:
+) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]:
     """
     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.

env_manager/rbs_gym/rbs_gym/envs/worlds/default.sdf

@@ -14,7 +14,7 @@
     <!--       -->
     <!-- Scene -->
     <!--       -->
-    <gravity>0 0 0</gravity>
+    <!-- <gravity>0 0 0</gravity> -->
     <scene>
       <ambient>1.0 1.0 1.0</ambient>
       <grid>false</grid>

env_manager/rbs_gym/scripts/test_agent.py

@@ -10,10 +10,13 @@ from rbs_gym import envs as gz_envs
 from rbs_gym.utils.utils import StoreDict, str2bool
 
 
-def main(args: Dict):
+def main(args: argparse.Namespace):
 
     # Create the environment
-    env = gym.make(args.env, **args.env_kwargs)
+    if args.env_kwargs is not None:
+        env = gym.make(args.env, **args.env_kwargs)
+    else:
+        env = gym.make(args.env)
 
     # Initialize random seed
     env.seed(args.seed)
@@ -58,7 +61,7 @@ if __name__ == "__main__":
 
     # Environment and its parameters
     parser.add_argument(
-        "--env", type=str, default="Reach-Gazebo-v0", help="Environment ID"
+        "--env", type=str, default="Reach-ColorImage-Gazebo-v0", help="Environment ID"
     )
     parser.add_argument(
         "--env-kwargs",

rbs_bringup/launch/rbs_robot.launch.py

@@ -158,12 +158,9 @@ def launch_setup(context, *args, **kwargs):
             ])
         ]),
         launch_arguments={
-            'with_gripper': with_gripper_condition,
+            'control_space': "task",
-            'robot_description': robot_description_content,
+            'control_strategy': "effort",
-            'start_joint_controller': start_joint_controller,
+            'is_gripper': "false",
-            'initial_joint_controller': initial_joint_controller,
-            'controllers_file': controllers_file,
-            'cartesian_controllers': cartesian_controllers,
             'namespace': namespace,
         }.items(),
         condition=IfCondition(launch_controllers))
@@ -234,6 +231,7 @@ def launch_setup(context, *args, **kwargs):
 
     asm_config = Node(
         package="rbs_utils",
+        namespace=namespace,
         executable="assembly_config_service.py"
     )
 

rbs_bringup/launch/single_robot.launch.py

@@ -11,7 +11,7 @@ from launch_ros.substitutions import FindPackageShare
 from launch_ros.actions import Node
 import os
 from ament_index_python.packages import get_package_share_directory
-from nav2_common.launch import RewrittenYaml
+# from nav2_common.launch import RewrittenYaml
 
 def launch_setup(context, *args, **kwargs):
     # Initialize Arguments
@@ -85,6 +85,7 @@ def launch_setup(context, *args, **kwargs):
     gz_spawner = Node(
         package='ros_gz_sim',
         executable='create',
+        # prefix=['gdbserver localhost:1234'],
         arguments=[
             '-name', robot_name.perform(context),
             '-topic', namespace + '/robot_description'],
@@ -103,7 +104,7 @@ def launch_setup(context, *args, **kwargs):
             "env_manager": env_manager,
             "with_gripper": with_gripper_condition,
             "gripper_name": gripper_name,
-            "controllers_file": controllers_file,
+            "controllers_file": initial_joint_controllers_file_path,
             "robot_type": robot_type,
             # "controllers_file": controller_paramfile,
             "cartesian_controllers": cartesian_controllers,

rbs_bt_executor/bt_trees/test_tree.xml

@@ -2,10 +2,19 @@
 <root BTCPP_format="4">
     <BehaviorTree ID="TestTree">
         <Sequence>
-      <Action ID="EnvManager" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment" 
+      <!-- <Action ID="EnvManager" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment"  -->
-        service_name="/env_manager/start_env" />
+        <!-- service_name="/env_manager/start_env" /> -->
-      <Action ID="GetWorkspace" type="box" service_name="/get_workspace" workspace="{workspace}"/>
+          <SubTree ID="WorkspaceMovement" robot_name="arm1" />
-      <SubTree ID="WorkspaceMovement" goal_pose="{workspace}" robot_name="rbs_arm" _autoremap="1" />
         </Sequence>
     </BehaviorTree>
+
+  <TreeNodesModel>
+
+    <Action ID="GetWorkspace">
+      <input_port name="type"/>
+      <input_port name="service_name"/>
+      <output_port name="workspace" />
+    </Action>
+
+  </TreeNodesModel>
   </root>

rbs_bt_executor/bt_trees/workspace_movement.xml

@@ -2,16 +2,32 @@
 <root BTCPP_format="4">
   <BehaviorTree ID="WorkspaceMovement">
     <Sequence>
-      <Action ID="MoveToPoseArray" pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}" 
+
-      robot_name="{robot_name}" action_name="/move_topose"  />
+      <!-- <Script code="action:='/'+robot_name+'/cartesian_move_to_pose'" /> -->
+      <!-- <Script code="get_workspace:='/'+robot_name+'/get_workspace'"/> -->
+
+      <Script code="action:='/cartesian_move_to_pose'"/>
+      <Script code="get_workspace:='/get_workspace'"/>
+
+      <Action ID="GetWorkspace" type="box" service_name="{get_workspace}" workspace="{workspace}"/>
+
+      <Action ID="MoveToPoseArray" pose_vec_in="{workspace}" pose_vec_out="{goal_pose}" 
+        robot_name="{robot_name}" action_name="{action}"  />
       <Action ID="MoveToPoseArray"  pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}" 
-      robot_name="{robot_name}" action_name="/move_cartesian"  />
+        robot_name="{robot_name}" action_name="{action}"  />
       <Action ID="MoveToPoseArray"  pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}" 
-      robot_name="{robot_name}" action_name="/move_cartesian"  />
+        robot_name="{robot_name}" action_name="{action}"  />
       <Action ID="MoveToPoseArray"  pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}" 
-      robot_name="{robot_name}" action_name="/move_cartesian"  />
+        robot_name="{robot_name}" action_name="{action}"  />
-      <Action ID="MoveToPoseArray"  pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}" 
-      robot_name="{robot_name}" action_name="/move_cartesian"  />
     </Sequence>
   </BehaviorTree>
+
+  <TreeNodesModel>
+    <Action ID="MoveToPoseArray">
+      <input_port name="pose_vec_in" />
+      <output_port name="pose_vec_out" />
+      <input_port name="robot_name" />
+      <input_port name="action_name" />
+    </Action>
+  </TreeNodesModel>
 </root>

rbs_bt_executor/src/MoveToPoseArray.cpp

@@ -32,8 +32,8 @@ public:
       return false;
     }
     goal.target_pose = m_pose_vec->poses[0];
-    goal.end_effector_velocity = 1.0;
+    goal.end_effector_velocity = 0.3;
-    goal.end_effector_acceleration = 1.0;
+    goal.end_effector_acceleration = 0.3;
     m_pose_vec->poses.erase(m_pose_vec->poses.begin());
 
     setOutput("pose_vec_out", m_pose_vec);

rbs_simulation/worlds/asm2.sdf

@@ -103,16 +103,16 @@
       </link>
     </model>
     <!-- Manipulating objects -->
-			<include>
+			<!-- <include> -->
-				<name>board</name>
+			<!-- 	<name>board</name> -->
-				<uri>model://board</uri>
+			<!-- 	<uri>model://board</uri> -->
-        <pose>0.45 0.0 0.0 0.0 0.0 0.0</pose>
+   <!--      <pose>0.45 0.0 0.0 0.0 0.0 0.0</pose> -->
-			</include>
+			<!-- </include> -->
-			<include>
+			<!-- <include> -->
-				<name>bishop</name>
+			<!-- 	<name>bishop</name> -->
-				<uri>model://bishop</uri>
+			<!-- 	<uri>model://bishop</uri> -->
-        <pose>0.35 0.0 0.0 0.0 0.0 0.0</pose>
+   <!--      <pose>0.35 0.0 0.0 0.0 0.0 0.0</pose> -->
-			</include>
+			<!-- </include> -->
     <!-- <include> -->
     <!--   <name>bishop</name> -->
     <!--   <uri>model://box1</uri> -->

rbs_skill_servers/CMakeLists.txt

@@ -34,7 +34,7 @@ find_package(tinyxml2_vendor REQUIRED)
 find_package(TinyXML2 REQUIRED)
 find_package(Eigen3 3.3 REQUIRED)
 find_package(rbs_utils REQUIRED)
-find_package(moveit_servo REQUIRED)
+# find_package(moveit_servo REQUIRED)
 
 # Default to Fortress
 set(SDF_VER 12)
@@ -80,7 +80,7 @@ set(deps
     moveit_ros_planning
     moveit_ros_planning_interface
     moveit_msgs
-    moveit_servo
+    # moveit_servo
     geometry_msgs
     tf2_ros
     rclcpp_components

rbs_skill_servers/launch/skills.launch.py

@@ -88,15 +88,17 @@ def launch_setup(context, *args, **kwargs):
             {"use_sim_time": use_sim_time},
         ]
     )
-        
+
-    grasp_pose_loader = Node(
+    assembly_config = Node(
-        package="rbs_skill_servers",
+        package="rbs_utils",
-        executable="pick_place_pose_loader_service_server",
+        executable="assembly_config_service.py",
         namespace=namespace,
         output="screen"
     )
+        
 
     nodes_to_start =[
+        assembly_config,
         move_topose_action_server,
         gripper_control_node,
         move_cartesian_path_action_server,

rbs_skill_servers/package.xml

@@ -13,7 +13,7 @@
   <depend>moveit_core</depend>
   <depend>moveit_ros_planning</depend>
   <depend>moveit_ros_planning_interface</depend>
-  <depend>moveit_servo</depend>
+  <!-- <depend>moveit_servo</depend> -->
   <depend>moveit_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>rclcpp_action</depend>

rbs_skill_servers/scripts/move_to_pose.py

@@ -48,11 +48,13 @@ class CartesianMoveToPose(Node):
         self.get_logger().debug(f"Executing goal {goal_handle.request.target_pose}")
         target_pose = goal_handle.request.target_pose
         start_pose = self.current_pose.pose if self.current_pose else None
+        result = MoveitSendPose.Result()
 
         if start_pose is None:
             self.get_logger().error("Current pose is not available")
             goal_handle.abort()
-            return MoveitSendPose.Result()
+            result.success = False
+            return result
 
         trajectory = self.generate_trajectory(start_pose, target_pose)
         for point in trajectory:
@@ -64,8 +66,6 @@ class CartesianMoveToPose(Node):
             rclpy.spin_once(self, timeout_sec=0.1)
 
         goal_handle.succeed()
-
-        result = MoveitSendPose.Result()
         result.success = True
         return result
 

rbs_skill_servers/scripts/test_cartesian_controller.py

@@ -39,7 +39,7 @@ class CartesianControllerPublisher(Node):
 def main(args=None):
     rclpy.init(args=args)
 
-    parser = argparse.ArgumentParser(description='ROS2 Minimal Publisher')
+    parser = argparse.ArgumentParser()
     parser.add_argument('--robot-name', type=str, default='arm0', help='Specify the robot name')
     args = parser.parse_args()
 

rbs_utils/rbs_utils/scripts/assembly_config_service.py

@@ -24,6 +24,7 @@ class AssemblyConfigService(Node):
         self.grasp_pose_service = self.create_service(GetGraspPose, "get_grasp_pose", self.get_grasp_pose_callback)
 
     def get_workspace_callback(self, request, response):
+        self.get_logger().info("Workspace request received")
         response.workspace = self.assembly_config.workspace
         response.ok = True
         return response