├── mamp
├── agents
│ ├── __init__.py
│ └── obstacle.py
├── configs
│ ├── __init__.py
│ └── config.py
├── envs
│ ├── rosport
│ │ ├── __init__.py
│ │ ├── car.py
│ │ └── bot.py
│ ├── __init__.py
│ ├── gazeboEnv.py
│ ├── carenv.py
│ └── macaenv.py
├── policies
│ ├── pcaPolicy
│ │ ├── __init__.py
│ │ ├── dubinsmaneuver2d.py
│ │ ├── rvoDubinsPolicy.py
│ │ └── pcaPolicy.py
│ ├── __init__.py
│ ├── policy.py
│ ├── rvoPolicy.py
│ ├── drvoPolicy.py
│ └── orcaPolicy.py
├── dynamics
│ ├── __init__.py
│ ├── Dynamics.py
│ ├── FullDynamics.py
│ └── UnicycleDynamics.py
├── sensors
│ ├── __init__.py
│ ├── Sensor.py
│ ├── OtherAgentsObsSensor.py
│ └── OtherAgentsStatesSensor.py
├── __init__.py
└── util.py
├── draw
├── figs
│ ├── s1.png
│ ├── cc1.jpg
│ ├── cc2.jpg
│ ├── cr1.jpg
│ ├── cr2.jpg
│ ├── large1.jpg
│ └── large2.jpg
├── animate.py
├── vis_util.py
└── plt2d.py
├── .gitignore
├── LICENSE
├── README.md
└── runpy
├── run_drvo.py
├── run_orca.py
├── run_rvo.py
├── run_rvodubins.py
└── run_pca.py
/mamp/agents/__init__.py:
--------------------------------------------------------------------------------
1 |
--------------------------------------------------------------------------------
/mamp/configs/__init__.py:
--------------------------------------------------------------------------------
1 |
--------------------------------------------------------------------------------
/mamp/envs/rosport/__init__.py:
--------------------------------------------------------------------------------
1 |
2 |
--------------------------------------------------------------------------------
/mamp/policies/pcaPolicy/__init__.py:
--------------------------------------------------------------------------------
1 |
--------------------------------------------------------------------------------
/draw/figs/s1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/wuuya1/PCA/HEAD/draw/figs/s1.png
--------------------------------------------------------------------------------
/draw/figs/cc1.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/wuuya1/PCA/HEAD/draw/figs/cc1.jpg
--------------------------------------------------------------------------------
/draw/figs/cc2.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/wuuya1/PCA/HEAD/draw/figs/cc2.jpg
--------------------------------------------------------------------------------
/draw/figs/cr1.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/wuuya1/PCA/HEAD/draw/figs/cr1.jpg
--------------------------------------------------------------------------------
/draw/figs/cr2.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/wuuya1/PCA/HEAD/draw/figs/cr2.jpg
--------------------------------------------------------------------------------
/draw/figs/large1.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/wuuya1/PCA/HEAD/draw/figs/large1.jpg
--------------------------------------------------------------------------------
/draw/figs/large2.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/wuuya1/PCA/HEAD/draw/figs/large2.jpg
--------------------------------------------------------------------------------
/mamp/dynamics/__init__.py:
--------------------------------------------------------------------------------
1 | from .Dynamics import Dynamics
2 | from .FullDynamics import FullDynamics
3 | from .UnicycleDynamics import UnicycleDynamics
--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | __pycache__
2 | .idea
3 | data_analysis
4 | draw/drvo
5 | draw/orca
6 | draw/pca
7 | draw/rvo
8 | draw/rvo_dubins
9 | data_analysis
10 |
11 |
--------------------------------------------------------------------------------
/mamp/sensors/__init__.py:
--------------------------------------------------------------------------------
1 | from .OtherAgentsObsSensor import OtherAgentsObsSensor
2 | from .OtherAgentsStatesSensor import OtherAgentsStatesSensor
3 | from .Sensor import Sensor
4 |
5 |
6 |
7 | sensor_dict = {
8 | 'empty_obs': Sensor,
9 | 'other_agents_obs': OtherAgentsObsSensor,
10 | 'other_agents_states': OtherAgentsStatesSensor,
11 | }
12 |
--------------------------------------------------------------------------------
/mamp/__init__.py:
--------------------------------------------------------------------------------
1 | import logging
2 | from gym.envs.registration import register
3 |
4 | logger = logging.getLogger(__name__)
5 |
6 | register(
7 | id='LineFollower-v0',
8 | entry_point='mamp.envs.carenv:CAREnv',
9 | )
10 |
11 | register(
12 | id='MAGazebo-v0',
13 | entry_point='mamp.envs.gazeboEnv:GazeboEnv',
14 | )
15 |
16 | register(
17 | id='MultiAgentCollisionAvoidance-v0',
18 | entry_point='mamp.envs.macaenv:MACAEnv',
19 | )
20 |
21 | __all__=['agent']
22 |
--------------------------------------------------------------------------------
/mamp/policies/__init__.py:
--------------------------------------------------------------------------------
1 | from .policy import Policy
2 | from .rvoPolicy import RVOPolicy
3 | from .rvoPolicy import RVOPolicy
4 | from .pcaPolicy.pcaPolicy import PCAPolicy
5 | from .drvoPolicy import DRVOPolicy
6 | from .orcaPolicy import ORCAPolicy
7 | from .pcaPolicy.rvoDubinsPolicy import RVODubinsPolicy
8 |
9 | policy_dict = {
10 | # rule
11 | 'rvo': RVOPolicy,
12 | 'pca': PCAPolicy,
13 | 'drvo': DRVOPolicy,
14 | 'orca': ORCAPolicy,
15 | 'rvo_dubins': RVODubinsPolicy,
16 | }
17 |
--------------------------------------------------------------------------------
/mamp/envs/__init__.py:
--------------------------------------------------------------------------------
1 | import os
2 | gym_config_path = os.environ.get('GYM_CONFIG_PATH', os.path.join(os.path.dirname(os.path.realpath(__file__)), 'config.py'))
3 | gym_config_class = os.environ.get('GYM_CONFIG_CLASS', 'Config')
4 |
5 | import importlib.util
6 | spec = importlib.util.spec_from_file_location(gym_config_class, gym_config_path)
7 | foo = importlib.util.module_from_spec(spec)
8 | spec.loader.exec_module(foo)
9 |
10 | config_class = getattr(foo, gym_config_class, None)
11 | assert(callable(config_class))
12 | Config = config_class()
--------------------------------------------------------------------------------
/mamp/agents/obstacle.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | import math
3 |
4 |
5 | class Obstacle(object):
6 | def __init__(self, pos, shape_dict, id):
7 | self.shape = shape = shape_dict['shape']
8 | self.feature = feature = shape_dict['feature']
9 | if shape == 'rect':
10 | self.width, self.heigh = shape_dict['feature']
11 | self.radius = math.sqrt(self.width ** 2 + self.heigh ** 2) / 2
12 | elif shape == 'circle':
13 | self.radius = shape_dict['feature']
14 | else:
15 | raise NotImplementedError
16 |
17 | self.pos_global_frame = np.array(pos, dtype='float64')
18 | self.vel_global_frame = np.array([0.0, 0.0, 0.0])
19 | self.pos = pos
20 | self.is_obstacle = True
21 | self.id = id
22 | self.t = 0.0
23 | self.step_num = 0
24 | self.is_at_goal = True
25 | self.was_in_collision_already = False
26 | self.in_collision = False
27 |
28 | self.x = pos[0]
29 | self.y = pos[1]
30 | self.r = self.radius
31 |
32 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2022 Gang Xu
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
--------------------------------------------------------------------------------
/mamp/sensors/Sensor.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | from mamp.envs import Config
3 |
4 | class Sensor(object):
5 | def __init__(self):
6 | self.name = 'empty_obs'
7 |
8 | def sense(self, agents, agent_index):
9 | """ Dummy method to be re-implemented by each Sensor subclass
10 | """
11 | host_agent = agents[agent_index]
12 | other_agents_states = np.zeros((Config.MAX_NUM_OTHER_AGENTS_OBSERVED, Config.OTHER_AGENT_OBSERVATION_LENGTH))
13 |
14 | other_agent_count = 0
15 | for other_agent in agents:
16 | if other_agent.id == host_agent.id: continue
17 | # rel_pos_to_other_global_frame = other_agent.pos_global_frame - host_agent.pos_global_frame
18 | # dist_2_other = np.linalg.norm(rel_pos_to_other_global_frame) - host_agent.radius - other_agent.radius
19 | # if dist_2_other > host_agent.view_radius: continue
20 |
21 | other_agent_count += 1
22 |
23 | host_agent.num_other_agents_observed = other_agent_count
24 | return other_agents_states
25 |
26 | def set_args(self, args):
27 | """ Update several class attributes (in dict format) of the Sensor object
28 |
29 | Args:
30 | args (dict): {'arg_name1': new_value1, ...} sets :code:`self.arg_name1 = new_value1`, etc.
31 |
32 | """
33 | # Supply a dict of arg key value pairs
34 | for arg, value in args.items():
35 | # print("Setting self.{} to {}".format(arg, value))
36 | setattr(self, arg, value)
37 |
--------------------------------------------------------------------------------
/mamp/policies/policy.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | from mamp.util import wrap
3 |
4 |
5 | class Policy(object):
6 | """ Each :class:`~gym_collision_avoidance.envs.agent.Agent` has one of these, which nominally converts an observation to an action
7 |
8 | :param is_still_learning: (bool) whether this policy is still being learned (i.e., weights are changing during execution)
9 | :param is_external: (bool) whether the Policy computes its own actions or relies on an external process to provide an action.
10 |
11 | """
12 |
13 | def __init__(self, str="NoPolicy"):
14 | self.str = str
15 | self.is_still_learning = False
16 | self.is_external = False
17 |
18 | def near_goal_smoother(self, dist_to_goal, pref_speed, heading, raw_action):
19 | """ Linearly ramp down speed/turning if agent is near goal, stop if close enough.
20 |
21 | I think this is just here for convenience, but nobody uses it? We used it on the jackal for sure.
22 | """
23 |
24 | kp_v = 0.5
25 | kp_r = 1
26 |
27 | if dist_to_goal < 2.0:
28 | near_goal_action = np.empty((2, 1))
29 | pref_speed = max(min(kp_v * (dist_to_goal - 0.1), pref_speed), 0.0)
30 | near_goal_action[0] = min(raw_action[0], pref_speed)
31 | turn_amount = max(min(kp_r * (dist_to_goal - 0.1), 1.0), 0.0) * raw_action[1]
32 | near_goal_action[1] = wrap(turn_amount + heading)
33 | if dist_to_goal < 0.3:
34 | near_goal_action = np.array([0., 0.])
35 | else:
36 | near_goal_action = raw_action
37 |
38 | return near_goal_action
39 |
--------------------------------------------------------------------------------
/mamp/dynamics/Dynamics.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | import math
3 | from mamp.util import wrap
4 |
5 |
6 | class Dynamics(object):
7 | """ Class to convert an agent's action to change in state
8 |
9 | There is a fundamental issue in passing the agent to the Dynamics. There should be a State object or something like that.
10 |
11 | :param agent: (:class:`~gym_collision_avoidance.envs.agent.Agent`) the Agent whose state should be updated
12 |
13 | """
14 |
15 | def __init__(self, agent):
16 | self.agent = agent
17 | pass
18 |
19 | def step(self, action, dt):
20 | """ Dummy method to be implemented by each Dynamics subclass
21 | """
22 | raise NotImplementedError
23 |
24 | def update_ego_frame(self):
25 | """ Update agent's heading and velocity by converting those values from the global to ego frame.
26 |
27 | This should be run every time :code:`step` is called (add to :code:`step`?)
28 |
29 | """
30 | # Compute heading w.r.t. ref_prll, ref_orthog coordinate axes
31 | self.agent.ref_prll, self.agent.ref_orth = self.agent.get_ref()
32 | ref_prll_angle_global_frame = np.arctan2(self.agent.ref_prll[1], self.agent.ref_prll[0])
33 | if self.agent.heading_global_frame is not None:
34 | self.agent.heading_ego_frame = wrap(self.agent.heading_global_frame - ref_prll_angle_global_frame)
35 |
36 | # Compute velocity w.r.t. ref_prll, ref_orthog coordinate axes
37 | cur_speed = math.sqrt(self.agent.vel_global_frame[0]**2 + self.agent.vel_global_frame[1]**2) # much faster than np.linalg.norm
38 | v_prll = cur_speed * np.cos(self.agent.heading_ego_frame)
39 | v_orthog = cur_speed * np.sin(self.agent.heading_ego_frame)
40 | self.agent.vel_ego_frame = np.array([v_prll, v_orthog])
41 |
--------------------------------------------------------------------------------
/mamp/dynamics/FullDynamics.py:
--------------------------------------------------------------------------------
1 | import math
2 | import numpy as np
3 | from mamp.util import wrap
4 | from mamp.dynamics.Dynamics import Dynamics
5 |
6 |
7 | class FullDynamics(Dynamics):
8 | """ Convert a speed & heading to a new state according to Unicycle Kinematics model.
9 |
10 | """
11 |
12 | def __init__(self, agent):
13 | Dynamics.__init__(self, agent)
14 | self.action_type = "XY"
15 |
16 | def step(self, action, dt):
17 | """
18 |
19 | In the global frame, assume the agent instantaneously turns by :code:`heading`
20 | and moves forward at :code:`speed` for :code:`dt` seconds.
21 | Add that offset to the current position. Update the velocity in the
22 | same way. Also update the agent's turning direction (only used by CADRL).
23 |
24 | Args:
25 | action (list): [vx, vy] command for this agent
26 | dt (float): time in seconds to execute :code:`action`
27 |
28 | """
29 | dx = action[0] * dt
30 | dy = action[1] * dt
31 | self.agent.pos_global_frame += np.array([dx, dy])
32 |
33 | selected_speed = math.sqrt(dx ** 2 + dy ** 2)
34 | selected_heading = np.arctan2(dy, dx)
35 |
36 | self.agent.vel_global_frame[0] = action[0]
37 | self.agent.vel_global_frame[1] = action[1]
38 |
39 | self.agent.speed_global_frame = selected_speed
40 | self.agent.delta_heading_global_frame = wrap(selected_heading - self.agent.heading_global_frame)
41 | self.agent.heading_global_frame = selected_heading
42 |
43 | # turning dir: needed for cadrl value fn
44 | if abs(self.agent.turning_dir) < 1e-5:
45 | self.agent.turning_dir = 0.11 * np.sign(selected_heading)
46 | elif self.agent.turning_dir * selected_heading < 0:
47 | self.agent.turning_dir = max(-np.pi, min(np.pi, -self.agent.turning_dir + selected_heading))
48 | else:
49 | self.agent.turning_dir = np.sign(self.agent.turning_dir) * max(0.0, abs(self.agent.turning_dir) - 0.1)
50 |
--------------------------------------------------------------------------------
/mamp/envs/gazeboEnv.py:
--------------------------------------------------------------------------------
1 | import rospy
2 | import time
3 | import threading
4 | import numpy as np
5 |
6 | from mamp.policies.kdTree import KDTree
7 |
8 | from std_srvs.srv import Empty
9 | from gazebo_msgs.msg import ModelState
10 | from gazebo_msgs.srv import SetModelState
11 |
12 | from .macaenv import MACAEnv
13 | from .rosport.bot import AgentPort
14 |
15 |
16 | def thread_job():
17 | rospy.spin()
18 |
19 |
20 | class GazeboEnv(MACAEnv):
21 | def __init__(self):
22 | self.env_name = 'gazebo'
23 | super(GazeboEnv, self).__init__()
24 | self.node = rospy.init_node('macaenv_node', anonymous=True)
25 | self.reset_world = rospy.ServiceProxy('/gazebo/reset_world', Empty)
26 | self.set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
27 | self.rate = rospy.Rate(1 / self.dt_nominal)
28 | add_thread = threading.Thread(target=thread_job)
29 | add_thread.start()
30 |
31 | def set_start(self, model_name, pos, heading):
32 | x, y = pos[0], pos[1]
33 | state = ModelState()
34 | state.model_name = model_name
35 | state.reference_frame = 'world' # ''ground_plane'
36 | # pose
37 | state.pose.position.x = x
38 | state.pose.position.y = y
39 | state.pose.position.z = 0
40 | # quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
41 | state.pose.orientation.x = 0
42 | state.pose.orientation.y = 0
43 | state.pose.orientation.z = np.sin(heading/2)
44 | state.pose.orientation.w = np.cos(heading/2)
45 | # twist
46 | state.twist.linear.x = 0
47 | state.twist.linear.y = 0
48 | state.twist.linear.z = 0
49 | state.twist.angular.x = 0
50 | state.twist.angular.y = 0
51 | state.twist.angular.z = 0
52 |
53 | rospy.wait_for_service('/gazebo/set_model_state')
54 | try:
55 | set_state = self.set_state
56 | result = set_state(state)
57 | assert result.success is True
58 | except rospy.ServiceException:
59 | print("/gazebo/get_model_state service call failed")
60 |
61 | def reset(self):
62 | super().reset()
63 | self.reset_world()
64 | # 将Agent和Obj进行绑定
65 | for agent in self.agents:
66 | agent.obj = AgentPort(agent.id, agent.name)
67 | self.set_start('Agent'+str(agent.id+1), agent.pos_global_frame, agent.heading_global_frame)
68 | agent.obj.stop_moving()
69 | time.sleep(0.5)
70 | for agent in self.agents:
71 | agent.update_from_obj() # update agent's dynamic state
72 |
73 | def _take_action(self, actions):
74 | super()._take_action(actions)
75 | self.rate.sleep()
76 |
77 | def step(self, actions):
78 | next_observations, rewards, game_over, info = super().step(actions)
79 | if game_over: self._stop_robots()
80 | return next_observations, rewards, game_over, info
81 |
82 | def _stop_robots(self):
83 | for agent in self.agents:
84 | agent.obj.stop_moving()
85 |
--------------------------------------------------------------------------------
/draw/animate.py:
--------------------------------------------------------------------------------
1 | import os
2 | import json
3 | import glob
4 | import imageio
5 | import numpy as np
6 | import pandas as pd
7 |
8 | from plt2d import plot_episode
9 |
10 | case_name = 'pca' # circle_case rand_case
11 | # safety_weight = 0
12 | # forname = 2 * safety_weight
13 |
14 | abs_path = os.path.abspath('.')
15 | plot_save_dir = abs_path + '/' + case_name + '/'
16 | log_save_dir = abs_path + '/' + case_name + '/'
17 |
18 |
19 | def get_agent_traj(info_file, traj_file):
20 | with open(info_file, "r") as f:
21 | json_info = json.load(f)
22 |
23 | traj_list = []
24 | step_num_list = []
25 | agents_info = json_info['all_agent_info']
26 | obstacles_info = json_info['all_obstacle']
27 | for agent_info in agents_info:
28 | agent_id = agent_info['id']
29 | agent_gp = agent_info['gp']
30 | agent_rd = agent_info['radius']
31 |
32 | df = pd.read_excel(traj_file, index_col=0, sheet_name='agent' + str(agent_id))
33 | step_num_list.append(df.shape[0])
34 | traj_list.append(df.to_dict('list'))
35 | # for indexs in df.index:
36 | # traj_info.append(df.loc[indexs].values[:].tolist())
37 | # traj_info = np.array(traj_info)
38 |
39 | return agents_info, traj_list, step_num_list, obstacles_info
40 |
41 |
42 | def png_to_gif(general_fig_name, last_fig_name, animation_filename, video_filename):
43 | all_filenames = plot_save_dir + general_fig_name
44 | last_filename = plot_save_dir + last_fig_name
45 | print(all_filenames)
46 |
47 | # Dump all those images into a gif (sorted by timestep)
48 | filenames = glob.glob(all_filenames)
49 | filenames.sort()
50 | images = []
51 | for filename in filenames:
52 | images.append(imageio.imread(filename))
53 | os.remove(filename)
54 | for i in range(10):
55 | images.append(imageio.imread(last_filename))
56 |
57 | # Save the gif in a new animations sub-folder
58 | animation_save_dir = plot_save_dir + "animations/"
59 | os.makedirs(animation_save_dir, exist_ok=True)
60 | video_filename = animation_save_dir + video_filename
61 | animation_filename = animation_save_dir + animation_filename
62 | imageio.mimsave(animation_filename, images, )
63 |
64 | # convert .gif to .mp4
65 | try:
66 | import moviepy.editor as mp
67 | except imageio.core.fetching.NeedDownloadError:
68 | imageio.plugins.ffmpeg.download()
69 | import moviepy.editor as mp
70 | clip = mp.VideoFileClip(animation_filename)
71 | clip.write_videofile(video_filename)
72 |
73 |
74 | info_file = log_save_dir + 'log/env_cfg.json'
75 | traj_file = log_save_dir + 'log/trajs.xlsx'
76 |
77 | agents_info, traj_list, step_num_list, obstacles_info = get_agent_traj(info_file, traj_file)
78 |
79 | agent_num = len(step_num_list)
80 | base_fig_name_style = "{test_case}_{policy}_{num_agents}agents"
81 | base_fig_name = base_fig_name_style.format(policy=case_name, num_agents=agent_num, test_case=str(0).zfill(3))
82 | general_fig_name = base_fig_name + '_*.png'
83 | last_fig_name = base_fig_name + '.png'
84 | animation_filename = base_fig_name + '.gif'
85 | video_filename = base_fig_name + '.mp4'
86 |
87 | plot_episode(obstacles_info, agents_info, traj_list, step_num_list, plot_save_dir=plot_save_dir,
88 | base_fig_name=base_fig_name, last_fig_name=last_fig_name, show=False)
89 |
90 | png_to_gif(general_fig_name, last_fig_name, animation_filename, video_filename)
91 |
--------------------------------------------------------------------------------
/mamp/dynamics/UnicycleDynamics.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | from mamp.util import wrap
3 | from mamp.dynamics.Dynamics import Dynamics
4 |
5 |
6 | class UnicycleDynamics(Dynamics):
7 | """ Convert a speed & heading to a new state according to Unicycle Kinematics model.
8 |
9 | """
10 |
11 | def __init__(self, agent):
12 | Dynamics.__init__(self, agent)
13 | self.action_type = "R_THETA"
14 |
15 | def step(self, action, dt):
16 | """
17 |
18 | In the global frame, assume the agent instantaneously turns by :code:`heading`
19 | and moves forward at :code:`speed` for :code:`dt` seconds.
20 | Add that offset to the current position. Update the velocity in the
21 | same way. Also update the agent's turning direction (only used by CADRL).
22 |
23 | Args:
24 | action (list): [delta heading angle, speed] command for this agent
25 | dt (float): time in seconds to execute :code:`action`
26 |
27 | """
28 | selected_speed = action[0]
29 | selected_heading = wrap(action[1] + self.agent.heading_global_frame)
30 |
31 | dx = selected_speed * np.cos(selected_heading) * dt
32 | dy = selected_speed * np.sin(selected_heading) * dt
33 | self.agent.pos_global_frame += np.array([dx, dy])
34 | self.agent.total_dist += np.sqrt(dx**2+dy**2)
35 |
36 | # 以下注释代码只是对环境范围做一下约束，当智能体在约束环境之外时，自动调为约束环境范围内
37 | # cmb_array = np.concatenate(
38 | # [np.array([[self.agent.max_x, self.agent.max_y]]), self.agent.pos_global_frame[np.newaxis, :]])
39 | # self.agent.pos_global_frame = np.min(cmb_array, axis=0)
40 | # cmb_array = np.concatenate(
41 | # [np.array([[self.agent.min_x, self.agent.min_y]]), self.agent.pos_global_frame[np.newaxis, :]])
42 | # self.agent.pos_global_frame = np.max(cmb_array, axis=0)
43 |
44 | self.agent.vel_global_frame[0] = round(selected_speed * np.cos(selected_heading), 5)
45 | self.agent.vel_global_frame[1] = round(selected_speed * np.sin(selected_heading), 5)
46 | self.agent.speed_global_frame = selected_speed
47 | self.agent.delta_heading_global_frame = wrap(selected_heading - self.agent.heading_global_frame)
48 | self.agent.heading_global_frame = selected_heading
49 |
50 | # turning dir: needed for cadrl value fn
51 | if abs(self.agent.turning_dir) < 1e-5:
52 | self.agent.turning_dir = 0.11 * np.sign(selected_heading)
53 | elif self.agent.turning_dir * selected_heading < 0:
54 | self.agent.turning_dir = max(-np.pi, min(np.pi, -self.agent.turning_dir + selected_heading))
55 | else:
56 | self.agent.turning_dir = np.sign(self.agent.turning_dir) * max(0.0, abs(self.agent.turning_dir) - 0.1)
57 |
58 | def update_no_step(self, action):
59 | if action is None: return
60 | selected_speed = action[0]
61 | # selected_heading = wrap(action[1] + self.agent.heading_global_frame)
62 | selected_heading = self.agent.heading_global_frame
63 |
64 | self.agent.vel_global_frame[0] = selected_speed * np.cos(selected_heading)
65 | self.agent.vel_global_frame[1] = selected_speed * np.sin(selected_heading)
66 | self.agent.speed_global_frame = selected_speed
67 | # self.agent.delta_heading_global_frame = wrap(selected_heading - self.agent.heading_global_frame)
68 | # self.agent.heading_global_frame = selected_heading
69 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | ## Multi-Vehicle Motion Planning with Posture Constraints in Real World
2 | Python Implementation of multi-vehicle motion planning, including our proposed method, namely postured collision avoidance (PCA), the reciprocal volocity obstacles (RVO), and the optimal reciprocal collision avoidance (ORCA, also namely RVO2) .
3 |
4 | -----
5 |
6 | Description
7 | -----
8 |
9 | This paper investigates more practical applications in the real world for non-holonomic unmanned ground vehicles. In this case, a strategy of diversion is designed to optimize the smoothness of motion. Considering the problem of the posture constraints, a postured collision avoidance (PCA) algorithm is proposed for the motion planning of the multiple non-holonomic unmanned ground vehicles. In addition, we provide the optimal reciprocal collision avoidance (ORCA) method and reciprocal velocity obstacles (RVO) method for comparison.
10 |
11 | About
12 | -----
13 |
14 | **Paper**: Multi-Vehicle Motion Planning with Posture Constraints in Real World, Gang Xu, Yansong Chen, Junjie Cao\*, Deye Zhu, Weiwei Liu, and Yong Liu\*, Accepted as TMECH/AIM Focused Section Paper in IEEE-ASME Transactions on Mechatronics (**TMECH**).
15 |
16 |
17 | -----
18 |
19 | Requirement
20 | -----
21 |
22 | ```python
23 | pip install numpy
24 | pip install pandas
25 | pip install matplotlib
26 | ```
27 |
28 | -----
29 |
30 | Applications
31 | -----
32 |
33 | ```python
34 | cd run_py
35 | python run_pca.py
36 | For the test, you can select the scenarios, including circle, random.
37 | ```
38 |
39 | #### Simulation 1:
40 |
41 |
42 | 
43 |
44 |
45 |
46 |
47 |
48 | #### Experimental Results (Ours: DRVO, PCA):
49 |
50 |
51 | 
52 |
53 |
54 | 
55 |
56 |
57 | 
58 |
59 |
60 | 
61 |
62 |
63 | #### Large Scale Results (Ours: DRVO, PCA):
64 |
65 |
66 | 
67 |
68 |
69 | 
70 |
71 |
72 |
73 | ----
74 |
75 | References
76 | ----
77 |
78 | * Papers on [RVO](https://www.cs.unc.edu/~geom/RVO/icra2008.pdf), [ORCA](http://gamma-web.iacs.umd.edu/ORCA/publications/ORCA.pdf).
79 |
80 |
81 | ----
82 |
83 | Discussion
84 | ----
85 |
86 | In the first simulation, the preferred speed is 0.22 m/s. In the compared experiments, the preferred speed is 1.0 m/s. All agents' radius is 0.2 m. The radius of circle benchmark is set as 15 m for DRVO, ORCA and RVO. The square of random benchmark is set as 30 m * 30 m for DRVO, ORCA and RVO. The radius of circle benchmark is set as 40 m for PCA and RVO + Dubins. The square of random benchmark is set as 80 m * 80 m for PCA and RVO + Dubins.
87 |
88 | In the large scale, the radius of circle benchmark is set as 20 m for DRVO, ORCA and RVO, the square of random benchmark is set as 60 m * 60 m for DRVO, ORCA and RVO, the radius of circle benchmark is set as 50 m for PCA and RVO + Dubins, and the square of random benchmark is set as 100 m * 100 m for PCA and RVO + Dubins.
89 |
--------------------------------------------------------------------------------
/mamp/envs/rosport/car.py:
--------------------------------------------------------------------------------
1 | #!/usr/env/bin python
2 | import numpy as np
3 | import math
4 | import rospy
5 | from nav_msgs.msg import Odometry
6 | from mrobot.msg import EnvInfo
7 | from geometry_msgs.msg import Twist
8 |
9 |
10 | class RosCarPort(object):
11 |
12 | """
13 | 循迹机器人ros接口类
14 | """
15 |
16 | def __init__(self, agent):
17 | self.id = agent.id
18 | self.name = agent.name
19 | self.group = agent.group # 0 表示 leader 1 表示 follower
20 | self.radianR = None
21 | self.radianP = None
22 | self.radianY = None
23 | self.angleR = None
24 | self.angleP = None
25 | self.angleY = None
26 |
27 | self.str_pub_twist = '/' + self.name + '/cmd_vel'
28 | self.pub_twist = rospy.Publisher(self.str_pub_twist,Twist,queue_size=1)
29 | self.str_odom = '/' + self.name + '/odom'
30 | self.sub_odom = rospy.Subscriber(self.str_odom, Odometry, self.pos_callback)
31 | self.agent = agent
32 | if self.group == 0:
33 | self.detect_sub = rospy.Subscriber("env_feedback", EnvInfo, \
34 | self.error_callback)
35 |
36 | def pos_callback(self, data):
37 | """require pos info"""
38 |
39 | x = data.pose.pose.position.x
40 | y = data.pose.pose.position.y
41 | dist = (x-self.agent.pose_global_frame[0])**2 + \
42 | (y-self.agent.pose_global_frame[1])**2
43 | self.agent.distance += math.sqrt(dist)
44 |
45 | self.agent.pose_list.append([x, y])
46 |
47 | self.agent.v_x = data.twist.twist.linear.x
48 | self.agent.w_z = data.twist.twist.angular.z
49 | self.agent.pose_global_frame = np.array([x,y])
50 |
51 | """
52 | require angle info
53 | """
54 |
55 | qx = data.pose.pose.orientation.x
56 | qy = data.pose.pose.orientation.y
57 | qz = data.pose.pose.orientation.z
58 | qw = data.pose.pose.orientation.w
59 |
60 | self.radianR = math.atan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx * qx + qy * qy))
61 | self.radianP = math.asin(2 * (qw * qy - qz * qx))
62 | self.radianY = math.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qz * qz + qy * qy))
63 |
64 | self.angleR = self.radianR * 180 / math.pi # 横滚角
65 | self.angleP = self.radianP * 180 / math.pi # 俯仰角
66 | self.angleY = self.radianY * 180 / math.pi # 偏航角
67 |
68 | self.agent.heading_global_frame = self.radianY
69 |
70 | self.agent.vel_global_frame = [math.cos(self.radianY)*self.agent.v_x,
71 | math.sin(self.radianY)* self.agent.v_x]
72 |
73 |
74 | def error_callback(self, data):
75 | """require error info"""
76 |
77 | e_x = data.error_x
78 | e_k = data.error_k
79 | self.agent.cx = data.cx
80 | self.agent.cy = data.cy
81 | self.agent.failed_flag = data.failed_flag
82 | self.agent.error.pop()
83 | self.agent.error.insert(0, e_x)
84 | self.agent.error_k.pop()
85 | self.agent.error_k.insert(0, e_k)
86 | self.agent.errorx_list.append(self.agent.error[0])
87 | self.agent.last_error = self.agent.error[0]
88 |
89 | def pubTwist(self, action, dt):
90 | twist = Twist()
91 | if self.group == 0 :
92 | twist.linear.x = action[0]
93 | twist.angular.z = action[1]
94 | else:
95 | twist.linear.x = action[0]
96 | twist.angular.z = action[1] / dt
97 | twist.angular.z = np.clip(twist.angular.z, -np.pi/2, np.pi/2)
98 |
99 | self.pub_twist.publish(twist)
100 |
101 | def stop_moving(self):
102 | twist = Twist()
103 | self.pub_twist.publish(twist)
104 |
105 |
106 |
107 |
108 |
109 |
110 |
111 |
112 |
--------------------------------------------------------------------------------
/draw/vis_util.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | import matplotlib.pyplot as plt
3 | from math import sin, cos, atan2, sqrt, pow
4 |
5 | plt_colors = [[0.8500, 0.3250, 0.0980], [0.0, 0.4470, 0.7410], [0.4660, 0.8740, 0.1880],
6 | [0.4940, 0.1840, 0.5560],
7 | [0.9290, 0.6940, 0.1250], [0.3010, 0.7450, 0.9330], [0.6350, 0.0780, 0.1840]]
8 |
9 |
10 | # plt_colors = ['red', 'green', 'blue', 'orange', 'yellow', 'purple']
11 |
12 |
13 | def get_2d_car_model(size):
14 | verts = [
15 | [-1. * size, 1. * size], # 矩形左下角的坐标(left,bottom)
16 | [-1. * size, -1. * size], # 矩形左上角的坐标(left,top)
17 | [1. * size, -1. * size], # 矩形右上角的坐标(right,top)
18 | [1. * size, 1. * size], # 矩形右下角的坐标(right, bottom)
19 | [-1. * size, 1. * size], # 封闭到起点
20 | ]
21 | return verts
22 |
23 |
24 | def get_2d_uav_model0(size):
25 | # size /= 2
26 | verts = [
27 | [0., 1. * size], # 矩形左下角的坐标(left, bottom)
28 | [-0.5 * size, -0.8 * size], # 矩形左上角的坐标(left, top)
29 | [0, -0.2 * size], # 矩形右上角的坐标(right,top)
30 | [0.5 * size, -0.8 * size], # 矩形右下角的坐标(right, bottom)
31 | [0., 1. * size], # 封闭到起点
32 | ]
33 | return verts
34 |
35 |
36 | def get_2d_uav_model(size):
37 | # size /= 2
38 | verts = [
39 | [0., 1. * size], # 矩形左下角的坐标(left, bottom)
40 | [-1. * size, -1. * size], # 矩形左上角的坐标(left, top)
41 | [0., -0.5 * size], # 矩形右上角的坐标(right,top)
42 | [1. * size, -1. * size], # 矩形右下角的坐标(right, bottom)
43 | [0., 1. * size], # 封闭到起点
44 | ]
45 | return verts
46 |
47 |
48 | def rgba2rgb(rgba):
49 | # rgba is a list of 4 color elements btwn [0.0, 1.0]
50 | # or a 2d np array (num_colors, 4)
51 | # returns a list of rgb values between [0.0, 1.0] accounting for alpha and background color [1, 1, 1] == WHITE
52 | if isinstance(rgba, list):
53 | alpha = rgba[3]
54 | r = max(min((1 - alpha) * 1.0 + alpha * rgba[0], 1.0), 0.0)
55 | g = max(min((1 - alpha) * 1.0 + alpha * rgba[1], 1.0), 0.0)
56 | b = max(min((1 - alpha) * 1.0 + alpha * rgba[2], 1.0), 0.0)
57 | return [r, g, b]
58 | elif rgba.ndim == 2:
59 | alphas = rgba[:, 3]
60 | r = np.clip((1 - alphas) * 1.0 + alphas * rgba[:, 0], 0, 1)
61 | g = np.clip((1 - alphas) * 1.0 + alphas * rgba[:, 1], 0, 1)
62 | b = np.clip((1 - alphas) * 1.0 + alphas * rgba[:, 2], 0, 1)
63 | return np.vstack([r, g, b]).T
64 |
65 |
66 | def draw_env(ax, buildings_obj_list, keypoints_obj_list, connection_matrix):
67 | pass
68 |
69 |
70 | def draw(origin_pos, objects_list, buildings_obj_list, keypoints_obj_list, connection_matrix):
71 | fig = plt.figure()
72 | plt.xlabel('x (m)')
73 | plt.ylabel('y (m)')
74 | plt.xlim([-500.0, 3500.0])
75 | plt.ylim([-1000.0, 1000.0])
76 | ax = fig.add_subplot(1, 1, 1)
77 |
78 | ax.set_aspect('equal')
79 |
80 | draw_objects(ax, objects_list)
81 | draw_buildings(ax, origin_pos, buildings_obj_list)
82 | draw_roads(ax, keypoints_obj_list, connection_matrix)
83 | plt.show()
84 |
85 |
86 | def draw_buildings(ax, origin_pos, buildings_obj):
87 | for building_obj in buildings_obj:
88 | id = building_obj.id
89 | pos = building_obj.pos
90 | draw_rectangle(ax, origin_pos, pos)
91 |
92 |
93 | def draw_roads(ax, keypoints_obj_list, connection_matrix):
94 | for index_i, info in enumerate(connection_matrix):
95 | self_pos = keypoints_obj_list[index_i].pos
96 | for index_j, distance in enumerate(info):
97 | if distance > 0:
98 | target_pos = keypoints_obj_list[index_j].pos
99 | x = [self_pos[0], target_pos[0]]
100 | y = [self_pos[1], target_pos[1]]
101 | plt.plot(x, y, color='r')
102 | plt.scatter(x, y, color='b')
103 |
104 |
105 | def draw_objects(ax, objects_obj):
106 | for object_obj in objects_obj:
107 | pass
108 |
109 |
110 | def draw_rectangle(ax, origin_pos, pos):
111 | pos_x = pos[0]
112 | pos_y = pos[1]
113 | ax.add_patch(
114 | plt.Rectangle(
115 | (pos_x - 5, pos_y - 5), # (x,y)矩形左下角
116 | 10, # width长
117 | 10, # height宽
118 | color='maroon',
119 | alpha=0.5
120 | ))
121 |
122 |
123 | def convert_to_actual_model_3d(agent_model, pos_global_frame, heading_global_frame):
124 | alpha = heading_global_frame[0]
125 | beta = heading_global_frame[1]
126 | gamma = heading_global_frame[2]
127 | for point in agent_model:
128 | x = point[0]
129 | y = point[1]
130 | z = point[2]
131 | # 进行俯仰计算
132 | r = sqrt(pow(y, 2) + pow(z, 2))
133 | beta_model = atan2(z, y)
134 | beta_ = beta + beta_model
135 | y = r * cos(beta_)
136 | z = r * sin(beta_)
137 | # 进行翻滚计算
138 | h = sqrt(pow(x, 2) + pow(z, 2))
139 | gama_model = atan2(z, x)
140 | gamma_ = gamma + gama_model
141 | x = h * cos(gamma_)
142 | z = h * sin(gamma_)
143 | # 进行航向计算
144 | l = sqrt(pow(x, 2) + pow(y, 2))
145 | alpha_model = atan2(y, x)
146 | alpha_ = alpha + alpha_model - np.pi / 2 # 改加 - np.pi / 2 因为画模型的时候UAV朝向就是正北方向，所以要减去90°
147 | point[0] = l * cos(alpha_) + pos_global_frame[0]
148 | point[1] = l * sin(alpha_) + pos_global_frame[1]
149 | point[2] = z + pos_global_frame[2]
150 |
--------------------------------------------------------------------------------
/mamp/policies/rvoPolicy.py:
--------------------------------------------------------------------------------
1 | import time
2 | import numpy as np
3 | from math import sqrt, sin, cos, atan2, asin, pi, floor, acos, asin
4 | from itertools import product
5 |
6 | from mamp.envs import Config
7 | from mamp.util import sqr, l2norm, norm, normalize, absSq, mod2pi, pi_2_pi, eps, angle_2_vectors, is_intersect
8 | from mamp.policies.policy import Policy
9 |
10 |
11 | class RVOPolicy(Policy):
12 | def __init__(self):
13 | Policy.__init__(self, str="RVOPolicy")
14 | self.type = "internal"
15 | self.now_goal = None
16 |
17 | def find_next_action(self, obs, dict_comm, agent, kdTree):
18 | ts = time.time()
19 | self.now_goal = agent.goal_global_frame
20 | v_pref = compute_v_pref(agent)
21 | vA = agent.vel_global_frame
22 | pA = agent.pos_global_frame
23 | agent_rad = agent.radius + 0.05
24 | computeNeighbors(agent, kdTree)
25 | RVO_BA_all = []
26 | for obj in agent.neighbors:
27 | obj = obj[0]
28 | pB = obj.pos_global_frame
29 | if obj.is_at_goal:
30 | transl_vB_vA = pA
31 | else:
32 | vB = obj.vel_global_frame
33 | transl_vB_vA = pA + 0.5 * (vB + vA) # Use RVO.
34 | obj_rad = obj.radius + 0.05
35 |
36 | RVO_BA = [transl_vB_vA, pA, pB, obj_rad + agent_rad]
37 | RVO_BA_all.append(RVO_BA)
38 | vA_post = intersect(v_pref, RVO_BA_all, agent)
39 | te = time.time()
40 | cost_step = te - ts
41 | agent.total_time += cost_step
42 | action = to_unicycle(vA_post, agent)
43 | theta = angle_2_vectors(vA, vA_post)
44 | agent.vel_global_unicycle[0] = int(1.0 * action[0]*eps)/eps
45 | agent.vel_global_unicycle[1] = int((0.22 * 0.5 * action[1] / Config.DT)*eps)/eps
46 | dist = l2norm(agent.pos_global_frame, agent.goal_global_frame)
47 | if theta > agent.max_heading_change:
48 | print('agent' + str(agent.id), len(agent.neighbors), action, '终点距离:', dist, 'theta:', theta)
49 | else:
50 | print('agent' + str(agent.id), len(agent.neighbors), action, '终点距离:', dist)
51 |
52 | return action
53 |
54 |
55 | def compute_v_pref(agent):
56 | goal = agent.goal_global_frame
57 | diff = goal - agent.pos_global_frame
58 | v_pref = agent.pref_speed * normalize(diff)
59 | if l2norm(goal, agent.pos_global_frame) < Config.NEAR_GOAL_THRESHOLD:
60 | v_pref = np.zeros_like(v_pref)
61 | return v_pref
62 |
63 |
64 | def intersect(v_pref, RVO_BA_all, agent):
65 | norm_v = np.linalg.norm(v_pref)
66 | suitable_V = []
67 | unsuitable_V = []
68 |
69 | Theta = np.arange(0, 2 * pi, step=pi / 32)
70 | RAD = np.linspace(0.02, 1.0, num=5)
71 | v_list = [v_pref[:]]
72 | for theta, rad in product(Theta, RAD):
73 | new_v = np.array([cos(theta), sin(theta)]) * rad * norm_v
74 | new_v = np.array([new_v[0], new_v[1]])
75 | v_list.append(new_v)
76 |
77 | for new_v in v_list:
78 | suit = True
79 | for RVO_BA in RVO_BA_all:
80 | p_0 = RVO_BA[0]
81 | pA = RVO_BA[1]
82 | pB = RVO_BA[2]
83 | combined_radius = RVO_BA[3]
84 | vAB = new_v + pA - p_0
85 | if is_intersect(pA, pB, combined_radius, vAB):
86 | suit = False
87 | break
88 | if suit:
89 | suitable_V.append(new_v)
90 | else:
91 | unsuitable_V.append(new_v)
92 |
93 | # ----------------------
94 | if suitable_V:
95 | suitable_V.sort(key=lambda v: l2norm(v, v_pref)) # sort begin at minimum and end at maximum
96 | vA_post = suitable_V[0]
97 | else:
98 | tc_V = dict()
99 | for unsuit_v in unsuitable_V:
100 | tc_V[tuple(unsuit_v)] = 0
101 | tc = [] # 时间
102 | for RVO_BA in RVO_BA_all:
103 | p_0 = RVO_BA[0]
104 | pA = RVO_BA[1]
105 | pB = RVO_BA[2]
106 | combined_radius = RVO_BA[3]
107 | vAB = unsuit_v + pA - p_0
108 | pApB = pB - pA
109 | if is_intersect(pA, pB, combined_radius, vAB):
110 | discr = sqr(np.dot(vAB, pApB)) - absSq(vAB) * (absSq(pApB) - sqr(combined_radius))
111 | if discr < 0.0:
112 | print(discr)
113 | continue
114 | tc_v = (np.dot(vAB, pApB) - sqrt(discr)) / absSq(vAB)
115 | if tc_v < 0:
116 | tc_v = 0.0
117 | tc.append(tc_v)
118 | if len(tc) == 0:
119 | tc = [0.0]
120 | tc_V[tuple(unsuit_v)] = min(tc) + 1e-5
121 | WT = agent.safetyFactor
122 | vA_post = min(unsuitable_V, key=lambda v: ((WT / tc_V[tuple(v)]) + l2norm(v, v_pref)))
123 | return vA_post
124 |
125 |
126 | def computeNeighbors(agent, kdTree):
127 | agent.neighbors.clear()
128 |
129 | # Check obstacle neighbors.
130 | rangeSq = agent.neighborDist ** 2
131 | if len(agent.neighbors) != agent.maxNeighbors:
132 | rangeSq = 1.0 * agent.neighborDist ** 2
133 | kdTree.computeObstacleNeighbors(agent, rangeSq)
134 |
135 | if agent.in_collision:
136 | return
137 |
138 | # Check other agents.
139 | if len(agent.neighbors) != agent.maxNeighbors:
140 | rangeSq = agent.neighborDist ** 2
141 | kdTree.computeAgentNeighbors(agent, rangeSq)
142 |
143 |
144 | def to_unicycle(vA_post, agent):
145 | vA_post = np.array(vA_post)
146 | norm_vA = norm(vA_post)
147 | yaw_next = mod2pi(atan2(vA_post[1], vA_post[0]))
148 | yaw_current = mod2pi(agent.heading_global_frame)
149 | delta_theta = yaw_next - yaw_current
150 | delta_theta = pi_2_pi(delta_theta)
151 | if delta_theta < -pi:
152 | delta_theta = delta_theta + 2 * pi
153 | if delta_theta > pi:
154 | delta_theta = delta_theta - 2 * pi
155 | if delta_theta >= 1.0:
156 | delta_theta = 1.0
157 | if delta_theta <= -1:
158 | delta_theta = -1.0
159 | action = np.array([norm_vA, delta_theta])
160 | return action
161 |
--------------------------------------------------------------------------------
/mamp/envs/rosport/bot.py:
--------------------------------------------------------------------------------
1 | #!/usr/env/bin python
2 |
3 | import math
4 | import rospy
5 | import numpy as np
6 | from nav_msgs.msg import Odometry, Path
7 | from geometry_msgs.msg import PoseStamped, Twist, Vector3, Point, PoseWithCovarianceStamped, Pose
8 |
9 |
10 | # from tf.transformations import euler_from_quaternion
11 |
12 | class AgentPort(object):
13 | def __init__(self, id, name):
14 | self.id = id
15 | self.name = name
16 | self.taken_action = None
17 | self.pos_global_frame = None
18 | self.radianR = None
19 | self.radianP = None
20 | self.radianY = None
21 | self.angleR = None
22 | self.angleP = None
23 | self.angleY = None
24 | self.goal_global_frame = None
25 | self.new_goal_received = False
26 |
27 | self.str_pub_twist = '/' + self.name + '/cmd_vel'
28 | self.pub_twist = rospy.Publisher(self.str_pub_twist, Twist, queue_size=1)
29 |
30 | self.str_pub_twist_real = '/' + 'robot0' + '/cmd_vel'
31 | self.pub_twist_real = rospy.Publisher(self.str_pub_twist_real, Twist, queue_size=1)
32 |
33 | self.str_pub_path = '/' + self.name + '/trajectory'
34 | self.pub_path = rospy.Publisher(self.str_pub_path, Path, queue_size=1)
35 |
36 | self.str_pose = '/' + self.name + '/pose'
37 | self.str_odom = '/' + self.name + '/odom'
38 | self.str_goal = '/' + self.name + '/move_base_simple/goal'
39 | self.str_robot_pose = '/' + self.name + '/' + self.name + '/robot_pose'
40 | self.sub_odom = rospy.Subscriber(self.str_odom, Odometry, self.cbOdom)
41 | # self.sub_robot_pose = rospy.Subscriber(self.str_robot_pose, Pose, self.cbPose)
42 |
43 | def cbOdom(self, msg):
44 | self.odom = msg
45 | self.pos_global_frame = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y])
46 |
47 | qx = msg.pose.pose.orientation.x
48 | qy = msg.pose.pose.orientation.y
49 | qz = msg.pose.pose.orientation.z
50 | qw = msg.pose.pose.orientation.w
51 | # (roll, pitch, yaw) = euler_from_quaternion([qx, qy, qz, qw]) # 将四元数转化为roll, pitch, yaw
52 | self.radianR = math.atan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx * qx + qy * qy))
53 | self.radianP = math.asin(2 * (qw * qy - qz * qx))
54 | self.radianY = math.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qz * qz + qy * qy))
55 |
56 | self.angleR = self.radianR * 180 / math.pi # 横滚角
57 | self.angleP = self.radianP * 180 / math.pi # 俯仰角
58 | self.angleY = self.radianY * 180 / math.pi # 偏航角
59 |
60 | self.v_x = msg.twist.twist.linear.x
61 | self.w_z = msg.twist.twist.angular.z
62 |
63 | self.vel_global_frame = np.array([math.cos(self.angleY), math.sin(self.angleY)]) * self.v_x
64 |
65 | def cbPose(self, msg):
66 | self.pos_global_frame = np.array([msg.position.x, msg.position.y])
67 |
68 | def stop_moving(self):
69 | twist = Twist()
70 | self.pub_twist.publish(twist)
71 |
72 | def pubTwist(self, action, dt, agent_id):
73 | self.taken_action = action
74 | twist = Twist()
75 | pose = Path()
76 | twist.linear.x = round(1.0 * action[0], 5)
77 | twist.angular.z = round(0.22 * 0.5 * action[1] / dt, 5)
78 |
79 | # test
80 | # twist.linear.x = round(0.2, 5)
81 | # twist.angular.z = 0.0
82 | # twist.angular.z = round(0.22 * 0.5 * action[1] / dt, 5)
83 | self.pub_twist.publish(twist)
84 |
85 | self.pub_path.publish(pose)
86 |
87 | # self.pub_twist_real.publish(twist)
88 |
89 | # RVO Para
90 | # if agent_id == 0: # Agent1
91 | # twist.linear.x = round(1.1 * action[0], 5)
92 | # twist.angular.z = round(0.22 * 0.5 * action[1] / dt, 5)
93 | # self.pub_twist_real.publish(twist)
94 | # if action[1] > 0:
95 | # twist.angular.z = round(1.5*0.18 * 0.5 * action[1] / dt, 5)
96 | # else:
97 | # twist.angular.z = round(1.5*0.11 * 0.5 * action[1] / dt, 5)
98 | # elif agent_id == 1: # Agent2
99 | # if action[1] > 0:
100 | # twist.angular.z = round(1.5*0.13 * 0.5 * action[1] / dt, 5)
101 | # else:
102 | # twist.angular.z = round(1.5*0.13 * 0.5 * action[1] / dt, 5)
103 | # elif agent_id == 2: # Agent3
104 | # if action[1] > 0:
105 | # twist.angular.z = round(1.6*0.10 * 0.5 * action[1] / dt, 5)
106 | # else:
107 | # twist.angular.z = round(1.6*0.15 * 0.5 * action[1] / dt, 5)
108 | # elif agent_id == 3: # Agent4
109 | # if action[1] > 0:
110 | # twist.angular.z = round(1.5*0.16 * 0.5 * action[1] / dt, 5)
111 | # else:
112 | # twist.angular.z = round(1.5*0.12 * 0.5 * action[1] / dt, 5)
113 |
114 | # SCA or Dubins Para
115 | # if agent_id == 0: # Agent1
116 | # if action[1] > 0:
117 | # twist.angular.z = round(1.5*0.12 * 0.5 * action[1] / dt, 5)
118 | # else:
119 | # twist.angular.z = round(1.5*0.12 * 0.5 * action[1] / dt, 5)
120 | # elif agent_id == 1: # Agent2
121 | # if action[1] > 0:
122 | # twist.angular.z = round(1.6*0.13 * 0.5 * action[1] / dt, 5)
123 | # else:
124 | # twist.angular.z = round(1.6*0.13 * 0.5 * action[1] / dt, 5)
125 | # elif agent_id == 2: # Agent3
126 | # if action[1] > 0:
127 | # twist.angular.z = round(1.5*0.10 * 0.5 * action[1] / dt, 5)
128 | # else:
129 | # twist.angular.z = round(1.5*0.15 * 0.5 * action[1] / dt, 5)
130 | # elif agent_id == 3: # Agent4
131 | # if action[1] > 0:
132 | # twist.angular.z = round(1.5*0.16 * 0.5 * action[1] / dt, 5)
133 | # else:
134 | # twist.angular.z = round(1.5*0.12 * 0.5 * action[1] / dt, 5)
135 | # self.pub_twist_real.publish(twist)
136 |
137 | # def cbRobotPose1(self, msg):
138 | # self.obstacle_list[0] = Obstacle(pos = np.array([msg.pose.position.x, msg.pose.position.y]), shape_dict = { 'shape' : "circle", 'feature' : 0.2})
139 |
140 | # def cbRobotPose2(self, msg):
141 | # self.obstacle_list[1] = Obstacle(pos = np.array([msg.pose.position.x, msg.pose.position.y]), shape_dict = { 'shape' : "circle", 'feature' : 0.2})
142 |
143 | def getRobotPose(self):
144 | return self.pos_global_frame
145 |
146 | def getEulerRadian(self):
147 | return self.radianR, self.radianP, self.radianY
148 |
149 | def getGoalPose(self):
150 | return self.goal_global_frame
151 |
--------------------------------------------------------------------------------
/runpy/run_drvo.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python3
2 | import os
3 | import sys
4 | import json
5 | import numpy as np
6 | import pandas as pd
7 |
8 | sys.path.append('/home/wuuya/PycharmProjects/maros')
9 | sys.path.append('/opt/ros/kinetic/lib/python2.7/dist-packages')
10 | import os
11 | import random
12 |
13 | os.environ['GYM_CONFIG_CLASS'] = 'Example'
14 | os.environ['GYM_CONFIG_PATH'] = '../mamp/configs/config.py'
15 | import gym
16 |
17 | gym.logger.set_level(40)
18 |
19 | from mamp.agents.agent import Agent
20 | # Policies
21 | from mamp.policies import policy_dict
22 | # Dynamics
23 | from mamp.dynamics.UnicycleDynamics import UnicycleDynamics
24 | # Sensors
25 | from mamp.sensors import Sensor
26 | from mamp.envs import Config
27 | from mamp.util import mod2pi
28 |
29 |
30 | def simulation_experiment2_in_circle(num_agents, rad):
31 | center = [0.0, 0.0]
32 | test_qi, test_qf = [], []
33 | k = 0 # np.random.uniform(0, 2)
34 | for n in range(num_agents):
35 | test_qi.append([center[0] + round(rad * np.cos(2 * n * np.pi / num_agents + k * np.pi / 4), 2),
36 | center[1] + round(rad * np.sin(2 * n * np.pi / num_agents + k * np.pi / 4), 2),
37 | mod2pi(2 * n * np.pi / num_agents + k * np.pi / 4 + np.pi)])
38 | for n in range(num_agents):
39 | test_qf.append([center[0] + round(rad * np.cos(2 * n * np.pi / num_agents + np.pi + k * np.pi / 4), 2),
40 | center[1] + round(rad * np.sin(2 * n * np.pi / num_agents + np.pi + k * np.pi / 4), 2),
41 | mod2pi(2 * n * np.pi / num_agents + k * np.pi / 4 + np.pi)])
42 | return test_qi, test_qf
43 |
44 |
45 | def set_random_pos(agent_num, r=15):
46 | """
47 | The square is 30 * 30
48 | The maximum number of agents is 961.
49 | """
50 | r = int(r)
51 | poses = []
52 | for i in range(-r, r+1):
53 | for j in range(-r, r+1):
54 | pos = np.array([i, j, 0.0])
55 | poses.append(pos)
56 | agent_pos = random.sample(poses, agent_num)
57 | agent_goal = random.sample(poses, agent_num)
58 |
59 | return agent_pos, agent_goal
60 |
61 |
62 | def build_agents():
63 | """
64 | simulation 1: agents' num is 10-150, circle's radius is 15, random's radius is 15
65 | large simulation: agents' num is 200, circle's radius is 20, random's radius is 30
66 | """
67 | num_agents = 100
68 | test_qi, test_qf = simulation_experiment2_in_circle(num_agents, rad=15.0)
69 | # test_qi, test_qf = set_random_pos(num_agents)
70 | radius = 0.2
71 | pref_speed = 1.0 # turtleBot3's max speed is 0.22 or 0.26 m/s, and 2.84 or 1.82 rad/s
72 | agents = []
73 | for j in range(len(test_qi)):
74 | agents.append(Agent(start_pos=test_qi[j][:2],
75 | goal_pos=test_qf[j][:2],
76 | name='Agent' + str(j+1),
77 | radius=radius, pref_speed=pref_speed,
78 | initial_heading=test_qi[j][2],
79 | goal_heading=test_qf[j][2],
80 | policy=policy_dict['drvo'],
81 | dynamics_model=UnicycleDynamics,
82 | sensors=[Sensor],
83 | id=j))
84 | return agents
85 |
86 |
87 | def build_obstacles():
88 | obstacles = []
89 | return obstacles
90 |
91 |
92 | if __name__ == '__main__':
93 | # Set agent configuration (start/goal pos, radius, size, policy)
94 | agents = build_agents()
95 | obstacles = build_obstacles()
96 | [agent.policy.initialize_network() for agent in agents if hasattr(agent.policy, 'initialize_network')]
97 |
98 | agents_num = len(agents)
99 |
100 | # env = gym.make("MAGazebo-v0")
101 | env = gym.make("MultiAgentCollisionAvoidance-v0")
102 | env.set_agents(agents, obstacles=obstacles)
103 |
104 | # is_back2start is True in experiment1 and is False in experiment2
105 | for agent in agents:
106 | agent.is_back2start = False
107 |
108 | epi_maximum = 1
109 | for epi in range(epi_maximum):
110 | env.reset()
111 | print("episode:", epi)
112 | game_over = False
113 | while not game_over:
114 | print("step is ", env.episode_step_number)
115 | actions = {}
116 | obs, rewards, game_over, which_agents_done = env.step(actions)
117 | print("All agents finished!", env.episode_step_number)
118 | print("Experiment over.")
119 |
120 | log_save_dir = os.path.dirname(os.path.realpath(__file__)) + '/../draw/drvo/log/'
121 | os.makedirs(log_save_dir, exist_ok=True)
122 |
123 | # trajectory
124 | writer = pd.ExcelWriter(log_save_dir + '/trajs_'+str(agents_num)+'.xlsx')
125 | for agent in agents:
126 | agent.history_info.to_excel(writer, sheet_name='agent' + str(agent.id))
127 | writer.save()
128 |
129 | # scenario information
130 | info_dict_to_visualize = {
131 | 'all_agent_info': [],
132 | 'all_obstacle': [],
133 | 'all_compute_time': 0.0,
134 | 'all_straight_distance': 0.0,
135 | 'all_distance': 0.0,
136 | 'successful_num': 0,
137 | 'all_desire_step_num': 0,
138 | 'all_step_num': 0,
139 | 'SuccessRate': 0.0,
140 | 'ExtraTime': 0.0,
141 | 'ExtraDistance': 0.0,
142 | 'AverageSpeed': 0.0,
143 | 'AverageCost': 0.0
144 | }
145 | all_straight_dist = 0.0
146 | all_agent_total_time = 0.0
147 | all_agent_total_dist = 0.0
148 | num_of_success = 0
149 | all_desire_step_num = 0
150 | all_step_num = 0
151 |
152 | SuccessRate = 0.0
153 | ExtraTime = 0.0
154 | ExtraDistance = 0.0
155 | AverageSpeed = 0.0
156 | AverageCost = 0.0
157 | for agent in agents:
158 | agent_info_dict = {'id': agent.id, 'gp': agent.group, 'radius': agent.radius,
159 | 'goal_pos': agent.goal_global_frame.tolist()}
160 | info_dict_to_visualize['all_agent_info'].append(agent_info_dict)
161 | if not agent.in_collision and not agent.ran_out_of_time:
162 | num_of_success += 1
163 | all_agent_total_time += agent.total_time
164 | all_straight_dist += agent.straight_path_length
165 | all_agent_total_dist += agent.total_dist
166 | all_desire_step_num += agent.desire_steps
167 | all_step_num += agent.step_num
168 | info_dict_to_visualize['all_compute_time'] = all_agent_total_time
169 | info_dict_to_visualize['all_straight_distance'] = all_straight_dist
170 | info_dict_to_visualize['all_distance'] = all_agent_total_dist
171 | info_dict_to_visualize['successful_num'] = num_of_success
172 | info_dict_to_visualize['all_desire_step_num'] = all_desire_step_num
173 | info_dict_to_visualize['all_step_num'] = all_step_num
174 |
175 | info_dict_to_visualize['SuccessRate'] = num_of_success / agents_num
176 | info_dict_to_visualize['ExtraTime'] = ((all_step_num - all_desire_step_num) * Config.DT) / num_of_success
177 | info_dict_to_visualize['ExtraDistance'] = (all_agent_total_dist - all_straight_dist) / num_of_success
178 | info_dict_to_visualize['AverageSpeed'] = all_agent_total_dist / all_step_num / Config.DT
179 | info_dict_to_visualize['AverageCost'] = 1000 * all_agent_total_time / all_step_num
180 |
181 | for obstacle in env.obstacles:
182 | obstacle_info_dict = {'position': obstacle.pos, 'shape': obstacle.shape, 'feature': obstacle.feature}
183 | info_dict_to_visualize['all_obstacle'].append(obstacle_info_dict)
184 |
185 | info_str = json.dumps(info_dict_to_visualize, indent=4)
186 | with open(log_save_dir + '/env_cfg_'+str(agents_num)+'.json', 'w') as json_file:
187 | json_file.write(info_str)
188 | json_file.close()
189 |
--------------------------------------------------------------------------------
/runpy/run_orca.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python3
2 | import os
3 | import sys
4 | import json
5 | import random
6 | import numpy as np
7 | import pandas as pd
8 |
9 | # set path
10 | sys.path.append('/home/wuuya/PycharmProjects/maros')
11 | sys.path.append('/opt/ros/kinetic/lib/python2.7/dist-packages')
12 | import os
13 |
14 | os.environ['GYM_CONFIG_CLASS'] = 'Example'
15 | os.environ['GYM_CONFIG_PATH'] = '../mamp/configs/config.py'
16 | import gym
17 |
18 | gym.logger.set_level(40)
19 |
20 | from mamp.agents.agent import Agent, Obstacle
21 | # Policies
22 | from mamp.policies import policy_dict
23 | # Dynamics
24 | from mamp.dynamics.UnicycleDynamics import UnicycleDynamics
25 | # Sensors
26 | from mamp.sensors import Sensor
27 | from mamp.envs import Config
28 | from mamp.util import mod2pi
29 |
30 |
31 | def simulation_experiment2_in_circle(num_agents, rad):
32 | center = [0.0, 0.0]
33 | test_qi, test_qf = [], []
34 | k = 0 # np.random.uniform(0, 2)
35 | for n in range(num_agents):
36 | test_qi.append([center[0] + round(rad * np.cos(2 * n * np.pi / num_agents + k * np.pi / 4), 2),
37 | center[1] + round(rad * np.sin(2 * n * np.pi / num_agents + k * np.pi / 4), 2),
38 | mod2pi(2 * n * np.pi / num_agents + k * np.pi / 4 + np.pi)])
39 | for n in range(num_agents):
40 | test_qf.append([center[0] + round(rad * np.cos(2 * n * np.pi / num_agents + np.pi + k * np.pi / 4), 2),
41 | center[1] + round(rad * np.sin(2 * n * np.pi / num_agents + np.pi + k * np.pi / 4), 2),
42 | mod2pi(2 * n * np.pi / num_agents + k * np.pi / 4 + np.pi)])
43 | return test_qi, test_qf
44 |
45 |
46 | def set_random_pos(agent_num, r=15):
47 | """
48 | The square is 30 * 30
49 | The maximum number of agents is 961.
50 | """
51 | r = int(r)
52 | poses = []
53 | for i in range(-r, r+1):
54 | for j in range(-r, r+1):
55 | pos = np.array([i, j, 0.0])
56 | poses.append(pos)
57 | agent_pos = random.sample(poses, agent_num)
58 | agent_goal = random.sample(poses, agent_num)
59 |
60 | return agent_pos, agent_goal
61 |
62 |
63 | def build_agents():
64 | """
65 | simulation 1: agents' num is 10-150, circle's radius is 15, random's radius is 15
66 | large simulation: agents' num is 200, circle's radius is 20, random's radius is 30
67 | """
68 | num_agents = 100
69 | test_qi, test_qf = simulation_experiment2_in_circle(num_agents, rad=15.0)
70 | # test_qi, test_qf = set_random_pos(num_agents, r=30)
71 | radius = 0.2
72 | pref_speed = 1.0 # turtleBot3's max speed is 0.22 or 0.26 m/s, and 2.84 or 1.82 rad/s
73 | agents = []
74 | for j in range(len(test_qi)):
75 | agents.append(Agent(start_pos=test_qi[j][:2],
76 | goal_pos=test_qf[j][:2],
77 | name='Agent' + str(j + 1),
78 | radius=radius, pref_speed=pref_speed,
79 | initial_heading=test_qi[j][2],
80 | goal_heading=test_qf[j][2],
81 | policy=policy_dict['orca'],
82 | dynamics_model=UnicycleDynamics,
83 | sensors=[Sensor],
84 | id=j))
85 | return agents
86 |
87 |
88 | def build_obstacles():
89 | """
90 | exp_2: r = 16.0, agents' num is 100, radius of circle is 40
91 | """
92 | obstacles = []
93 | r = 16.0
94 | # obstacles.append(Obstacle(pos=[0.0, 0.0], shape_dict={'shape': "rect", 'feature': (r, r)}, id=0))
95 | return obstacles
96 |
97 |
98 | if __name__ == '__main__':
99 | # Set agent configuration (start/goal pos, radius, size, policy)
100 | agents = build_agents()
101 | obstacles = build_obstacles()
102 | [agent.policy.initialize_network() for agent in agents if hasattr(agent.policy, 'initialize_network')]
103 |
104 | agents_num = len(agents)
105 |
106 | # env = gym.make("MAGazebo-v0")
107 | env = gym.make("MultiAgentCollisionAvoidance-v0")
108 | env.set_agents(agents, obstacles=obstacles)
109 |
110 | epi_maximum = 1
111 | for epi in range(epi_maximum):
112 | env.reset()
113 | print("episode:", epi)
114 | game_over = False
115 | while not game_over:
116 | print("step is ", env.episode_step_number)
117 | actions = {}
118 | obs, rewards, game_over, which_agents_done = env.step(actions)
119 | print("All agents finished!", env.episode_step_number)
120 | print("Experiment over.")
121 |
122 | log_save_dir = os.path.dirname(os.path.realpath(__file__)) + '/../draw/orca/log/'
123 | os.makedirs(log_save_dir, exist_ok=True)
124 |
125 | # trajectory
126 | writer = pd.ExcelWriter(log_save_dir + '/trajs_'+str(agents_num)+'.xlsx')
127 | for agent in agents:
128 | agent.history_info.to_excel(writer, sheet_name='agent' + str(agent.id))
129 | writer.save()
130 |
131 | # scenario information
132 | info_dict_to_visualize = {
133 | 'all_agent_info': [],
134 | 'all_obstacle': [],
135 | 'all_compute_time': 0.0,
136 | 'all_straight_distance': 0.0,
137 | 'all_distance': 0.0,
138 | 'successful_num': 0,
139 | 'all_desire_step_num': 0,
140 | 'all_step_num': 0,
141 | 'SuccessRate': 0.0,
142 | 'ExtraTime': 0.0,
143 | 'ExtraDistance': 0.0,
144 | 'AverageSpeed': 0.0,
145 | 'AverageCost': 0.0
146 | }
147 | all_straight_dist = 0.0
148 | all_agent_total_time = 0.0
149 | all_agent_total_dist = 0.0
150 | num_of_success = 0
151 | all_desire_step_num = 0
152 | all_step_num = 0
153 |
154 | SuccessRate = 0.0
155 | ExtraTime = 0.0
156 | ExtraDistance = 0.0
157 | AverageSpeed = 0.0
158 | AverageCost = 0.0
159 | for agent in agents:
160 | agent_info_dict = {'id': agent.id, 'gp': agent.group, 'radius': agent.radius,
161 | 'goal_pos': agent.goal_global_frame.tolist()}
162 | info_dict_to_visualize['all_agent_info'].append(agent_info_dict)
163 | if not agent.in_collision and not agent.ran_out_of_time:
164 | num_of_success += 1
165 | all_agent_total_time += agent.total_time
166 | all_straight_dist += agent.straight_path_length
167 | all_agent_total_dist += agent.total_dist
168 | all_desire_step_num += agent.desire_steps
169 | all_step_num += agent.step_num
170 | info_dict_to_visualize['all_compute_time'] = all_agent_total_time
171 | info_dict_to_visualize['all_straight_distance'] = all_straight_dist
172 | info_dict_to_visualize['all_distance'] = all_agent_total_dist
173 | info_dict_to_visualize['successful_num'] = num_of_success
174 | info_dict_to_visualize['all_desire_step_num'] = all_desire_step_num
175 | info_dict_to_visualize['all_step_num'] = all_step_num
176 |
177 | info_dict_to_visualize['SuccessRate'] = num_of_success / agents_num
178 | info_dict_to_visualize['ExtraTime'] = ((all_step_num - all_desire_step_num) * Config.DT) / num_of_success
179 | info_dict_to_visualize['ExtraDistance'] = (all_agent_total_dist - all_straight_dist) / num_of_success
180 | info_dict_to_visualize['AverageSpeed'] = all_agent_total_dist / all_step_num / Config.DT
181 | info_dict_to_visualize['AverageCost'] = 1000 * all_agent_total_time / all_step_num
182 |
183 | for obstacle in env.obstacles:
184 | obstacle_info_dict = {'position': obstacle.pos, 'shape': obstacle.shape, 'feature': obstacle.feature}
185 | info_dict_to_visualize['all_obstacle'].append(obstacle_info_dict)
186 |
187 | info_str = json.dumps(info_dict_to_visualize, indent=4)
188 | with open(log_save_dir + '/env_cfg_'+str(agents_num)+'.json', 'w') as json_file:
189 | json_file.write(info_str)
190 | json_file.close()
191 |
--------------------------------------------------------------------------------
/runpy/run_rvo.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python3
2 | import os
3 | import sys
4 | import json
5 | import numpy as np
6 | import pandas as pd
7 |
8 | # set path
9 | sys.path.append('/home/wuuya/PycharmProjects/maros')
10 | sys.path.append('/opt/ros/kinetic/lib/python2.7/dist-packages')
11 | import os
12 | import random
13 |
14 | os.environ['GYM_CONFIG_CLASS'] = 'Example'
15 | os.environ['GYM_CONFIG_PATH'] = '../mamp/configs/config.py'
16 | import gym
17 |
18 | gym.logger.set_level(40)
19 |
20 | from mamp.agents.agent import Agent, Obstacle
21 | # Policies
22 | from mamp.policies import policy_dict
23 | # Dynamics
24 | from mamp.dynamics.UnicycleDynamics import UnicycleDynamics
25 | # Sensors
26 | from mamp.sensors import Sensor
27 | from mamp.envs import Config
28 | from mamp.util import mod2pi
29 |
30 |
31 | def simulation_experiment2_in_circle(num_agents, rad):
32 | center = [0.0, 0.0]
33 | test_qi, test_qf = [], []
34 | k = 0 # np.random.uniform(0, 2)
35 | for n in range(num_agents):
36 | test_qi.append([center[0] + round(rad * np.cos(2 * n * np.pi / num_agents + k * np.pi / 4), 2),
37 | center[1] + round(rad * np.sin(2 * n * np.pi / num_agents + k * np.pi / 4), 2),
38 | mod2pi(2 * n * np.pi / num_agents + k * np.pi / 4 + np.pi)])
39 | for n in range(num_agents):
40 | test_qf.append([center[0] + round(rad * np.cos(2 * n * np.pi / num_agents + np.pi + k * np.pi / 4), 2),
41 | center[1] + round(rad * np.sin(2 * n * np.pi / num_agents + np.pi + k * np.pi / 4), 2),
42 | mod2pi(2 * n * np.pi / num_agents + k * np.pi / 4 + np.pi)])
43 | return test_qi, test_qf
44 |
45 |
46 | def set_random_pos(agent_num, r=15):
47 | """
48 | The square is 30 * 30
49 | The maximum number of agents is 961.
50 | """
51 | r = int(r)
52 | poses = []
53 | for i in range(-r, r+1):
54 | for j in range(-r, r+1):
55 | pos = np.array([i, j, 0.0])
56 | poses.append(pos)
57 | agent_pos = random.sample(poses, agent_num)
58 | agent_goal = random.sample(poses, agent_num)
59 |
60 | return agent_pos, agent_goal
61 |
62 |
63 | def build_agents():
64 | """
65 | simulation 1: agents' num is 10-150, circle's radius is 15, random's radius is 15
66 | large simulation: agents' num is 200, circle's radius is 20, random's radius is 30
67 | """
68 | num_agents = 100
69 | test_qi, test_qf = simulation_experiment2_in_circle(num_agents, rad=15.0)
70 | # test_qi, test_qf = set_random_pos(num_agents, r=15)
71 | radius = 0.2
72 | pref_speed = 1.0 # turtleBot3's max speed is 0.22 or 0.26 m/s, and 2.84 or 1.82 rad/s
73 | agents = []
74 | for j in range(len(test_qi)):
75 | agents.append(Agent(start_pos=test_qi[j][:2],
76 | goal_pos=test_qf[j][:2],
77 | name='Agent' + str(j+1),
78 | radius=radius, pref_speed=pref_speed,
79 | initial_heading=test_qi[j][2],
80 | goal_heading=test_qf[j][2],
81 | policy=policy_dict['rvo'],
82 | dynamics_model=UnicycleDynamics,
83 | sensors=[Sensor],
84 | id=j))
85 | return agents
86 |
87 |
88 | def build_obstacles():
89 | """
90 | exp_2: r = 16.0, agents' num is 100, radius of circle is 40
91 | """
92 | obstacles = []
93 | r = 16.0
94 | # obstacles.append(Obstacle(pos=[0.0, 0.0], shape_dict={'shape': "rect", 'feature': (r, r)}, id=0))
95 | return obstacles
96 |
97 |
98 | if __name__ == '__main__':
99 | # Set agent configuration (start/goal pos, radius, size, policy)
100 | agents = build_agents()
101 | obstacles = build_obstacles()
102 | [agent.policy.initialize_network() for agent in agents if hasattr(agent.policy, 'initialize_network')]
103 |
104 | agents_num = len(agents)
105 |
106 | # env = gym.make("MAGazebo-v0")
107 | env = gym.make("MultiAgentCollisionAvoidance-v0")
108 | env.set_agents(agents, obstacles=obstacles)
109 |
110 | epi_maximum = 1
111 | for epi in range(epi_maximum):
112 | env.reset()
113 | print("episode:", epi)
114 | game_over = False
115 | while not game_over:
116 | print("step is ", env.episode_step_number)
117 | actions = {}
118 | obs, rewards, game_over, which_agents_done = env.step(actions)
119 | print("All agents finished!", env.episode_step_number)
120 | print("Experiment over.")
121 |
122 | log_save_dir = os.path.dirname(os.path.realpath(__file__)) + '/../draw/rvo/log/'
123 | os.makedirs(log_save_dir, exist_ok=True)
124 |
125 | # trajectory
126 | writer = pd.ExcelWriter(log_save_dir + '/trajs_'+str(agents_num)+'.xlsx')
127 | for agent in agents:
128 | agent.history_info.to_excel(writer, sheet_name='agent' + str(agent.id))
129 | writer.save()
130 |
131 | # scenario information
132 | info_dict_to_visualize = {
133 | 'all_agent_info': [],
134 | 'all_obstacle': [],
135 | 'all_compute_time': 0.0,
136 | 'all_straight_distance': 0.0,
137 | 'all_distance': 0.0,
138 | 'successful_num': 0,
139 | 'all_desire_step_num': 0,
140 | 'all_step_num': 0,
141 | 'SuccessRate': 0.0,
142 | 'ExtraTime': 0.0,
143 | 'ExtraDistance': 0.0,
144 | 'AverageSpeed': 0.0,
145 | 'AverageCost': 0.0
146 | }
147 | all_straight_dist = 0.0
148 | all_agent_total_time = 0.0
149 | all_agent_total_dist = 0.0
150 | num_of_success = 0
151 | all_desire_step_num = 0
152 | all_step_num = 0
153 |
154 | SuccessRate = 0.0
155 | ExtraTime = 0.0
156 | ExtraDistance = 0.0
157 | AverageSpeed = 0.0
158 | AverageCost = 0.0
159 | for agent in agents:
160 | agent_info_dict = {'id': agent.id, 'gp': agent.group, 'radius': agent.radius,
161 | 'goal_pos': agent.goal_global_frame.tolist()}
162 | info_dict_to_visualize['all_agent_info'].append(agent_info_dict)
163 | if not agent.in_collision and not agent.ran_out_of_time:
164 | num_of_success += 1
165 | all_agent_total_time += agent.total_time
166 | all_straight_dist += agent.straight_path_length
167 | all_agent_total_dist += agent.total_dist
168 | all_desire_step_num += agent.desire_steps
169 | all_step_num += agent.step_num
170 | if num_of_success == 0:
171 | num_of_success = 1e-5
172 | if all_step_num == 0:
173 | all_step_num = 1e-5
174 | info_dict_to_visualize['all_compute_time'] = all_agent_total_time
175 | info_dict_to_visualize['all_straight_distance'] = all_straight_dist
176 | info_dict_to_visualize['all_distance'] = all_agent_total_dist
177 | info_dict_to_visualize['successful_num'] = num_of_success
178 | info_dict_to_visualize['all_desire_step_num'] = all_desire_step_num
179 | info_dict_to_visualize['all_step_num'] = all_step_num
180 |
181 | info_dict_to_visualize['SuccessRate'] = num_of_success / agents_num
182 | info_dict_to_visualize['ExtraTime'] = ((all_step_num - all_desire_step_num) * Config.DT) / num_of_success
183 | info_dict_to_visualize['ExtraDistance'] = (all_agent_total_dist - all_straight_dist) / num_of_success
184 | info_dict_to_visualize['AverageSpeed'] = all_agent_total_dist / all_step_num / Config.DT
185 | info_dict_to_visualize['AverageCost'] = 1000 * all_agent_total_time / all_step_num
186 |
187 | for obstacle in env.obstacles:
188 | obstacle_info_dict = {'position': obstacle.pos, 'shape': obstacle.shape, 'feature': obstacle.feature}
189 | info_dict_to_visualize['all_obstacle'].append(obstacle_info_dict)
190 |
191 | info_str = json.dumps(info_dict_to_visualize, indent=4)
192 | with open(log_save_dir + '/env_cfg_'+str(agents_num)+'.json', 'w') as json_file:
193 | json_file.write(info_str)
194 | json_file.close()
195 |
--------------------------------------------------------------------------------
/mamp/policies/drvoPolicy.py:
--------------------------------------------------------------------------------
1 | import time
2 | import numpy as np
3 | from math import sqrt, sin, cos, atan2, asin, pi, floor, acos, asin
4 | from itertools import combinations, product
5 |
6 | from mamp.envs import Config
7 | from mamp.util import sqr, l2norm, norm, normalize, absSq, mod2pi, pi_2_pi, get_phi, angle_2_vectors, is_intersect, eps
8 | from mamp.policies.policy import Policy
9 |
10 |
11 | class DRVOPolicy(Policy):
12 | def __init__(self):
13 | Policy.__init__(self, str="DRVOPolicy")
14 | self.type = "internal"
15 | self.now_goal = None
16 |
17 | def find_next_action(self, obs, dict_comm, agent, kdTree):
18 | ts = time.time()
19 | self.now_goal = agent.goal_global_frame
20 | v_pref = compute_v_pref(agent)
21 | vA = agent.vel_global_frame
22 | pA = agent.pos_global_frame
23 | agent_rad = agent.radius + 0.05
24 | computeNeighbors(agent, kdTree)
25 | RVO_BA_all = []
26 | for obj in agent.neighbors:
27 | obj = obj[0]
28 | pB = obj.pos_global_frame
29 | if obj.is_at_goal:
30 | transl_vB_vA = pA
31 | else:
32 | vB = obj.vel_global_frame
33 | transl_vB_vA = pA + 0.5 * (vB + vA) # Use RVO.
34 | obj_rad = obj.radius + 0.05
35 |
36 | RVO_BA = [transl_vB_vA, pA, pB, obj_rad + agent_rad]
37 | RVO_BA_all.append(RVO_BA)
38 | vA_post = intersect(v_pref, RVO_BA_all, agent)
39 | te = time.time()
40 | cost_step = te - ts
41 | agent.total_time += cost_step
42 | action = to_unicycle(vA_post, agent)
43 | theta = angle_2_vectors(vA, vA_post)
44 | agent.vel_global_unicycle[0] = int(1.0 * action[0]*eps)/eps
45 | agent.vel_global_unicycle[1] = int((0.22 * 0.5 * action[1] / Config.DT)*eps)/eps
46 | dist = l2norm(agent.pos_global_frame, agent.goal_global_frame)
47 | if theta > agent.max_heading_change:
48 | print('agent' + str(agent.id), len(agent.neighbors), action, '终点距离:', dist, 'theta:', theta)
49 | else:
50 | print('agent' + str(agent.id), len(agent.neighbors), action, '终点距离:', dist)
51 |
52 | return action
53 |
54 |
55 | def compute_v_pref(agent):
56 | goal = agent.goal_global_frame
57 | diff = goal - agent.pos_global_frame
58 | v_pref = agent.pref_speed * normalize(diff)
59 | if l2norm(goal, agent.pos_global_frame) < Config.NEAR_GOAL_THRESHOLD:
60 | v_pref[0] = 0.0
61 | v_pref[1] = 0.0
62 | return np.array(v_pref)
63 |
64 |
65 | def intersect(v_pref, RVO_BA_all, agent):
66 | norm_v = np.linalg.norm(v_pref)
67 | suitable_V = []
68 | unsuitable_V = []
69 |
70 | Theta = np.arange(0, 2 * pi, step=pi / 32)
71 | RAD = np.linspace(0.02, 1.0, num=5)
72 | v_list = [v_pref[:]]
73 | for theta, rad in product(Theta, RAD):
74 | new_v = np.array([cos(theta), sin(theta)]) * rad * norm_v
75 | new_v = np.array([new_v[0], new_v[1]])
76 | v_list.append(new_v)
77 |
78 | for new_v in v_list:
79 | suit = True
80 | for RVO_BA in RVO_BA_all:
81 | p_0 = RVO_BA[0]
82 | pA = RVO_BA[1]
83 | pB = RVO_BA[2]
84 | combined_radius = RVO_BA[3]
85 | v_dif = np.array(new_v + pA - p_0) # new_v-0.5*(vA+vB) or new_v
86 | if is_intersect(pA, pB, combined_radius, v_dif):
87 | suit = False
88 | break
89 | if suit:
90 | suitable_V.append(new_v)
91 | else:
92 | unsuitable_V.append(new_v)
93 |
94 | # ----------------------
95 | if suitable_V:
96 | suitable_V.sort(key=lambda v: l2norm(v, v_pref)) # Sort begin at minimum and end at maximum.
97 | if len(suitable_V) > 1:
98 | vA_post = select_right_side(suitable_V, agent.vel_global_frame, epsilon=1e-1, opt_num=6)
99 | else:
100 | vA_post = suitable_V[0]
101 | else:
102 | print('not find feasible velocity--------------------------------------', len(unsuitable_V), len(suitable_V))
103 | tc_V = dict()
104 | for unsuit_v in unsuitable_V:
105 | tc_V[tuple(unsuit_v)] = 0
106 | tc = [] # Collision time.
107 | for RVO_BA in RVO_BA_all:
108 | p_0 = RVO_BA[0]
109 | pA = RVO_BA[1]
110 | pB = RVO_BA[2]
111 | combined_radius = RVO_BA[3]
112 | v_dif = np.array(unsuit_v + pA - p_0)
113 | pApB = pB - pA
114 | if is_intersect(pA, pB, combined_radius, v_dif):
115 | discr = sqr(np.dot(v_dif, pApB)) - absSq(v_dif) * (absSq(pApB) - sqr(combined_radius))
116 | if discr < 0.0:
117 | print(discr)
118 | continue
119 | tc_v = max((np.dot(v_dif, pApB) - sqrt(discr)) / absSq(v_dif), 0.0)
120 | tc.append(tc_v)
121 | if len(tc) == 0:
122 | tc = [0.0]
123 | tc_V[tuple(unsuit_v)] = min(tc) + 1e-5
124 | WT = agent.safetyFactor
125 | unsuitable_V.sort(key=lambda v: ((WT / tc_V[tuple(v)]) + l2norm(v, v_pref)))
126 | if len(unsuitable_V) > 1:
127 | vA_post = select_right_side(unsuitable_V, agent.vel_global_frame, epsilon=1e-1, opt_num=6)
128 | else:
129 | vA_post = unsuitable_V[0]
130 |
131 | return vA_post
132 |
133 |
134 | def satisfied_constraint(agent, vCand):
135 | vA = agent.vel_global_frame
136 | costheta = np.dot(vA, vCand) / (np.linalg.norm(vA) * np.linalg.norm(vCand))
137 | if costheta > 1.0:
138 | costheta = 1.0
139 | elif costheta < -1.0:
140 | costheta = -1.0
141 | theta = acos(costheta) # Rotational constraints.
142 | if theta <= agent.max_heading_change:
143 | return True
144 | else:
145 | return False
146 |
147 |
148 | def select_right_side(v_list, vA, epsilon=1e-5, opt_num=4):
149 | opt_v = [v_list[0]]
150 | i = 1
151 | while True:
152 | if abs(l2norm(v_list[0], vA) - l2norm(v_list[i], vA)) < epsilon:
153 | opt_v.append(v_list[i])
154 | i += 1
155 | if i == len(v_list) or i == opt_num:
156 | break
157 | else:
158 | break
159 | vA_phi_min = min(opt_v, key=lambda v: get_phi(v))
160 | vA_phi_max = max(opt_v, key=lambda v: get_phi(v))
161 | phi_min = get_phi(vA_phi_min)
162 | phi_max = get_phi(vA_phi_max)
163 | if abs(phi_max - phi_min) <= pi:
164 | vA_opti = vA_phi_min
165 | else:
166 | vA_opti = vA_phi_max
167 | return vA_opti
168 |
169 |
170 | def computeNeighbors(agent, kdTree):
171 | agent.neighbors.clear()
172 |
173 | # Check obstacle neighbors.
174 | rangeSq = agent.neighborDist ** 2
175 | if len(agent.neighbors) != agent.maxNeighbors:
176 | rangeSq = 1.0 * agent.neighborDist ** 2
177 | kdTree.computeObstacleNeighbors(agent, rangeSq)
178 |
179 | if agent.in_collision:
180 | return
181 |
182 | # Check other agents.
183 | if len(agent.neighbors) != agent.maxNeighbors:
184 | rangeSq = agent.neighborDist ** 2
185 | kdTree.computeAgentNeighbors(agent, rangeSq)
186 |
187 |
188 | def to_unicycle(vA_post, agent):
189 | vA_post = np.array(vA_post)
190 | norm_vA = norm(vA_post)
191 | yaw_next = mod2pi(atan2(vA_post[1], vA_post[0]))
192 | yaw_current = mod2pi(agent.heading_global_frame)
193 | delta_theta = yaw_next - yaw_current
194 | delta_theta = pi_2_pi(delta_theta)
195 | if delta_theta < -pi:
196 | delta_theta = delta_theta + 2 * pi
197 | if delta_theta > pi:
198 | delta_theta = delta_theta - 2 * pi
199 | if delta_theta >= 1.0:
200 | delta_theta = 1.0
201 | if delta_theta <= -1:
202 | delta_theta = -1.0
203 | action = np.array([norm_vA, delta_theta])
204 | return action
205 |
--------------------------------------------------------------------------------
/runpy/run_rvodubins.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python3
2 | import os
3 | import sys
4 | import json
5 | import random
6 | import numpy as np
7 | import pandas as pd
8 |
9 | sys.path.append('/home/wuuya/PycharmProjects/maros')
10 | sys.path.append('/opt/ros/kinetic/lib/python2.7/dist-packages')
11 |
12 | os.environ['GYM_CONFIG_CLASS'] = 'Example'
13 | os.environ['GYM_CONFIG_PATH'] = '../mamp/configs/config.py'
14 | import gym
15 |
16 | gym.logger.set_level(40)
17 |
18 | from mamp.agents.agent import Agent
19 | from mamp.agents.obstacle import Obstacle
20 | # Policies
21 | from mamp.policies import policy_dict
22 | # Dynamics
23 | from mamp.dynamics.UnicycleDynamics import UnicycleDynamics
24 | # Sensors
25 | from mamp.sensors import Sensor
26 |
27 | from mamp.util import mod2pi
28 | from mamp.envs import Config
29 |
30 |
31 | def simulation1_in_circle(num_agents, rad=40.0):
32 | center = [0.0, 0.0]
33 | test_qi, test_qf = [], []
34 | k = 0 # rotation parameter
35 | for n in range(num_agents):
36 | test_qi.append([center[0] + round(rad * np.cos(2 * n * np.pi / num_agents + k * np.pi / 4), 2),
37 | center[1] + round(rad * np.sin(2 * n * np.pi / num_agents + k * np.pi / 4), 2),
38 | mod2pi(2 * n * np.pi / num_agents + k * np.pi / 4 + np.pi)])
39 | for n in range(num_agents):
40 | test_qf.append([center[0] + round(rad * np.cos(2 * n * np.pi / num_agents + np.pi + k * np.pi / 4), 2),
41 | center[1] + round(rad * np.sin(2 * n * np.pi / num_agents + np.pi + k * np.pi / 4), 2),
42 | mod2pi(2 * n * np.pi / num_agents + k * np.pi / 4)])
43 | return test_qi, test_qf
44 |
45 |
46 | def set_random_pos(agent_num, r=40):
47 | """
48 | The square is 80 * 80
49 | The maximum number of agents is 81*81.
50 | """
51 | r = int(r)
52 | poses = []
53 | for i in range(-r, r+1):
54 | for j in range(-r, r+1):
55 | pos = np.array([i, j, random.uniform(0.0, 2*np.pi)])
56 | poses.append(pos)
57 | agent_pos = random.sample(poses, agent_num)
58 | agent_goal = random.sample(poses, agent_num)
59 |
60 | return agent_pos, agent_goal
61 |
62 |
63 | def build_agents():
64 | """
65 | simulation1: agents' num is 10-150, circle's radius is 40, random's radius is 40
66 | large simulation : agents' num is 200, circle's radius is 50, random's radius is 50
67 | """
68 | num_agents = 100
69 | test_qi, test_qf = simulation1_in_circle(num_agents, rad=40.0)
70 | # test_qi, test_qf = set_random_pos(num_agents, r=50)
71 | radius = 0.2
72 | pref_speed = 1.0 # turtleBot3's max speed is 0.22 or 0.26 m/s, and 2.84 or 1.82 rad/s
73 | agents = []
74 | for j in range(len(test_qi)):
75 | agents.append(Agent(start_pos=test_qi[j][:2],
76 | goal_pos=test_qf[j][:2],
77 | name='Agent' + str(j+1),
78 | radius=radius, pref_speed=pref_speed,
79 | initial_heading=test_qi[j][2],
80 | goal_heading=test_qf[j][2],
81 | policy=policy_dict['rvo_dubins'],
82 | dynamics_model=UnicycleDynamics,
83 | sensors=[Sensor],
84 | id=j))
85 | return agents
86 |
87 |
88 | def build_obstacles():
89 | """
90 | simulation1: r = 16.0, agents' num is 100, radius of circle is 40
91 | simulation2: r = 16.0, agents' num is 100, radius of circle is 40
92 | exp_real-world: obstacles is None
93 | """
94 | obstacles = []
95 | r = 16.0
96 | # obstacles.append(Obstacle(pos=[0.0, 0.0], shape_dict={'shape': "rect", 'feature': (r, r)}, id=0))
97 | return obstacles
98 |
99 |
100 | if __name__ == '__main__':
101 | # Set agent configuration (start/goal pos, radius, size, policy)
102 | agents = build_agents()
103 | obstacles = build_obstacles()
104 | [agent.policy.initialize_network() for agent in agents if hasattr(agent.policy, 'initialize_network')]
105 |
106 | agents_num = len(agents)
107 |
108 | # env = gym.make("MAGazebo-v0")
109 | env = gym.make("MultiAgentCollisionAvoidance-v0")
110 | env.set_agents(agents, obstacles=obstacles)
111 |
112 | # is_back2start is True in experiment1 and is False in experiment2
113 | for agent in agents:
114 | agent.is_back2start = False
115 |
116 | epi_maximum = 1
117 | for epi in range(epi_maximum):
118 | env.reset()
119 | # print("episode:", epi)
120 | game_over = False
121 | while not game_over:
122 | print("step is ", env.episode_step_number)
123 | actions = {}
124 | obs, rewards, game_over, which_agents_done = env.step(actions)
125 | print("All agents finished!", env.episode_step_number)
126 | print("Experiment over.")
127 |
128 | log_save_dir = os.path.dirname(os.path.realpath(__file__)) + '/../draw/rvo_dubins/log/'
129 | os.makedirs(log_save_dir, exist_ok=True)
130 |
131 | # trajectory
132 | writer = pd.ExcelWriter(log_save_dir + '/trajs_'+str(agents_num)+'.xlsx')
133 | for agent in agents:
134 | agent.history_info.to_excel(writer, sheet_name='agent' + str(agent.id))
135 | writer.save()
136 |
137 | # scenario information
138 | info_dict_to_visualize = {
139 | 'all_agent_info': [],
140 | 'all_obstacle': [],
141 | 'all_compute_time': 0.0,
142 | 'all_straight_distance': 0.0,
143 | 'all_distance': 0.0,
144 | 'successful_num': 0,
145 | 'all_desire_step_num': 0,
146 | 'all_step_num': 0,
147 | 'SuccessRate': 0.0,
148 | 'ExtraTime': 0.0,
149 | 'ExtraDistance': 0.0,
150 | 'AverageSpeed': 0.0,
151 | 'AverageCost': 0.0
152 | }
153 | all_straight_dist = 0.0
154 | all_agent_total_time = 0.0
155 | all_agent_total_dist = 0.0
156 | num_of_success = 0
157 | all_desire_step_num = 0
158 | all_step_num = 0
159 |
160 | SuccessRate = 0.0
161 | ExtraTime = 0.0
162 | ExtraDistance = 0.0
163 | AverageSpeed = 0.0
164 | AverageCost = 0.0
165 | for agent in agents:
166 | agent_info_dict = {'id': agent.id, 'gp': agent.group, 'radius': agent.radius,
167 | 'goal_pos': agent.goal_global_frame.tolist()}
168 | info_dict_to_visualize['all_agent_info'].append(agent_info_dict)
169 | if not agent.in_collision and not agent.ran_out_of_time:
170 | num_of_success += 1
171 | all_agent_total_time += agent.total_time
172 | all_straight_dist += agent.straight_path_length
173 | all_agent_total_dist += agent.total_dist
174 | all_desire_step_num += agent.desire_steps
175 | all_step_num += agent.step_num
176 | info_dict_to_visualize['all_compute_time'] = all_agent_total_time
177 | info_dict_to_visualize['all_straight_distance'] = all_straight_dist
178 | info_dict_to_visualize['all_distance'] = all_agent_total_dist
179 | info_dict_to_visualize['successful_num'] = num_of_success
180 | info_dict_to_visualize['all_desire_step_num'] = all_desire_step_num
181 | info_dict_to_visualize['all_step_num'] = all_step_num
182 |
183 | info_dict_to_visualize['SuccessRate'] = num_of_success / agents_num
184 | info_dict_to_visualize['ExtraTime'] = ((all_step_num - all_desire_step_num) * Config.DT) / num_of_success
185 | info_dict_to_visualize['ExtraDistance'] = (all_agent_total_dist - all_straight_dist) / num_of_success
186 | info_dict_to_visualize['AverageSpeed'] = all_agent_total_dist / all_step_num / Config.DT
187 | info_dict_to_visualize['AverageCost'] = 1000 * all_agent_total_time / all_step_num
188 |
189 | for obstacle in env.obstacles:
190 | obstacle_info_dict = {'position': obstacle.pos, 'shape': obstacle.shape, 'feature': obstacle.feature}
191 | info_dict_to_visualize['all_obstacle'].append(obstacle_info_dict)
192 |
193 | info_str = json.dumps(info_dict_to_visualize, indent=4)
194 | with open(log_save_dir + '/env_cfg_'+str(agents_num)+'.json', 'w') as json_file:
195 | json_file.write(info_str)
196 | json_file.close()
197 |
--------------------------------------------------------------------------------
/mamp/sensors/OtherAgentsObsSensor.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | from mamp.sensors.Sensor import Sensor
3 | from mamp.envs import Config
4 | from mamp.util import compute_time_to_impact, vec2_l2_norm
5 | import operator
6 |
7 | class OtherAgentsObsSensor(Sensor):
8 | """ A dense matrix of relative states of other agents (e.g., their positions, vel, radii)
9 |
10 | :param max_num_other_agents_observed: (int) only can observe up to this many agents (the closest ones)
11 | :param agent_sorting_method: (str) definition of closeness in words (one of ['closest_last', 'closest_first', 'time_to_impact'])
12 |
13 | """
14 | def __init__(self, max_num_other_agents_observed=Config.MAX_NUM_OTHER_AGENTS_OBSERVED, agent_sorting_method=Config.AGENT_SORTING_METHOD):
15 | Sensor.__init__(self)
16 | self.name = 'other_agents_obs'
17 | self.max_num_other_agents_observed = max_num_other_agents_observed
18 | self.agent_sorting_method = agent_sorting_method
19 |
20 | def get_clipped_sorted_inds(self, sorting_criteria):
21 | """ Determine the closest N agents using the desired sorting criteria
22 |
23 | Args:
24 | sorting_criteria (str): how to sort the list of agents (one of ['closest_last', 'closest_first', 'time_to_impact']).
25 | See journal paper.
26 |
27 | Returns:
28 | clipped_sorted_inds (list): indices of the "closest" max_num_other_agents_observed
29 | agents sorted by "closeness" ("close" defined by sorting criteria),
30 |
31 | """
32 |
33 | # Grab first N agents (where N=Config.MAX_NUM_OTHER_AGENTS_OBSERVED)
34 | if self.agent_sorting_method in ['closest_last', 'closest_first']:
35 | # where "first" == closest
36 | sorted_sorting_criteria = sorted(sorting_criteria, key = lambda x: (x[1], x[2]))
37 | elif self.agent_sorting_method in ['time_to_impact']:
38 | # where "first" == lowest time-to-impact
39 | sorted_sorting_criteria = sorted(sorting_criteria, key = lambda x: (-x[3], -x[1], x[2]))
40 | clipped_sorting_criteria = sorted_sorting_criteria[:self.max_num_other_agents_observed]
41 |
42 | # Then sort those N agents by the preferred ordering scheme
43 | if self.agent_sorting_method == "closest_last":
44 | # sort by inverse distance away, then by lateral position
45 | sorted_dists = sorted(clipped_sorting_criteria, key = lambda x: (-x[1], x[2]))
46 | elif self.agent_sorting_method == "closest_first":
47 | # sort by distance away, then by lateral position
48 | sorted_dists = sorted(clipped_sorting_criteria, key = lambda x: (x[1], x[2]))
49 | elif self.agent_sorting_method == "time_to_impact":
50 | # sort by time_to_impact, break ties by distance away, then by lateral position (e.g. in case inf TTC)
51 | sorted_dists = sorted(clipped_sorting_criteria, key = lambda x: (-x[3], -x[1], x[2]))
52 | else:
53 | raise ValueError("Did not supply proper self.agent_sorting_method in Agent.py.")
54 |
55 | clipped_sorted_inds = [x[0] for x in sorted_dists]
56 | return clipped_sorted_inds
57 |
58 |
59 | def sense(self, agents, agent_index):
60 | """ Go through each agent in the environment, and compute its relative position, vel, etc. and put into an array
61 |
62 | This is a denser measurement of other agents' states vs. a LaserScan or OccupancyGrid
63 |
64 | Args:
65 | agents (list): all :class:`~gym_collision_avoidance.envs.agent.Agent` in the environment
66 | agent_index (int): index of this agent (the one with this sensor) in :code:`agents`
67 | top_down_map (2D np array): binary image with 0 if that pixel is free space, 1 if occupied (not used!)
68 |
69 | Returns:
70 | other_agents_states (np array): (max_num_other_agents_observed x 7) the 7 states about each other agent, :code:`[p_parallel_ego_frame, p_orthog_ego_frame, v_parallel_ego_frame, v_orthog_ego_frame, other_agent.radius, combined_radius, dist_2_other]`
71 |
72 | """
73 | host_agent = agents[agent_index]
74 | other_agent_dists = {}
75 | sorted_pairs = sorted(other_agent_dists.items(), key=operator.itemgetter(1))
76 |
77 | sorting_criteria = []
78 | for i, other_agent in enumerate(agents):
79 | if host_agent.pos_global_frame is not None and other_agent.pos_global_frame is not None:
80 | if other_agent.id == host_agent.id:
81 | continue
82 | # project other elements onto the new reference frame
83 | rel_pos_to_other_global_frame = other_agent.pos_global_frame - host_agent.pos_global_frame
84 | p_parallel_ego_frame = np.dot(rel_pos_to_other_global_frame, host_agent.ref_prll)
85 | p_orthog_ego_frame = np.dot(rel_pos_to_other_global_frame, host_agent.ref_orth)
86 | dist_between_agent_centers = vec2_l2_norm(rel_pos_to_other_global_frame)
87 | dist_2_other = dist_between_agent_centers - host_agent.radius - other_agent.radius
88 | combined_radius = host_agent.radius + other_agent.radius
89 |
90 | if dist_between_agent_centers > Config.SENSING_HORIZON:
91 | # print("Agent too far away")
92 | continue
93 |
94 | if self.agent_sorting_method != "time_to_impact":
95 | time_to_impact = None
96 | else:
97 | time_to_impact = compute_time_to_impact(host_agent.pos_global_frame,
98 | other_agent.pos_global_frame,
99 | host_agent.vel_global_frame,
100 | other_agent.vel_global_frame,
101 | combined_radius)
102 |
103 | sorting_criteria.append([i, round(dist_2_other,2), p_orthog_ego_frame, time_to_impact])
104 |
105 | clipped_sorted_inds = self.get_clipped_sorted_inds(sorting_criteria)
106 | clipped_sorted_agents = [agents[i] for i in clipped_sorted_inds]
107 |
108 | other_agents_states = np.zeros((Config.MAX_NUM_OTHER_AGENTS_OBSERVED, Config.OTHER_AGENT_OBSERVATION_LENGTH))
109 | other_agent_count = 0
110 | for other_agent in clipped_sorted_agents:
111 | if other_agent.id == host_agent.id:
112 | continue
113 |
114 | # project other elements onto the new reference frame
115 | rel_pos_to_other_global_frame = other_agent.pos_global_frame - host_agent.pos_global_frame
116 | dist_2_other = np.linalg.norm(rel_pos_to_other_global_frame) - host_agent.radius - other_agent.radius
117 |
118 | p_parallel_ego_frame = np.dot(rel_pos_to_other_global_frame, host_agent.ref_prll)
119 | p_orthog_ego_frame = np.dot(rel_pos_to_other_global_frame, host_agent.ref_orth)
120 | v_parallel_ego_frame = np.dot(other_agent.vel_global_frame, host_agent.ref_prll)
121 | v_orthog_ego_frame = np.dot(other_agent.vel_global_frame, host_agent.ref_orth)
122 | combined_radius = host_agent.radius + other_agent.radius
123 |
124 | other_obs = np.array([p_parallel_ego_frame,
125 | p_orthog_ego_frame,
126 | v_parallel_ego_frame,
127 | v_orthog_ego_frame,
128 | other_agent.radius,
129 | combined_radius,
130 | dist_2_other])
131 |
132 | if other_agent_count == 0:
133 | host_agent.other_agent_obs[:] = other_obs
134 |
135 | other_agents_states[other_agent_count,:] = other_obs
136 | other_agent_count += 1
137 |
138 | host_agent.num_other_agents_observed = other_agent_count
139 | return other_agents_states
140 |
141 |
--------------------------------------------------------------------------------
/draw/plt2d.py:
--------------------------------------------------------------------------------
1 | import time
2 | import numpy as np
3 | import pandas as pd
4 | import matplotlib.cm as cmx
5 | import matplotlib.colors as colors
6 | import matplotlib.pyplot as plt
7 | from matplotlib.path import Path
8 | import matplotlib.patches as patches
9 | from math import sin, cos, atan2, sqrt, pow
10 |
11 | from vis_util import get_2d_car_model, get_2d_uav_model
12 | from vis_util import rgba2rgb
13 |
14 | simple_plot = True
15 |
16 |
17 | # img = plt.imread('beijing.jpg', 100)
18 |
19 |
20 | def convert_to_actual_model_2d(agent_model, pos_global_frame, heading_global_frame):
21 | alpha = heading_global_frame
22 | for point in agent_model:
23 | x = point[0]
24 | y = point[1]
25 | # 进行航向计算
26 | ll = sqrt(pow(x, 2) + pow(y, 2))
27 | alpha_model = atan2(y, x)
28 | alpha_ = alpha + alpha_model - np.pi / 2 # 改加 - np.pi / 2 因为画模型的时候UAV朝向就是正北方向，所以要减去90°
29 | point[0] = ll * cos(alpha_) + pos_global_frame[0]
30 | point[1] = ll * sin(alpha_) + pos_global_frame[1]
31 |
32 |
33 | def draw_agent_2d(ax, pos_global_frame, heading_global_frame, my_agent_model, color='blue'):
34 | agent_model = my_agent_model
35 | convert_to_actual_model_2d(agent_model, pos_global_frame, heading_global_frame)
36 |
37 | codes = [Path.MOVETO,
38 | Path.LINETO,
39 | Path.LINETO,
40 | Path.LINETO,
41 | Path.CLOSEPOLY,
42 | ]
43 |
44 | path = Path(agent_model, codes)
45 | # 第二步：创建一个patch，路径依然也是通过patch实现的，只不过叫做pathpatch
46 | col = [0.8, 0.8, 0.8]
47 | patch = patches.PathPatch(path, fc=col, ec=col, lw=1.5)
48 |
49 | ax.add_patch(patch)
50 |
51 |
52 | def draw_traj_2d(ax, obstacles_info, agents_info, agents_traj_list, step_num_list, current_step):
53 | cmap = get_cmap(64)
54 | plt_colors = get_colors()
55 | for idx, agent_traj in enumerate(agents_traj_list):
56 | # ax.imshow(img, extent=[-1, 4, -1, 4]) # 添加仿真背景
57 | agent_id = agents_info[idx]['id']
58 | agent_gp = agents_info[idx]['gp']
59 | agent_rd = agents_info[idx]['radius']
60 | agent_goal = agents_info[idx]['goal_pos']
61 | info = agents_info[idx]
62 | group = info['gp']
63 | radius = info['radius'] / 1
64 | color_ind = idx % len(plt_colors)
65 | # p_color = p_colors[color_ind]
66 | plt_color = plt_colors[color_ind]
67 |
68 | ag_step_num = step_num_list[idx]
69 | if current_step > ag_step_num - 1:
70 | plot_step = ag_step_num - 1
71 | else:
72 | plot_step = current_step
73 |
74 | pos_x = agent_traj['pos_x']
75 | pos_y = agent_traj['pos_y']
76 | alpha = agent_traj['alpha']
77 |
78 | # # 绘制目标点
79 | plt.plot(agent_goal[0], agent_goal[1], color=[0.933, 0.933, 0.0], marker='*', markersize=4)
80 |
81 | # 绘制探测区域
82 | # rect = patches.Rectangle((agent_goal[0] - 0.5, agent_goal[1] - 0.5), 1.0, 1.0, linewidth=1,
83 | # edgecolor=plt_color, facecolor='none', alpha=0.5)
84 | # ax.add_patch(rect)
85 | # plt.text(agent_goal[0]-0.85, agent_goal[1]-0.65, 'UGV ' + str(agent_id+1) +
86 | # ' Exploration Region', fontsize=12, color=p_color)
87 |
88 | # 绘制实线
89 | # plt.plot(pos_x[:plot_step], pos_y[:plot_step], color=plt_color)
90 |
91 | # 绘制箭头
92 | plt.arrow(pos_x[plot_step], pos_y[plot_step], 0.6 * cos(alpha[plot_step]), 0.6 * sin(alpha[plot_step]),
93 | fc=plt_color, ec=plt_color, head_width=0.3, head_length=0.4)
94 |
95 | # 绘制渐变线
96 | # pcolors = np.zeros((plot_step, 4))
97 | # pcolors[:, :3] = plt_color[:3]
98 | # pcolors[:, 3] = np.linspace(0.2, 1., plot_step)
99 | # pcolors = rgba2rgb(pcolors)
100 | endure_time = 320
101 | # if plot_step <= endure_time:
102 | ax.scatter(pos_x[:plot_step], pos_y[:plot_step], marker='.', color=plt_color, s=0.2, alpha=0.5)
103 | # else:
104 | # nt = plot_step - endure_time
105 | # ax.scatter(pos_x[nt:plot_step], pos_y[nt:plot_step], color=pcolors[nt:plot_step], s=0.1, alpha=0.5)
106 |
107 | if simple_plot:
108 | ax.add_patch(plt.Circle((pos_x[plot_step], pos_y[plot_step]), radius=agent_rd, fc=plt_color, ec=plt_color))
109 | text_offset = agent_rd
110 | ax.text(pos_x[plot_step] + text_offset, pos_y[plot_step] + text_offset, str(agent_id + 1), color=plt_color)
111 | else:
112 | if group == 0:
113 | my_model = get_2d_car_model(size=agent_rd)
114 | else:
115 | my_model = get_2d_uav_model(size=agent_rd)
116 | pos = [pos_x[plot_step], pos_y[plot_step]]
117 | heading = alpha[plot_step]
118 | draw_agent_2d(ax, pos, heading, my_model)
119 | for i in range(len(obstacles_info)):
120 | pos = obstacles_info[i]['position']
121 | heading = 0.0
122 | rd = obstacles_info[i]['feature']
123 | agent_rd = rd[0] / 2
124 | my_model = get_2d_car_model(size=agent_rd)
125 | draw_agent_2d(ax, pos, heading, my_model)
126 |
127 |
128 | def plot_save_one_pic(obstacles_info, agents_info, agents_traj_list, step_num_list, filename, current_step):
129 | fig = plt.figure(0)
130 | fig_size = (10, 8)
131 | fig.set_size_inches(fig_size[0], fig_size[1])
132 | ax = fig.add_subplot(1, 1, 1)
133 | ax.set(xlabel='X',
134 | ylabel='Y',
135 | )
136 | ax.axis('equal')
137 | # plt.grid(alpha=0.2)
138 | draw_traj_2d(ax, obstacles_info, agents_info, agents_traj_list, step_num_list, current_step)
139 | fig.savefig(filename, bbox_inches="tight")
140 | if current_step == 0: plt.show()
141 | # fig.savefig(filename)
142 | plt.close()
143 |
144 |
145 | def plot_episode(obstacles_info, agents_info, traj_list, step_num_list, plot_save_dir, base_fig_name, last_fig_name,
146 | show=False):
147 | current_step = 0
148 | num_agents = len(step_num_list)
149 | total_step = max(step_num_list)
150 | print('num_agents:', num_agents, 'total_step:', total_step)
151 | while current_step < total_step:
152 | fig_name = base_fig_name + "_{:05}".format(current_step) + '.png'
153 | filename = plot_save_dir + fig_name
154 | plot_save_one_pic(obstacles_info, agents_info, traj_list, step_num_list, filename, current_step)
155 | print(filename)
156 | current_step += 3
157 | filename = plot_save_dir + last_fig_name
158 | plot_save_one_pic(obstacles_info, agents_info, traj_list, step_num_list, filename, total_step)
159 |
160 |
161 | def get_cmap(N):
162 | """Returns a function that maps each index in 0, 1, ... N-1 to a distinct RGB color."""
163 | color_norm = colors.Normalize(vmin=0, vmax=N - 1)
164 | scalar_map = cmx.ScalarMappable(norm=color_norm, cmap='hsv')
165 |
166 | def map_index_to_rgb_color(index):
167 | return scalar_map.to_rgba(index)
168 |
169 | return map_index_to_rgb_color
170 |
171 |
172 | def get_colors():
173 | py_colors = np.array(
174 | [
175 | [255, 0, 0], [255, 69, 0], [255, 165, 0], [0, 255, 0], [152, 251, 152], [0, 0, 255], [160, 32, 240],
176 | [255, 99, 71], [132, 112, 255], [0, 255, 255], [255, 69, 0], [148, 0, 211], [255, 192, 203],
177 | [255, 127, 0], [0, 191, 255], [255, 0, 255],
178 | ]
179 | )
180 | # py_colors = np.array(
181 | # [
182 | # [255, 99, 71], [255, 69, 0], [255, 0, 0], [255, 105, 180], [255, 192, 203], [238, 130, 238],
183 | # [78, 238, 148], [67, 205, 128], [46, 139, 87], [154, 255, 154], [144, 238, 144], [0, 255, 127],
184 | # [0, 238, 118], [0, 205, 102], [0, 139, 69], [0, 255, 0], [0, 139, 0], [127, 255, 0], [102, 205, 0],
185 | # [192, 255, 62], [202, 255, 112], [255, 255, 0], [255, 215, 0], [255, 193, 37], [255, 193, 193],
186 | # [250, 128, 114], [255, 160, 122], [255, 165, 0], [255, 140, 0], [255, 127, 80], [240, 128, 128],
187 | # [221, 160, 221], [218, 112, 214], [186, 85, 211], [148, 0, 211], [138, 43, 226], [160, 32, 240],
188 | # [106, 90, 205], [123, 104, 238], [0, 0, 205], [0, 0, 255], [30, 144, 255], [0, 191, 255], [152, 251, 152],
189 | # [0, 255, 127], [124, 252, 0], [255, 250, 240], [253, 245, 230], [250, 235, 215],
190 | # [255, 235, 205], [255, 218, 185], [255, 228, 181], [255, 248, 220], [255, 255, 240], [240, 255, 240],
191 | # [230, 230, 250], [0, 245, 255], [187, 255, 255], [0, 255, 255], [127, 255, 212], [118, 238, 198],
192 | # [102, 205, 170], [193, 255, 193], [84, 255, 159],
193 | # ]
194 | # )
195 | return py_colors / 255
196 |
--------------------------------------------------------------------------------
/runpy/run_pca.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python3
2 | import os
3 | import sys
4 | import json
5 | import random
6 | import numpy as np
7 | import pandas as pd
8 |
9 | sys.path.append('/home/wuuya/PycharmProjects/maros')
10 | sys.path.append('/opt/ros/kinetic/lib/python2.7/dist-packages')
11 | import os
12 |
13 | os.environ['GYM_CONFIG_CLASS'] = 'Example'
14 | os.environ['GYM_CONFIG_PATH'] = '../mamp/configs/config.py'
15 | import gym
16 |
17 | gym.logger.set_level(40)
18 |
19 | from mamp.agents.agent import Agent
20 | from mamp.agents.obstacle import Obstacle
21 | # Policies
22 | from mamp.policies import policy_dict
23 | # Dynamics
24 | from mamp.dynamics.UnicycleDynamics import UnicycleDynamics
25 | # Sensors
26 | from mamp.sensors import Sensor
27 |
28 | from mamp.util import mod2pi
29 | from mamp.envs import Config
30 |
31 |
32 | def simulation1_in_circle(num_agents, rad=10.0):
33 | center = [0.0, 0.0]
34 | test_qi, test_qf = [], []
35 | k = 0 # rotation parameter
36 | for n in range(num_agents):
37 | test_qi.append([center[0] + round(rad * np.cos(2 * n * np.pi / num_agents + k * np.pi / 4), 2),
38 | center[1] + round(rad * np.sin(2 * n * np.pi / num_agents + k * np.pi / 4), 2),
39 | mod2pi(2 * n * np.pi / num_agents + k * np.pi / 4 + np.pi)])
40 | for n in range(num_agents):
41 | test_qf.append([center[0] + round(rad * np.cos(2 * n * np.pi / num_agents + np.pi + k * np.pi / 4), 2),
42 | center[1] + round(rad * np.sin(2 * n * np.pi / num_agents + np.pi + k * np.pi / 4), 2),
43 | mod2pi(2 * n * np.pi / num_agents + k * np.pi / 4)])
44 | return test_qi, test_qf
45 |
46 |
47 | def real_world_experiment_in_circle(num_agents):
48 | center = [1.5, 1.5]
49 | rad = 1.5
50 | test_qi, test_qf = [], []
51 | k = 0 # np.random.uniform(0, 2)
52 | for n in range(num_agents):
53 | test_qi.append([center[0] + round(rad * np.cos(2 * n * np.pi / num_agents + k * np.pi / 4), 2),
54 | center[1] + round(rad * np.sin(2 * n * np.pi / num_agents + k * np.pi / 4), 2),
55 | mod2pi(2 * n * np.pi / num_agents + k * np.pi / 4 + np.pi)])
56 | for n in range(num_agents):
57 | test_qf.append([center[0] + round(rad * np.cos(2 * n * np.pi / num_agents + np.pi + k * np.pi / 4), 2),
58 | center[1] + round(rad * np.sin(2 * n * np.pi / num_agents + np.pi + k * np.pi / 4), 2),
59 | mod2pi(2 * n * np.pi / num_agents + k * np.pi / 4)])
60 | return test_qi, test_qf
61 |
62 |
63 | def set_random_pos(agent_num, r=40):
64 | """
65 | The square is 80 * 80
66 | The maximum number of agents is 81*81.
67 | """
68 | r = int(r)
69 | poses = []
70 | for i in range(-r, r+1):
71 | for j in range(-r, r+1):
72 | pos = np.array([i, j, random.uniform(0.0, 2*np.pi)])
73 | poses.append(pos)
74 | agent_pos = random.sample(poses, agent_num)
75 | agent_goal = random.sample(poses, agent_num)
76 |
77 | return agent_pos, agent_goal
78 |
79 |
80 | def build_agents():
81 | """
82 | simulation 1: agents' num is 10-150, circle's radius is 40, random's radius is 40
83 | large simulation: agents' num is 200, circle's radius is 50, random's radius is 50
84 | real-world: agents' num is 4, circle's is radius 1.5
85 | """
86 | num_agents = 100
87 | test_qi, test_qf = simulation1_in_circle(num_agents, rad=40.0)
88 | # test_qi, test_qf = set_random_pos(num_agents, r=50)
89 | # test_qi, test_qf = exp_realworld_in_circle()
90 | radius = 0.2
91 | pref_speed = 0.22 # turtleBot3's max speed is 0.22 or 0.26 m/s, and 2.84 or 1.82 rad/s
92 | agents = []
93 | for j in range(len(test_qi)):
94 | agents.append(Agent(start_pos=test_qi[j][:2],
95 | goal_pos=test_qf[j][:2],
96 | name='Agent' + str(j+1),
97 | radius=radius, pref_speed=pref_speed,
98 | initial_heading=test_qi[j][2],
99 | goal_heading=test_qf[j][2],
100 | policy=policy_dict['pca'],
101 | dynamics_model=UnicycleDynamics,
102 | sensors=[Sensor],
103 | id=j))
104 | return agents
105 |
106 |
107 | def build_obstacles():
108 | """
109 | simulation1: r = 16.0, agents' num is 100, radius of circle is 40
110 | simulation2: r = 16.0, agents' num is 100, radius of circle is 40
111 | exp_real-world: obstacles is None
112 | """
113 | obstacles = []
114 | r = 16.0
115 | obstacles.append(Obstacle(pos=[0.0, 0.0], shape_dict={'shape': "rect", 'feature': (r, r)}, id=0))
116 | return obstacles
117 |
118 |
119 | if __name__ == '__main__':
120 | # Set agent configuration (start/goal pos, radius, size, policy)
121 | agents = build_agents()
122 | obstacles = build_obstacles()
123 | [agent.policy.initialize_network() for agent in agents if hasattr(agent.policy, 'initialize_network')]
124 |
125 | agents_num = len(agents)
126 |
127 | # env = gym.make("MAGazebo-v0")
128 | env = gym.make("MultiAgentCollisionAvoidance-v0")
129 | env.set_agents(agents, obstacles=obstacles)
130 |
131 | # is_back2start is True in experiment1 and is False in experiment2
132 | for agent in agents:
133 | agent.is_back2start = False
134 |
135 | epi_maximum = 1
136 | for epi in range(epi_maximum):
137 | env.reset()
138 | # print("episode:", epi)
139 | game_over = False
140 | while not game_over:
141 | print("step is ", env.episode_step_number)
142 | actions = {}
143 | obs, rewards, game_over, which_agents_done = env.step(actions)
144 | print("All agents finished!", env.episode_step_number)
145 | print("Experiment over.")
146 |
147 | log_save_dir = os.path.dirname(os.path.realpath(__file__)) + '/../draw/pca/log/'
148 | os.makedirs(log_save_dir, exist_ok=True)
149 |
150 | # trajectory
151 | writer = pd.ExcelWriter(log_save_dir + '/trajs_'+str(agents_num)+'.xlsx')
152 | for agent in agents:
153 | agent.history_info.to_excel(writer, sheet_name='agent' + str(agent.id))
154 | writer.save()
155 |
156 | # scenario information
157 | info_dict_to_visualize = {
158 | 'all_agent_info': [],
159 | 'all_obstacle': [],
160 | 'all_compute_time': 0.0,
161 | 'all_straight_distance': 0.0,
162 | 'all_distance': 0.0,
163 | 'successful_num': 0,
164 | 'all_desire_step_num': 0,
165 | 'all_step_num': 0,
166 | 'SuccessRate': 0.0,
167 | 'ExtraTime': 0.0,
168 | 'ExtraDistance': 0.0,
169 | 'AverageSpeed': 0.0,
170 | 'AverageCost': 0.0
171 | }
172 | all_straight_dist = 0.0
173 | all_agent_total_time = 0.0
174 | all_agent_total_dist = 0.0
175 | num_of_success = 0
176 | all_desire_step_num = 0
177 | all_step_num = 0
178 |
179 | SuccessRate = 0.0
180 | ExtraTime = 0.0
181 | ExtraDistance = 0.0
182 | AverageSpeed = 0.0
183 | AverageCost = 0.0
184 | for agent in agents:
185 | agent_info_dict = {'id': agent.id, 'gp': agent.group, 'radius': agent.radius,
186 | 'goal_pos': agent.goal_global_frame.tolist()}
187 | info_dict_to_visualize['all_agent_info'].append(agent_info_dict)
188 | if not agent.in_collision and not agent.ran_out_of_time:
189 | num_of_success += 1
190 | all_agent_total_time += agent.total_time
191 | all_straight_dist += agent.straight_path_length
192 | all_agent_total_dist += agent.total_dist
193 | all_desire_step_num += agent.desire_steps
194 | all_step_num += agent.step_num
195 | info_dict_to_visualize['all_compute_time'] = all_agent_total_time
196 | info_dict_to_visualize['all_straight_distance'] = all_straight_dist
197 | info_dict_to_visualize['all_distance'] = all_agent_total_dist
198 | info_dict_to_visualize['successful_num'] = num_of_success
199 | info_dict_to_visualize['all_desire_step_num'] = all_desire_step_num
200 | info_dict_to_visualize['all_step_num'] = all_step_num
201 |
202 | info_dict_to_visualize['SuccessRate'] = num_of_success / agents_num
203 | info_dict_to_visualize['ExtraTime'] = ((all_step_num - all_desire_step_num) * Config.DT) / num_of_success
204 | info_dict_to_visualize['ExtraDistance'] = (all_agent_total_dist - all_straight_dist) / num_of_success
205 | info_dict_to_visualize['AverageSpeed'] = all_agent_total_dist / all_step_num / Config.DT
206 | info_dict_to_visualize['AverageCost'] = 1000 * all_agent_total_time / all_step_num
207 |
208 | for obstacle in env.obstacles:
209 | obstacle_info_dict = {'position': obstacle.pos, 'shape': obstacle.shape, 'feature': obstacle.feature}
210 | info_dict_to_visualize['all_obstacle'].append(obstacle_info_dict)
211 |
212 | info_str = json.dumps(info_dict_to_visualize, indent=4)
213 | with open(log_save_dir + '/env_cfg_'+str(agents_num)+'.json', 'w') as json_file:
214 | json_file.write(info_str)
215 | json_file.close()
216 |
--------------------------------------------------------------------------------
/mamp/envs/carenv.py:
--------------------------------------------------------------------------------
1 | #! /usr/bin/env python3
2 | import gym
3 | import rospy
4 | import numpy as np
5 | import time
6 | from mamp.envs.rosport.car import RosCarPort
7 | from gazebo_msgs.msg import ModelState
8 | from gazebo_msgs.srv import SetModelState
9 | from std_srvs.srv import Empty
10 | from math import sqrt, sin, cos, atan2
11 | from copy import deepcopy
12 |
13 | ERROR_MAX = 1
14 |
15 |
16 | class CAREnv(gym.Env):
17 | """
18 | line follower robot environment
19 | 多机器人编队环境:
20 | 1位领导者:通过强化学习算法来学习PID系数完成循迹任务
21 | n位跟随者:跟随领导者前进,并且保持编队秩序
22 | """
23 |
24 | def __init__(self):
25 | super().__init__()
26 | # information for rl
27 | self.reward = 0
28 | self.state = np.zeros(13)
29 | self.action_space = np.zeros(6)
30 | self.state_space = np.zeros(13)
31 | # information recorded
32 | self.success_record = []
33 | self.game_over = False
34 | self.node = rospy.init_node('line_following_node', anonymous=False)
35 | self.reset_world = rospy.ServiceProxy('/gazebo/reset_world', Empty)
36 | self.set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
37 | self.rate = rospy.Rate(10)
38 | self.count = 0
39 | # agent
40 | self.agents = None
41 |
42 | def reset(self):
43 | # print('mean_v:', self.agents[0].distance/(time.time()-self.time))
44 | self.reset_world()
45 | for ag in self.agents:
46 | ag.stop()
47 | time.sleep(1)
48 | self.game_over = False
49 | # if self.agents[0].failed_flag:
50 | # self.success_record.append(0)
51 | # print("fail!")
52 | if self.agents[0].success_flag:
53 | self.success_record.append(1)
54 | print("success!")
55 | else:
56 | self.success_record.append(0)
57 | self.time = time.time()
58 | for ag in self.agents:
59 | ag.reset()
60 | self.agents[0].set_pos([0, 0])
61 | self.agents[1].set_pos([-1, 1])
62 | self.agents[2].set_pos([-1, -1])
63 | self.agents[3].set_pos([-2, 0])
64 | obs = self._get_obs()
65 | return obs
66 |
67 | def set_start(self, model_name, pos, heading):
68 | """
69 | 设置gazebo中机器人的位置和姿态
70 | """
71 | x, y = pos[0], pos[1]
72 | state = ModelState()
73 | state.model_name = model_name
74 | state.reference_frame = 'world' # ''ground_plane'
75 | # pose
76 | state.pose.position.x = x
77 | state.pose.position.y = y
78 | state.pose.position.z = 0
79 | # quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
80 | state.pose.orientation.x = 0
81 | state.pose.orientation.y = 0
82 | state.pose.orientation.z = np.sin(heading / 2)
83 | state.pose.orientation.w = np.cos(heading / 2)
84 | # twist
85 | state.twist.linear.x = 0
86 | state.twist.linear.y = 0
87 | state.twist.linear.z = 0
88 | state.twist.angular.x = 0
89 | state.twist.angular.y = 0
90 | state.twist.angular.z = 0
91 |
92 | rospy.wait_for_service('/gazebo/set_model_state')
93 | try:
94 | set_state = self.set_state
95 | result = set_state(state)
96 | assert result.success is True
97 | except rospy.ServiceException:
98 | print("/gazebo/get_model_state service call failed")
99 |
100 | def set_agents(self, agents, neighbor_info):
101 | '''
102 | neighbor_info = [{1: [-1, 0], 3: [0, 1]},
103 | {0: [1, 0], 2: [0, 1]},
104 | {1: [0, -1], 3: [1, 0]},
105 | {0: [0, -1], 2: [-1, 0]},]
106 | '''
107 | self.agents = agents
108 | self.neighbor_info = deepcopy(neighbor_info) # complete formation matrix
109 | # 设置临接矩阵
110 | num_agent = len(agents)
111 | adj_matrix = np.zeros((num_agent, num_agent))
112 | for i, info in enumerate(neighbor_info):
113 | neighbor = info.keys()
114 | for j in neighbor:
115 | adj_matrix[i, j] = 1
116 | self.agents[i].neighbor.append(j)
117 | print(self.agents[i].name, self.agents[i].neighbor)
118 | self.adjMatrix = adj_matrix
119 | # 绑定智能体
120 | for ag in self.agents:
121 | ag.neighbor_info = neighbor_info[ag.id]
122 | ag.reset()
123 | ag.rosport = RosCarPort(ag)
124 |
125 | def _check_if_done(self):
126 | """
127 | 检查是否回合结束
128 | """
129 | dones = []
130 | # check which agent is already done
131 | for ag in self.agents:
132 | if ag.done == True:
133 | ag.quit = True
134 | if abs(self.agents[0].pose_global_frame[0]) < 0.5 and \
135 | abs(self.agents[0].pose_global_frame[1]) < 0.5 and (time.time() - self.time) > 20:
136 | for ag in self.agents:
137 | ag.success_flag = True
138 |
139 | for ag in self.agents:
140 | if ag.group == 1:
141 | if abs(ag.error[0][0]) > ERROR_MAX or abs(ag.error[0][1]) > ERROR_MAX:
142 | ag.failed_flag = True
143 | if ag.success_flag == True or ag.failed_flag == True:
144 | ag.done = True
145 | dones.append(ag.done)
146 | if self.agents[0].done == True:
147 | self.game_over = True
148 | if any([ag.done for ag in self.agents if ag.group == 1]):
149 | self.game_over = True
150 | return dones
151 |
152 | def _compute_rewards(self):
153 | rewards = []
154 | for ag in self.agents:
155 | if ag.quit:
156 | rewards.append(0)
157 | else:
158 | var_e = np.var(ag.error)
159 | dist = ag.distance - ag.last_distance
160 | reward = 2 * dist - var_e
161 | if ag.group == 0:
162 | reward = 2 * dist - 10 * var_e
163 | reward -= 0.1
164 | if ag.done:
165 | if ag.success_flag:
166 | reward = 5
167 | else:
168 | reward = -5
169 | rewards.append(reward)
170 | ag.last_distance = ag.distance
171 | return rewards
172 |
173 | def _get_obs(self):
174 | """
175 | 获取机器人完整的状态观测值
176 | """
177 | ob = []
178 | self.agents[0].state = np.append(self.agents[0].cx, self.agents[0].cy)
179 | state2 = [self.agents[0].v_x, self.agents[0].w_z, self.agents[0].error_k[0],
180 | self.agents[0].distance / 45.0]
181 | self.agents[0].state = np.append(self.agents[0].state, state2)
182 | ob.append(self.agents[0].state)
183 | for ag in self.agents:
184 | if ag.group == 1:
185 | ag.state = np.array([ag.error[0][0], ag.error[0][1], ag.v_x, ag.w_z,
186 | self.agents[ag.neighbor[0]].v_x, self.agents[ag.neighbor[0]].w_z,
187 | self.agents[ag.neighbor[1]].v_x, self.agents[ag.neighbor[1]].w_z,
188 | ag.distance / 45.0]) # 或许可以多加几个偏差
189 |
190 | ob.append(ag.state)
191 | return ob
192 |
193 | def _get_dict_comm(self):
194 | dict_comm = []
195 | for ag in self.agents:
196 | info = {'pose_global_frame': ag.pose_global_frame,
197 | 'vel_global_frame': ag.vel_global_frame}
198 | dict_comm.append(info)
199 | return dict_comm
200 |
201 | def step(self, actions, dt=None):
202 | """
203 | 执行单次动作（ros控制周期循环3次）
204 | 返回新的状态，奖励，回合结束标志位
205 | """
206 | time_step = 0
207 | all_command = []
208 | while time_step < 1:
209 | dones = self._check_if_done()
210 | dict_comm = self._get_dict_comm()
211 | if self.game_over:
212 | break
213 | for idx, ag in enumerate(self.agents):
214 | if ag.quit:
215 | all_command.append([0, 0])
216 | else:
217 | if ag.group == 1:
218 | self._dynamic_update()
219 | command = ag.generate_next_command(actions[idx], dict_comm)
220 | all_command.append(command)
221 | self._control(all_command)
222 | time_step += 1
223 | obs = self._get_obs()
224 | rewards = self._compute_rewards()
225 | info = {}
226 | return obs, rewards, dones, info
227 |
228 | def _control(self, commands):
229 | """
230 | 发送ROS控制指令
231 | """
232 | for idx, ag in enumerate(self.agents):
233 | ag.rosport.pubTwist(commands[idx], dt=1 / 10)
234 | self.rate.sleep()
235 |
236 | def stop(self):
237 | """
238 | 停止机器人运行
239 | """
240 | for ag in self.agents:
241 | ag.stop()
242 |
243 | def _dynamic_update(self):
244 | """
245 | 编队机器人动态矩阵信息的更新
246 | """
247 | # update formation matrix
248 | for ag in self.agents:
249 | if ag.group == 1:
250 | for key, value in ag.neighbor_info.items():
251 | yaw = self.agents[key].heading_global_frame
252 | Rz = np.array([[cos(yaw), -sin(yaw)], [sin(yaw), cos(yaw)]], dtype=float)
253 | value = Rz @ np.array(self.neighbor_info[ag.id][key])
254 | ag.neighbor_info[key] = value
255 |
--------------------------------------------------------------------------------
/mamp/configs/config.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | '''
3 | formation movement: from pos to goal_pos with other agent observation
4 | '''
5 | class Config(object):
6 | def __init__(self):
7 | #########################################################################
8 | # GENERAL PARAMETERS
9 | self.COLLISION_AVOIDANCE = True
10 | self.continuous, self.discrete = range(2) # Initialize game types as enum
11 | self.ACTION_SPACE_TYPE = self.continuous
12 |
13 | ### DISPLAY
14 | self.ANIMATE_EPISODES = False
15 | self.SHOW_EPISODE_PLOTS = False
16 | self.SAVE_EPISODE_PLOTS = False
17 | if not hasattr(self, "PLOT_CIRCLES_ALONG_TRAJ"):
18 | self.PLOT_CIRCLES_ALONG_TRAJ = True
19 | self.ANIMATION_PERIOD_STEPS = 5 # plot every n-th DT step (if animate mode on)
20 | self.PLT_LIMITS = None
21 | self.PLT_FIG_SIZE = (10, 8)
22 |
23 | if not hasattr(self, "USE_STATIC_MAP"):
24 | self.USE_STATIC_MAP = False
25 |
26 | ### TRAIN / PLAY / EVALUATE
27 | self.TRAIN_MODE = True # Enable to see the trained agent in action (for testing)
28 | self.PLAY_MODE = False # Enable to see the trained agent in action (for testing)
29 | self.EVALUATE_MODE = False # Enable to see the trained agent in action (for testing)
30 |
31 | ### REWARDS
32 | self.REWARD_MODE = "no formation" # formation
33 | self.REWARD_AT_GOAL = 1.0 # reward given when agent reaches goal position
34 | self.REWARD_AT_FORMAT = 0.1 #
35 | self.REWARD_TO_GOAL_RATE = 0.0 #
36 | self.REWARD_COLLISION_WITH_AGENT = -0.25 # reward given when agent collides with another agent
37 | self.REWARD_COLLISION_WITH_WALL = -0.25 # reward given when agent collides with wall
38 | self.REWARD_GETTING_CLOSE = -0.1 # reward when agent gets close to another agent (unused?)
39 | self.REWARD_ENTERED_NORM_ZONE = -0.05 # reward when agent enters another agent's social zone
40 | self.REWARD_TIME_STEP = 0.0 # default reward given if none of the others apply (encourage speed)
41 | self.REWARD_WIGGLY_BEHAVIOR = 0.0
42 | self.WIGGLY_BEHAVIOR_THRESHOLD = np.inf
43 | self.COLLISION_DIST = 0.0 # meters between agents' boundaries for collision
44 | self.GETTING_CLOSE_RANGE = 0.2 # meters between agents' boundaries for collision
45 | # self.SOCIAL_NORMS = "right"
46 | # self.SOCIAL_NORMS = "left"
47 | self.SOCIAL_NORMS = "none"
48 |
49 | ### SIMULATION
50 | self.DT = 0.2 # seconds between simulation time steps
51 | self.NEAR_GOAL_THRESHOLD = 0.2
52 | self.NEAR_FORMAT_THRESHOLD = 0.2
53 | self.MAX_TIME_RATIO = 10. # agent has this number times the straight-line-time to reach its goal before "timing out"
54 |
55 | ### PARAMETERS THAT OVERWRITE/IMPACT THE ENV'S PARAMETERS
56 | if not hasattr(self, "MAX_NUM_OBS_IN_ENVIRONMENT"):
57 | self.MAX_NUM_OBS_IN_ENVIRONMENT = 0
58 | if not hasattr(self, "MAX_NUM_AGENTS_IN_ENVIRONMENT"):
59 | self.MAX_NUM_AGENTS_IN_ENVIRONMENT = 4
60 | if not hasattr(self, "MAX_NUM_AGENTS_TO_SIM"):
61 | self.MAX_NUM_AGENTS_TO_SIM = 4
62 | self.MAX_NUM_OTHER_AGENTS_IN_ENVIRONMENT = self.MAX_NUM_AGENTS_IN_ENVIRONMENT - 1
63 | if not hasattr(self, "MAX_NUM_OTHER_AGENTS_OBSERVED"):
64 | self.MAX_NUM_OTHER_AGENTS_OBSERVED = self.MAX_NUM_AGENTS_IN_ENVIRONMENT - 1
65 |
66 | ### EXPERIMENTS
67 | self.PLOT_EVERY_N_EPISODES = 100 # for tensorboard visualization
68 |
69 | ### SENSORS
70 | self.SENSING_HORIZON = np.inf
71 | # self.SENSING_HORIZON = 3.0
72 | self.LASERSCAN_LENGTH = 512 # num range readings in one scan
73 | self.LASERSCAN_NUM_PAST = 3 # num range readings in one scan
74 | self.NUM_STEPS_IN_OBS_HISTORY = 1 # number of time steps to store in observation vector
75 | self.NUM_PAST_ACTIONS_IN_STATE = 0
76 | self.WITH_COMM = False
77 |
78 | ### RVO AGENTS
79 | self.RVO_TIME_HORIZON = 5.0
80 | self.RVO_COLLAB_COEFF = 0.5
81 | self.RVO_ANTI_COLLAB_T = 1.0
82 |
83 | ### OBSERVATION VECTOR
84 | self.TRAIN_SINGLE_AGENT = False
85 | self.STATE_INFO_DICT = {
86 | 'dist_to_goal': {
87 | 'dtype': np.float32,
88 | 'size': 1,
89 | 'bounds': [-np.inf, np.inf],
90 | 'attr': 'get_agent_data("dist_to_goal")',
91 | 'std': np.array([5.], dtype=np.float32),
92 | 'mean': np.array([0.], dtype=np.float32)
93 | },
94 | 'radius': {
95 | 'dtype': np.float32,
96 | 'size': 1,
97 | 'bounds': [0, np.inf],
98 | 'attr': 'get_agent_data("radius")',
99 | 'std': np.array([1.0], dtype=np.float32),
100 | 'mean': np.array([0.5], dtype=np.float32)
101 | },
102 | 'heading_ego_frame': {
103 | 'dtype': np.float32,
104 | 'size': 1,
105 | 'bounds': [-np.pi, np.pi],
106 | 'attr': 'get_agent_data("heading_ego_frame")',
107 | 'std': np.array([3.14], dtype=np.float32),
108 | 'mean': np.array([0.], dtype=np.float32)
109 | },
110 | 'pref_speed': {
111 | 'dtype': np.float32,
112 | 'size': 1,
113 | 'bounds': [0, np.inf],
114 | 'attr': 'get_agent_data("pref_speed")',
115 | 'std': np.array([1.0], dtype=np.float32),
116 | 'mean': np.array([1.0], dtype=np.float32)
117 | },
118 | 'num_other_agents': {
119 | 'dtype': np.float32,
120 | 'size': 1,
121 | 'bounds': [0, np.inf],
122 | 'attr': 'get_agent_data("num_other_agents_observed")',
123 | 'std': np.array([1.0], dtype=np.float32),
124 | 'mean': np.array([1.0], dtype=np.float32)
125 | },
126 | 'other_agent_states': {
127 | 'dtype': np.float32,
128 | 'size': 7,
129 | 'bounds': [-np.inf, np.inf],
130 | 'attr': 'get_agent_data("other_agent_states")',
131 | 'std': np.array([5.0, 5.0, 1.0, 1.0, 1.0, 5.0, 1.0], dtype=np.float32),
132 | 'mean': np.array([0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 1.0], dtype=np.float32)
133 | },
134 | 'other_agents_states': {
135 | 'dtype': np.float32,
136 | 'size': (self.MAX_NUM_OTHER_AGENTS_OBSERVED,7),
137 | 'bounds': [-np.inf, np.inf],
138 | 'attr': 'get_sensor_data("other_agents_states")',
139 | 'std': np.tile(np.array([5.0, 5.0, 1.0, 1.0, 1.0, 5.0, 1.0], dtype=np.float32), (self.MAX_NUM_OTHER_AGENTS_OBSERVED, 1)),
140 | 'mean': np.tile(np.array([0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 1.0], dtype=np.float32), (self.MAX_NUM_OTHER_AGENTS_OBSERVED, 1)),
141 | },
142 | 'laserscan': {
143 | 'dtype': np.float32,
144 | 'size': (self.LASERSCAN_NUM_PAST, self.LASERSCAN_LENGTH),
145 | 'bounds': [0., 6.],
146 | 'attr': 'get_sensor_data("laserscan")',
147 | 'std': 5.*np.ones((self.LASERSCAN_NUM_PAST, self.LASERSCAN_LENGTH), dtype=np.float32),
148 | 'mean': 5.*np.ones((self.LASERSCAN_NUM_PAST, self.LASERSCAN_LENGTH), dtype=np.float32)
149 | },
150 | 'is_learning': {
151 | 'dtype': np.float32,
152 | 'size': 1,
153 | 'bounds': [0., 1.],
154 | 'attr': 'get_agent_data_equiv("policy.str", "learning")'
155 | },
156 | }
157 |
158 | self.ANIMATION_COLUMNS = ['pos_x', 'pos_y', 'alpha', 'vel_x', 'vel_y', 'vel_linear', 'vel_angular', 'total_time']
159 | if not hasattr(self, "STATES_IN_OBS_MULTI"):
160 | self.STATES_IN_OBS_MULTI = [
161 | ['is_learning', 'num_other_agents', 'dist_to_goal', 'heading_ego_frame', 'pref_speed', 'radius'],
162 | ]
163 |
164 | if not hasattr(self, "STATES_NOT_USED_IN_POLICY"):
165 | self.STATES_NOT_USED_IN_POLICY = ['is_learning']
166 |
167 | self.HOST_AGENT_OBSERVATION_LENGTH = 4 # dist to goal, heading to goal, pref speed, radius
168 | self.OTHER_AGENT_STATE_LENGTH = 7 # other px, other py, other vx, other vy, other radius, combined radius, distance between
169 | self.OTHER_AGENT_OBSERVATION_LENGTH = 7 # other px, other py, other vx, other vy, other radius, combined radius, distance between
170 | self.OTHER_AGENT_FULL_OBSERVATION_LENGTH = self.OTHER_AGENT_OBSERVATION_LENGTH
171 | self.HOST_AGENT_STATE_SIZE = self.HOST_AGENT_OBSERVATION_LENGTH
172 |
173 | # self.AGENT_SORTING_METHOD = "closest_last"
174 | self.AGENT_SORTING_METHOD = "closest_first"
175 | # self.AGENT_SORTING_METHOD = "time_to_impact"
176 |
177 | class Example(Config):
178 | def __init__(self):
179 | self.MAX_NUM_AGENTS_IN_ENVIRONMENT = 1024
180 | Config.__init__(self)
181 | self.EVALUATE_MODE = True
182 | self.TRAIN_MODE = False
183 | self.DT = 0.1
184 | self.SAVE_EPISODE_PLOTS = True
185 | self.PLOT_CIRCLES_ALONG_TRAJ = True
186 | self.ANIMATE_EPISODES = True
187 | # self.SENSING_HORIZON = 4
188 | # self.PLT_LIMITS = [[-20, 20], [-20, 20]]
189 | # self.PLT_FIG_SIZE = (10,10)
190 |
191 | class AStar(Config):
192 | def __init__(self):
193 | self.MAX_NUM_AGENTS_IN_ENVIRONMENT = 512
194 | Config.__init__(self)
195 | self.EVALUATE_MODE = True
196 | self.TRAIN_MODE = False
197 | self.DT = 0.1
198 | self.SAVE_EPISODE_PLOTS = True
199 | self.PLOT_CIRCLES_ALONG_TRAJ = True
200 | self.ANIMATE_EPISODES = True
201 | # self.SENSING_HORIZON = 4
202 | # self.PLT_LIMITS = [[-20, 20], [-20, 20]]
203 | # self.PLT_FIG_SIZE = (10,10)
204 |
--------------------------------------------------------------------------------
/mamp/envs/macaenv.py:
--------------------------------------------------------------------------------
1 | '''
2 | '''
3 | import os
4 | import gym
5 | import copy
6 | import itertools
7 | import gym.spaces
8 | import numpy as np
9 |
10 | from mamp.envs import Config
11 | from mamp.util import l2norm
12 | from mamp.policies.kdTree import KDTree
13 |
14 |
15 | class MACAEnv(gym.Env):
16 | def __init__(self):
17 | self.id = 0
18 | self.episode_number = 0
19 | self.episode_step_number = None
20 |
21 | # Initialize Rewards
22 | # self._initialize_rewards()
23 |
24 | # Simulation Parameters
25 | self.num_agents = Config.MAX_NUM_AGENTS_IN_ENVIRONMENT
26 | self.dt_nominal = Config.DT
27 |
28 | # Collision Parameters
29 | self.collision_dist = Config.COLLISION_DIST
30 | self.getting_close_range = Config.GETTING_CLOSE_RANGE
31 | self.evaluate = Config.EVALUATE_MODE
32 |
33 | ### The gym.spaces library doesn't support Python2.7 (syntax of Super().__init__())
34 | self.action_space_type = Config.ACTION_SPACE_TYPE
35 | # Upper/Lower bounds on Actions
36 | self.max_heading_change = np.pi / 4
37 | self.min_heading_change = -self.max_heading_change
38 | self.min_speed = 0.0
39 | self.max_speed = 1.0
40 | if self.action_space_type == Config.discrete:
41 | self.action_space = gym.spaces.Discrete(self.actions.num_actions, dtype=np.float32)
42 | elif self.action_space_type == Config.continuous:
43 | self.low_action = np.array([self.min_speed, self.min_heading_change])
44 | self.high_action = np.array([self.max_speed, self.max_heading_change])
45 | self.action_space = gym.spaces.Box(self.low_action, self.high_action, dtype=np.float32)
46 |
47 | # single agent dict obs
48 | self.observation = {}
49 | for agent in range(Config.MAX_NUM_AGENTS_IN_ENVIRONMENT):
50 | self.observation[agent] = {}
51 |
52 | self.agents = None
53 | self.centralized_planner = None
54 | self.obstacles = []
55 | self.kdTree = None
56 |
57 | def set_agents(self, agents, obstacles=None):
58 | self.agents = agents
59 | self.obstacles = obstacles
60 | self.kdTree = KDTree(self.agents, self.obstacles)
61 | self.kdTree.buildObstacleTree()
62 |
63 | def reset(self):
64 | for ag in self.agents: ag.reset()
65 | if self.episode_step_number is not None and self.episode_step_number > 0:
66 | self.episode_number += 1
67 | self.begin_episode = True
68 | self.episode_step_number = 0
69 |
70 | def step(self, actions):
71 | self.episode_step_number += 1
72 |
73 | # Take action
74 | self._take_action(actions)
75 |
76 | self._update_after_action()
77 |
78 | collision_with_obstacle, collision_with_agent = self._check_for_collisions()
79 |
80 | # Take observation
81 | next_observations = self._get_obs()
82 |
83 | # Check which agents' games are finished (at goal/collided/out of time)
84 | which_agents_done, game_over = self._check_which_agents_done()
85 |
86 | which_agents_done_dict = {}
87 | which_agents_learning_dict = {}
88 | for i, agent in enumerate(self.agents):
89 | which_agents_done_dict[agent.id] = which_agents_done[i]
90 | which_agents_learning_dict[agent.id] = agent.policy.is_still_learning
91 | info = {
92 | 'which_agents_done': which_agents_done_dict,
93 | 'which_agents_learning': which_agents_learning_dict,
94 | }
95 | return next_observations, 0, game_over, info
96 |
97 | def _take_action(self, actions):
98 | self.kdTree.buildAgentTree()
99 |
100 | num_actions_per_agent = 2 # speed, delta heading angle
101 | all_actions = np.zeros((len(self.agents), num_actions_per_agent), dtype=np.float32)
102 |
103 | # Agents set their action (either from external or w/ find_next_action)
104 | for agent_index, agent in enumerate(self.agents):
105 | if agent.is_done:
106 | continue
107 | dict_obs = self.observation[agent_index]
108 | other_agents = copy.copy(self.agents)
109 | other_agents.remove(agent)
110 | dict_comm = {'other_agents': other_agents, 'obstacles': self.obstacles}
111 | all_actions[agent_index, :] = agent.find_next_action(dict_obs, dict_comm, actions, self.kdTree)
112 |
113 | # After all agents have selected actions, run one dynamics update
114 | for i, agent in enumerate(self.agents):
115 | agent.take_action(all_actions[i, :])
116 |
117 | def set_cbs_planner(self, planner):
118 | self.centralized_planner = planner
119 | solutions = self.centralized_planner.search()
120 |
121 | if not solutions:
122 | print("agent" + str(self.id) + "'s Solution not found", '\n')
123 | return
124 | for aid, solution in solutions.items():
125 | if len(solution) <= 1: continue
126 | for time_sol in solution[1:][::-1]:
127 | self.agents[aid].path.append(time_sol)
128 | print("agent" + str(aid) + "'s trajectory is:", solution)
129 |
130 | def _check_for_collisions(self):
131 | """ Check whether each agent has collided with another agent or a static obstacle in the map
132 | This method doesn't compute social zones currently!!!!!
133 | Returns:
134 | - collision_with_agent (list): for each agent, bool True if that agent is in collision with another agent
135 | - collision_with_wall (list): for each agent, bool True if that agent is in collision with object in map
136 | - entered_norm_zone (list): for each agent, bool True if that agent entered another agent's social zone
137 | - dist_btwn_nearest_agent (list): for each agent, float closest distance to another agent
138 | """
139 | collision_with_agent = [False for _ in self.agents]
140 | collision_with_wall = [False for _ in self.agents]
141 | entered_norm_zone = [False for _ in self.agents]
142 | dist_btwn_nearest_agent = [np.inf for _ in self.agents]
143 | agent_shapes = []
144 | agent_front_zones = []
145 | agent_inds = list(range(len(self.agents)))
146 | agent_pairs = list(itertools.combinations(agent_inds, 2))
147 | for i, j in agent_pairs:
148 | dist_btwn = l2norm(self.agents[i].pos_global_frame, self.agents[j].pos_global_frame)
149 | combined_radius = self.agents[i].radius + self.agents[j].radius
150 | dist_btwn_nearest_agent[i] = min(dist_btwn_nearest_agent[i], dist_btwn - combined_radius)
151 | if dist_btwn <= combined_radius:
152 | # Collision with another agent!
153 | collision_with_agent[i] = True
154 | collision_with_agent[j] = True
155 | self.agents[i].in_collision = True
156 | self.agents[j].in_collision = True
157 | print('collision:', 'agent' + str(i), 'and agent' + str(j))
158 |
159 | agent_inds = list(range(len(self.agents)))
160 | collision_with_obstacle = [False for _ in self.agents]
161 | dist_to_nearest_obstacle = [np.inf for _ in self.agents]
162 | for obstacle in self.obstacles:
163 | for j in agent_inds:
164 | dist_btwn = l2norm(obstacle.pos_global_frame, self.agents[j].pos_global_frame)
165 | combined_radius = obstacle.radius + self.agents[j].radius
166 | dist_to_nearest_obstacle[j] = min(dist_to_nearest_obstacle[j], dist_btwn - combined_radius)
167 | # diff_verct = self.agents[j].pos_global_frame - obstacle.pos_global_frame
168 | # norm = np.sqrt(diff_verct[0]**2 + diff_verct[1]**2)
169 | if dist_btwn <= combined_radius:
170 | collision_with_obstacle[j] = True
171 | self.agents[j].in_collision = True
172 | print('collision:', 'agent' + str(j), 'and obstacle' + str(obstacle.id))
173 |
174 | return collision_with_obstacle, collision_with_agent
175 |
176 | def _check_which_agents_done(self):
177 | at_goal_condition = np.array([a.is_at_goal for a in self.agents])
178 | ran_out_of_time_condition = np.array([a.ran_out_of_time for a in self.agents])
179 | in_collision_condition = np.array([a.in_collision for a in self.agents])
180 | which_agents_done = np.logical_or.reduce((at_goal_condition, ran_out_of_time_condition, in_collision_condition))
181 | for agent_index, agent in enumerate(self.agents):
182 | agent.is_done = which_agents_done[agent_index]
183 |
184 | if Config.EVALUATE_MODE:
185 | # Episode ends when every agent is done
186 | game_over = np.all(which_agents_done)
187 | elif Config.TRAIN_SINGLE_AGENT:
188 | # Episode ends when ego agent is done
189 | game_over = which_agents_done[0]
190 | else:
191 | # Episode is done when all *learning* agents are done
192 | learning_agent_inds = [i for i in range(len(self.agents)) if self.agents[i].policy.is_still_learning]
193 | game_over = np.all(which_agents_done[learning_agent_inds])
194 |
195 | return which_agents_done, game_over
196 |
197 | def _update_after_action(self):
198 | for i, agent in enumerate(self.agents):
199 | if agent.is_at_goal: continue
200 | agent.update_after_action()
201 |
202 | def _get_obs(self):
203 | """ Update the map now that agents have moved, have each agent sense the world, and fill in their observations
204 | Returns:
205 | observation (list): for each agent, a dictionary observation.
206 | """
207 | # Agents collect a reading from their map-based sensors
208 | for i, agent in enumerate(self.agents):
209 | agent.sense(self.agents, i)
210 | # Agents fill in their element of the multiagent observation vector
211 | for i, agent in enumerate(self.agents):
212 | self.observation[i] = agent.get_observation_dict()
213 |
214 | return self.observation
215 |
--------------------------------------------------------------------------------
/mamp/sensors/OtherAgentsStatesSensor.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | from mamp.sensors.Sensor import Sensor
3 | from mamp.envs import Config
4 | from mamp.util import compute_time_to_impact, vec2_l2_norm
5 | import operator
6 |
7 | class OtherAgentsStatesSensor(Sensor):
8 | """ A dense matrix of relative states of other agents (e.g., their positions, vel, radii)
9 |
10 | :param max_num_other_agents_observed: (int) only can observe up to this many agents (the closest ones)
11 | :param agent_sorting_method: (str) definition of closeness in words (one of ['closest_last', 'closest_first', 'time_to_impact'])
12 |
13 | """
14 | def __init__(self, max_num_other_agents_observed=Config.MAX_NUM_OTHER_AGENTS_OBSERVED, agent_sorting_method=Config.AGENT_SORTING_METHOD):
15 | Sensor.__init__(self)
16 | self.name = 'other_agents_states'
17 | self.max_num_other_agents_observed = max_num_other_agents_observed
18 | self.agent_sorting_method = agent_sorting_method
19 |
20 | def get_clipped_sorted_inds(self, sorting_criteria):
21 | """ Determine the closest N agents using the desired sorting criteria
22 |
23 | Args:
24 | sorting_criteria (str): how to sort the list of agents (one of ['closest_last', 'closest_first', 'time_to_impact']).
25 | See journal paper.
26 |
27 | Returns:
28 | clipped_sorted_inds (list): indices of the "closest" max_num_other_agents_observed
29 | agents sorted by "closeness" ("close" defined by sorting criteria),
30 |
31 | """
32 |
33 | # Grab first N agents (where N=Config.MAX_NUM_OTHER_AGENTS_OBSERVED)
34 | if self.agent_sorting_method in ['closest_last', 'closest_first']:
35 | # where "first" == closest
36 | sorted_sorting_criteria = sorted(sorting_criteria, key = lambda x: (x[1], x[2]))
37 | elif self.agent_sorting_method in ['time_to_impact']:
38 | # where "first" == lowest time-to-impact
39 | sorted_sorting_criteria = sorted(sorting_criteria, key = lambda x: (-x[3], -x[1], x[2]))
40 | clipped_sorting_criteria = sorted_sorting_criteria[:self.max_num_other_agents_observed]
41 |
42 | # Then sort those N agents by the preferred ordering scheme
43 | if self.agent_sorting_method == "closest_last":
44 | # sort by inverse distance away, then by lateral position
45 | sorted_dists = sorted(clipped_sorting_criteria, key = lambda x: (-x[1], x[2]))
46 | elif self.agent_sorting_method == "closest_first":
47 | # sort by distance away, then by lateral position
48 | sorted_dists = sorted(clipped_sorting_criteria, key = lambda x: (x[1], x[2]))
49 | elif self.agent_sorting_method == "time_to_impact":
50 | # sort by time_to_impact, break ties by distance away, then by lateral position (e.g. in case inf TTC)
51 | sorted_dists = sorted(clipped_sorting_criteria, key = lambda x: (-x[3], -x[1], x[2]))
52 | else:
53 | raise ValueError("Did not supply proper self.agent_sorting_method in Agent.py.")
54 |
55 | clipped_sorted_inds = [x[0] for x in sorted_dists]
56 | return clipped_sorted_inds
57 |
58 |
59 | def sense(self, agents, agent_index):
60 | """ Go through each agent in the environment, and compute its relative position, vel, etc. and put into an array
61 |
62 | This is a denser measurement of other agents' states vs. a LaserScan or OccupancyGrid
63 |
64 | Args:
65 | agents (list): all :class:`~gym_collision_avoidance.envs.agent.Agent` in the environment
66 | agent_index (int): index of this agent (the one with this sensor) in :code:`agents`
67 | top_down_map (2D np array): binary image with 0 if that pixel is free space, 1 if occupied (not used!)
68 |
69 | Returns:
70 | other_agents_states (np array): (max_num_other_agents_observed x 7) the 7 states about each other agent, :code:`[p_parallel_ego_frame, p_orthog_ego_frame, v_parallel_ego_frame, v_orthog_ego_frame, other_agent.radius, combined_radius, dist_2_other]`
71 |
72 | """
73 | host_agent = agents[agent_index]
74 | other_agents_states = []
75 | other_agent_count = 0
76 | for other_agent in agents:
77 | if other_agent.id == host_agent.id:
78 | continue
79 | # project other elements onto the new reference frame
80 | rel_pos_to_other_global_frame = other_agent.pos_global_frame - host_agent.pos_global_frame
81 | dist_2_other = np.linalg.norm(rel_pos_to_other_global_frame) - host_agent.radius - other_agent.radius
82 |
83 | if dist_2_other > host_agent.view_radius: continue
84 |
85 | other_obs = np.array([other_agent.group,
86 | other_agent.pos_global_frame[0],
87 | other_agent.pos_global_frame[1],
88 | dist_2_other])
89 |
90 | if other_agent_count == 0:
91 | host_agent.other_agent_states[:] = other_obs
92 |
93 | other_agents_states.append(other_obs)
94 | other_agent_count += 1
95 |
96 | host_agent.num_other_agents_observed = other_agent_count
97 | other_agents_states_array = np.reshape(other_agents_states, (-1, 4))
98 | return other_agents_states_array
99 |
100 | def sense1(self, agents, agent_index, top_down_map=None):
101 | """ Go through each agent in the environment, and compute its relative position, vel, etc. and put into an array
102 |
103 | This is a denser measurement of other agents' states vs. a LaserScan or OccupancyGrid
104 |
105 | Args:
106 | agents (list): all :class:`~gym_collision_avoidance.envs.agent.Agent` in the environment
107 | agent_index (int): index of this agent (the one with this sensor) in :code:`agents`
108 | top_down_map (2D np array): binary image with 0 if that pixel is free space, 1 if occupied (not used!)
109 |
110 | Returns:
111 | other_agents_states (np array): (max_num_other_agents_observed x 7) the 7 states about each other agent, :code:`[p_parallel_ego_frame, p_orthog_ego_frame, v_parallel_ego_frame, v_orthog_ego_frame, other_agent.radius, combined_radius, dist_2_other]`
112 |
113 | """
114 | host_agent = agents[agent_index]
115 | other_agent_dists = {}
116 | sorted_pairs = sorted(other_agent_dists.items(), key=operator.itemgetter(1))
117 |
118 | sorting_criteria = []
119 | for i, other_agent in enumerate(agents):
120 | if other_agent.id == host_agent.id:
121 | continue
122 | # project other elements onto the new reference frame
123 | rel_pos_to_other_global_frame = other_agent.pos_global_frame - host_agent.pos_global_frame
124 | p_parallel_ego_frame = np.dot(rel_pos_to_other_global_frame, host_agent.ref_prll)
125 | p_orthog_ego_frame = np.dot(rel_pos_to_other_global_frame, host_agent.ref_orth)
126 | dist_between_agent_centers = vec2_l2_norm(rel_pos_to_other_global_frame)
127 | dist_2_other = dist_between_agent_centers - host_agent.radius - other_agent.radius
128 | combined_radius = host_agent.radius + other_agent.radius
129 |
130 | if dist_between_agent_centers > Config.SENSING_HORIZON:
131 | # print("Agent too far away")
132 | continue
133 |
134 | if self.agent_sorting_method != "time_to_impact":
135 | time_to_impact = None
136 | else:
137 | time_to_impact = compute_time_to_impact(host_agent.pos_global_frame,
138 | other_agent.pos_global_frame,
139 | host_agent.vel_global_frame,
140 | other_agent.vel_global_frame,
141 | combined_radius)
142 |
143 | sorting_criteria.append([i, round(dist_2_other,2), p_orthog_ego_frame, time_to_impact])
144 |
145 | clipped_sorted_inds = self.get_clipped_sorted_inds(sorting_criteria)
146 | clipped_sorted_agents = [agents[i] for i in clipped_sorted_inds]
147 |
148 | other_agents_states = np.zeros((Config.MAX_NUM_OTHER_AGENTS_OBSERVED, Config.OTHER_AGENT_OBSERVATION_LENGTH))
149 | other_agent_count = 0
150 | for other_agent in clipped_sorted_agents:
151 | if other_agent.id == host_agent.id:
152 | continue
153 |
154 | rel_pos_to_other_global_frame_exp = other_agent.goal_global_frame - host_agent.goal_global_frame
155 | # project other elements onto the new reference frame
156 | rel_pos_to_other_global_frame = other_agent.pos_global_frame - host_agent.pos_global_frame
157 | dist_2_other = np.linalg.norm(rel_pos_to_other_global_frame) - host_agent.radius - other_agent.radius
158 | rel_pos_diff = rel_pos_to_other_global_frame - rel_pos_to_other_global_frame_exp
159 |
160 | p_parallel_ego_frame = np.dot(rel_pos_to_other_global_frame, host_agent.ref_prll)
161 | p_orthog_ego_frame = np.dot(rel_pos_to_other_global_frame, host_agent.ref_orth)
162 | v_parallel_ego_frame = np.dot(other_agent.vel_global_frame, host_agent.ref_prll)
163 | v_orthog_ego_frame = np.dot(other_agent.vel_global_frame, host_agent.ref_orth)
164 | combined_radius = host_agent.radius + other_agent.radius
165 |
166 | other_obs = np.array([rel_pos_diff[0],
167 | rel_pos_diff[1],
168 | p_parallel_ego_frame,
169 | p_orthog_ego_frame,
170 | v_parallel_ego_frame,
171 | v_orthog_ego_frame,
172 | other_agent.radius,
173 | combined_radius,
174 | dist_2_other])
175 |
176 | if other_agent_count == 0:
177 | host_agent.other_agent_obs[:] = other_obs
178 |
179 | other_agents_states[other_agent_count,:] = other_obs
180 | other_agent_count += 1
181 |
182 | host_agent.num_other_agents_observed = other_agent_count
183 | return other_agents_states
184 |
--------------------------------------------------------------------------------
/mamp/policies/pcaPolicy/dubinsmaneuver2d.py:
--------------------------------------------------------------------------------
1 | """
2 | @ Revise: Gang Xu
3 | @ Date: 2021.8.13
4 | @ Details: dubins path planner
5 | @ reference: https://github.com/phanikiran1169/RRTstar_3D-Dubins.git
6 | """
7 |
8 |
9 | import math
10 | import matplotlib.pyplot as plt
11 | import numpy as np
12 |
13 |
14 | class DubinsManeuver(object):
15 | def __init__(self, qi, qf, r_min):
16 | self.qi = qi
17 | self.qf = qf
18 | self.r_min = r_min
19 | self.t = -1.0
20 | self.p = -1.0
21 | self.q = -1.0
22 | self.px = []
23 | self.py = []
24 | self.pyaw = []
25 | self.path = []
26 | self.mode = None
27 | self.length = -1.0
28 | self.sampling_size = np.deg2rad(6.0)
29 |
30 |
31 | def mod2pi(theta): # to 0-2*pi
32 | return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi)
33 |
34 |
35 | def pi_2_pi(angle): # to -pi-pi
36 | return (angle + math.pi) % (2 * math.pi) - math.pi
37 |
38 |
39 | def LSL(alpha, beta, d):
40 | sa = math.sin(alpha)
41 | sb = math.sin(beta)
42 | ca = math.cos(alpha)
43 | cb = math.cos(beta)
44 | c_ab = math.cos(alpha - beta)
45 |
46 | tmp0 = d + sa - sb
47 |
48 | mode = ["L", "S", "L"]
49 | p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sa - sb))
50 | if p_squared < 0:
51 | return None, None, None, mode
52 | tmp1 = math.atan2((cb - ca), tmp0)
53 | t = mod2pi(-alpha + tmp1)
54 | p = math.sqrt(p_squared)
55 | q = mod2pi(beta - tmp1)
56 |
57 | return t, p, q, mode
58 |
59 |
60 | def RSR(alpha, beta, d):
61 | sa = math.sin(alpha)
62 | sb = math.sin(beta)
63 | ca = math.cos(alpha)
64 | cb = math.cos(beta)
65 | c_ab = math.cos(alpha - beta)
66 |
67 | tmp0 = d - sa + sb
68 | mode = ["R", "S", "R"]
69 | p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa))
70 | if p_squared < 0:
71 | return None, None, None, mode
72 | tmp1 = math.atan2((ca - cb), tmp0)
73 | t = mod2pi(alpha - tmp1)
74 | p = math.sqrt(p_squared)
75 | q = mod2pi(-beta + tmp1)
76 |
77 | return t, p, q, mode
78 |
79 |
80 | def LSR(alpha, beta, d):
81 | sa = math.sin(alpha)
82 | sb = math.sin(beta)
83 | ca = math.cos(alpha)
84 | cb = math.cos(beta)
85 | c_ab = math.cos(alpha - beta)
86 |
87 | p_squared = -2 + (d * d) + (2 * c_ab) + (2 * d * (sa + sb))
88 | mode = ["L", "S", "R"]
89 | if p_squared < 0:
90 | return None, None, None, mode
91 | p = math.sqrt(p_squared)
92 | tmp2 = math.atan2((-ca - cb), (d + sa + sb)) - math.atan2(-2.0, p)
93 | t = mod2pi(-alpha + tmp2)
94 | q = mod2pi(-mod2pi(beta) + tmp2)
95 |
96 | return t, p, q, mode
97 |
98 |
99 | def RSL(alpha, beta, d):
100 | sa = math.sin(alpha)
101 | sb = math.sin(beta)
102 | ca = math.cos(alpha)
103 | cb = math.cos(beta)
104 | c_ab = math.cos(alpha - beta)
105 |
106 | p_squared = (d * d) - 2 + (2 * c_ab) - (2 * d * (sa + sb))
107 | mode = ["R", "S", "L"]
108 | if p_squared < 0:
109 | return None, None, None, mode
110 | p = math.sqrt(p_squared)
111 | tmp2 = math.atan2((ca + cb), (d - sa - sb)) - math.atan2(2.0, p)
112 | t = mod2pi(alpha - tmp2)
113 | q = mod2pi(beta - tmp2)
114 |
115 | return t, p, q, mode
116 |
117 |
118 | def RLR(alpha, beta, d):
119 | sa = math.sin(alpha)
120 | sb = math.sin(beta)
121 | ca = math.cos(alpha)
122 | cb = math.cos(beta)
123 | c_ab = math.cos(alpha - beta)
124 |
125 | mode = ["R", "L", "R"]
126 | tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0
127 | if abs(tmp_rlr) > 1.0:
128 | return None, None, None, mode
129 |
130 | p = mod2pi(2 * math.pi - math.acos(tmp_rlr))
131 | t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0))
132 | q = mod2pi(alpha - beta - t + mod2pi(p))
133 | return t, p, q, mode
134 |
135 |
136 | def LRL(alpha, beta, d):
137 | sa = math.sin(alpha)
138 | sb = math.sin(beta)
139 | ca = math.cos(alpha)
140 | cb = math.cos(beta)
141 | c_ab = math.cos(alpha - beta)
142 |
143 | mode = ["L", "R", "L"]
144 | tmp_lrl = (6. - d * d + 2 * c_ab + 2 * d * (- sa + sb)) / 8.
145 | if abs(tmp_lrl) > 1:
146 | return None, None, None, mode
147 | p = mod2pi(2 * math.pi - math.acos(tmp_lrl))
148 | t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.)
149 | q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p))
150 |
151 | return t, p, q, mode
152 |
153 |
154 | def dubins_path_planning_from_origin(ex, ey, syaw, eyaw, c):
155 | # nomalize
156 | dx = ex
157 | dy = ey
158 | D = math.sqrt(dx ** 2.0 + dy ** 2.0)
159 | d = D / c
160 |
161 | theta = mod2pi(math.atan2(dy, dx))
162 | alpha = mod2pi(syaw - theta)
163 | beta = mod2pi(eyaw - theta)
164 |
165 | planners = [LSL, RSR, LSR, RSL, RLR, LRL]
166 |
167 | bcost = float("inf")
168 | bt, bp, bq, bmode = None, None, None, None
169 |
170 | for planner in planners:
171 | t, p, q, mode = planner(alpha, beta, d)
172 | if t is None:
173 | continue
174 |
175 | cost = c*(abs(t) + abs(p) + abs(q))
176 | if bcost > cost:
177 | bt, bp, bq, bmode = t, p, q, mode
178 | bcost = cost
179 |
180 | px, py, pyaw = generate_course([bt, bp, bq], bmode, c)
181 |
182 | return px, py, pyaw, bmode, bcost, bt, bp, bq
183 |
184 |
185 | def dubins_path_planning(start_pos, end_pos, c):
186 | """
187 | Dubins path plannner
188 |
189 | input:start_pos, end_pos, c
190 | start_pos[0] x position of start point [m]
191 | start_pos[1] y position of start point [m]
192 | start_pos[2] yaw angle of start point [rad]
193 | end_pos[0] x position of end point [m]
194 | end_pos[1] y position of end point [m]
195 | end_pos[2] yaw angle of end point [rad]
196 | c radius [m]
197 |
198 | output: maneuver
199 | maneuver.t the first segment curve of dubins
200 | maneuver.p the second segment line of dubins
201 | maneuver.q the third segment curve of dubins
202 | maneuver.px x position sets [m]
203 | maneuver.py y position sets [m]
204 | maneuver.pyaw heading angle sets [rad]
205 | maneuver.length length of dubins
206 | maneuver.mode mode of dubins
207 | """
208 | maneuver = DubinsManeuver(start_pos, end_pos, c)
209 | sx, sy = start_pos[0], start_pos[1]
210 | ex, ey = end_pos[0], end_pos[1]
211 | syaw, eyaw = start_pos[2], end_pos[2]
212 |
213 | ex = ex - sx
214 | ey = ey - sy
215 |
216 | lpx, lpy, lpyaw, mode, clen, t, p, q = dubins_path_planning_from_origin(ex, ey, syaw, eyaw, c)
217 |
218 | px = [math.cos(-syaw) * x + math.sin(-syaw) * y + sx for x, y in zip(lpx, lpy)]
219 | py = [-math.sin(-syaw) * x + math.cos(-syaw) * y + sy for x, y in zip(lpx, lpy)]
220 | pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw]
221 | maneuver.t, maneuver.p, maneuver.q, maneuver.mode = t, p, q, mode
222 | maneuver.px, maneuver.py, maneuver.pyaw, maneuver.length = px, py, pyaw, clen
223 | for index in range(len(maneuver.px)):
224 | maneuver.path.append([maneuver.px[index], maneuver.py[index], maneuver.pyaw[index]])
225 |
226 | return maneuver
227 |
228 |
229 | def generate_course(length, mode, c):
230 | px = [0.0]
231 | py = [0.0]
232 | pyaw = [0.0]
233 |
234 | for m, l in zip(mode, length):
235 | pd = 0.0
236 | if m == "S":
237 | d = 1.0 / c
238 | else: # turning course
239 | d = np.deg2rad(6.0)
240 |
241 | while pd < abs(l - d):
242 | px.append(px[-1] + d * c * math.cos(pyaw[-1]))
243 | py.append(py[-1] + d * c * math.sin(pyaw[-1]))
244 |
245 | if m == "L": # left turn
246 | pyaw.append(pyaw[-1] + d)
247 | elif m == "S": # Straight
248 | pyaw.append(pyaw[-1])
249 | elif m == "R": # right turn
250 | pyaw.append(pyaw[-1] - d)
251 | pd += d
252 |
253 | d = l - pd
254 | px.append(px[-1] + d * c * math.cos(pyaw[-1]))
255 | py.append(py[-1] + d * c * math.sin(pyaw[-1]))
256 |
257 | if m == "L": # left turn
258 | pyaw.append(pyaw[-1] + d)
259 | elif m == "S": # Straight
260 | pyaw.append(pyaw[-1])
261 | elif m == "R": # right turn
262 | pyaw.append(pyaw[-1] - d)
263 | pd += d
264 |
265 | return px, py, pyaw
266 |
267 |
268 | def get_coordinates(maneuver, offset):
269 | noffset = offset / maneuver.r_min
270 | qi = [0., 0., maneuver.qi[2]]
271 |
272 | l1 = maneuver.t
273 | l2 = maneuver.p
274 | q1 = get_position_in_segment(l1, qi, maneuver.mode[0]) # Final do segmento 1
275 | q2 = get_position_in_segment(l2, q1, maneuver.mode[1]) # Final do segmento 2
276 |
277 | if noffset < l1:
278 | q = get_position_in_segment(noffset, qi, maneuver.mode[0])
279 | elif noffset < (l1 + l2):
280 | q = get_position_in_segment(noffset - l1, q1, maneuver.mode[1])
281 | else:
282 | q = get_position_in_segment(noffset - l1 - l2, q2, maneuver.mode[2])
283 |
284 | q[0] = q[0] * maneuver.r_min + qi[0]
285 | q[1] = q[1] * maneuver.r_min + qi[1]
286 | q[2] = mod2pi(q[2])
287 |
288 | return q
289 |
290 |
291 | def get_position_in_segment(offset, qi, mode):
292 | q = [0.0, 0.0, 0.0]
293 | if mode == 'L':
294 | q[0] = qi[0] + math.sin(qi[2] + offset) - math.sin(qi[2])
295 | q[1] = qi[1] - math.cos(qi[2] + offset) + math.cos(qi[2])
296 | q[2] = qi[2] + offset
297 | elif mode == 'R':
298 | q[0] = qi[0] - math.sin(qi[2] - offset) + math.sin(qi[2])
299 | q[1] = qi[1] + math.cos(qi[2] - offset) - math.cos(qi[2])
300 | q[2] = qi[2] - offset
301 | elif mode == 'S':
302 | q[0] = qi[0] + math.cos(qi[2]) * offset
303 | q[1] = qi[1] + math.sin(qi[2]) * offset
304 | q[2] = qi[2]
305 | return q
306 |
307 |
308 | def get_sampling_points(maneuver, sampling_size=0.1):
309 | points = []
310 | for offset in np.arange(0.0, maneuver.length+sampling_size, sampling_size):
311 | points.append(get_coordinates(maneuver, offset))
312 | return points
313 |
314 |
315 | def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
316 | """
317 | Plot arrow
318 | """
319 |
320 | if not isinstance(x, float):
321 | for (ix, iy, iyaw) in zip(x, y, yaw):
322 | plot_arrow(ix, iy, iyaw)
323 | else:
324 | plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
325 | fc=fc, ec=ec, head_width=width, head_length=width)
326 | plt.plot(x, y)
327 |
328 |
329 | if __name__ == '__main__':
330 | print("Dubins Path Planner Start!!")
331 |
332 | test_qi = [[0.0, 5.0, np.deg2rad(180.0)], [10.0, 5.0, np.deg2rad(0.0)]] # start_x, start_y, start_yaw
333 | test_qf = [[10.0, 5.0, np.deg2rad(180.0)], [0.0, 5.0, np.deg2rad(0.0)]] # end_x, end_y, end_yaw
334 |
335 | rmin = 0.5
336 |
337 | test_maneuver = dubins_path_planning(test_qi[0], test_qf[0], rmin)
338 | test_maneuver1 = dubins_path_planning(test_qi[1], test_qf[1], rmin)
339 |
340 | path = []
341 | for i in range(len(test_maneuver.px)):
342 | path.append([test_maneuver.px[i], test_maneuver.py[i], test_maneuver.pyaw[i]])
343 | print(len(path), test_maneuver.length, test_maneuver.path)
344 | print(len(path), test_maneuver.length, test_maneuver1.path)
345 |
346 | plt.plot(test_maneuver.px, test_maneuver.py, label="(0->10),(180°->180°) " + "".join(test_maneuver.mode))
347 | plt.plot(test_maneuver1.px, test_maneuver1.py, label="(10->0),(0°->0°) " + "".join(test_maneuver1.mode))
348 |
349 |
350 | # plotting
351 | plot_arrow(test_qi[0][0], test_qi[0][1], test_qi[0][2])
352 | plot_arrow(test_qf[0][0], test_qf[0][1], test_qf[0][2])
353 | plot_arrow(test_qi[1][0], test_qi[1][1], test_qi[1][2])
354 | plot_arrow(test_qf[1][0], test_qf[1][1], test_qf[1][2])
355 |
356 | plt.legend()
357 | plt.grid(True)
358 | plt.axis("equal")
359 | plt.show()
360 |
--------------------------------------------------------------------------------
/mamp/policies/pcaPolicy/rvoDubinsPolicy.py:
--------------------------------------------------------------------------------
1 | import time
2 | import numpy as np
3 | from math import sqrt, sin, cos, atan2, pi, acos
4 | from itertools import product
5 |
6 | from mamp.envs import Config
7 | from mamp.policies.policy import Policy
8 | from mamp.policies.pcaPolicy import dubinsmaneuver2d
9 | from mamp.util import sqr, l2norm, norm, is_parallel, absSq, mod2pi, pi_2_pi, get_phi, angle_2_vectors, is_intersect
10 |
11 |
12 | class RVODubinsPolicy(Policy):
13 | def __init__(self):
14 | Policy.__init__(self, str="RVODubinsPolicy")
15 | self.type = "internal"
16 | self.now_goal = None
17 |
18 | def find_next_action(self, obs, dict_comm, agent, kdTree):
19 | ts = time.time()
20 | self.now_goal = agent.goal_global_frame
21 | v_pref = compute_v_pref(agent)
22 | vA = agent.vel_global_frame
23 | pA = agent.pos_global_frame
24 | if l2norm(vA, [0, 0, 0]) <= 1e-5:
25 | vA_post = 0.2 * v_pref
26 | te = time.time()
27 | cost_step = te - ts
28 | agent.total_time += cost_step
29 | action = to_unicycle(vA_post, agent)
30 | theta = 0.0
31 | else:
32 | agent_rad = agent.radius + 0.05
33 | computeNeighbors(agent, kdTree)
34 | RVO_BA_all = []
35 | for obj in agent.neighbors:
36 | obj = obj[0]
37 | pB = obj.pos_global_frame
38 | if obj.is_at_goal:
39 | transl_vB_vA = pA
40 | else:
41 | vB = obj.vel_global_frame
42 | transl_vB_vA = pA + 0.5 * (vB + vA) # Use RVO.
43 | obj_rad = obj.radius + 0.05
44 |
45 | RVO_BA = [transl_vB_vA, pA, pB, obj_rad + agent_rad]
46 | RVO_BA_all.append(RVO_BA)
47 | vA_post = intersect(v_pref, RVO_BA_all, agent)
48 | te = time.time()
49 | cost_step = te - ts
50 | agent.total_time += cost_step
51 | action = to_unicycle(vA_post, agent)
52 | theta = angle_2_vectors(vA, vA_post)
53 | agent.vel_global_unicycle[0] = round(1.0 * action[0], 5)
54 | agent.vel_global_unicycle[1] = round(0.22 * 0.5 * action[1] / Config.DT, 5)
55 | dist = l2norm(agent.pos_global_frame, agent.goal_global_frame)
56 | if theta > agent.max_heading_change:
57 | print('agent' + str(agent.id), len(agent.neighbors), action, '终点距离:', dist, 'theta:', theta)
58 | else:
59 | print('agent' + str(agent.id), len(agent.neighbors), action, '终点距离:', dist)
60 |
61 | return action
62 |
63 |
64 | def update_dubins(agent):
65 | dis = l2norm(agent.pos_global_frame, agent.dubins_now_goal)
66 | sampling_size = agent.dubins_sampling_size
67 | if dis <= sampling_size:
68 | if agent.dubins_path:
69 | agent.dubins_now_goal = np.array(agent.dubins_path.pop()[:2], dtype='float64')
70 | else:
71 | agent.dubins_now_goal = agent.goal_global_frame
72 |
73 |
74 | def compute_dubins(agent):
75 | qi = np.hstack((agent.pos_global_frame, agent.heading_global_frame))
76 | qf = np.hstack((agent.goal_global_frame, agent.goal_heading_frame))
77 | rmin, pitchlims = agent.turning_radius, agent.pitchlims
78 | maneuver = dubinsmaneuver2d.dubins_path_planning(qi, qf, rmin)
79 | dubins_path = maneuver.path.copy()
80 | agent.dubins_sampling_size = maneuver.sampling_size
81 | desire_length = maneuver.length
82 | points_num = len(dubins_path)
83 | path = []
84 | for i in range(points_num):
85 | path.append((dubins_path.pop()))
86 | return path, desire_length, points_num
87 |
88 |
89 | def is_parallel_neighbor(agent):
90 | range_para = 0.15
91 | max_parallel_steps = int((agent.radius + agent.neighbors[0][0].radius + 0.2) / (agent.pref_speed * agent.timeStep))
92 | if len(agent.is_parallel_neighbor) < max_parallel_steps and len(agent.neighbors) > 0:
93 | agent.is_parallel_neighbor.append((agent.neighbors[0][0].id, agent.neighbors[0][1]))
94 | return False
95 | else:
96 | agent.is_parallel_neighbor.pop(0)
97 | agent.is_parallel_neighbor.append((agent.neighbors[0][0].id, agent.neighbors[0][1]))
98 | nei = agent.is_parallel_neighbor
99 | is_same_id = True
100 | is_in_range = True
101 | for i in range(len(nei) - 1):
102 | if nei[0][0] != nei[i + 1][0]:
103 | is_same_id = False
104 | dis = abs(sqrt(nei[0][1]) - sqrt(nei[i + 1][1]))
105 | if dis > range_para:
106 | is_in_range = False
107 | if is_same_id and is_in_range:
108 | return True
109 | else:
110 | return False
111 |
112 |
113 | def dubins_path_node_pop(agent):
114 | agent.dubins_last_goal = np.array(agent.dubins_path.pop()[:3], dtype='float64')
115 | agent.dubins_last_goal = np.array(agent.dubins_path.pop()[:3], dtype='float64')
116 | agent.dubins_last_goal = np.array(agent.dubins_path.pop()[:3], dtype='float64')
117 | agent.dubins_last_goal = np.array(agent.dubins_path.pop()[:3], dtype='float64')
118 |
119 |
120 | def compute_v_pref(agent):
121 | """
122 | is_parallel(vA, v_pref): —— Whether to leave Dubins trajectory as collision avoidance.
123 | dis_goal <= k: —— Regard as obstacles-free when the distance is less than k.
124 | dis < 6 * sampling_size: —— Follow the current Dubins path when not moving away from the current Dubins path.
125 | theta >= np.deg2rad(100): —— Avoid the agent moving away from the goal position after update Dubins path.
126 | """
127 | pA = agent.pos_global_frame
128 | pG = agent.goal_global_frame
129 | dist_goal = l2norm(pA, pG)
130 | k = 3.0 * agent.turning_radius
131 | if not agent.is_use_dubins: # first
132 | agent.is_use_dubins = True
133 | dubins_path, desire_length, points_num = compute_dubins(agent)
134 | agent.dubins_path = dubins_path
135 | dubins_path_node_pop(agent)
136 | agent.dubins_now_goal = np.array(agent.dubins_path.pop()[:2], dtype='float64')
137 | dif_x = agent.dubins_now_goal - pA
138 | else:
139 | update_dubins(agent)
140 | vA = np.array(agent.vel_global_frame)
141 | v_pref = agent.v_pref
142 | dis = l2norm(pA, agent.dubins_now_goal)
143 | max_size = round(6 * agent.dubins_sampling_size, 5)
144 | pApG = pG - pA
145 | theta = angle_2_vectors(vA, pApG)
146 | deg135 = np.deg2rad(135)
147 | if ((is_parallel(vA, v_pref) or dist_goal <= k) and dis < max_size) or (theta >= deg135):
148 | update_dubins(agent)
149 | if agent.dubins_path:
150 | dif_x = agent.dubins_now_goal - pA
151 | else:
152 | dif_x = pG - pA
153 | else:
154 | dubins_path, length, points_num = compute_dubins(agent)
155 | agent.dubins_path = dubins_path
156 | dubins_path_node_pop(agent)
157 | agent.dubins_now_goal = np.array(agent.dubins_path.pop()[:2], dtype='float64')
158 | dif_x = agent.dubins_now_goal - pA
159 |
160 | norm_dif_x = dif_x * agent.pref_speed / l2norm(dif_x, [0, 0])
161 | v_pref = np.array(norm_dif_x)
162 | if dist_goal < Config.NEAR_GOAL_THRESHOLD:
163 | v_pref[0] = 0.0
164 | v_pref[1] = 0.0
165 | agent.v_pref = v_pref
166 | v_pref = np.array([round(v_pref[0], 5), round(v_pref[1], 5)])
167 | return v_pref
168 |
169 |
170 | def intersect(v_pref, RVO_BA_all, agent):
171 | norm_v = np.linalg.norm(v_pref)
172 | suitable_V = []
173 | unsuitable_V = []
174 |
175 | Theta = np.arange(0, 2 * pi, step=pi / 32)
176 | RAD = np.linspace(0.02, 1.0, num=5)
177 | v_list = [v_pref[:]]
178 | for theta, rad in product(Theta, RAD):
179 | new_v = np.array([cos(theta), sin(theta)]) * rad * norm_v
180 | new_v = np.array([new_v[0], new_v[1]])
181 | v_list.append(new_v)
182 |
183 | for new_v in v_list:
184 | suit = True
185 | for RVO_BA in RVO_BA_all:
186 | p_0 = RVO_BA[0]
187 | pA = RVO_BA[1]
188 | pB = RVO_BA[2]
189 | combined_radius = RVO_BA[3]
190 | v_dif = np.array(new_v + pA - p_0) # new_v-0.5*(vA+vB) or new_v-vB
191 | if is_intersect(pA, pB, combined_radius, v_dif):
192 | suit = False
193 | break
194 | if suit:
195 | suitable_V.append(new_v)
196 | else:
197 | unsuitable_V.append(new_v)
198 |
199 | # ----------------------
200 | if suitable_V:
201 | suitable_V.sort(key=lambda v: l2norm(v, v_pref)) # Sort begin at minimum and end at maximum.
202 | vA_post = suitable_V[0]
203 | else:
204 | tc_V = dict()
205 | for unsuit_v in unsuitable_V:
206 | tc_V[tuple(unsuit_v)] = 0
207 | tc = [] # 时间
208 | for RVO_BA in RVO_BA_all:
209 | p_0 = RVO_BA[0]
210 | pA = RVO_BA[1]
211 | pB = RVO_BA[2]
212 | combined_radius = RVO_BA[3]
213 | v_dif = np.array(unsuit_v + pA - p_0)
214 | pApB = pB - pA
215 | if is_intersect(pA, pB, combined_radius, v_dif):
216 | discr = sqr(np.dot(v_dif, pApB)) - absSq(v_dif) * (absSq(pApB) - sqr(combined_radius))
217 | if discr < 0.0:
218 | print(discr)
219 | continue
220 | tc_v = max((np.dot(v_dif, pApB) - sqrt(discr)) / absSq(v_dif), 0.0)
221 | tc.append(tc_v)
222 | if len(tc) == 0:
223 | tc = [0.0]
224 | tc_V[tuple(unsuit_v)] = min(tc) + 1e-5
225 | WT = agent.safetyFactor
226 | vA_post = min(unsuitable_V, key=lambda v: ((WT / tc_V[tuple(v)]) + l2norm(v, v_pref)))
227 |
228 | return vA_post
229 |
230 |
231 | def satisfied_constraint(agent, vCand):
232 | vA = agent.vel_global_frame
233 | costheta = np.dot(vA, vCand) / (np.linalg.norm(vA) * np.linalg.norm(vCand))
234 | if costheta > 1.0:
235 | costheta = 1.0
236 | elif costheta < -1.0:
237 | costheta = -1.0
238 | theta = acos(costheta) # Rotational constraints.
239 | if theta <= agent.max_heading_change:
240 | return True
241 | else:
242 | return False
243 |
244 |
245 | def select_right_side(v_list, vA):
246 | opt_v = [v_list[0]]
247 | i = 1
248 | while True:
249 | if abs(l2norm(v_list[0], vA) - l2norm(v_list[i], vA)) < 1e-1:
250 | opt_v.append(v_list[i])
251 | i += 1
252 | if i == len(v_list):
253 | break
254 | else:
255 | break
256 | vA_phi_min = min(opt_v, key=lambda v: get_phi(v))
257 | vA_phi_max = max(opt_v, key=lambda v: get_phi(v))
258 | phi_min = get_phi(vA_phi_min)
259 | phi_max = get_phi(vA_phi_max)
260 | if abs(phi_max - phi_min) <= pi:
261 | vA_opti = vA_phi_min
262 | else:
263 | vA_opti = vA_phi_max
264 | return vA_opti
265 |
266 |
267 | def computeNeighbors(agent, kdTree):
268 | agent.neighbors.clear()
269 |
270 | # Check obstacle neighbors.
271 | rangeSq = agent.neighborDist ** 2
272 | if len(agent.neighbors) != agent.maxNeighbors:
273 | rangeSq = 1.0 * agent.neighborDist ** 2
274 | kdTree.computeObstacleNeighbors(agent, rangeSq)
275 |
276 | if agent.in_collision:
277 | return
278 |
279 | # Check other agents.
280 | if len(agent.neighbors) != agent.maxNeighbors:
281 | rangeSq = agent.neighborDist ** 2
282 | kdTree.computeAgentNeighbors(agent, rangeSq)
283 |
284 |
285 | def to_unicycle(vA_post, agent):
286 | vA_post = np.array(vA_post)
287 | norm_vA = norm(vA_post)
288 | yaw_next = mod2pi(atan2(vA_post[1], vA_post[0]))
289 | yaw_current = mod2pi(agent.heading_global_frame)
290 | delta_theta = yaw_next - yaw_current
291 | delta_theta = pi_2_pi(delta_theta)
292 | if delta_theta < -pi:
293 | delta_theta = delta_theta + 2 * pi
294 | if delta_theta > pi:
295 | delta_theta = delta_theta - 2 * pi
296 | if delta_theta >= 1.0:
297 | delta_theta = 1.0
298 | if delta_theta <= -1:
299 | delta_theta = -1.0
300 | action = np.array([norm_vA, delta_theta])
301 | return action
302 |
--------------------------------------------------------------------------------
/mamp/policies/pcaPolicy/pcaPolicy.py:
--------------------------------------------------------------------------------
1 | import time
2 | import numpy as np
3 | from math import sqrt, sin, cos, atan2, pi, acos
4 | from itertools import product
5 |
6 | from mamp.envs import Config
7 | from mamp.policies.policy import Policy
8 | from mamp.policies.pcaPolicy import dubinsmaneuver2d
9 | from mamp.util import sqr, l2norm, norm, is_parallel, absSq, mod2pi, pi_2_pi, get_phi, angle_2_vectors, is_intersect
10 |
11 |
12 | class PCAPolicy(Policy):
13 | def __init__(self):
14 | Policy.__init__(self, str="PCAPolicy")
15 | self.type = "internal"
16 | self.now_goal = None
17 |
18 | def find_next_action(self, obs, dict_comm, agent, kdTree):
19 | ts = time.time()
20 | self.now_goal = agent.goal_global_frame
21 | v_pref = compute_v_pref(agent)
22 | vA = agent.vel_global_frame
23 | pA = agent.pos_global_frame
24 | if l2norm(vA, [0, 0, 0]) <= 1e-5:
25 | vA_post = 0.2 * v_pref
26 | te = time.time()
27 | cost_step = te - ts
28 | agent.total_time += cost_step
29 | action = to_unicycle(vA_post, agent)
30 | theta = 0.0
31 | else:
32 | agent_rad = agent.radius + 0.05
33 | computeNeighbors(agent, kdTree)
34 | RVO_BA_all = []
35 | for obj in agent.neighbors:
36 | obj = obj[0]
37 | pB = obj.pos_global_frame
38 | if obj.is_at_goal:
39 | transl_vB_vA = pA
40 | else:
41 | vB = obj.vel_global_frame
42 | transl_vB_vA = pA + 0.5 * (vB + vA) # Use RVO.
43 | obj_rad = obj.radius + 0.05
44 |
45 | RVO_BA = [transl_vB_vA, pA, pB, obj_rad + agent_rad]
46 | RVO_BA_all.append(RVO_BA)
47 | vA_post = intersect(v_pref, RVO_BA_all, agent)
48 | te = time.time()
49 | cost_step = te - ts
50 | agent.total_time += cost_step
51 | action = to_unicycle(vA_post, agent)
52 | theta = angle_2_vectors(vA, vA_post)
53 | agent.vel_global_unicycle[0] = round(1.0 * action[0], 5)
54 | agent.vel_global_unicycle[1] = round(0.22 * 0.5 * action[1] / Config.DT, 5)
55 | dist = l2norm(agent.pos_global_frame, agent.goal_global_frame)
56 | if theta > agent.max_heading_change:
57 | print('agent' + str(agent.id), len(agent.neighbors), action, '终点距离:', dist, 'theta:', theta)
58 | else:
59 | print('agent' + str(agent.id), len(agent.neighbors), action, '终点距离:', dist)
60 |
61 | return action
62 |
63 |
64 | def update_dubins(agent):
65 | dis = l2norm(agent.pos_global_frame, agent.dubins_now_goal)
66 | sampling_size = agent.dubins_sampling_size
67 | if dis <= sampling_size:
68 | if agent.dubins_path:
69 | agent.dubins_now_goal = np.array(agent.dubins_path.pop()[:2], dtype='float64')
70 | else:
71 | agent.dubins_now_goal = agent.goal_global_frame
72 |
73 |
74 | def compute_dubins(agent):
75 | qi = np.hstack((agent.pos_global_frame, agent.heading_global_frame))
76 | qf = np.hstack((agent.goal_global_frame, agent.goal_heading_frame))
77 | rmin, pitchlims = agent.turning_radius, agent.pitchlims
78 | maneuver = dubinsmaneuver2d.dubins_path_planning(qi, qf, rmin)
79 | dubins_path = maneuver.path.copy()
80 | agent.dubins_sampling_size = maneuver.sampling_size
81 | desire_length = maneuver.length
82 | points_num = len(dubins_path)
83 | path = []
84 | for i in range(points_num):
85 | path.append((dubins_path.pop()))
86 | return path, desire_length, points_num
87 |
88 |
89 | def is_parallel_neighbor(agent):
90 | range_para = 0.15
91 | max_parallel_steps = int((agent.radius + agent.neighbors[0][0].radius + 0.2) / (agent.pref_speed * agent.timeStep))
92 | if len(agent.is_parallel_neighbor) < max_parallel_steps and len(agent.neighbors) > 0:
93 | agent.is_parallel_neighbor.append((agent.neighbors[0][0].id, agent.neighbors[0][1]))
94 | return False
95 | else:
96 | agent.is_parallel_neighbor.pop(0)
97 | agent.is_parallel_neighbor.append((agent.neighbors[0][0].id, agent.neighbors[0][1]))
98 | nei = agent.is_parallel_neighbor
99 | is_same_id = True
100 | is_in_range = True
101 | for i in range(len(nei) - 1):
102 | if nei[0][0] != nei[i + 1][0]:
103 | is_same_id = False
104 | dis = abs(sqrt(nei[0][1]) - sqrt(nei[i + 1][1]))
105 | if dis > range_para:
106 | is_in_range = False
107 | if is_same_id and is_in_range:
108 | return True
109 | else:
110 | return False
111 |
112 |
113 | def dubins_path_node_pop(agent):
114 | agent.dubins_last_goal = np.array(agent.dubins_path.pop()[:3], dtype='float64')
115 | agent.dubins_last_goal = np.array(agent.dubins_path.pop()[:3], dtype='float64')
116 | agent.dubins_last_goal = np.array(agent.dubins_path.pop()[:3], dtype='float64')
117 | agent.dubins_last_goal = np.array(agent.dubins_path.pop()[:3], dtype='float64')
118 |
119 |
120 | def compute_v_pref(agent):
121 | """
122 | is_parallel(vA, v_pref): —— Whether to leave Dubins trajectory as collision avoidance.
123 | dis_goal <= k: —— Regard as obstacles-free when the distance is less than k.
124 | dis < 6 * sampling_size: —— Follow the current Dubins path when not moving away from the current Dubins path.
125 | theta >= np.deg2rad(100): —— Avoid the agent moving away from the goal position after update Dubins path.
126 | """
127 | pA = agent.pos_global_frame
128 | pG = agent.goal_global_frame
129 | dist_goal = l2norm(pA, pG)
130 | k = 3.0 * agent.turning_radius
131 | if not agent.is_use_dubins: # first
132 | agent.is_use_dubins = True
133 | dubins_path, desire_length, points_num = compute_dubins(agent)
134 | agent.dubins_path = dubins_path
135 | dubins_path_node_pop(agent)
136 | agent.dubins_now_goal = np.array(agent.dubins_path.pop()[:2], dtype='float64')
137 | dif_x = agent.dubins_now_goal - pA
138 | else:
139 | update_dubins(agent)
140 | vA = np.array(agent.vel_global_frame)
141 | v_pref = agent.v_pref
142 | dis = l2norm(pA, agent.dubins_now_goal)
143 | max_size = round(6 * agent.dubins_sampling_size, 5)
144 | pApG = pG - pA
145 | theta = angle_2_vectors(vA, pApG)
146 | deg135 = np.deg2rad(135)
147 | if ((is_parallel(vA, v_pref) or dist_goal <= k) and dis < max_size) or (theta >= deg135):
148 | update_dubins(agent)
149 | if agent.dubins_path:
150 | dif_x = agent.dubins_now_goal - pA
151 | else:
152 | dif_x = pG - pA
153 | else:
154 | dubins_path, length, points_num = compute_dubins(agent)
155 | agent.dubins_path = dubins_path
156 | dubins_path_node_pop(agent)
157 | agent.dubins_now_goal = np.array(agent.dubins_path.pop()[:2], dtype='float64')
158 | dif_x = agent.dubins_now_goal - pA
159 |
160 | norm_dif_x = dif_x * agent.pref_speed / l2norm(dif_x, [0, 0])
161 | v_pref = np.array(norm_dif_x)
162 | if dist_goal < Config.NEAR_GOAL_THRESHOLD:
163 | v_pref[0] = 0.0
164 | v_pref[1] = 0.0
165 | agent.v_pref = v_pref
166 | v_pref = np.array([v_pref[0], v_pref[1]])
167 | return v_pref
168 |
169 |
170 | def intersect(v_pref, RVO_BA_all, agent):
171 | norm_v = agent.pref_speed
172 | suitable_V = []
173 | unsuitable_V = []
174 |
175 | Theta = np.arange(0, 2 * pi, step=pi / 32)
176 | RAD = np.linspace(0.02, norm_v, num=5)
177 | v_list = [v_pref[:]]
178 | for theta, rad in product(Theta, RAD):
179 | new_v = np.array([cos(theta), sin(theta)]) * rad
180 | new_v = np.array([new_v[0], new_v[1]])
181 | v_list.append(new_v)
182 |
183 | for new_v in v_list:
184 | suit = True
185 | if len(RVO_BA_all) == 0:
186 | if not satisfied_constraint(agent, new_v):
187 | suit = False
188 | for RVO_BA in RVO_BA_all:
189 | p_0 = RVO_BA[0]
190 | pA = RVO_BA[1]
191 | pB = RVO_BA[2]
192 | combined_radius = RVO_BA[3]
193 | v_dif = np.array(new_v + pA - p_0) # new_v-0.5*(vA+vB) or new_v-vB
194 | if is_intersect(pA, pB, combined_radius, v_dif) or not satisfied_constraint(agent, new_v):
195 | suit = False
196 | break
197 | if suit:
198 | suitable_V.append(new_v)
199 | else:
200 | unsuitable_V.append(new_v)
201 |
202 | # ----------------------
203 | if agent.pref_speed >= 0.44: # Adjust this parameter according to agents' preferred speed.
204 | delta_vopt = 1e-1
205 | else:
206 | delta_vopt = 1e-2
207 |
208 | if suitable_V:
209 | suitable_V.sort(key=lambda v: l2norm(v, v_pref)) # Sort begin at minimum and end at maximum.
210 | if len(suitable_V) > 1:
211 | vA_post = select_right_side(suitable_V, agent.vel_global_frame, epsilon=delta_vopt, opt_num=6)
212 | else:
213 | vA_post = suitable_V[0]
214 | else:
215 | tc_V = dict()
216 | for unsuit_v in unsuitable_V:
217 | tc_V[tuple(unsuit_v)] = 0
218 | tc = [] # 时间
219 | for RVO_BA in RVO_BA_all:
220 | p_0 = RVO_BA[0]
221 | pA = RVO_BA[1]
222 | pB = RVO_BA[2]
223 | combined_radius = RVO_BA[3]
224 | v_dif = np.array(unsuit_v + pA - p_0)
225 | pApB = pB - pA
226 | if is_intersect(pA, pB, combined_radius, v_dif) and satisfied_constraint(agent, unsuit_v):
227 | discr = sqr(np.dot(v_dif, pApB)) - absSq(v_dif) * (absSq(pApB) - sqr(combined_radius))
228 | if discr < 0.0:
229 | print(discr)
230 | continue
231 | tc_v = max((np.dot(v_dif, pApB) - sqrt(discr)) / absSq(v_dif), 0.0)
232 | tc.append(tc_v)
233 | if len(tc) == 0:
234 | tc = [0.0]
235 | tc_V[tuple(unsuit_v)] = min(tc) + 1e-5
236 | WT = agent.safetyFactor
237 | unsuitable_V.sort(key=lambda v: ((WT / tc_V[tuple(v)]) + l2norm(v, v_pref)))
238 | if len(unsuitable_V) > 1:
239 | vA_post = select_right_side(unsuitable_V, agent.vel_global_frame, epsilon=delta_vopt, opt_num=6)
240 | else:
241 | vA_post = unsuitable_V[0]
242 |
243 | return vA_post
244 |
245 |
246 | def satisfied_constraint(agent, vCand):
247 | vA = agent.vel_global_frame
248 | costheta = np.dot(vA, vCand) / (np.linalg.norm(vA) * np.linalg.norm(vCand))
249 | if costheta > 1.0:
250 | costheta = 1.0
251 | elif costheta < -1.0:
252 | costheta = -1.0
253 | theta = acos(costheta) # Rotational constraints.
254 | if theta <= agent.max_heading_change:
255 | return True
256 | else:
257 | return False
258 |
259 |
260 | def select_right_side(v_list, vA, epsilon=1e-5, opt_num=4):
261 | opt_v = [v_list[0]]
262 | i = 1
263 | while True:
264 | if abs(l2norm(v_list[0], vA) - l2norm(v_list[i], vA)) < epsilon:
265 | opt_v.append(v_list[i])
266 | i += 1
267 | if i == len(v_list) or i == opt_num:
268 | break
269 | else:
270 | break
271 | vA_phi_min = min(opt_v, key=lambda v: get_phi(v))
272 | vA_phi_max = max(opt_v, key=lambda v: get_phi(v))
273 | phi_min = get_phi(vA_phi_min)
274 | phi_max = get_phi(vA_phi_max)
275 | if abs(phi_max - phi_min) <= pi:
276 | vA_opti = vA_phi_min
277 | else:
278 | vA_opti = vA_phi_max
279 | return vA_opti
280 |
281 |
282 | def computeNeighbors(agent, kdTree):
283 | agent.neighbors.clear()
284 |
285 | # Check obstacle neighbors.
286 | rangeSq = agent.neighborDist ** 2
287 | if len(agent.neighbors) != agent.maxNeighbors:
288 | rangeSq = 1.0 * agent.neighborDist ** 2
289 | kdTree.computeObstacleNeighbors(agent, rangeSq)
290 |
291 | if agent.in_collision:
292 | return
293 |
294 | # Check other agents.
295 | if len(agent.neighbors) != agent.maxNeighbors:
296 | rangeSq = agent.neighborDist ** 2
297 | kdTree.computeAgentNeighbors(agent, rangeSq)
298 |
299 |
300 | def to_unicycle(vA_post, agent):
301 | vA_post = np.array(vA_post)
302 | norm_vA = norm(vA_post)
303 | yaw_next = mod2pi(atan2(vA_post[1], vA_post[0]))
304 | yaw_current = mod2pi(agent.heading_global_frame)
305 | delta_theta = yaw_next - yaw_current
306 | delta_theta = pi_2_pi(delta_theta)
307 | if delta_theta < -pi:
308 | delta_theta = delta_theta + 2 * pi
309 | if delta_theta > pi:
310 | delta_theta = delta_theta - 2 * pi
311 | if delta_theta >= 1.0:
312 | delta_theta = 1.0
313 | if delta_theta <= -1:
314 | delta_theta = -1.0
315 | action = np.array([norm_vA, delta_theta])
316 | return action
317 |
--------------------------------------------------------------------------------
/mamp/util.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | import matplotlib.pyplot as plt
3 | from math import cos, sin, sqrt, atan2, acos, asin
4 | eps = 1e5
5 |
6 |
7 | def is_intersect(pA, pB, combined_radius, vAB):
8 | pApB = pB - pA
9 | dist_pAB = l2norm(pA, pB)
10 | if dist_pAB <= combined_radius:
11 | dist_pAB = combined_radius
12 | theta_pABBound = asin(combined_radius / dist_pAB)
13 | if (dist_pAB * np.linalg.norm(vAB)) == 0.0: # The v_dif is zero vector
14 | return False
15 |
16 | theta_pABvAB = angle_2_vectors(pApB, vAB)
17 | if theta_pABBound < theta_pABvAB: # No intersecting or tangent.
18 | return False
19 | else:
20 | return True
21 |
22 |
23 | def angle_2_vectors(v1, v2):
24 | v1v2_norm = (np.linalg.norm(v1) * np.linalg.norm(v2))
25 | if v1v2_norm == 0.0:
26 | v1v2_norm = 1e-5
27 | cosdv = np.dot(v1, v2) / v1v2_norm
28 | if cosdv > 1.0:
29 | cosdv = 1.0
30 | elif cosdv < -1.0:
31 | cosdv = -1.0
32 | else:
33 | cosdv = cosdv
34 | angle = acos(cosdv)
35 | return angle
36 |
37 |
38 | def reached(p1, p2, bound=0.5):
39 | if l2norm(p1, p2) < bound:
40 | return True
41 | else:
42 | return False
43 |
44 |
45 | def is_in_between(theta_right, vec_AB, theta_v, theta_left):
46 | """
47 | 判断一条射线（速度）在不在一个角度范围内
48 | """
49 | b_left = [cos(theta_left), sin(theta_left)]
50 | b_right = [cos(theta_right), sin(theta_right)]
51 | v = [cos(theta_v), sin(theta_v)]
52 | norm_b1 = norm(b_left + vec_AB)
53 | norm_b2 = norm(b_right + vec_AB)
54 | norm_bV = norm(v + vec_AB)
55 | return norm_bV >= norm_b1 or norm_bV >= norm_b2
56 |
57 |
58 | def absSq(vec):
59 | return np.dot(vec, vec)
60 |
61 |
62 | def left_of_line(p, p1, p2):
63 | tmpx = (p1[0] - p2[0]) / (p1[1] - p2[1]) * (p[1] - p2[1]) + p2[0]
64 | if tmpx > p[0]:
65 | return True
66 | else:
67 | return False
68 |
69 |
70 | def cross(p1, p2, p3): # 跨立实验
71 | x1 = p2[0] - p1[0]
72 | y1 = p2[1] - p1[1]
73 | x2 = p3[0] - p1[0]
74 | y2 = p3[1] - p1[1]
75 | return x1 * y2 - x2 * y1
76 |
77 |
78 | def IsIntersec(p1, p2, p3, p4): # 判断两线段是否相交
79 | # 快速排斥，以l1、l2为对角线的矩形必相交，否则两线段不相交
80 | if (max(p1[0], p2[0]) >= min(p3[0], p4[0]) # 矩形1最右端大于矩形2最左端
81 | and max(p3[0], p4[0]) >= min(p1[0], p2[0]) # 矩形2最右端大于矩形最左端
82 | and max(p1[1], p2[1]) >= min(p3[1], p4[1]) # 矩形1最高端大于矩形最低端
83 | and max(p3[1], p4[1]) >= min(p1[1], p2[1])): # 矩形2最高端大于矩形最低端
84 |
85 | # 若通过快速排斥则进行跨立实验
86 | if (cross(p1, p2, p3) * cross(p1, p2, p4) <= 0
87 | and cross(p3, p4, p1) * cross(p3, p4, p2) <= 0):
88 | D = True
89 | else:
90 | D = False
91 | else:
92 | D = False
93 | return D
94 |
95 |
96 | """
97 | 用于列表排序，指定每个数组第二个元素
98 | Example:
99 | a=[(2, 2), (3, 4), (4, 1), (1, 3)]
100 | a.sort(key=takeSecond)
101 | """
102 |
103 |
104 | def takeSecond(elem):
105 | return elem[1]
106 |
107 |
108 | def sqr(a):
109 | return a ** 2
110 |
111 |
112 | def leftOf(a, b, c):
113 | return det(a - c, b - a) #
114 |
115 |
116 | def det(p, q):
117 | return p[0] * q[1] - p[1] * q[0]
118 |
119 |
120 | def is_parallel(vec1, vec2):
121 | """ 判断二个向量是否平行 """
122 | assert vec1.shape == vec2.shape, r'输入的参数 shape 必须相同'
123 | norm_vec1 = np.linalg.norm(vec1)
124 | norm_vec2 = np.linalg.norm(vec2)
125 | vec1_normalized = vec1 / norm_vec1
126 | vec2_normalized = vec2 / norm_vec2
127 | if norm_vec1 <= 1e-3 or norm_vec2 <= 1e-3:
128 | return True
129 | elif 1.0 - abs(np.dot(vec1_normalized, vec2_normalized)) < 1e-3:
130 | return True
131 | else:
132 | return False
133 |
134 |
135 | def get_phi(vec): # 计算两个向量投影后的夹角, 返回值是0-2*pi
136 | phi = mod2pi(atan2(vec[1], vec[0]))
137 | return int(phi*eps)/eps
138 |
139 |
140 | def pi_2_pi(angle): # to -pi-pi
141 | return int(((angle + np.pi) % (2 * np.pi) - np.pi)*eps)/eps
142 |
143 |
144 | def mod2pi(theta): # to 0-2*pi
145 | return int((theta - 2.0 * np.pi * np.floor(theta / 2.0 / np.pi))*eps)/eps
146 |
147 |
148 | def norm(vec):
149 | return int(np.linalg.norm(vec)*eps)/eps
150 |
151 |
152 | def normalize(vec):
153 | return vec / np.linalg.norm(vec)
154 |
155 |
156 | def l2norm(x, y):
157 | return int(sqrt(l2normsq(x, y))*eps)/eps
158 |
159 |
160 | def l2normsq(x, y):
161 | return (x[0] - y[0]) ** 2 + (x[1] - y[1]) ** 2
162 |
163 |
164 | def plot_learning_curve(x, scores, figure_file, time, title='average reward'):
165 | running_avg = np.zeros(len(scores))
166 | for i in range(len(running_avg)):
167 | running_avg[i] = np.mean(scores[max(0, i - time):(i + 1)])
168 | plt.plot(x, running_avg)
169 | plt.title(title)
170 | plt.savefig(figure_file)
171 |
172 |
173 | ##################
174 | # Utils
175 | ################
176 |
177 | def compute_time_to_impact(host_pos, other_pos, host_vel, other_vel, combined_radius):
178 | # http://www.ambrsoft.com/TrigoCalc/Circles2/CirclePoint/CirclePointDistance.htm
179 | v_rel = host_vel - other_vel
180 | coll_cone_vec1, coll_cone_vec2 = tangent_vecs_from_external_pt(host_pos[0],
181 | host_pos[1],
182 | other_pos[0],
183 | other_pos[1],
184 | combined_radius)
185 |
186 | if coll_cone_vec1 is None:
187 | # collision already occurred ==> collision cone isn't meaningful anymore
188 | return 0.0
189 | else:
190 | # check if v_rel btwn coll_cone_vecs
191 | # (B btwn A, C): https://stackoverflow.com/questions/13640931/how-to-determine-if-a-vector-is-between-two-other-vectors)
192 |
193 | if (np.cross(coll_cone_vec1, v_rel) * np.cross(coll_cone_vec1, coll_cone_vec2) >= 0 and
194 | np.cross(coll_cone_vec2, v_rel) * np.cross(coll_cone_vec2, coll_cone_vec1) >= 0):
195 | # quadratic eqn for soln to line from host agent pos along v_rel vector to collision circle
196 | # circle: (x-a)**2 + (y-b)**2 = r**2
197 | # line: y = v1/v0 *(x-px) + py
198 | # solve for x: (x-a)**2 + ((v1/v0)*(x-px)+py-a)**2 = r**2
199 | v0, v1 = v_rel
200 | if abs(v0) < 1e-5 and abs(v1) < 1e-5:
201 | # agents aren't moving toward each other ==> inf TTC
202 | return np.inf
203 |
204 | px, py = host_pos
205 | a, b = other_pos
206 | r = combined_radius
207 | if abs(v0) < 1e-5: # vertical v_rel (solve for y, x known)
208 | print("[warning] v0=0, and not yet handled")
209 | x1 = x2 = px
210 | A = 1
211 | B = -2 * b
212 | C = b ** 2 + (px - a) ** 2 - r ** 2
213 | y1 = (-B + np.sqrt(B ** 2 - 4 * A * C)) / (2 * A)
214 | y2 = (-B - np.sqrt(B ** 2 - 4 * A * C)) / (2 * A)
215 | else: # non-vertical v_rel (solve for x)
216 | A = 1 + (v1 / v0) ** 2
217 | B = -2 * a + 2 * (v1 / v0) * (py - b - (v1 / v0) * px)
218 | C = a ** 2 - r ** 2 + ((v1 / v0) * px - (py - b)) ** 2
219 |
220 | det = B ** 2 - 4 * A * C
221 | if det == 0:
222 | print("[warning] det == 0, so only one tangent pt")
223 | elif det < 0:
224 | print("[warning] det < 0, so no tangent pts...")
225 |
226 | x1 = (-B + np.sqrt(B ** 2 - 4 * A * C)) / (2 * A)
227 | x2 = (-B - np.sqrt(B ** 2 - 4 * A * C)) / (2 * A)
228 | y1 = (v1 / v0) * (x1 - px) + py
229 | y2 = (v1 / v0) * (x2 - px) + py
230 |
231 | d1 = np.linalg.norm([x1 - px, y1 - py])
232 | d2 = np.linalg.norm([x2 - px, y2 - py])
233 | d = min(d1, d2)
234 | spd = np.linalg.norm(v_rel)
235 | return d / spd
236 | else:
237 | return np.inf
238 |
239 |
240 | def tangent_vecs_from_external_pt(xp, yp, a, b, r):
241 | # http://www.ambrsoft.com/TrigoCalc/Circles2/CirclePoint/CirclePointDistance.htm
242 | # (xp, yp) is coords of pt outside of circle
243 | # (x-a)**2 + (y-b)**2 = r**2 is defn of circle
244 |
245 | sq_dist_to_perimeter = (xp - a) ** 2 + (yp - b) ** 2 - r ** 2
246 | if sq_dist_to_perimeter < 0:
247 | # print("sq_dist_to_perimeter < 0 ==> agent center is already within coll zone??")
248 | return None, None
249 |
250 | sqrt_term = np.sqrt((xp - a) ** 2 + (yp - b) ** 2 - r ** 2)
251 | xnum1 = r ** 2 * (xp - a)
252 | xnum2 = r * (yp - b) * sqrt_term
253 |
254 | ynum1 = r ** 2 * (yp - b)
255 | ynum2 = r * (xp - a) * sqrt_term
256 |
257 | den = (xp - a) ** 2 + (yp - b) ** 2
258 |
259 | # pt1, pt2 are the tangent pts on the circle perimeter
260 | pt1 = np.array([(xnum1 + xnum2) / den + a, (ynum1 - ynum2) / den + b])
261 | pt2 = np.array([(xnum1 - xnum2) / den + a, (ynum1 + ynum2) / den + b])
262 |
263 | # vec1, vec2 are the vecs from (xp,yp) to the tangent pts on the circle perimeter
264 | vec1 = pt1 - np.array([xp, yp])
265 | vec2 = pt2 - np.array([xp, yp])
266 |
267 | return vec1, vec2
268 |
269 |
270 | def vec2_l2_norm(vec):
271 | # return np.linalg.norm(vec)
272 | return sqrt(vec2_l2_norm_squared(vec))
273 |
274 |
275 | def vec2_l2_norm_squared(vec):
276 | return vec[0] ** 2 + vec[1] ** 2
277 |
278 |
279 | # speed filter
280 | # input: past velocity in (x,y) at time_past
281 | # output: filtered velocity in (speed, theta)
282 | def filter_vel(dt_vec, agent_past_vel_xy):
283 | average_x = np.sum(dt_vec * agent_past_vel_xy[:, 0]) / np.sum(dt_vec)
284 | average_y = np.sum(dt_vec * agent_past_vel_xy[:, 1]) / np.sum(dt_vec)
285 | speeds = np.linalg.norm(agent_past_vel_xy, axis=1)
286 | speed = np.linalg.norm(np.array([average_x, average_y]))
287 | angle = np.arctan2(average_y, average_x)
288 |
289 | return np.array([speed, angle])
290 |
291 |
292 | # angle_1 - angle_2
293 | # contains direction in range [-3.14, 3.14]
294 | def find_angle_diff(angle_1, angle_2):
295 | angle_diff_raw = angle_1 - angle_2
296 | angle_diff = (angle_diff_raw + np.pi) % (2 * np.pi) - np.pi
297 | return angle_diff
298 |
299 |
300 | # keep angle between [-pi, pi]
301 | def wrap(angle):
302 | while angle >= np.pi:
303 | angle -= 2 * np.pi
304 | while angle < -np.pi:
305 | angle += 2 * np.pi
306 | return angle
307 |
308 |
309 | def find_nearest(array, value):
310 | # array is a 1D np array
311 | # value is an scalar or 1D np array
312 | tiled_value = np.tile(np.expand_dims(value, axis=0).transpose(), (1, np.shape(array)[0]))
313 | idx = (np.abs(array - tiled_value)).argmin(axis=1)
314 | return array[idx], idx
315 |
316 |
317 | def rad2deg(rad):
318 | return rad * 180 / np.pi
319 |
320 |
321 | def rgba2rgb(rgba):
322 | # rgba is a list of 4 color elements btwn [0.0, 1.0]
323 | # or a 2d np array (num_colors, 4)
324 | # returns a list of rgb values between [0.0, 1.0] accounting for alpha and background color [1, 1, 1] == WHITE
325 | if isinstance(rgba, list):
326 | alpha = rgba[3]
327 | r = max(min((1 - alpha) * 1.0 + alpha * rgba[0], 1.0), 0.0)
328 | g = max(min((1 - alpha) * 1.0 + alpha * rgba[1], 1.0), 0.0)
329 | b = max(min((1 - alpha) * 1.0 + alpha * rgba[2], 1.0), 0.0)
330 | return [r, g, b]
331 | elif rgba.ndim == 2:
332 | alphas = rgba[:, 3]
333 | r = np.clip((1 - alphas) * 1.0 + alphas * rgba[:, 0], 0, 1)
334 | g = np.clip((1 - alphas) * 1.0 + alphas * rgba[:, 1], 0, 1)
335 | b = np.clip((1 - alphas) * 1.0 + alphas * rgba[:, 2], 0, 1)
336 | return np.vstack([r, g, b]).T
337 |
338 |
339 | def yaw_to_quaternion(yaw):
340 | pitch = 0;
341 | roll = 0
342 | cy = np.cos(yaw * 0.5)
343 | sy = np.sin(yaw * 0.5)
344 | cp = np.cos(pitch * 0.5)
345 | sp = np.sin(pitch * 0.5)
346 | cr = np.cos(roll * 0.5)
347 | sr = np.sin(roll * 0.5)
348 |
349 | qw = cy * cp * cr + sy * sp * sr
350 | qx = cy * cp * sr - sy * sp * cr
351 | qy = sy * cp * sr + cy * sp * cr
352 | qz = sy * cp * cr - cy * sp * sr
353 | return qx, qy, qz, qw
354 |
355 |
356 | def run_episode(env, one_env):
357 | total_reward = 0
358 | step = 0
359 | done = False
360 | while not done:
361 | obs, rew, done, info = env.step([None])
362 | total_reward += rew[0]
363 | step += 1
364 |
365 | # After end of episode, store some statistics about the environment
366 | # Some stats apply to every gym env...
367 | generic_episode_stats = {'total_reward': total_reward, 'steps': step, }
368 |
369 | agents = one_env.agents
370 | # agents = one_env.prev_episode_agents
371 | time_to_goal = np.array([a.t for a in agents])
372 | extra_time_to_goal = np.array([a.t - a.straight_line_time_to_reach_goal for a in agents])
373 | collision = np.array(np.any([a.in_collision for a in agents])).tolist()
374 | all_at_goal = np.array(np.all([a.is_at_goal for a in agents])).tolist()
375 | any_stuck = np.array(np.any([not a.in_collision and not a.is_at_goal for a in agents])).tolist()
376 | outcome = "collision" if collision else "all_at_goal" if all_at_goal else "stuck"
377 | specific_episode_stats = {'num_agents': len(agents), 'time_to_goal': time_to_goal,
378 | 'total_time_to_goal': np.sum(time_to_goal),
379 | 'extra_time_to_goal': extra_time_to_goal, 'collision': collision,
380 | 'all_at_goal': all_at_goal, 'any_stuck': any_stuck, 'outcome': outcome,
381 | 'policies': [agent.policy.str for agent in agents], }
382 |
383 | # Merge all stats into a single dict
384 | episode_stats = {**generic_episode_stats, **specific_episode_stats}
385 |
386 | env.reset()
387 |
388 | return episode_stats, agents
389 |
390 |
391 | def store_stats(df, hyperparameters, episode_stats):
392 | # Add a new row to the pandas DataFrame (a table of results, where each row is an episode)
393 | # that contains the hyperparams and stats from that episode, for logging purposes
394 | df_columns = {**hyperparameters, **episode_stats}
395 | df = df.append(df_columns, ignore_index=True)
396 | return df
397 |
--------------------------------------------------------------------------------
/mamp/policies/orcaPolicy.py:
--------------------------------------------------------------------------------
1 | import time
2 | import numpy as np
3 | from math import sqrt, sin, cos, atan2, asin, pi, floor, acos, asin
4 | from itertools import combinations, product
5 |
6 | from mamp.envs import Config
7 | from mamp.util import absSq, l2norm, norm, sqr, det, mod2pi, pi_2_pi, normalize, angle_2_vectors
8 | from mamp.policies.policy import Policy
9 |
10 |
11 | class Line(object):
12 | def __init__(self):
13 | self.direction = np.array([0.0, 1.0]) # The direction of the directed line.
14 | self.point = np.array([0.0, 0.0]) # A point on the directed line.
15 |
16 |
17 | class ORCAPolicy(Policy):
18 | def __init__(self):
19 | Policy.__init__(self, str="ORCAPolicy")
20 | self.type = "internal"
21 | self.now_goal = None
22 | self.epsilon = 1e-5
23 | self.orcaLines = []
24 | self.newVelocity = np.array([0.0, 0.0])
25 |
26 | def find_next_action(self, obs, dict_comm, agent, kdTree):
27 | ts = time.time()
28 | self.now_goal = agent.goal_global_frame
29 | self.orcaLines.clear()
30 | self.newVelocity = np.array([0.0, 0.0])
31 | invTimeHorizonObst = 1.0 / agent.timeHorizonObst
32 | invTimeHorizon = 1.0 / agent.timeHorizon
33 | v_pref = compute_v_pref(agent)
34 | computeNeighbors(agent, kdTree)
35 |
36 | for obj in agent.neighbors:
37 | obj = obj[0]
38 | relativePosition = obj.pos_global_frame - agent.pos_global_frame
39 | relativeVelocity = agent.vel_global_frame - obj.vel_global_frame
40 | distSq = absSq(relativePosition)
41 | agent_rad = agent.radius + 0.05
42 | obj_rad = obj.radius + 0.05
43 | combinedRadius = agent_rad + obj_rad
44 | combinedRadiusSq = sqr(combinedRadius)
45 |
46 | line = Line()
47 |
48 | if distSq > combinedRadiusSq: # No collision.
49 | w = relativeVelocity - invTimeHorizon * relativePosition
50 | # Vector from cutoff center to relative velocity.
51 | wLengthSq = absSq(w)
52 |
53 | dotProduct1 = np.dot(w, relativePosition)
54 |
55 | if dotProduct1 < 0.0 and sqr(dotProduct1) > combinedRadiusSq * wLengthSq:
56 | # Project on cut-off circle.
57 | wLength = sqrt(wLengthSq)
58 | unitW = w / wLength
59 |
60 | line.direction = np.array([unitW[1], -unitW[0]])
61 | u = (combinedRadius * invTimeHorizon - wLength) * unitW
62 | else:
63 | # Project on legs.
64 | leg = sqrt(distSq - combinedRadiusSq)
65 |
66 | if det(relativePosition, w) > 0.0 > 0.0:
67 | # Project on left leg.
68 | x = relativePosition[0] * leg - relativePosition[1] * combinedRadius
69 | y = relativePosition[0] * combinedRadius + relativePosition[1] * leg
70 | line.direction = np.array([x, y]) / distSq
71 | else:
72 | # Project on right leg.
73 | x = relativePosition[0] * leg + relativePosition[1] * combinedRadius
74 | y = -relativePosition[0] * combinedRadius + relativePosition[1] * leg
75 | line.direction = -np.array([x, y]) / distSq
76 |
77 | dotProduct2 = np.dot(relativeVelocity, line.direction)
78 |
79 | u = dotProduct2 * line.direction - relativeVelocity
80 | else:
81 | # Collision.Project on cut - off circle of time timeStep.
82 | invTimeStep = 1.0 / agent.timeStep
83 |
84 | # Vector from cutoff center to relative velocity.
85 | w = relativeVelocity - invTimeStep * relativePosition
86 | wLength = norm(w)
87 | unitW = w / wLength
88 |
89 | line.direction = np.array([unitW[1], -unitW[0]])
90 | u = (combinedRadius * invTimeStep - wLength) * unitW
91 |
92 | line.point = agent.vel_global_frame + 0.5 * u
93 | self.orcaLines.append(line)
94 |
95 | lineFail = self.linearProgram2(self.orcaLines, agent.maxSpeed, v_pref, False)
96 |
97 | if lineFail < len(self.orcaLines):
98 | self.linearProgram3(self.orcaLines, 0, lineFail, agent.maxSpeed)
99 | # print(222, self.newVelocity)
100 |
101 | vA_post = np.array([self.newVelocity[0], self.newVelocity[1]])
102 | vA = agent.vel_global_frame
103 | te = time.time()
104 | cost_step = te - ts
105 | agent.total_time += cost_step
106 | action = to_unicycle(vA_post, agent)
107 | agent.vel_global_unicycle[0] = round(1.0 * action[0], 5)
108 | agent.vel_global_unicycle[1] = round(0.22 * 0.5 * action[1] / Config.DT, 5)
109 | theta = angle_2_vectors(vA, vA_post)
110 | dist = l2norm(agent.pos_global_frame, agent.goal_global_frame)
111 | if theta > agent.max_heading_change:
112 | print('agent' + str(agent.id), len(agent.neighbors), action, '终点距离:', dist, 'theta:', theta)
113 | else:
114 | print('agent' + str(agent.id), len(agent.neighbors), action, '终点距离:', dist)
115 |
116 | return action
117 |
118 | def linearProgram1(self, lines, lineNo, radius, optVelocity, directionOpt):
119 | """
120 | Solves a one-dimensional linear program on a specified line subject to linear
121 | constraints defined by lines and a circular constraint.
122 | Args:
123 | lines (list): Lines defining the linear constraints.
124 | lineNo (int): The specified line constraint.
125 | radius (float): The radius of the circular constraint.
126 | optVelocity (Vector2): The optimization velocity.
127 | directionOpt (bool): True if the direction should be optimized.
128 | Returns:
129 | bool: True if successful.
130 | Vector2: A reference to the result of the linear program.
131 | """
132 | dotProduct = np.dot(lines[lineNo].point, lines[lineNo].direction)
133 | discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo].point)
134 |
135 | if discriminant < 0.0:
136 | # Max speed circle fully invalidates line lineNo.
137 | return False
138 |
139 | sqrtDiscriminant = sqrt(discriminant)
140 | tLeft = -dotProduct - sqrtDiscriminant
141 | tRight = -dotProduct + sqrtDiscriminant
142 |
143 | for i in range(lineNo):
144 | denominator = det(lines[lineNo].direction, lines[i].direction)
145 | numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point)
146 |
147 | if abs(denominator) <= self.epsilon:
148 | # Lines lineNo and i are (almost) parallel.
149 | if numerator < 0.0:
150 | return False
151 | else:
152 | continue
153 |
154 | t = numerator / denominator
155 |
156 | if denominator >= 0.0:
157 | # Line i bounds line lineNo on the right.
158 | tRight = min(tRight, t)
159 | else:
160 | # * Line i bounds line lineNo on the left.
161 | tLeft = max(tLeft, t)
162 |
163 | if tLeft > tRight:
164 | return False
165 |
166 | if directionOpt:
167 | # Optimize direction.
168 | if np.dot(optVelocity, lines[lineNo].direction) > 0.0:
169 | # Take right extreme.
170 | self.newVelocity = lines[lineNo].point + tRight * lines[lineNo].direction
171 | else:
172 | # Take left extreme.
173 | self.newVelocity = lines[lineNo].point + tLeft * lines[lineNo].direction
174 | else:
175 | # Optimize closest point.
176 | t = np.dot(lines[lineNo].direction, optVelocity - lines[lineNo].point)
177 |
178 | if t < tLeft:
179 | self.newVelocity = lines[lineNo].point + tLeft * lines[lineNo].direction
180 | elif t > tRight:
181 | self.newVelocity = lines[lineNo].point + tRight * lines[lineNo].direction
182 | else:
183 | self.newVelocity = lines[lineNo].point + t * lines[lineNo].direction
184 |
185 | return True
186 |
187 | def linearProgram2(self, lines, radius, optVelocity, directionOpt):
188 | """
189 | Solves a two-dimensional linear program subject to linear constraints defined by
190 | lines and a circular constraint.
191 | Args:
192 | lines (list): Lines defining the linear constraints.
193 | radius (float): The radius of the circular constraint.
194 | optVelocity (Vector2): The optimization velocity.
195 | directionOpt (bool): True if the direction should be optimized.
196 | Returns:
197 | int: The number of the line it fails on, and the number of lines if successful.
198 | Vector2: A reference to the result of the linear program.
199 | """
200 | if directionOpt:
201 | # Optimize direction. Note that the optimization velocity is of unit length in this case.
202 | self.newVelocity = optVelocity * radius
203 | elif absSq(optVelocity) > sqr(radius):
204 | # Optimize closest point and outside circle.
205 | self.newVelocity = normalize(optVelocity) * radius
206 | else:
207 | # Optimize closest point and inside circle.
208 | self.newVelocity = optVelocity
209 | for i in range(len(lines)):
210 | if det(lines[i].direction, lines[i].point - self.newVelocity) > 0.0:
211 | # Result does not satisfy constraint i.Compute new optimal result.
212 | tempResult = self.newVelocity
213 |
214 | if not self.linearProgram1(lines, i, radius, optVelocity, directionOpt):
215 | self.newVelocity = tempResult
216 | return i
217 |
218 | return len(lines)
219 |
220 | def linearProgram3(self, lines, numObstLines, beginLine, radius):
221 | """
222 | Solves a two-dimensional linear program subject to linear constraints defined by lines and a circular constraint.
223 | Args:
224 | lines (list): Lines defining the linear constraints.
225 | numObstLines (int): Count of obstacle lines.
226 | beginLine (int): The line on which the 2-d linear program failed.
227 | radius (float): The radius of the circular constraint.
228 | """
229 | distance = 0.0
230 |
231 | for i in range(beginLine, len(lines)):
232 | if det(lines[i].direction, lines[i].point - self.newVelocity) > distance:
233 | # Result does not satisfy constraint of line i.
234 | projLines = []
235 | for j in range(numObstLines):
236 | projLines.append(lines[j])
237 |
238 | for j in range(numObstLines, i):
239 | line = Line()
240 | determinant = det(lines[i].direction, lines[j].direction)
241 |
242 | if abs(determinant) <= self.epsilon:
243 | # Line i and line j are parallel.
244 | if np.dot(lines[i].direction, lines[j].direction) > 0.0:
245 | # Line i and line j point in the same direction.
246 | continue
247 | else:
248 | # Line i and line j point in opposite direction.
249 | line.point = 0.5 * (lines[i].point + lines[j].point)
250 | else:
251 | p1 = (det(lines[j].direction, lines[i].point - lines[j].point) / determinant)
252 | line.point = lines[i].point + p1 * lines[i].direction
253 |
254 | line.direction = normalize(lines[j].direction - lines[i].direction)
255 | projLines.append(line)
256 |
257 | tempResult = self.newVelocity
258 | optVelocity = np.array([-lines[i].direction[1], lines[i].direction[0]])
259 | if self.linearProgram2(projLines, radius, optVelocity, True) < len(projLines):
260 | """
261 | This should in principle not happen. The result is by definition already
262 | in the feasible region of this linear program. If it fails, it is due to
263 | small floating point error, and the current result is kept.
264 | """
265 | self.newVelocity = tempResult
266 |
267 | distance = det(lines[i].direction, lines[i].point - self.newVelocity)
268 |
269 |
270 | def compute_v_pref(agent):
271 | goal = agent.goal_global_frame
272 | diff = goal - agent.pos_global_frame
273 | v_pref = agent.pref_speed * normalize(diff)
274 | if l2norm(goal, agent.pos_global_frame) < Config.NEAR_GOAL_THRESHOLD:
275 | v_pref = np.zeros_like(v_pref)
276 | return v_pref
277 |
278 |
279 | def computeNeighbors(agent, kdTree):
280 | agent.neighbors.clear()
281 |
282 | # check obstacle neighbors
283 | rangeSq = agent.neighborDist ** 2
284 | if len(agent.neighbors) != agent.maxNeighbors:
285 | rangeSq = 1.0 * agent.neighborDist ** 2
286 | kdTree.computeObstacleNeighbors(agent, rangeSq)
287 |
288 | if agent.in_collision:
289 | return
290 |
291 | # check other agents
292 | if len(agent.neighbors) != agent.maxNeighbors:
293 | rangeSq = agent.neighborDist ** 2
294 | kdTree.computeAgentNeighbors(agent, rangeSq)
295 |
296 |
297 | def to_unicycle(vA_post, agent):
298 | vA_post = np.array(vA_post)
299 | norm_vA = norm(vA_post)
300 | yaw_next = mod2pi(atan2(vA_post[1], vA_post[0]))
301 | yaw_current = mod2pi(agent.heading_global_frame)
302 | delta_theta = yaw_next - yaw_current
303 | delta_theta = pi_2_pi(delta_theta)
304 | if delta_theta < -pi:
305 | delta_theta = delta_theta + 2 * pi
306 | if delta_theta > pi:
307 | delta_theta = delta_theta - 2 * pi
308 | if delta_theta >= 1.0:
309 | delta_theta = 1.0
310 | if delta_theta <= -1:
311 | delta_theta = -1.0
312 | action = np.array([norm_vA, delta_theta])
313 | return action
314 |
315 |
--------------------------------------------------------------------------------