refactor launch configuration for robot and environment setup

- Simplified and consolidated launch arguments by removing unused configurations.
- Enhanced `test_env.launch.py` to support dynamic loading of xacro and SRDF arguments via YAML files.
- Added new launch parameters for control space, strategy, and interactive mode to improve flexibility.
- Updated robot description and MoveIt integration to use dynamic configurations.
- Improved clarity and modularity in scene and robot setup processes.

env_manager/rbs_gym/launch/test_env.launch.py

@@ -15,6 +15,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
+import yaml
 
 
 def launch_setup(context, *args, **kwargs):
@@ -22,25 +23,21 @@ def launch_setup(context, *args, **kwargs):
     robot_type = LaunchConfiguration("robot_type")
     # General arguments
     with_gripper_condition = LaunchConfiguration("with_gripper")
-    controllers_file = LaunchConfiguration("controllers_file")
-    cartesian_controllers = LaunchConfiguration("cartesian_controllers")
     description_package = LaunchConfiguration("description_package")
     description_file = LaunchConfiguration("description_file")
-    robot_name = LaunchConfiguration("robot_name")
+    use_moveit = LaunchConfiguration("use_moveit")
-    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")
+    scene_config_file = LaunchConfiguration("scene_config_file").perform(context)
-    hardware = LaunchConfiguration("hardware")
+
-    env_manager = LaunchConfiguration("env_manager")
+    ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
-    launch_controllers = LaunchConfiguration("launch_controllers")
+    base_link_name = LaunchConfiguration("base_link_name").perform(context)
-    gripper_name = LaunchConfiguration("gripper_name")
+
+    control_space = LaunchConfiguration("control_space").perform(context)
+    control_strategy = LaunchConfiguration("control_strategy").perform(context)
+
+    interactive = LaunchConfiguration("interactive").perform(context)
 
     # training arguments
     env = LaunchConfiguration("env")
@@ -48,28 +45,72 @@ def launch_setup(context, *args, **kwargs):
     log_level = LaunchConfiguration("log_level")
     env_kwargs = LaunchConfiguration("env_kwargs")
 
-    sim_gazebo = LaunchConfiguration("sim_gazebo")
+    description_package_abs_path = get_package_share_directory(
-    launch_simulation = LaunchConfiguration("launch_sim")
+        description_package.perform(context)
-
-    initial_joint_controllers_file_path = os.path.join(
-        get_package_share_directory("rbs_arm"), "config", "controllers.yaml"
     )
 
+    simulation_controllers = os.path.join(
+        description_package_abs_path, "config", "controllers.yaml"
+    )
+
+
     xacro_file = os.path.join(
-        get_package_share_directory(description_package.perform(context)),
+        description_package_abs_path,
         "urdf",
         description_file.perform(context),
     )
 
-    robot_description_doc = xacro.process_file(
+    xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml"
-        xacro_file,
+
-        mappings={
+
-            "gripper_name": gripper_name.perform(context),
+    # TODO: hide this to another place
-            "hardware": hardware.perform(context),
+    # Load xacro_args
-            "simulation_controllers": initial_joint_controllers_file_path,
+    def param_constructor(loader, node, local_vars):
-            "namespace": "",
+        value = loader.construct_scalar(node)
-        },
+        return LaunchConfiguration(value).perform(
-    )
+            local_vars.get("context", "Launch context if not defined")
+        )
+
+    def variable_constructor(loader, node, local_vars):
+        value = loader.construct_scalar(node)
+        return local_vars.get(value, f"Variable '{value}' not found")
+
+    def load_xacro_args(yaml_file, local_vars):
+        # Get valut from ros2 argument
+        yaml.add_constructor(
+            "!param", lambda loader, node: param_constructor(loader, node, local_vars)
+        )
+
+        # Get value from local variable in this code
+        # The local variable should be initialized before the loader was called
+        yaml.add_constructor(
+            "!variable",
+            lambda loader, node: variable_constructor(loader, node, local_vars),
+        )
+
+        with open(yaml_file, "r") as file:
+            return yaml.load(file, Loader=yaml.FullLoader)
+
+    mappings_data = load_xacro_args(xacro_config_file, locals())
+
+    robot_description_doc = xacro.process_file(xacro_file, mappings=mappings_data)
+
+    robot_description_semantic_content = ""
+
+    if use_moveit.perform(context) == "true":
+        srdf_config_file = f"{description_package_abs_path}/config/srdf_xacro_args.yaml"
+        srdf_file = os.path.join(
+            get_package_share_directory(moveit_config_package.perform(context)),
+            "srdf",
+            moveit_config_file.perform(context),
+        )
+        srdf_mappings = load_xacro_args(srdf_config_file, locals())
+        robot_description_semantic_content = xacro.process_file(srdf_file, mappings=srdf_mappings)
+        robot_description_semantic_content = robot_description_semantic_content.toprettyxml(indent=" ")
+        control_space = "joint"
+        control_strategy = "position"
+        interactive = "false"
+
 
     robot_description_content = robot_description_doc.toprettyxml(indent="  ")
     robot_description = {"robot_description": robot_description_content}
@@ -83,28 +124,25 @@ def launch_setup(context, *args, **kwargs):
             ]
         ),
         launch_arguments={
-            "env_manager": env_manager,
             "with_gripper": with_gripper_condition,
-            "gripper_name": gripper_name,
+            "controllers_file": simulation_controllers,
-            "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_type,
-            "start_joint_controller": start_joint_controller,
+            "use_moveit": "false",
-            "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,
+            "use_sim_time": "true",
-            "sim_gazebo": sim_gazebo,
+            "use_skills": "false",
-            "hardware": hardware,
+            "use_controllers": "true",
-            "launch_controllers": "true",
             "robot_description": robot_description_content,
+            "robot_description_semantic": robot_description_semantic_content,
+            "base_link_name": base_link_name,
+            "ee_link_name": ee_link_name,
+            "control_space": control_space,
+            "control_strategy": "effort",
+            "interactive_control": "false",
         }.items(),
     )
 
