diff --git a/config_runner/configs/1_31_23/door/sacadrl.json b/config_runner/configs/1_31_23/door/sacadrl.json index 388d9bfd..d09fd058 100644 --- a/config_runner/configs/1_31_23/door/sacadrl.json +++ b/config_runner/configs/1_31_23/door/sacadrl.json @@ -1,5 +1,5 @@ { - "num_agents": 2, + "num_agents": [[0,2]], "eval_num_agents": [3, 4, 5, 7, 10, 20], "train_length": 250000, "ending_eval_trials": 25, diff --git a/config_runner/run_config.sh b/config_runner/run_config.sh index 2d8afdbb..4a2fe07b 100755 --- a/config_runner/run_config.sh +++ b/config_runner/run_config.sh @@ -11,7 +11,7 @@ docker run -d --name $1 -w /home/rosdev/social_gym/ --gpus all --privileged \ -e DISPLAY=unix$DISPLAY \ -v ${DIR}/data:/home/rosdev/social_gym/data \ -v ${DIR}/config_runner/configs:/home/rosdev/social_gym/config_runner/configs \ --v ${DIR}/submodules/amrl_maps/envs:/home/rosdev/social_gym/submodules/amrl_maps/envs \ +-v ${DIR}/submodules/amrl_maps:/home/rosdev/social_gym/submodules/amrl_maps \ -v ${DIR}/submodules/ut_multirobot_sim/maps:/home/rosdev/social_gym/submodules/ut_multirobot_sim/maps \ --network host \ -v ${DIR}/src:/home/rosdev/social_gym/src \ diff --git a/docker/vectordisplay/maps/multienv/multienv.navigation.json b/docker/vectordisplay/maps/multienv/multienv.navigation.json new file mode 100644 index 00000000..44b3b25b --- /dev/null +++ b/docker/vectordisplay/maps/multienv/multienv.navigation.json @@ -0,0 +1,94 @@ +{ + "edges": [ + { + "has_automated_door": false, + "has_door": false, + "has_elevator": false, + "has_stairs": false, + "max_clearance": 1.0, + "max_speed": 2.0, + "s0_id": 0, + "s1_id": 2 + }, + { + "has_automated_door": false, + "has_door": false, + "has_elevator": false, + "has_stairs": false, + "max_clearance": 1.0, + "max_speed": 2.0, + "s0_id": 0, + "s1_id": 1 + }, + { + "has_automated_door": false, + "has_door": false, + "has_elevator": false, + "has_stairs": false, + "max_clearance": 1.0, + "max_speed": 2.0, + "s0_id": 1, + "s1_id": 3 + }, + { + "has_automated_door": false, + "has_door": false, + "has_elevator": false, + "has_stairs": false, + "max_clearance": 1.0, + "max_speed": 2.0, + "s0_id": 2, + "s1_id": 3 + }, + { + "has_automated_door": false, + "has_door": false, + "has_elevator": false, + "has_stairs": false, + "max_clearance": 1.0, + "max_speed": 2.0, + "s0_id": 0, + "s1_id": 3 + }, + { + "has_automated_door": false, + "has_door": false, + "has_elevator": false, + "has_stairs": false, + "max_clearance": 1.0, + "max_speed": 2.0, + "s0_id": 2, + "s1_id": 1 + } + ], + "nodes": [ + { + "id": 0, + "loc": { + "x": -11.379310607910156, + "y": 15.17241382598877 + } + }, + { + "id": 1, + "loc": { + "x": 8.448275566101074, + "y": 15.0 + } + }, + { + "id": 2, + "loc": { + "x": -8.793103218078613, + "y": -6.724138259887695 + } + }, + { + "id": 3, + "loc": { + "x": 9.82758617401123, + "y": -8.103448867797852 + } + } + ] +} diff --git a/docker/vectordisplay/maps/multienv/multienv.navigation.txt b/docker/vectordisplay/maps/multienv/multienv.navigation.txt new file mode 100644 index 00000000..e69de29b diff --git a/docker/vectordisplay/maps/multienv/multienv.vectormap.txt b/docker/vectordisplay/maps/multienv/multienv.vectormap.txt new file mode 100644 index 00000000..bd9ad719 --- /dev/null +++ b/docker/vectordisplay/maps/multienv/multienv.vectormap.txt @@ -0,0 +1,3 @@ +-19.655172, 21.724140, -19.655172, -7.241378 +-19.827587, 22.241381, 13.275863, 22.586208 +13.275863, 22.586208, 13.793104, -5.689654 diff --git a/logs_multienv.txt b/logs_multienv.txt new file mode 100644 index 00000000..a3d63202 --- /dev/null +++ b/logs_multienv.txt @@ -0,0 +1,38 @@ +/home/rosdev/social_gym +/home/rosdev/social_gym +Name: SuperSuit +Version: 3.9.0 +Summary: Wrappers for Gymnasium and PettingZoo +Home-page: None +Author: None +Author-email: Farama Foundation +License: MIT License +Location: /usr/local/lib/python3.8/dist-packages +Requires: tinyscaler, gymnasium, numpy +Required-by: +Name: pettingzoo +Version: 1.23.1 +Summary: Gymnasium for multi-agent reinforcement learning. +Home-page: None +Author: None +Author-email: Farama Foundation +License: MIT License +Location: /usr/local/lib/python3.8/dist-packages +Requires: gymnasium, numpy +Required-by: +Name: stable-baselines3 +Version: 2.1.0 +Summary: Pytorch version of Stable Baselines, implementations of reinforcement learning algorithms. +Home-page: https://github.com/DLR-RM/stable-baselines3 +Author: Antonin Raffin +Author-email: antonin.raffin@dlr.de +License: MIT +Location: /usr/local/lib/python3.8/dist-packages +Requires: matplotlib, gymnasium, pandas, torch, cloudpickle, numpy +Required-by: sb3-contrib, imitation +Python 3.8.10 +[ INFO] [1693431990.500186587]: Updated sim with live config: Rate=40 incoming rate=40 +[ INFO] [1693431990.508012836]: Using default queue size of 10 for publisher queues...  +[ INFO] [1693431990.520342905]: Loading scene [/home/rosdev/social_gym/config/gym_gen/scene.xml] for simulation +[ INFO] [1693431990.540756953]: node initialized, now running  +Service Mode diff --git a/scripts/create_env_template.py b/scripts/create_env_template.py index 1908246f..bdd8558d 100644 --- a/scripts/create_env_template.py +++ b/scripts/create_env_template.py @@ -82,6 +82,8 @@ def create_new_env(name: str, template: str = None): out = subprocess.Popen(cmd, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE) out.wait() + print("HI") + subprocess.Popen(["sudo", "chmod", "-R", "a+rwX", f"{ROOT_FOLDER}"], stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE) subprocess.Popen(["sudo", "chmod", "-R", "a+rwX", f"{AMRL_MAPS_FOLDER}"], stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE) subprocess.Popen(["sudo", "chmod", "-R", "a+rwX", f"{UT_MULTI_ROBOT_SIM_MAPS_FOLDER}"], stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE) diff --git a/src/config_run.py b/src/config_run.py index 1f26213e..7e691916 100644 --- a/src/config_run.py +++ b/src/config_run.py @@ -34,7 +34,7 @@ from src.environment.scenarios.common_scenarios import envs_door, envs_hallway, envs_intersection, envs_round_about, \ envs_open -from src.environment.scenarios import CycleScenario, GraphNavScenario +from src.environment.scenarios import CycleScenario, GraphNavScenario, ManualScenario from src.environment.utils.utils import DATA_FOLDER from src.environment.utils.evaluate_policy import evaluate_policy import datetime @@ -320,8 +320,12 @@ def run( scenarios.append(scenario) zones.append(conflict_zone) if 'envs_multi' in experiment_names: - scenario = GraphNavScenario("tmp/multi-scenario") + # scenario = GraphNavScenario("multienv", partially_observable=partially_observable, config_runner=True if not monitor and not local else False, all_config=monitor and not local) + scenario = ManualScenario("multienv", agent_paths=[[0, 1], [1,0], [1, 2]], human_paths=[[0, 1]], partially_observable=partially_observable, config_runner=True if not monitor and not local else False, all_config=monitor and not local) + scenarios.append(scenario) + # conflict_zone = (0, 1, 1) + # zones.append(conflict_zone) observations = [] diff --git a/src/environment/environment_types/manual_zone.py b/src/environment/environment_types/manual_zone.py index 23de1d08..921d4707 100644 --- a/src/environment/environment_types/manual_zone.py +++ b/src/environment/environment_types/manual_zone.py @@ -153,6 +153,10 @@ def build_zone(self): start = np.array(scenario.robot_positions[start_point]) end = np.array(scenario.robot_positions[end_point]) + if start.shape[0] > 1: + start = start[0] + end = end[0] + line = end - start length = np.linalg.norm(line) line /= length diff --git a/src/environment/ros_social_gym.py b/src/environment/ros_social_gym.py index 19a16e92..676b14ed 100644 --- a/src/environment/ros_social_gym.py +++ b/src/environment/ros_social_gym.py @@ -335,7 +335,10 @@ def reset(self, seed=None, return_info=True, options=None): self.last_reward_maps = [] if not return_info: - return {agent: obs for agent, obs in zip(self.agents, observations)} + infos = { + agent: {} for agent in self.possible_agents if agent in self.agents + } + return {agent: obs for agent, obs in zip(self.agents, observations)}, infos else: infos = { agent: {} for agent in self.possible_agents if agent in self.agents @@ -423,13 +426,14 @@ def step(self, action_dict): } for agent, obs_map in zip(self.possible_agents, observation_maps) if agent in self.agents } - agent_infos["robot_data"] = agent_infos.pop(self.agents[0]) - agent_infos["pedestrian_data"] = agent_infos.pop(self.agents[1]) + agent_infos["robot_data"] = agent_infos[self.agents[0]] + agent_infos["pedestrian_data"] = agent_infos[self.agents[1]] agent_infos["pedestrian_data"] = [agent_infos["pedestrian_data"]] agent_infos["collisions"] = 0 agent_infos["success"] = False agent_infos["timestep"] = int(time.time()) + # if all([x.get('succeeded') for x in agent_infos.values()]): # print('major reward') # agent_rewards = {k: 100_000 for k in agent_rewards.keys()} @@ -437,6 +441,7 @@ def step(self, action_dict): self.terminations_ = list(agent_terminations.values()) truncs = {agent: False if obs_map.get('success_observation', 0) == 0 else True for agent, obs_map in zip(self.agents, observation_maps)} + #print(agent_infos, flush=True) # return agent_observations, agent_rewards, agent_terminations, truncs, agent_infos return agent_observations, agent_rewards, agent_terminations, truncs, agent_infos diff --git a/src/environment/scenarios/types/graph_nav.py b/src/environment/scenarios/types/graph_nav.py index 20a4a3c4..76eafebd 100644 --- a/src/environment/scenarios/types/graph_nav.py +++ b/src/environment/scenarios/types/graph_nav.py @@ -140,9 +140,12 @@ def __init__( allowed_agent_start_positions: List[int] = None, allowed_agent_goal_positions: List[int] = None, allowed_human_start_positions: List[int] = None, - allowed_human_goal_positions: List[int] = None + allowed_human_goal_positions: List[int] = None, + config_runner: bool = False, + all_config: bool = False, + partially_observable: bool = False ): - super().__init__(env_name=env_name) + super().__init__(env_name=env_name, config_runner=config_runner, all_config=all_config, partially_observable=partially_observable) self.allowed_agent_start_positions = allowed_agent_start_positions self.allowed_agent_goal_positions = allowed_agent_goal_positions diff --git a/src/templates/multienv/all_launch.launch b/src/templates/multienv/all_launch.launch new file mode 100644 index 00000000..438b1832 --- /dev/null +++ b/src/templates/multienv/all_launch.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + - 'image_transport/compressed' + - 'image_transport/compressedDepth' + - 'image_transport/theora' + + + + + + + + \ No newline at end of file diff --git a/src/templates/multienv/config_launch.launch b/src/templates/multienv/config_launch.launch new file mode 100644 index 00000000..a793590d --- /dev/null +++ b/src/templates/multienv/config_launch.launch @@ -0,0 +1,12 @@ + + + + + + + + + + diff --git a/src/templates/multienv/greedy_launch.launch b/src/templates/multienv/greedy_launch.launch new file mode 100644 index 00000000..56fca365 --- /dev/null +++ b/src/templates/multienv/greedy_launch.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + diff --git a/src/templates/multienv/humans.lua b/src/templates/multienv/humans.lua new file mode 100644 index 00000000..ff63c602 --- /dev/null +++ b/src/templates/multienv/humans.lua @@ -0,0 +1,31 @@ +function Vector2(x, y) + return {x = x, y = y} +end + +-- Human shape information +hu_radius = 0.2 +hu_num_segments = 20 + +-- Human speed information +hu_max_speed = 0.8 -- +hu_avg_speed = 0.8 -- +hu_max_omega = 0.2 +hu_avg_omega = 0. +hu_reach_goal_threshold = 0.1 + +-- Human walking mode +local HumanMode = { + Singleshot=0, + Repeat=1, + Controlled=2, + Cycle=3, +} + +hu_mode = HumanMode.Controlled +hu_control_topic = "/command" + +{% for i in range(human_count) %} + hu{{ i }}_waypoints = { + { {{ human_positions[i][0] }}, {{ human_positions[i][1] }}, {{ human_positions[i][2] }} } + } +{% endfor %} diff --git a/src/templates/multienv/launch.launch b/src/templates/multienv/launch.launch new file mode 100644 index 00000000..4a13bc0b --- /dev/null +++ b/src/templates/multienv/launch.launch @@ -0,0 +1,37 @@ + + + + + + + + + + + - 'image_transport/compressed' + - 'image_transport/compressedDepth' + - 'image_transport/theora' + + + + + + + + + + + + + diff --git a/src/templates/multienv/multienv.navigation.json b/src/templates/multienv/multienv.navigation.json new file mode 100644 index 00000000..44b3b25b --- /dev/null +++ b/src/templates/multienv/multienv.navigation.json @@ -0,0 +1,94 @@ +{ + "edges": [ + { + "has_automated_door": false, + "has_door": false, + "has_elevator": false, + "has_stairs": false, + "max_clearance": 1.0, + "max_speed": 2.0, + "s0_id": 0, + "s1_id": 2 + }, + { + "has_automated_door": false, + "has_door": false, + "has_elevator": false, + "has_stairs": false, + "max_clearance": 1.0, + "max_speed": 2.0, + "s0_id": 0, + "s1_id": 1 + }, + { + "has_automated_door": false, + "has_door": false, + "has_elevator": false, + "has_stairs": false, + "max_clearance": 1.0, + "max_speed": 2.0, + "s0_id": 1, + "s1_id": 3 + }, + { + "has_automated_door": false, + "has_door": false, + "has_elevator": false, + "has_stairs": false, + "max_clearance": 1.0, + "max_speed": 2.0, + "s0_id": 2, + "s1_id": 3 + }, + { + "has_automated_door": false, + "has_door": false, + "has_elevator": false, + "has_stairs": false, + "max_clearance": 1.0, + "max_speed": 2.0, + "s0_id": 0, + "s1_id": 3 + }, + { + "has_automated_door": false, + "has_door": false, + "has_elevator": false, + "has_stairs": false, + "max_clearance": 1.0, + "max_speed": 2.0, + "s0_id": 2, + "s1_id": 1 + } + ], + "nodes": [ + { + "id": 0, + "loc": { + "x": -11.379310607910156, + "y": 15.17241382598877 + } + }, + { + "id": 1, + "loc": { + "x": 8.448275566101074, + "y": 15.0 + } + }, + { + "id": 2, + "loc": { + "x": -8.793103218078613, + "y": -6.724138259887695 + } + }, + { + "id": 3, + "loc": { + "x": 9.82758617401123, + "y": -8.103448867797852 + } + } + ] +} diff --git a/src/templates/multienv/multienv.navigation.txt b/src/templates/multienv/multienv.navigation.txt new file mode 100644 index 00000000..e69de29b diff --git a/src/templates/multienv/multienv.vectormap.json b/src/templates/multienv/multienv.vectormap.json new file mode 100644 index 00000000..43e58fa6 --- /dev/null +++ b/src/templates/multienv/multienv.vectormap.json @@ -0,0 +1 @@ +[{"p0": {"x": "-19.655172", "y": "21.724140"}, "p1": {"x": "-19.655172", "y": "-7.241378"}}, {"p0": {"x": "-19.827587", "y": "22.241381"}, "p1": {"x": "13.275863", "y": "22.586208"}}, {"p0": {"x": "13.275863", "y": "22.586208"}, "p1": {"x": "13.793104", "y": "-5.689654"}}] \ No newline at end of file diff --git a/src/templates/multienv/multienv.vectormap.txt b/src/templates/multienv/multienv.vectormap.txt new file mode 100644 index 00000000..bd9ad719 --- /dev/null +++ b/src/templates/multienv/multienv.vectormap.txt @@ -0,0 +1,3 @@ +-19.655172, 21.724140, -19.655172, -7.241378 +-19.827587, 22.241381, 13.275863, 22.586208 +13.275863, 22.586208, 13.793104, -5.689654 diff --git a/src/templates/multienv/pedsim_launch.launch b/src/templates/multienv/pedsim_launch.launch new file mode 100644 index 00000000..70ac0248 --- /dev/null +++ b/src/templates/multienv/pedsim_launch.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/templates/multienv/pips_launch.launch b/src/templates/multienv/pips_launch.launch new file mode 100644 index 00000000..6ce1b629 --- /dev/null +++ b/src/templates/multienv/pips_launch.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/templates/multienv/ref_launch.launch b/src/templates/multienv/ref_launch.launch new file mode 100644 index 00000000..6d7d0c6c --- /dev/null +++ b/src/templates/multienv/ref_launch.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/templates/multienv/scene.xml b/src/templates/multienv/scene.xml new file mode 100644 index 00000000..d0a5a336 --- /dev/null +++ b/src/templates/multienv/scene.xml @@ -0,0 +1,33 @@ + + + + + + + +{% for i in range(position_count) %} + +{% endfor %} + +{% for i in range(nav_count) %} + +{% endfor %} + + + + + + + + + + + {% for human_position in human_positions %} + + {% for i in range(3, human_position|length) %} + + {% endfor %} + + {% endfor %} + + diff --git a/src/templates/multienv/sim_config.lua b/src/templates/multienv/sim_config.lua new file mode 100644 index 00000000..3d04b6a3 --- /dev/null +++ b/src/templates/multienv/sim_config.lua @@ -0,0 +1,104 @@ + +function Vector2(x, y) + return {x = x, y = y} + end + + function Vector3(x, y, z) + return {x = x, y = y, z = z} + end + + function DegToRad(d) + return math.pi * d / 180 + end +map_name = "maps/multienv/multienv.vectormap.txt" +nav_map_name = "multienv" +-- Simulator starting location. +start_poses = { + {% for i in range(robot_count) %} + { + {{ robot_start[i][0] }}, {{ robot_start[i][1] }}, {{ robot_start[i][2] }} + }, + {% endfor %} +} + + +goal_poses = { + {% for i in range(robot_count) %} + { + {{ robot_end[i][0] }}, {{ robot_end[i][1] }}, {{ robot_end[i][2] }} + }, + {% endfor %} +} + + + num_humans = {{ human_count }} + human_config = "../../config/gym_gen/humans.lua" + + partially_observable = {{ partially_observable }} + + door_config_list = { + -- "/home/jaholtz/code/amrl_maps/GDC1/door_list.lua" + } + + -- Time-step for simulation. + delta_t = 0.025 + -- max_steps = 12000 + max_steps = 3200 + + -- Simulator TF publications + publish_tfs = true; + -- publish_foot_to_base = true; + -- publish_map_to_odom = true; + + -- -- Car dimensions. + -- car_width = 0.281 + -- car_length = 0.535 + -- car_height = 0.15; + + -- -- Location of the robot's rear wheel axle relative to the center of the body. + -- rear_axle_offset = -0.162 + -- laser_loc = Vector3(0.2, 0, 0.15) + + -- Kinematic and dynamic constraints for the car. + -- min_turn_radius = 0.98 + + -- Laser rangefinder parameters. + laser_noise_stddev = 0.01; + -- laser_angle_min = DegToRad(-135.0); + -- laser_angle_max = DegToRad(135.0); + -- laser_angle_increment = DegToRad(0.25); + -- laser_min_range = 0.02; + -- laser_max_range = 100.0; + + -- Turning error simulation. + angular_error_bias = DegToRad(0); + angular_error_rate = 0.1; + + -- Defining robot type enumerator + local RobotType = { + ACKERMANN_DRIVE="ACKERMANN_DRIVE", + OMNIDIRECTIONAL_DRIVE="OMNIDIRECTIONAL_DRIVE", + DIFF_DRIVE="DIFF_DRIVE" + } + + -- robot_type = RobotType.ACKERMANN_DRIVE + -- robot_config = "config/ut_automata_config.lua" + -- robot_type = RobotType.DIFF_DRIVE + -- robot_config = "config/bwibot_config.lua" + -- robot_type = RobotType.OMNIDIRECTIONAL_DRIVE + -- robot_config = "config/cobot_config.lua" + robot_types = { + {% for i in range(robot_count) %} + {% if i+1 < robot_count %} + RobotType.DIFF_DRIVE, + {% else %} + RobotType.DIFF_DRIVE + {% endif %} + {% endfor %} + } + + robot_config = "config/ut_jackal_config.lua" + + laser_topic = "/Cobot/Laser" + laser_frame = "base_laser" + \ No newline at end of file