From 2240ca1d45af0e44d6fd844d5379f129b80f7ad7 Mon Sep 17 00:00:00 2001 From: Tejas Shah Date: Wed, 11 Dec 2024 06:43:53 -0800 Subject: [PATCH 1/3] added util method to construct roslaunch flags --- robo_gym/utils/cmd_utils.py | 30 +++++++++++++++++++ tests/robo-gym/utils/test_cmd_utils.py | 41 ++++++++++++++++++++++++++ 2 files changed, 71 insertions(+) create mode 100644 robo_gym/utils/cmd_utils.py create mode 100644 tests/robo-gym/utils/test_cmd_utils.py 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 From b8e9d115d533a66e3b455d86771df7e1efabe1e0 Mon Sep 17 00:00:00 2001 From: Tejas Shah Date: Wed, 11 Dec 2024 06:45:38 -0800 Subject: [PATCH 2/3] testing cmd construction util for mir100 simulations --- robo_gym/envs/mir100/mir100.py | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/robo_gym/envs/mir100/mir100.py b/robo_gym/envs/mir100/mir100.py index 4aa7b184..6a11b32e 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 @@ -421,8 +421,12 @@ def _reward(self, rs_state, action): return reward, done, info 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) NoObstacleNavigationMir100.__init__(self, rs_address=self.robot_server_ip, **kwargs) @@ -640,8 +644,15 @@ def _generate_obstacles_positions(self,): self.sim_obstacles = [[x_0, y_0, yaw_0],[x_1, y_1, yaw_1],[x_2, y_2, yaw_2]] class ObstacleAvoidanceMir100Sim(Simulation, ObstacleAvoidanceMir100): - cmd = "roslaunch mir100_robot_server sim_robot_server.launch world_name:=lab_6x8.world" 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' + } + ) + Simulation.__init__(self, self.cmd, ip, lower_bound_port, upper_bound_port, gui, **kwargs) ObstacleAvoidanceMir100.__init__(self, rs_address=self.robot_server_ip, **kwargs) From 4a39f4aed6a51a7c2799f35029f6d310fbeb832d Mon Sep 17 00:00:00 2001 From: Tejas Shah Date: Wed, 11 Dec 2024 07:10:24 -0800 Subject: [PATCH 3/3] testing cmd construction util for universal robots --- robo_gym/envs/ur/ur_avoidance_basic.py | 33 ++++++++----- robo_gym/envs/ur/ur_avoidance_raad.py | 67 +++++++++++++++----------- robo_gym/envs/ur/ur_base_env.py | 26 ++++++---- robo_gym/envs/ur/ur_ee_positioning.py | 34 +++++++------ 4 files changed, 96 insertions(+), 64 deletions(-) diff --git a/robo_gym/envs/ur/ur_avoidance_basic.py b/robo_gym/envs/ur/ur_avoidance_basic.py index 7c907f45..8c144cdc 100644 --- a/robo_gym/envs/ur/ur_avoidance_basic.py +++ b/robo_gym/envs/ur/ur_avoidance_basic.py @@ -9,6 +9,7 @@ from __future__ import annotations import numpy as np from typing import Tuple +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 @@ -147,20 +148,26 @@ def step(self, action) -> Tuple[np.array, float, bool, dict]: return state, reward, done, info 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, lower_bound_port=None, upper_bound_port=None, gui=False, 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) BasicAvoidanceUR.__init__(self, rs_address=self.robot_server_ip, ur_model=ur_model, **kwargs) diff --git a/robo_gym/envs/ur/ur_avoidance_raad.py b/robo_gym/envs/ur/ur_avoidance_raad.py index 9d08ee5a..dc04910b 100644 --- a/robo_gym/envs/ur/ur_avoidance_raad.py +++ b/robo_gym/envs/ur/ur_avoidance_raad.py @@ -11,6 +11,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 @@ -263,20 +264,26 @@ def _get_joint_positions_as_array(self) -> np.ndarray: return np.array(temp) 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, lower_bound_port=None, upper_bound_port=None, gui=False, 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) AvoidanceRaad2022UR.__init__(self, rs_address=self.robot_server_ip, ur_model=ur_model, **kwargs) @@ -327,21 +334,27 @@ def reset(self, *, seed: int | None = None, options: dict[str, Any] | None = Non return super_result 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, lower_bound_port=None, upper_bound_port=None, gui=False, 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) AvoidanceRaad2022TestUR.__init__(self, rs_address=self.robot_server_ip, ur_model=ur_model, **kwargs) diff --git a/robo_gym/envs/ur/ur_base_env.py b/robo_gym/envs/ur/ur_base_env.py index 4fefaaec..963ebbe5 100644 --- a/robo_gym/envs/ur/ur_base_env.py +++ b/robo_gym/envs/ur/ur_base_env.py @@ -4,7 +4,7 @@ import numpy as np import gymnasium as gym from typing import Tuple, Any -from robo_gym.utils import ur_utils +from robo_gym.utils import cmd_utils, ur_utils from robo_gym.utils.exceptions import InvalidStateError, RobotServerError, InvalidActionError import robo_gym_server_modules.robot_server.client as rs_client from robo_gym_server_modules.robot_server.grpc_msgs.python import robot_server_pb2 @@ -379,16 +379,22 @@ 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, lower_bound_port=None, upper_bound_port=None, gui=False, 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) URBaseEnv.__init__(self, rs_address=self.robot_server_ip, ur_model=ur_model, **kwargs) diff --git a/robo_gym/envs/ur/ur_ee_positioning.py b/robo_gym/envs/ur/ur_ee_positioning.py index 8bc8f531..eea15fee 100644 --- a/robo_gym/envs/ur/ur_ee_positioning.py +++ b/robo_gym/envs/ur/ur_ee_positioning.py @@ -5,7 +5,7 @@ from typing import Tuple, Any from scipy.spatial.transform import Rotation as R from robo_gym.utils.exceptions import InvalidStateError, RobotServerError -from robo_gym.utils import utils +from robo_gym.utils import utils, 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_env import URBaseEnv @@ -366,20 +366,26 @@ 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, lower_bound_port=None, upper_bound_port=None, gui=False, 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) EndEffectorPositioningUR.__init__(self, rs_address=self.robot_server_ip, ur_model=ur_model, **kwargs)