@@ -137,7 +175,7 @@ def launch_setup(context, *args, **kwargs):
 
     nodes_to_start = [
         # env,
-        rl_task,
+        # rl_task,
         clock_bridge,
         delay_robot_control_stack,
     ]
@@ -150,18 +188,20 @@ 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",
+                "ar4",
+                "ur3",
+                "ur3e",
+                "ur5",
+                "ur5e",
+                "ur10",
+                "ur10e",
+                "ur16e",
+            ],
             default_value="rbs_arm",
         )
     )
-    # General arguments
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "controllers_file",
-            default_value="controllers_gazebosim.yaml",
-            description="YAML file with the controllers configuration.",
-        )
-    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "description_package",
@@ -184,20 +224,6 @@ def generate_launch_description():
             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",
@@ -221,14 +247,6 @@ def generate_launch_description():
             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?"
@@ -236,25 +254,7 @@ def generate_launch_description():
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "sim_gazebo", default_value="true", description="Gazebo Simulation"
+            "use_moveit", default_value="false", description="Launch moveit?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "env_manager", default_value="false", description="Launch env_manager?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_sim",
-            default_value="true",
-            description="Launch simulator (Gazebo)?\
-                              Most general arg",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_moveit", default_value="false", description="Launch moveit?"
         )
     )
     declared_arguments.append(
@@ -264,39 +264,55 @@ def generate_launch_description():
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "launch_task_planner",
+            "use_controllers",
-            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?"
+            "scene_config_file",
+            default_value="",
+            description="Path to a scene configuration file",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ee_link_name",
+            default_value="",
+            description="End effector name of robot arm",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "base_link_name",
+            default_value="",
+            description="Base link name if robot arm",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_space",
+            default_value="task",
+            choices=["task", "joint"],
+            description="Specify the control space for the robot (e.g., task space).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_strategy",
+            default_value="position",
+            choices=["position", "velocity", "effort"],
+            description="Specify the control strategy (e.g., position control).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "interactive",
+            default_value="true",
+            description="Wheter to run the motion_control_handle controller",
+        ),
+    )
     # training arguments
     declared_arguments.append(
         DeclareLaunchArgument(

env_manager/rbs_gym/launch/train.launch.py

@@ -13,31 +13,29 @@ from launch_ros.actions import Node
 import os
 from os import cpu_count
 from ament_index_python.packages import get_package_share_directory
+import yaml
+import xacro
 
 def launch_setup(context, *args, **kwargs):
     # Initialize Arguments
     robot_type = LaunchConfiguration("robot_type")
     # General arguments
     with_gripper_condition = LaunchConfiguration("with_gripper")
-    controllers_file = LaunchConfiguration("controllers_file")
-    cartesian_controllers = LaunchConfiguration("cartesian_controllers")
     description_package = LaunchConfiguration("description_package")
     description_file = LaunchConfiguration("description_file")
-    robot_name = LaunchConfiguration("robot_name")
+    use_moveit = LaunchConfiguration("use_moveit")
-    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")
+    scene_config_file = LaunchConfiguration("scene_config_file").perform(context)
-    hardware = LaunchConfiguration("hardware")
+
-    env_manager = LaunchConfiguration("env_manager")
+    ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
-    launch_controllers = LaunchConfiguration("launch_controllers")
+    base_link_name = LaunchConfiguration("base_link_name").perform(context)
-    gripper_name = LaunchConfiguration("gripper_name")
+
+    control_space = LaunchConfiguration("control_space").perform(context)
+    control_strategy = LaunchConfiguration("control_strategy").perform(context)
+
+    interactive = LaunchConfiguration("interactive").perform(context)
 
     # training arguments
     env = LaunchConfiguration("env")
@@ -64,47 +62,104 @@ def launch_setup(context, *args, **kwargs):
 
     track = LaunchConfiguration("track")
 
-
+    description_package_abs_path = get_package_share_directory(
-    sim_gazebo = LaunchConfiguration("sim_gazebo")
+        description_package.perform(context)
-    launch_simulation = LaunchConfiguration("launch_sim")
-
-    initial_joint_controllers_file_path = os.path.join(
-        get_package_share_directory('rbs_arm'), 'config', 'controllers.yaml'
     )
 
+    simulation_controllers = os.path.join(
+        description_package_abs_path, "config", "controllers.yaml"
+    )
+
+
+    xacro_file = os.path.join(
+        description_package_abs_path,
+        "urdf",
+        description_file.perform(context),
+    )
+
+    xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml"
+
+
+    # TODO: hide this to another place
+    # Load xacro_args
+    def param_constructor(loader, node, local_vars):
+        value = loader.construct_scalar(node)
+        return LaunchConfiguration(value).perform(
+            local_vars.get("context", "Launch context if not defined")
+        )
+
+    def variable_constructor(loader, node, local_vars):
+        value = loader.construct_scalar(node)
+        return local_vars.get(value, f"Variable '{value}' not found")
+
+    def load_xacro_args(yaml_file, local_vars):
+        # Get valut from ros2 argument
+        yaml.add_constructor(
+            "!param", lambda loader, node: param_constructor(loader, node, local_vars)
+        )
+
+        # Get value from local variable in this code
+        # The local variable should be initialized before the loader was called
+        yaml.add_constructor(
+            "!variable",
+            lambda loader, node: variable_constructor(loader, node, local_vars),
+        )
+
+        with open(yaml_file, "r") as file:
+            return yaml.load(file, Loader=yaml.FullLoader)
+
+    mappings_data = load_xacro_args(xacro_config_file, locals())
+
+    robot_description_doc = xacro.process_file(xacro_file, mappings=mappings_data)
+
+    robot_description_semantic_content = ""
+
+    if use_moveit.perform(context) == "true":
+        srdf_config_file = f"{description_package_abs_path}/config/srdf_xacro_args.yaml"
+        srdf_file = os.path.join(
+            get_package_share_directory(moveit_config_package.perform(context)),
+            "srdf",
+            moveit_config_file.perform(context),
+        )
+        srdf_mappings = load_xacro_args(srdf_config_file, locals())
+        robot_description_semantic_content = xacro.process_file(srdf_file, mappings=srdf_mappings)
+        robot_description_semantic_content = robot_description_semantic_content.toprettyxml(indent=" ")
+        control_space = "joint"
+        control_strategy = "position"
+        interactive = "false"
+
+
+    robot_description_content = robot_description_doc.toprettyxml(indent="  ")
+    robot_description = {"robot_description": robot_description_content}
+
     single_robot_setup = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([
+        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": simulation_controllers,
-            "controllers_file": controllers_file,
             "robot_type": robot_type,
-            "controllers_file": initial_joint_controllers_file_path,
-            "cartesian_controllers": cartesian_controllers,
             "description_package": description_package,
             "description_file": description_file,
-            "robot_name": robot_name,
+            "robot_name": robot_type,
-            "start_joint_controller": start_joint_controller,
+            "use_moveit": use_moveit,
-            "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,
+            "use_controllers": "true",
-            "hardware": hardware,
+            "robot_description": robot_description_content,
-            "launch_controllers": launch_controllers,
+            "robot_description_semantic": robot_description_semantic_content,
-            # "gazebo_gui": gazebo_gui
+            "base_link_name": base_link_name,
-        }.items()
+            "ee_link_name": ee_link_name,
+            "control_space": control_space,
+            "control_strategy": control_strategy,
+            "interactive_control": interactive,
+        }.items(),
     )
 
     args = [
@@ -188,18 +243,20 @@ 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",
+                "ar4",
+                "ur3",
+                "ur3e",
+                "ur5",
+                "ur5e",
+                "ur10",
+                "ur10e",
+                "ur16e",
+            ],
             default_value="rbs_arm",
         )
     )
