diff --git a/robo_gym/envs/mir100/mir100.py b/robo_gym/envs/mir100/mir100.py index 2c508f3e..21670857 100644 --- a/robo_gym/envs/mir100/mir100.py +++ b/robo_gym/envs/mir100/mir100.py @@ -7,7 +7,7 @@ import gymnasium as gym from gymnasium import spaces, logger from gymnasium.utils import seeding -from robo_gym.utils import utils, mir100_utils +from robo_gym.utils import utils, mir100_utils, cmd_utils from robo_gym.utils.exceptions import InvalidStateError, RobotServerError import robo_gym_server_modules.robot_server.client as rs_client from robo_gym.envs.simulation_wrapper import Simulation @@ -441,11 +441,14 @@ def _reward(self, rs_state, action): class NoObstacleNavigationMir100Sim(Simulation, NoObstacleNavigationMir100): - cmd = "roslaunch mir100_robot_server sim_robot_server.launch" - def __init__( self, ip=None, lower_bound_port=None, upper_bound_port=None, gui=False, **kwargs ): + self.cmd = cmd_utils.construct_roslaunch_command( + module='mir100_robot_server', + launch_file='sim_robot_server.launch' + ) + Simulation.__init__( self, self.cmd, ip, lower_bound_port, upper_bound_port, gui, **kwargs ) @@ -453,7 +456,6 @@ def __init__( self, rs_address=self.robot_server_ip, **kwargs ) - class NoObstacleNavigationMir100Rob(NoObstacleNavigationMir100): real_robot = True @@ -692,11 +694,18 @@ def _generate_obstacles_positions( class ObstacleAvoidanceMir100Sim(Simulation, ObstacleAvoidanceMir100): - cmd = "roslaunch mir100_robot_server sim_robot_server.launch world_name:=lab_6x8.world gazebo_gui:=true" - def __init__( self, ip=None, lower_bound_port=None, upper_bound_port=None, gui=False, **kwargs ): + self.cmd = cmd_utils.construct_roslaunch_command( + module='mir100_robot_server', + launch_file='sim_robot_server.launch', + launch_args={ + 'world_name': 'lab_6x8.world', + 'gazebo_gui': True, + } + ) + Simulation.__init__( self, self.cmd, ip, lower_bound_port, upper_bound_port, gui, **kwargs ) @@ -704,6 +713,5 @@ def __init__( self, rs_address=self.robot_server_ip, **kwargs ) - class ObstacleAvoidanceMir100Rob(ObstacleAvoidanceMir100): real_robot = True diff --git a/robo_gym/envs/ur/ur_avoidance_basic.py b/robo_gym/envs/ur/ur_avoidance_basic.py index bcb16368..528744a3 100644 --- a/robo_gym/envs/ur/ur_avoidance_basic.py +++ b/robo_gym/envs/ur/ur_avoidance_basic.py @@ -10,6 +10,7 @@ from __future__ import annotations import numpy as np from typing import Tuple, Any +from robo_gym.utils import cmd_utils from robo_gym_server_modules.robot_server.grpc_msgs.python import robot_server_pb2 from robo_gym.envs.simulation_wrapper import Simulation from robo_gym.envs.ur.ur_base_avoidance_env import URBaseAvoidanceEnv @@ -190,19 +191,6 @@ def step(self, action) -> Tuple[np.array, float, bool, bool, dict]: class BasicAvoidanceURSim(BasicAvoidanceUR, Simulation): - cmd = "roslaunch ur_robot_server ur_robot_server.launch \ - world_name:=tabletop_sphere50.world \ - reference_frame:=base_link \ - max_velocity_scale_factor:=0.2 \ - action_cycle_rate:=20 \ - rviz_gui:=false \ - gazebo_gui:=true \ - objects_controller:=true \ - rs_mode:=1moving2points \ - n_objects:=1.0 \ - object_0_model_name:=sphere50 \ - object_0_frame:=target" - def __init__( self, ip=None, @@ -212,7 +200,25 @@ def __init__( ur_model="ur5", **kwargs, ): - self.cmd = self.cmd + " " + "ur_model:=" + ur_model + self.cmd = cmd_utils.construct_roslaunch_command( + module='ur_robot_server', + launch_file='ur_robot_server.launch', + launch_args={ + 'world_name': 'tabletop_sphere50.world', + 'reference_frame': 'base_link', + 'max_velocity_scale_factor': 0.2, + 'action_cycle_rate': 20, + 'rviz_gui': False, + 'gazebo_gui': True, + 'objects_controller': True, + 'rs_mode': '1moving2points', + 'n_objects': 1.0, + 'object_0_model_name': 'sphere50', + 'object_0_frame': 'target', + 'ur_model': ur_model + } + ) + Simulation.__init__( self, self.cmd, ip, lower_bound_port, upper_bound_port, gui, **kwargs ) @@ -220,7 +226,6 @@ def __init__( self, rs_address=self.robot_server_ip, ur_model=ur_model, **kwargs ) - class BasicAvoidanceURRob(BasicAvoidanceUR): real_robot = True diff --git a/robo_gym/envs/ur/ur_avoidance_raad.py b/robo_gym/envs/ur/ur_avoidance_raad.py index fb9161bd..c70b68e8 100644 --- a/robo_gym/envs/ur/ur_avoidance_raad.py +++ b/robo_gym/envs/ur/ur_avoidance_raad.py @@ -12,6 +12,7 @@ import numpy as np import gymnasium as gym from typing import Tuple, Any +from robo_gym.utils import cmd_utils from robo_gym_server_modules.robot_server.grpc_msgs.python import robot_server_pb2 from robo_gym.envs.simulation_wrapper import Simulation from robo_gym.envs.ur.ur_base_avoidance_env import URBaseAvoidanceEnv @@ -386,19 +387,6 @@ def _get_joint_positions_as_array(self) -> np.ndarray: class AvoidanceRaad2022URSim(AvoidanceRaad2022UR, Simulation): - cmd = "roslaunch ur_robot_server ur_robot_server.launch \ - world_name:=tabletop_sphere50.world \ - reference_frame:=base_link \ - max_velocity_scale_factor:=0.2 \ - action_cycle_rate:=20 \ - rviz_gui:=false \ - gazebo_gui:=true \ - objects_controller:=true \ - rs_mode:=1moving2points \ - n_objects:=1.0 \ - object_0_model_name:=sphere50 \ - object_0_frame:=target" - def __init__( self, ip=None, @@ -408,7 +396,25 @@ def __init__( ur_model="ur5", **kwargs, ): - self.cmd = self.cmd + " " + "ur_model:=" + ur_model + self.cmd = cmd_utils.construct_roslaunch_command( + module='ur_robot_server', + launch_file='ur_robot_server.launch', + launch_args={ + 'world_name': 'tabletop_sphere50.world', + 'reference_frame': 'base_link', + 'max_velocity_scale_factor': 0.2, + 'action_cycle_rate': 20, + 'rviz_gui': False, + 'gazebo_gui': True, + 'objects_controller': True, + 'rs_mode': '1moving2points', + 'n_objects': 1.0, + 'object_0_model_name': 'sphere50', + 'object_0_frame': 'target', + 'ur_model': ur_model + } + ) + Simulation.__init__( self, self.cmd, ip, lower_bound_port, upper_bound_port, gui, **kwargs ) @@ -416,7 +422,6 @@ def __init__( self, rs_address=self.robot_server_ip, ur_model=ur_model, **kwargs ) - class AvoidanceRaad2022URRob(AvoidanceRaad2022UR): real_robot = True @@ -477,20 +482,6 @@ def reset( class AvoidanceRaad2022TestURSim(AvoidanceRaad2022TestUR, Simulation): - cmd = "roslaunch ur_robot_server ur_robot_server.launch \ - world_name:=tabletop_sphere50.world \ - reference_frame:=base_link \ - max_velocity_scale_factor:=0.2 \ - action_cycle_rate:=20 \ - rviz_gui:=false \ - gazebo_gui:=true \ - objects_controller:=true \ - rs_mode:=1moving2points \ - n_objects:=1.0 \ - object_trajectory_file_name:=splines_ur5 \ - object_0_model_name:=sphere50 \ - object_0_frame:=target" - def __init__( self, ip=None, @@ -500,7 +491,26 @@ def __init__( ur_model="ur5", **kwargs, ): - self.cmd = self.cmd + " " + "ur_model:=" + ur_model + self.cmd = cmd_utils.construct_roslaunch_command( + module='ur_robot_server', + launch_file='ur_robot_server.launch', + launch_args={ + 'world_name': 'tabletop_sphere50.world', + 'reference_frame': 'base_link', + 'max_velocity_scale_factor': 0.2, + 'action_cycle_rate': 20, + 'rviz_gui': False, + 'gazebo_gui': True, + 'objects_controller': True, + 'rs_mode': '1moving2points', + 'n_objects': 1.0, + 'object_trajectory_file_name': 'splines_ur5', + 'object_0_model_name': 'sphere50', + 'object_0_frame': 'target', + 'ur_model': ur_model + } + ) + Simulation.__init__( self, self.cmd, ip, lower_bound_port, upper_bound_port, gui, **kwargs ) @@ -508,7 +518,6 @@ def __init__( self, rs_address=self.robot_server_ip, ur_model=ur_model, **kwargs ) - class AvoidanceRaad2022TestURRob(AvoidanceRaad2022TestUR): real_robot = True diff --git a/robo_gym/envs/ur/ur_base_env.py b/robo_gym/envs/ur/ur_base_env.py index 1e0c5d3d..29ff1ce3 100644 --- a/robo_gym/envs/ur/ur_base_env.py +++ b/robo_gym/envs/ur/ur_base_env.py @@ -3,13 +3,11 @@ import copy from typing import Tuple, Any - import gymnasium as gym import numpy as np - import robo_gym_server_modules.robot_server.client as rs_client from robo_gym.envs.simulation_wrapper import Simulation -from robo_gym.utils import ur_utils +from robo_gym.utils import cmd_utils, ur_utils from robo_gym.utils.exceptions import ( InvalidStateError, RobotServerError, @@ -439,15 +437,6 @@ def _get_action_space(self) -> gym.spaces.Box: class EmptyEnvironmentURSim(URBaseEnv, Simulation): - cmd = "roslaunch ur_robot_server ur_robot_server.launch \ - world_name:=empty.world \ - reference_frame:=base_link \ - max_velocity_scale_factor:=0.2 \ - action_cycle_rate:=20 \ - rviz_gui:=true \ - gazebo_gui:=true \ - rs_mode:=only_robot" - def __init__( self, ip=None, @@ -457,7 +446,21 @@ def __init__( ur_model="ur5", **kwargs, ): - self.cmd = self.cmd + " " + "ur_model:=" + ur_model + self.cmd = cmd_utils.construct_roslaunch_command( + module='ur_robot_server', + launch_file='ur_robot_server.launch', + launch_args={ + 'world_name': 'empty.world', + 'reference_frame': 'base_link', + 'max_velocity_scale_factor': 0.2, + 'action_cycle_rate': 20, + 'rviz_gui': True, + 'gazebo_gui': True, + 'rs_mode': 'only_robot', + 'ur_model': ur_model, + } + ) + Simulation.__init__( self, self.cmd, ip, lower_bound_port, upper_bound_port, gui, **kwargs ) @@ -465,7 +468,6 @@ def __init__( self, rs_address=self.robot_server_ip, ur_model=ur_model, **kwargs ) - class EmptyEnvironmentURRob(URBaseEnv): real_robot = True diff --git a/robo_gym/envs/ur/ur_ee_positioning.py b/robo_gym/envs/ur/ur_ee_positioning.py index eed5620d..fdaab8cf 100644 --- a/robo_gym/envs/ur/ur_ee_positioning.py +++ b/robo_gym/envs/ur/ur_ee_positioning.py @@ -6,10 +6,9 @@ import gymnasium as gym import numpy as np from scipy.spatial.transform import Rotation as R - from robo_gym.envs.simulation_wrapper import Simulation from robo_gym.envs.ur.ur_base_env import URBaseEnv -from robo_gym.utils import utils +from robo_gym.utils import utils, cmd_utils from robo_gym.utils.exceptions import InvalidStateError, RobotServerError from robo_gym_server_modules.robot_server.grpc_msgs.python import robot_server_pb2 @@ -485,19 +484,6 @@ def env_action_to_rs_action(self, action) -> np.array: class EndEffectorPositioningURSim(EndEffectorPositioningUR, Simulation): - cmd = "roslaunch ur_robot_server ur_robot_server.launch \ - world_name:=tabletop_sphere50_no_collision.world \ - reference_frame:=base_link \ - max_velocity_scale_factor:=0.1 \ - action_cycle_rate:=10 \ - rviz_gui:=true \ - gazebo_gui:=true \ - objects_controller:=true \ - rs_mode:=1object \ - n_objects:=1.0 \ - object_0_model_name:=sphere50_no_collision \ - object_0_frame:=target" - def __init__( self, ip=None, @@ -507,7 +493,25 @@ def __init__( ur_model="ur5", **kwargs, ): - self.cmd = self.cmd + " " + "ur_model:=" + ur_model + self.cmd = cmd_utils.construct_roslaunch_command( + module='ur_robot_server', + launch_file='ur_robot_server.launch', + launch_args={ + 'world_name': 'tabletop_sphere50_no_collision.world', + 'reference_frame': 'base_link', + 'max_velocity_scale_factor': 0.1, + 'action_cycle_rate': 10, + 'rviz_gui': True, + 'gazebo_gui': True, + 'objects_controller': True, + 'rs_mode': '1object', + 'n_objects': 1.0, + 'object_0_model_name': 'sphere50_no_collision', + 'object_0_frame': 'target', + 'ur_model': ur_model + } + ) + Simulation.__init__( self, self.cmd, ip, lower_bound_port, upper_bound_port, gui, **kwargs ) @@ -515,7 +519,6 @@ def __init__( self, rs_address=self.robot_server_ip, ur_model=ur_model, **kwargs ) - class EndEffectorPositioningURRob(EndEffectorPositioningUR): real_robot = True diff --git a/robo_gym/utils/cmd_utils.py b/robo_gym/utils/cmd_utils.py new file mode 100644 index 00000000..499a65f6 --- /dev/null +++ b/robo_gym/utils/cmd_utils.py @@ -0,0 +1,30 @@ +from typing import Any +import numpy as np + +def construct_roslaunch_command(module: str, launch_file: str, launch_args: dict[str, Any] = {}) -> str: + """Construct a full roslaunch command to run for simulations from a base command and its arguments. + + Args: + base_cmd (string): The base roslaunch command containing the module and launch file + launch_args (float): y coordinate of the point. + + Returns: + constructed_cmd (string): The fully constructed roslaunch command. + + """ + + launch_args_merged = '' + for (name, value) in launch_args.items(): + if type(value) == bool: + # roslaunch expects the lowercase version of Python's boolean values + value = 'true' if value else 'false' + elif type(value) in [int, float, np.number]: + # TODO: This is redundant (and will be removed later) but helpful to highlight that numeric conversions are supported + value = str(value) + else: + value = str(value) + + launch_args_merged += f'{name}:={value} ' + + constructed_cmd = f'roslaunch {module} {launch_file} {launch_args_merged}'.strip() + return constructed_cmd diff --git a/tests/robo-gym/utils/test_cmd_utils.py b/tests/robo-gym/utils/test_cmd_utils.py new file mode 100644 index 00000000..eb79ec4c --- /dev/null +++ b/tests/robo-gym/utils/test_cmd_utils.py @@ -0,0 +1,41 @@ +import pytest +from robo_gym.utils import cmd_utils + +# Input command argument definitions +BASIC_ARGS = {} + +COMMAND_ARGS_SIMPLE = { + 'string_var1': 'string1', + 'string_var2': 'string2' +} + +COMMAND_ARGS_COMPLEX = { + 'string_var': 'string', + 'int_var': 6, + 'float_var': 3.14, + 'bool_var': True, +} + +# Expected command string definitions (NOTE: indentation sensitive) +EXP_BASIC_COMMAND = "roslaunch example_module_1 example_launch_1.launch" + +EXP_COMMAND_WITH_ARGS_SIMPLE = "roslaunch example_module_2 example_launch_2.launch \ +string_var1:=string1 \ +string_var2:=string2" + +EXP_COMMAND_WITH_ARGS_COMPLEX = "roslaunch example_module_3 example_launch_3.launch \ +string_var:=string \ +int_var:=6 \ +float_var:=3.14 \ +bool_var:=true" + +### correct_launch_commands ### +test_correct_launch_commands = [ + ('example_module_1', 'example_launch_1.launch', BASIC_ARGS, EXP_BASIC_COMMAND), + ('example_module_2', 'example_launch_2.launch', COMMAND_ARGS_SIMPLE, EXP_COMMAND_WITH_ARGS_SIMPLE), + ('example_module_3', 'example_launch_3.launch', COMMAND_ARGS_COMPLEX, EXP_COMMAND_WITH_ARGS_COMPLEX) +] +@pytest.mark.parametrize('module, launch_file, launch_args, expected_result', test_correct_launch_commands) +def test_construct_roslaunch_command(module, launch_file, launch_args, expected_result): + constructed_cmd = cmd_utils.construct_roslaunch_command(module, launch_file, launch_args) + assert constructed_cmd == expected_result