-    # General arguments
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "controllers_file",
-            default_value="controllers_gazebosim.yaml",
-            description="YAML file with the controllers configuration.",
-        )
-    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "description_package",
@@ -222,20 +279,6 @@ def generate_launch_description():
             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",
@@ -261,69 +304,69 @@ def generate_launch_description():
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "gripper_name",
+            "with_gripper", default_value="true", description="With gripper or not?"
-            default_value="rbs_gripper",
-            choices=["rbs_gripper", ""],
-            description="choose gripper by name (leave empty if hasn't)",
         )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("with_gripper", 
+        DeclareLaunchArgument(
-                              default_value="true", 
+            "use_moveit", default_value="false", description="Launch moveit?"
-                              description="With gripper or not?")
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("sim_gazebo", 
+        DeclareLaunchArgument(
-                              default_value="true", 
+            "launch_perception", default_value="false", description="Launch perception?"
-                              description="Gazebo Simulation")
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("env_manager", 
+        DeclareLaunchArgument(
-                              default_value="false", 
+            "use_controllers",
-                              description="Launch env_manager?")
+            default_value="true",
+            description="Launch controllers?",
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_sim", 
+        DeclareLaunchArgument(
-                              default_value="true", 
+            "scene_config_file",
-                              description="Launch simulator (Gazebo)?\
+            default_value="",
-                              Most general arg")
+            description="Path to a scene configuration file",
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_moveit", 
+        DeclareLaunchArgument(
-                              default_value="false", 
+            "ee_link_name",
-                              description="Launch moveit?")
+            default_value="",
+            description="End effector name of robot arm",
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_perception", 
+        DeclareLaunchArgument(
-                              default_value="false", 
+            "base_link_name",
-                              description="Launch perception?")
+            default_value="",
+            description="Base link name if robot arm",
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_task_planner", 
+        DeclareLaunchArgument(
-                              default_value="false", 
+            "control_space",
-                              description="Launch task_planner?")
+            default_value="task",
+            choices=["task", "joint"],
+            description="Specify the control space for the robot (e.g., task space).",
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("cartesian_controllers", 
+        DeclareLaunchArgument(
-                              default_value="true", 
+            "control_strategy",
-                              description="Load cartesian\
+            default_value="position",
-                              controllers?")
+            choices=["position", "velocity", "effort"],
+            description="Specify the control strategy (e.g., position control).",
+        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("hardware", 
+        DeclareLaunchArgument(
-                              choices=["gazebo", "mock"],
+            "interactive",
-                              default_value="gazebo", 
+            default_value="true",
-                              description="Choose your harware_interface")
+            description="Wheter to run the motion_control_handle controller",
-    )
+        ),
-    declared_arguments.append(
-        DeclareLaunchArgument("launch_controllers", 
-                              default_value="true", 
-                              description="Launch controllers?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("gazebo_gui", 
-                              default_value="true", 
-                              description="Launch gazebo with gui?")
     )
     # training arguments
     declared_arguments.append(
@@ -448,13 +491,12 @@ def generate_launch_description():
             default_value="1",
             description="Verbose mode (0: no output, 1: INFO).",
         ))
-        # HER specifics
     declared_arguments.append(
         DeclareLaunchArgument(
             "truncate_last_trajectory",
             default_value="True",
             description="When using HER with online sampling the last trajectory in the replay buffer will be truncated after) reloading the replay buffer."
-        )),
+        ))
     declared_arguments.append(
         DeclareLaunchArgument(
             "log_level",

env_manager/rbs_gym/rbs_gym/envs/__init__.py

@@ -105,14 +105,14 @@ SCENE_CONFIGURATION: SceneData = SceneData(
         name="rbs_arm",
         joint_positions=[0.0, 0.5, 3.14159, 1.5, 0.0, 1.4, 0.0],
         with_gripper=True,
-        gripper_joint_positions=0.064,
+        gripper_joint_positions=0.00,
         randomizer=RobotRandomizerData(joint_positions=True),
     ),
     objects=[
         SphereObjectData(
             name="sphere",
             relative_to="base_link",
-            position=(0.0, 0.4, 1.0),
+            position=(0.0, 0.3, 0.5),
             static=True,
             collision=False,
             color=(0.0, 1.0, 0.0, 0.8),

env_manager/rbs_runtime/rbs_runtime/__init__.py

@@ -6,22 +6,25 @@ from env_manager.models.configs import SceneData
 
 from typing import Dict, Any
 from env_manager.models.configs import (
-    BoxObjectData, CylinderObjectData, MeshData, ModelData, ObjectData
+    BoxObjectData, CylinderObjectData, MeshData, ModelData, ObjectData, ObjectRandomizerData 
 )
 
 def object_factory(obj_data: Dict[str, Any]) -> ObjectData:
     obj_type = obj_data.get('type', '')
     
+    if "randomize" in obj_data and isinstance(obj_data["randomize"], dict):
+        obj_data["randomize"] = from_dict(data_class=ObjectRandomizerData, data=obj_data["randomize"])
+    
     if obj_type == 'box':
-        return BoxObjectData(**obj_data)
+        return from_dict(data_class=BoxObjectData, data=obj_data)
     elif obj_type == 'cylinder':
-        return CylinderObjectData(**obj_data)
+        return from_dict(data_class=CylinderObjectData, data=obj_data)
     elif obj_type == 'mesh':
-        return MeshData(**obj_data)
+        return from_dict(data_class=MeshData, data=obj_data)
     elif obj_type == 'model':
-        return ModelData(**obj_data)
+        return from_dict(data_class=ModelData, data=obj_data)
     else:
-        return ObjectData(**obj_data)
+        return from_dict(data_class=ObjectData, data=obj_data)
 
 def scene_config_loader(file: str | Path) -> SceneData:
     def tuple_constructor(loader, node):
@@ -35,6 +38,4 @@ def scene_config_loader(file: str | Path) -> SceneData:
 
     scene_data.objects = [object_factory(obj) for obj in scene_cfg.get('objects', [])]
 
-    print(scene_data)
-
     return scene_data

rbs_bringup/launch/rbs_bringup.launch.py

@@ -25,7 +25,7 @@ def launch_setup(context, *args, **kwargs):
             "description_package": "rbs_arm",
             "description_file": "rbs_arm_modular.xacro",
             "robot_name": "rbs_arm",
-            "use_moveit": "true",
+            "use_moveit": "false",
             "moveit_config_package": "rbs_arm",
             "moveit_config_file": "rbs_arm.srdf.xacro",
             "use_sim_time": "true",

rbs_bringup/launch/rbs_robot.launch.py

@@ -2,7 +2,7 @@ import os
 
 import xacro
 from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
+from launch import LaunchDescription, condition
 from launch.actions import (
     DeclareLaunchArgument,
     IncludeLaunchDescription,
@@ -30,6 +30,7 @@ def launch_setup(context, *args, **kwargs):
     moveit_config_package = LaunchConfiguration("moveit_config_package")
     use_sim_time = LaunchConfiguration("use_sim_time")
     use_controllers = LaunchConfiguration("use_controllers")
+    use_skills = LaunchConfiguration("use_skills")
     namespace = LaunchConfiguration("namespace")
     multi_robot = LaunchConfiguration("multi_robot")
     robot_name = robot_name.perform(context)
@@ -144,6 +145,7 @@ def launch_setup(context, *args, **kwargs):
             "ee_link_name": ee_link_name,
             "base_link_name": base_link_name,
         }.items(),
+        condition=IfCondition(use_skills),
     )
 
     nodes_to_start = [
@@ -247,6 +249,14 @@ def generate_launch_description():
             description="Specify if MoveIt should be launched.",
         )
     )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_skills",
+            default_value="true",
+            description="Specify if skills should be launched.",
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "use_controllers",