├── .gitignore ├── LICENSE ├── PathPlanning ├── AStar │ ├── AStar.png │ ├── AStar.py │ ├── README.md │ ├── __init__.py │ └── run.py ├── DStar │ ├── DStar.py │ ├── __init__.py │ └── run.py ├── Dijkstra │ ├── Dijkstra.png │ ├── Dijkstra.py │ ├── README.md │ ├── __init__.py │ └── run.py ├── DynamicWindowApproach │ ├── DynamicWindowApproach.py │ ├── __init__.py │ └── run.py ├── HybridAStar │ ├── HybridAStar.py │ ├── __init__.py │ └── run.py ├── PotentialField │ ├── PotentialField.png │ ├── PotentialField.py │ ├── __init__.py │ └── run.py ├── Qlearning │ ├── RL_brain.py │ ├── main.py │ ├── qlearning.gif │ └── view.qml ├── RRT │ ├── README.md │ ├── RRT.png │ ├── RRT.py │ ├── __init__.py │ └── run.py ├── RRTConnect │ ├── README.md │ ├── RRTConnect.py │ ├── __init__.py │ └── run.py ├── RRTStar │ ├── RRTStar.png │ ├── RRTStar.py │ ├── __init__.py │ └── run.py ├── RRTStarSmart │ ├── RRTStarSmart.png │ ├── RRTStarSmart.py │ ├── __init__.py │ └── run.py ├── __init__.py ├── geometry.py ├── icon.png ├── render.py └── utils.py ├── README.md ├── requirements.txt └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | 106 | # pycharm 107 | .idea/ 108 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 0AQZ0 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 | -------------------------------------------------------------------------------- /PathPlanning/AStar/AStar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0aqz0/Robotics-Notebook/f94dfa1f1a76e6148e073cf0b3929975628d6f3c/PathPlanning/AStar/AStar.png -------------------------------------------------------------------------------- /PathPlanning/AStar/AStar.py: -------------------------------------------------------------------------------- 1 | """ 2 | A* path planning implementation with python 3 | """ 4 | from PathPlanning.utils import * 5 | 6 | class AStarPlanner(PathPlanner): 7 | def __init__(self, map, iterations=1e4, step_size=5, heuristic_dist='Manhattan'): 8 | PathPlanner.__init__(self) 9 | self.map = map 10 | self.iterations = iterations 11 | self.step_size = step_size 12 | self.heuristic_dist = heuristic_dist 13 | self.motions = [ 14 | Vector(1, 0), # right cost: 1 15 | Vector(0, 1), # up cost: 1 16 | Vector(-1, 0), # left cost: 1 17 | Vector(0, -1), # down cost: 1 18 | Vector(-1, -1), # left and down cost: 2^0.5 19 | Vector(-1, 1), # left and up cost: 2^0.5 20 | Vector(1, -1), # right and down cost: 2^0.5 21 | Vector(1, 1), # right and up cost: 2^0.5 22 | ] 23 | self.open_list = [] 24 | self.close_list = [] 25 | 26 | def plan(self, start, target): 27 | self.open_list = [] 28 | self.close_list = [] 29 | self.open_list.append(Node(start)) 30 | 31 | for iteration in range(int(self.iterations)): 32 | current_node = min(self.open_list, key=lambda node: node.cost + self.heuristic_func(pos=node.pos, target=target, mode=self.heuristic_dist)) 33 | 34 | if current_node.pos.dist(target) < self.step_size: 35 | print("final") 36 | finalNode = Node(pos=target, parent=current_node, cost=current_node.cost + current_node.pos.dist(target)) 37 | self.close_list.append(finalNode) 38 | self.generate_final_path() 39 | break 40 | 41 | # remove from open list 42 | self.open_list.remove(current_node) 43 | # add to close list 44 | self.close_list.append(current_node) 45 | # explore 46 | for motion in self.motions: 47 | newNode = Node(pos=current_node.pos + motion*self.step_size, parent=current_node, cost=current_node.cost + motion.mod()*self.step_size) 48 | 49 | # in close list 50 | if self.find_in_close_list(newNode): 51 | continue 52 | # outside the map 53 | if self.map.out_of_map(newNode.pos): 54 | continue 55 | # meet obstacle 56 | if self.check_obstacle(newNode.pos): 57 | continue 58 | 59 | # add to open list 60 | if self.find_in_open_list(newNode): 61 | sameNode = self.find_in_open_list(newNode) 62 | if sameNode.cost > newNode.cost: 63 | sameNode = newNode 64 | else: 65 | self.open_list.append(newNode) 66 | 67 | def heuristic_func(self, pos, target, mode='Manhattan'): 68 | if mode == 'Manhattan': 69 | return abs(pos.x - target.x) + abs(pos.y - target.y) 70 | elif mode == 'Euclidean': 71 | return pos.dist(target) 72 | elif mode == 'Chebyshev': 73 | return max(abs(pos.x - target.x), abs(pos.y - target.y)) 74 | else: 75 | raise ValueError('Wrong mode') 76 | 77 | def find_in_close_list(self, node): 78 | for candidate in self.close_list: 79 | if candidate.pos == node.pos: 80 | return candidate 81 | return None 82 | 83 | def find_in_open_list(self, node): 84 | for candidate in self.open_list: 85 | if candidate.pos == node.pos: 86 | return candidate 87 | return None 88 | 89 | def check_obstacle(self, pos): 90 | for obs in self.map.obstacles: 91 | if obs.check_collision(pos, 10): 92 | return True 93 | return False 94 | 95 | def generate_final_path(self): 96 | self.finalPath = [] 97 | self.finalPath.append(self.close_list[-1].pos) 98 | currentNode = self.close_list[-1] 99 | while currentNode.parent is not None: 100 | currentNode = currentNode.parent 101 | self.finalPath.append(currentNode.pos) 102 | self.finalPath.reverse() -------------------------------------------------------------------------------- /PathPlanning/AStar/README.md: -------------------------------------------------------------------------------- 1 | # A* implementation with Python 2 | 3 | Wikipedia explanation: [A* algorithm](https://en.wikipedia.org/wiki/A*_search_algorithm) 4 | 5 | F(n) = G + H 6 | 7 | Heuristics Function: Manhattan, Euclidean or Chebyshev 8 | 9 | 10 | 11 | ## Exploration 12 | 13 | - less time complexity 14 | -------------------------------------------------------------------------------- /PathPlanning/AStar/__init__.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.AStar.AStar import AStarPlanner -------------------------------------------------------------------------------- /PathPlanning/AStar/run.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.AStar import AStarPlanner 2 | from PathPlanning.utils import * 3 | 4 | def main(): 5 | map = Map(top=480, down=0, left=0, right=640) 6 | map.add_obstacle(CircleObstacle(pos=Point(500, 300), radius=50)) 7 | map.add_obstacle(CircleObstacle(pos=Point(100, 100), radius=45)) 8 | map.add_obstacle(RectangleObstacle(top=480, down=200, left=300, right=350)) 9 | astarPlanner = AStarPlanner(map=map, step_size=30, heuristic_dist='Euclidean') 10 | start = Point(0, 0) 11 | end = Point(640, 480) 12 | astarPlanner.plan(start=start, target=end) 13 | while map.is_open: 14 | map.add_geometry(type='point', pos=start.tuple(), size=30, color=(100, 0, 0)) 15 | map.add_geometry(type='point', pos=end.tuple(), size = 30, color=(0, 100, 0)) 16 | for i in range(len(astarPlanner.finalPath)-1): 17 | map.add_geometry(type='line', start=astarPlanner.finalPath[i].tuple(), end=astarPlanner.finalPath[i+1].tuple(), color=(0, 100, 0)) 18 | map.render() 19 | 20 | if __name__ == '__main__': 21 | main() -------------------------------------------------------------------------------- /PathPlanning/DStar/DStar.py: -------------------------------------------------------------------------------- 1 | """ 2 | D* path planning algorithm 3 | """ 4 | from PathPlanning.utils import * 5 | 6 | class DStarPlanner(PathPlanner): 7 | def __init__(self): 8 | DStarPlanner.__init__(self) 9 | 10 | def plan(self, start, target): 11 | pass -------------------------------------------------------------------------------- /PathPlanning/DStar/__init__.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.DStar.DStar import DStarPlanner -------------------------------------------------------------------------------- /PathPlanning/DStar/run.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0aqz0/Robotics-Notebook/f94dfa1f1a76e6148e073cf0b3929975628d6f3c/PathPlanning/DStar/run.py -------------------------------------------------------------------------------- /PathPlanning/Dijkstra/Dijkstra.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0aqz0/Robotics-Notebook/f94dfa1f1a76e6148e073cf0b3929975628d6f3c/PathPlanning/Dijkstra/Dijkstra.png -------------------------------------------------------------------------------- /PathPlanning/Dijkstra/Dijkstra.py: -------------------------------------------------------------------------------- 1 | """ 2 | Dijkstra path planning implementation with python 3 | """ 4 | from PathPlanning.utils import * 5 | 6 | class DijkstraPlanner(PathPlanner): 7 | def __init__(self, map, iterations=1e4, step_size=10): 8 | PathPlanner.__init__(self) 9 | self.map = map 10 | self.iterations = iterations 11 | self.step_size = step_size 12 | self.motions = [ 13 | Vector(1, 0), # right cost: 1 14 | Vector(0, 1), # up cost: 1 15 | Vector(-1, 0), # left cost: 1 16 | Vector(0, -1), # down cost: 1 17 | Vector(-1, -1), # left and down cost: 2^0.5 18 | Vector(-1, 1), # left and up cost: 2^0.5 19 | Vector(1, -1), # right and down cost: 2^0.5 20 | Vector(1, 1), # right and up cost: 2^0.5 21 | ] 22 | self.open_list = [] 23 | self.close_list = [] 24 | 25 | def plan(self, start, target): 26 | self.open_list = [] 27 | self.close_list = [] 28 | self.open_list.append(Node(start)) 29 | 30 | for iteration in range(int(self.iterations)): 31 | current_node = min(self.open_list, 32 | key=lambda node: node.cost) 33 | 34 | if current_node.pos.dist(target) < self.step_size: 35 | print("final") 36 | finalNode = Node(pos=target, parent=current_node, 37 | cost=current_node.cost + current_node.pos.dist(target)) 38 | self.close_list.append(finalNode) 39 | self.generate_final_path() 40 | break 41 | 42 | # remove from open list 43 | self.open_list.remove(current_node) 44 | # add to close list 45 | self.close_list.append(current_node) 46 | # explore 47 | for motion in self.motions: 48 | newNode = Node(pos=current_node.pos + motion * self.step_size, parent=current_node, 49 | cost=current_node.cost + motion.mod() * self.step_size) 50 | 51 | # in close list 52 | if self.find_in_close_list(newNode): 53 | continue 54 | # outside the map 55 | if self.map.out_of_map(newNode.pos): 56 | continue 57 | # meet obstacle 58 | if self.check_obstacle(newNode.pos): 59 | continue 60 | 61 | # add to open list 62 | if self.find_in_open_list(newNode): 63 | sameNode = self.find_in_open_list(newNode) 64 | if sameNode.cost > newNode.cost: 65 | sameNode = newNode 66 | else: 67 | self.open_list.append(newNode) 68 | 69 | def find_in_close_list(self, node): 70 | for candidate in self.close_list: 71 | if candidate.pos == node.pos: 72 | return candidate 73 | return None 74 | 75 | def find_in_open_list(self, node): 76 | for candidate in self.open_list: 77 | if candidate.pos == node.pos: 78 | return candidate 79 | return None 80 | 81 | def check_obstacle(self, pos): 82 | for obs in self.map.obstacles: 83 | if obs.check_collision(pos, 10): 84 | return True 85 | return False 86 | 87 | def generate_final_path(self): 88 | self.finalPath = [] 89 | self.finalPath.append(self.close_list[-1].pos) 90 | currentNode = self.close_list[-1] 91 | while currentNode.parent is not None: 92 | currentNode = currentNode.parent 93 | self.finalPath.append(currentNode.pos) 94 | self.finalPath.reverse() -------------------------------------------------------------------------------- /PathPlanning/Dijkstra/README.md: -------------------------------------------------------------------------------- 1 | # Dijkstra implementation with Python 2 | 3 | Wikipedia explanation: [Dijkstra](https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm) 4 | 5 | Path planning: F(n) = G 6 | 7 | 8 | 9 | ## Exploration 10 | 11 | - less time complexity 12 | -------------------------------------------------------------------------------- /PathPlanning/Dijkstra/__init__.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.Dijkstra.Dijkstra import DijkstraPlanner -------------------------------------------------------------------------------- /PathPlanning/Dijkstra/run.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.Dijkstra import DijkstraPlanner 2 | from PathPlanning.utils import * 3 | 4 | def main(): 5 | map = Map(top=480, down=0, left=0, right=640) 6 | map.add_obstacle(CircleObstacle(pos=Point(400, 300), radius=80)) 7 | map.add_obstacle(CircleObstacle(pos=Point(300, 100), radius=30)) 8 | map.add_obstacle(RectangleObstacle(top=150, down=100, left=0, right=200)) 9 | dijkstraPlanner = DijkstraPlanner(map=map, step_size=30) 10 | start = Point(0, 0) 11 | end = Point(640, 480) 12 | while map.is_open: 13 | dijkstraPlanner.plan(start=start, target=end) 14 | map.add_geometry(type='point', pos=start.tuple(), size=30, color=(100, 0, 0)) 15 | map.add_geometry(type='point', pos=end.tuple(), size = 30, color=(0, 100, 0)) 16 | for i in range(len(dijkstraPlanner.finalPath)-1): 17 | map.add_geometry(type='line', start=dijkstraPlanner.finalPath[i].tuple(), end=dijkstraPlanner.finalPath[i+1].tuple(), color=(0, 100, 0)) 18 | map.render() 19 | 20 | if __name__ == '__main__': 21 | main() -------------------------------------------------------------------------------- /PathPlanning/DynamicWindowApproach/DynamicWindowApproach.py: -------------------------------------------------------------------------------- 1 | """ 2 | Dynamic Window Approach Path Planning Algorithm 3 | """ 4 | from PathPlanning.utils import * 5 | 6 | class DWAPlanner(PathPlanner): 7 | def __init__(self): 8 | PathPlanner.__init__(self) 9 | 10 | def plan(self, start, target): 11 | pass -------------------------------------------------------------------------------- /PathPlanning/DynamicWindowApproach/__init__.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.DynamicWindowApproach.DynamicWindowApproach import DWAPlanner -------------------------------------------------------------------------------- /PathPlanning/DynamicWindowApproach/run.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0aqz0/Robotics-Notebook/f94dfa1f1a76e6148e073cf0b3929975628d6f3c/PathPlanning/DynamicWindowApproach/run.py -------------------------------------------------------------------------------- /PathPlanning/HybridAStar/HybridAStar.py: -------------------------------------------------------------------------------- 1 | """ 2 | Hybrid AStar Path Planning Algorithm 3 | """ 4 | from PathPlanning.utils import * 5 | 6 | class HybridAStarPlanner(PathPlanner): 7 | def __init__(self): 8 | PathPlanner.__init__(self) 9 | 10 | def plan(self, start, target): 11 | pass -------------------------------------------------------------------------------- /PathPlanning/HybridAStar/__init__.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.HybridAStar.HybridAStar import HybridAStarPlanner -------------------------------------------------------------------------------- /PathPlanning/HybridAStar/run.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0aqz0/Robotics-Notebook/f94dfa1f1a76e6148e073cf0b3929975628d6f3c/PathPlanning/HybridAStar/run.py -------------------------------------------------------------------------------- /PathPlanning/PotentialField/PotentialField.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0aqz0/Robotics-Notebook/f94dfa1f1a76e6148e073cf0b3929975628d6f3c/PathPlanning/PotentialField/PotentialField.png -------------------------------------------------------------------------------- /PathPlanning/PotentialField/PotentialField.py: -------------------------------------------------------------------------------- 1 | """ 2 | Potential Field Path Planning Algorithm 3 | """ 4 | from PathPlanning.utils import * 5 | 6 | class PotentialFieldPlanner(PathPlanner): 7 | def __init__(self, map, iterations=1e4, step_size=2, ka=1, kr=1e4, da=10, dr=100): 8 | PathPlanner.__init__(self) 9 | self.map = map 10 | self.iterations = iterations 11 | self.step_size = step_size 12 | self.motions = [ 13 | Vector(1, 0), # right 14 | Vector(0, 1), # up 15 | Vector(-1, 0), # left 16 | Vector(0, -1), # down 17 | Vector(-1, -1), # left and down 18 | Vector(-1, 1), # left and up 19 | Vector(1, -1), # right and down 20 | Vector(1, 1), # right and up 21 | ] 22 | self.ka = ka 23 | self.kr = kr 24 | self.da = da 25 | self.dr = dr 26 | 27 | def plan(self, start, target): 28 | self.finalPath = [] 29 | self.finalPath.append(start) 30 | for iteration in range(int(self.iterations)): 31 | currentPos = self.finalPath[-1] 32 | if currentPos.dist(target) < self.step_size: 33 | print('final') 34 | break 35 | 36 | lowestPotential = float('inf') 37 | nextPos = currentPos 38 | for motion in self.motions: 39 | newPos = currentPos + motion * self.step_size 40 | newPotential = self.calculate_attractive_potential(newPos, target) + self.calculate_repulsive_potential(newPos) 41 | if newPotential < lowestPotential: 42 | lowestPotential = newPotential 43 | nextPos = newPos 44 | self.finalPath.append(nextPos) 45 | 46 | def calculate_attractive_potential(self, pos, target): 47 | if pos.dist(target) <= self.da: 48 | return self.ka * pos.dist(target) ** 2 49 | else: 50 | return self.ka * (2 * self.da * pos.dist(target) - self.da ** 2) 51 | 52 | def calculate_repulsive_potential(self, pos): 53 | pr = 0 54 | for obs in self.map.obstacles: 55 | if obs.dist(pos) == 0: 56 | return float('inf') 57 | if obs.dist(pos) <= self.dr: 58 | pr = pr + 0.5 * self.kr * (1/obs.dist(pos) - 1/self.dr) ** 2 59 | return pr 60 | 61 | def calculate_potential_force(self, pos, target): 62 | # attractive force(simple version) 63 | attractive_force = Vector(pos.x - target.x, pos.y - target.y) * (-self.ka) 64 | # repulsive force 65 | repulsive_force = Vector(0, 0) 66 | for obs in self.map.obstacles: 67 | if obs.dist(pos) == 0: 68 | repulsive_force += Vector(float('inf'), float('inf')) 69 | elif obs.dist(pos) <= self.dr: 70 | repulsive_force += Vector(pos.x - obs.pos.x, pos.y - obs.pos.y) * self.kr * (1/obs.dist(pos) - 1/self.dr) * ((1/obs.dist(pos))**2) * (1/obs.dist(pos)) 71 | # sum up 72 | potential_force = attractive_force + repulsive_force 73 | 74 | return potential_force 75 | -------------------------------------------------------------------------------- /PathPlanning/PotentialField/__init__.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.PotentialField.PotentialField import PotentialFieldPlanner -------------------------------------------------------------------------------- /PathPlanning/PotentialField/run.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.PotentialField import PotentialFieldPlanner 2 | from PathPlanning.utils import * 3 | 4 | def main(): 5 | map = Map(left=0, right=640, top=480, down=0, refresh=False) 6 | map.add_obstacle(CircleObstacle(pos=Point(200, 200), radius=50)) 7 | map.add_obstacle(CircleObstacle(pos=Point(400, 300), radius=40)) 8 | # map.add_obstacle(CircleObstacle(pos=Point(450, 400), radius=80)) 9 | potentialFieldPlanner = PotentialFieldPlanner(map=map, iterations=1e4, step_size=2, ka=1, kr=1e7, da=10, dr=50) 10 | start = Point(100, 100) 11 | end = Point(500, 250) 12 | potentialFieldPlanner.plan(start=start, target=end) 13 | map.add_geometry(type='point', pos=start.tuple(), size=10, color=(100, 0, 0)) 14 | map.add_geometry(type='point', pos=end.tuple(), size=10, color=(0, 100, 0)) 15 | for i in range(len(potentialFieldPlanner.finalPath)-1): 16 | map.add_geometry(type='line', start=potentialFieldPlanner.finalPath[i].tuple(), end=potentialFieldPlanner.finalPath[i+1].tuple(), color=(0, 100, 0)) 17 | # draw potential force 18 | for x in range(0, map.right+1, 10): 19 | for y in range(0, map.top+1, 10): 20 | force = potentialFieldPlanner.calculate_potential_force(Point(x, y), end) 21 | force_start = Point(x, y) 22 | force_end = force_start + Polar2Vector(5, force.dir()) 23 | # map.add_geometry(type='point', pos=(x,y), size=3, color=(0, 0, 100)) 24 | map.add_geometry(type='line', start=force_start.tuple(), end=force_end.tuple(), width=2, color=(120,50,80)) 25 | # draw potential line 26 | potential_lines = [i for i in range(0, 20000, 1000)] 27 | for x in range(0, map.right+1, 1): 28 | for y in range(0, map.top+1, 1): 29 | valid = True 30 | for obs in map.obstacles: 31 | if obs.dist(Point(x, y)) <= 32: 32 | valid = False 33 | 34 | if valid: 35 | offset = 20 36 | else: 37 | offset = 100 38 | potential = potentialFieldPlanner.calculate_attractive_potential(Point(x, y), end) + potentialFieldPlanner.calculate_repulsive_potential(Point(x, y)) 39 | # print(potential, potentialFieldPlanner.calculate_attractive_potential(Point(x, y), end)) 40 | for p in potential_lines: 41 | if math.fabs(potential-p) < offset: 42 | map.add_geometry(type='point', pos=(x,y), size=1, color=(0, 0, 100)) 43 | 44 | while map.is_open: 45 | map.render() 46 | 47 | if __name__ == '__main__': 48 | main() -------------------------------------------------------------------------------- /PathPlanning/Qlearning/RL_brain.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pandas as pd 3 | 4 | 5 | class QLearningTable: 6 | def __init__(self, actions, learning_rate=0.01, reward_decay=0.9, e_greedy=0.9): 7 | self.actions = actions # a list 8 | self.lr = learning_rate # 学习率 9 | self.gamma = reward_decay # 奖励衰减 10 | self.epsilon = e_greedy # 贪婪度 11 | self.q_table = pd.DataFrame(columns=self.actions, dtype=np.float64) # 初始 q_table 12 | 13 | def choose_action(self, observation): 14 | self.check_state_exist(observation) # 检测本 state 是否在 q_table 中存在(见后面标题内容) 15 | 16 | # 选择 action 17 | if np.random.uniform() < self.epsilon: # 选择 Q value 最高的 action 18 | state_action = self.q_table.loc[observation, :] 19 | 20 | # 同一个 state, 可能会有多个相同的 Q action value, 所以我们乱序一下 21 | action = np.random.choice(state_action[state_action == np.max(state_action)].index) 22 | 23 | else: # 随机选择 action 24 | action = np.random.choice(self.actions) 25 | 26 | return action 27 | 28 | def learn(self, s, a, r, s_): 29 | self.check_state_exist(s_) # 检测 q_table 中是否存在 s_ (见后面标题内容) 30 | # print(s + ' ' + s_) 31 | q_predict = self.q_table.loc[s, a] 32 | if s_ != 'terminal': 33 | q_target = r + self.gamma * self.q_table.loc[s_, :].max() # 下个 state 不是 终止符 34 | else: 35 | q_target = r # 下个 state 是终止符 36 | self.q_table.loc[s, a] += self.lr * (q_target - q_predict) # 更新对应的 state-action 值 37 | 38 | def check_state_exist(self, state): 39 | if state not in self.q_table.index: 40 | # append new state to q table 41 | self.q_table = self.q_table.append( 42 | pd.Series( 43 | [0] * len(self.actions), 44 | index=self.q_table.columns, 45 | name=state, 46 | ) 47 | ) 48 | 49 | -------------------------------------------------------------------------------- /PathPlanning/Qlearning/main.py: -------------------------------------------------------------------------------- 1 | # author : 0aqz0 2 | # date: 2018/11/1 3 | import os 4 | import sys 5 | import time 6 | import _thread 7 | 8 | from PyQt5.QtCore import QObject, QUrl, pyqtSlot 9 | from PyQt5.QtQuick import QQuickView 10 | from PyQt5.QtGui import QGuiApplication 11 | 12 | from RL_brain import QLearningTable 13 | 14 | 15 | class Maze(QObject): 16 | def __init__(self, parent=None): 17 | super().__init__(parent) 18 | # Initialise the value of the properties. 19 | self._robot = [1, 1] 20 | self._start = [1, 1] 21 | self._end = [9, 9] 22 | self._obs = [] 23 | self._maxepisode = 100 24 | self._learningrate = 0.01 25 | self._discountfactor = 0.9 26 | self._egreedy = 0.9 27 | self.action_space = ['u', 'd', 'l', 'r'] 28 | self.n_actions = len(self.action_space) 29 | self.newpathplanning = False 30 | self.finalpath = [] 31 | self.isfinalpath = False 32 | self.currentepisode = 0 33 | self.currentqtable = "" 34 | 35 | @pyqtSlot(int, int) 36 | def setstart(self, x, y): 37 | global root 38 | self._start[0] = x 39 | self._start[1] = y 40 | self._robot = self._start 41 | # print(self._start) 42 | 43 | @pyqtSlot(int, int) 44 | def setend(self, x, y): 45 | global root 46 | self._end[0] = x 47 | self._end[1] = y 48 | # print(self._end) 49 | 50 | @pyqtSlot(int, int) 51 | def setobs(self, x, y): 52 | global root 53 | if [x,y] not in self._obs: 54 | self._obs.append([x,y]) 55 | # print(self._obs) 56 | 57 | @pyqtSlot() 58 | def reset(self): 59 | global root 60 | self._robot = [1, 1] 61 | self._start = [1, 1] 62 | self._end = [9, 9] 63 | self._obs = [] 64 | self.finalpath = [] 65 | self.isfinalpath = False 66 | self.currentepisode = 0 67 | self.currentqtable = "" 68 | root.resetqml() 69 | 70 | @pyqtSlot(bool) 71 | def setnewpathplanning(self, setter): 72 | self.newpathplanning = setter 73 | # print(self.newpathplanning) 74 | 75 | @pyqtSlot(result = str) 76 | def robotstring(self): 77 | return str("(" + str(int(self._robot[0])) + "," + str(int(self._robot[1])) + ")") 78 | 79 | @pyqtSlot(result = str) 80 | def endstring(self): 81 | return str("(" + str(int(self._end[0])) + "," + str(int(self._end[1])) + ")") 82 | 83 | @pyqtSlot(result = 'QStringList') 84 | def obsstring(self): 85 | return [str("(" + str(int(obs[0])) + "," + str(int(obs[1])) + ")") for obs in self._obs] 86 | 87 | @pyqtSlot(result = int) 88 | def obsnum(self): 89 | return len(self._obs) 90 | 91 | @pyqtSlot() 92 | def quit(self): 93 | sys.exit(0) 94 | 95 | @pyqtSlot(int) 96 | def maxepisode(self, maxepisode): 97 | self._maxepisode = maxepisode 98 | # print(self._maxepisode) 99 | 100 | @pyqtSlot(float) 101 | def learningrate(self, learningrate): 102 | self._learningrate = learningrate 103 | # print(self._learningrate) 104 | 105 | @pyqtSlot(float) 106 | def discountfactor(self, discountfactor): 107 | self._discountfactor = discountfactor 108 | # print(self._discountfactor) 109 | 110 | @pyqtSlot(float) 111 | def egreedy(self, egreedy): 112 | self._egreedy = egreedy 113 | # print(self._egreedy) 114 | 115 | # @pyqtSlot() 116 | # def printinfo(self): 117 | # print("updated") 118 | 119 | @pyqtSlot(result = 'QStringList') 120 | def finalpathlist(self): 121 | return self.finalpath 122 | 123 | @pyqtSlot(result = int) 124 | def finalpathlen(self): 125 | return len(self.finalpath) 126 | 127 | @pyqtSlot(result = bool) 128 | def requestisfinalpath(self): 129 | return self.isfinalpath 130 | 131 | @pyqtSlot(result = int) 132 | def requestepisode(self): 133 | return self.currentepisode 134 | 135 | @pyqtSlot(result = "QString") 136 | def requestqtable(self): 137 | return self.currentqtable 138 | 139 | @pyqtSlot() 140 | def go(self): 141 | self.newpathplanning = True 142 | 143 | # observation after action 144 | @pyqtSlot() 145 | def step(self, action): 146 | global root 147 | # next state 148 | next_s = self._robot 149 | if action == 0: 150 | if next_s[1] > 1: 151 | next_s[1] -= 1 152 | elif action == 1: 153 | if next_s[1] < 9: 154 | next_s[1] += 1 155 | elif action == 2: 156 | if next_s[0] > 1: 157 | next_s[0] -= 1 158 | elif action == 3: 159 | if next_s[0] < 9: 160 | next_s[0] += 1 161 | 162 | # move to next state 163 | self._robot = next_s 164 | 165 | # reward 166 | if next_s == self._end: 167 | reward = 10 168 | done = True 169 | next_s = 'terminal' 170 | # print("win") 171 | return next_s, reward, done 172 | # punish 173 | for obs in self._obs: 174 | if next_s == obs: 175 | reward = -10 176 | done = True 177 | next_s = 'terminal' 178 | # print("fail") 179 | return next_s, reward, done 180 | # nothing 181 | reward = -1 182 | done = False 183 | return str(next_s), reward, done 184 | 185 | @pyqtSlot() 186 | def pathplanning(self): 187 | global root 188 | global view 189 | RL = QLearningTable(actions=list(range(self.n_actions)), learning_rate=self._learningrate, 190 | reward_decay=self._discountfactor, e_greedy=self._egreedy) 191 | # update qtable 192 | self.currentqtable = str(RL.q_table) 193 | for episode in range(self._maxepisode): 194 | # update episode 195 | self.currentepisode = episode + 1 196 | 197 | # reset 198 | self._robot = self._start.copy() 199 | # initialize observation 200 | observation = str(self._robot) 201 | time.sleep(1) 202 | 203 | while True: 204 | # record the final path 205 | if(episode == self._maxepisode - 1): 206 | self.finalpath.append(str("(" + str(int(self._robot[0])) + "," + str(int(self._robot[1])) + ")")) 207 | 208 | # choose action 209 | action = RL.choose_action(observation) 210 | # get new observation 211 | next_observation, reward, done = self.step(action) 212 | # learn from this observation 213 | RL.learn(observation,action,reward,next_observation) 214 | # update observation 215 | observation = next_observation 216 | 217 | # update qtable 218 | self.currentqtable = str(RL.q_table) 219 | # sleep for qml's update 220 | time.sleep(0.2) 221 | # print("#######") 222 | if done: 223 | break 224 | # print(self.finalpath) 225 | self.isfinalpath = True 226 | 227 | 228 | if __name__ == '__main__': 229 | # create maze instance 230 | maze = Maze() 231 | # start a new thread 232 | def threadforpathplanning(): 233 | while True: 234 | if maze.newpathplanning: 235 | maze.pathplanning() 236 | maze.newpathplanning = False 237 | time.sleep(0.5) 238 | _thread.start_new_thread(threadforpathplanning,()) 239 | 240 | # create application instance 241 | app = QGuiApplication(sys.argv) 242 | view = QQuickView() 243 | 244 | # register the python type 245 | # qmlRegisterType(Maze, 'Maze', 1, 0, 'Maze') 246 | 247 | qmlFile = os.path.join(os.path.dirname(__file__), 'view.qml') 248 | 249 | context = view.rootContext() 250 | context.setContextProperty("maze", maze) 251 | 252 | view.setSource(QUrl.fromLocalFile(os.path.abspath(qmlFile))) 253 | if view.status() == QQuickView.Error: 254 | sys.exit(-1) 255 | view.show() 256 | 257 | root = view.rootObject() 258 | 259 | res = app.exec_() 260 | 261 | # delete the view 262 | del view 263 | sys.exit(res) 264 | -------------------------------------------------------------------------------- /PathPlanning/Qlearning/qlearning.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0aqz0/Robotics-Notebook/f94dfa1f1a76e6148e073cf0b3929975628d6f3c/PathPlanning/Qlearning/qlearning.gif -------------------------------------------------------------------------------- /PathPlanning/Qlearning/view.qml: -------------------------------------------------------------------------------- 1 | // author : 0aqz0 2 | // date: 2018/11/1 3 | import QtQuick 2.6 4 | import QtQuick.Controls 2.4 5 | 6 | Rectangle{ 7 | id : root 8 | 9 | width : 1400; height: 930 10 | 11 | SystemPalette { id: activePalette } 12 | 13 | // white #ffffff 14 | // red #ff0000 15 | property string redposition 16 | function resetqml(){ 17 | for (var i = 0; i < mazeview.contentItem.children.length - 1; i++){ 18 | mazeview.contentItem.children[i].color = "white" 19 | } 20 | } 21 | // function finalroot(){ 22 | //// console.log(maze.finalpathlen()) 23 | // for (var i = 0; i < maze.finalpathlen(); i++){ 24 | //// console.log(maze.finalpathlist()[i]) 25 | // for (var j = 0; j < mazeview.contentItem.children.length - 1; j++){ 26 | //// console.log(mazeview.contentItem.children[j].id) 27 | // if(mazeview.contentItem.children[j].id == maze.finalpathlist()[i]){ 28 | //// console.log(mazeview.contentItem.children[j].id) 29 | // mazeview.contentItem.children[j].color = "red" 30 | // break 31 | // } 32 | // } 33 | // } 34 | // } 35 | 36 | property int mode : 0 37 | // 1-----set the start 38 | // 2-----set the end 39 | // 3-----set obstacle 40 | // 4-----set new path-planning 41 | // 5-----reset 42 | // 6-----quit 43 | 44 | Rectangle { 45 | id : map 46 | width: 900; height: 900 47 | color: "white" 48 | 49 | ListModel { 50 | id: appModel 51 | ListElement { name: "(1,1)"; colors : "white"; ix : 1; iy : 1 } 52 | ListElement { name: "(1,2)"; colors : "white"; ix : 1; iy : 2 } 53 | ListElement { name: "(1,3)"; colors : "white"; ix : 1; iy : 3 } 54 | ListElement { name: "(1,4)"; colors : "white"; ix : 1; iy : 4 } 55 | ListElement { name: "(1,5)"; colors : "white"; ix : 1; iy : 5 } 56 | ListElement { name: "(1,6)"; colors : "white"; ix : 1; iy : 6 } 57 | ListElement { name: "(1,7)"; colors : "white"; ix : 1; iy : 7 } 58 | ListElement { name: "(1,8)"; colors : "white"; ix : 1; iy : 8 } 59 | ListElement { name: "(1,9)"; colors : "white"; ix : 1; iy : 9 } 60 | 61 | ListElement { name: "(2,1)"; colors : "white"; ix : 2; iy : 1 } 62 | ListElement { name: "(2,2)"; colors : "white"; ix : 2; iy : 2 } 63 | ListElement { name: "(2,3)"; colors : "white"; ix : 2; iy : 3 } 64 | ListElement { name: "(2,4)"; colors : "white"; ix : 2; iy : 4 } 65 | ListElement { name: "(2,5)"; colors : "white"; ix : 2; iy : 5 } 66 | ListElement { name: "(2,6)"; colors : "white"; ix : 2; iy : 6 } 67 | ListElement { name: "(2,7)"; colors : "white"; ix : 2; iy : 7 } 68 | ListElement { name: "(2,8)"; colors : "white"; ix : 2; iy : 8 } 69 | ListElement { name: "(2,9)"; colors : "white"; ix : 2; iy : 9 } 70 | 71 | ListElement { name: "(3,1)"; colors : "white"; ix : 3; iy : 1 } 72 | ListElement { name: "(3,2)"; colors : "white"; ix : 3; iy : 2 } 73 | ListElement { name: "(3,3)"; colors : "white"; ix : 3; iy : 3 } 74 | ListElement { name: "(3,4)"; colors : "white"; ix : 3; iy : 4 } 75 | ListElement { name: "(3,5)"; colors : "white"; ix : 3; iy : 5 } 76 | ListElement { name: "(3,6)"; colors : "white"; ix : 3; iy : 6 } 77 | ListElement { name: "(3,7)"; colors : "white"; ix : 3; iy : 7 } 78 | ListElement { name: "(3,8)"; colors : "white"; ix : 3; iy : 8 } 79 | ListElement { name: "(3,9)"; colors : "white"; ix : 3; iy : 9 } 80 | 81 | ListElement { name: "(4,1)"; colors : "white"; ix : 4; iy : 1 } 82 | ListElement { name: "(4,2)"; colors : "white"; ix : 4; iy : 2 } 83 | ListElement { name: "(4,3)"; colors : "white"; ix : 4; iy : 3 } 84 | ListElement { name: "(4,4)"; colors : "white"; ix : 4; iy : 4 } 85 | ListElement { name: "(4,5)"; colors : "white"; ix : 4; iy : 5 } 86 | ListElement { name: "(4,6)"; colors : "white"; ix : 4; iy : 6 } 87 | ListElement { name: "(4,7)"; colors : "white"; ix : 4; iy : 7 } 88 | ListElement { name: "(4,8)"; colors : "white"; ix : 4; iy : 8 } 89 | ListElement { name: "(4,9)"; colors : "white"; ix : 4; iy : 9 } 90 | 91 | ListElement { name: "(5,1)"; colors : "white"; ix : 5; iy : 1 } 92 | ListElement { name: "(5,2)"; colors : "white"; ix : 5; iy : 2 } 93 | ListElement { name: "(5,3)"; colors : "white"; ix : 5; iy : 3 } 94 | ListElement { name: "(5,4)"; colors : "white"; ix : 5; iy : 4 } 95 | ListElement { name: "(5,5)"; colors : "white"; ix : 5; iy : 5 } 96 | ListElement { name: "(5,6)"; colors : "white"; ix : 5; iy : 6 } 97 | ListElement { name: "(5,7)"; colors : "white"; ix : 5; iy : 7 } 98 | ListElement { name: "(5,8)"; colors : "white"; ix : 5; iy : 8 } 99 | ListElement { name: "(5,9)"; colors : "white"; ix : 5; iy : 9 } 100 | 101 | ListElement { name: "(6,1)"; colors : "white"; ix : 6; iy : 1 } 102 | ListElement { name: "(6,2)"; colors : "white"; ix : 6; iy : 2 } 103 | ListElement { name: "(6,3)"; colors : "white"; ix : 6; iy : 3 } 104 | ListElement { name: "(6,4)"; colors : "white"; ix : 6; iy : 4 } 105 | ListElement { name: "(6,5)"; colors : "white"; ix : 6; iy : 5 } 106 | ListElement { name: "(6,6)"; colors : "white"; ix : 6; iy : 6 } 107 | ListElement { name: "(6,7)"; colors : "white"; ix : 6; iy : 7 } 108 | ListElement { name: "(6,8)"; colors : "white"; ix : 6; iy : 8 } 109 | ListElement { name: "(6,9)"; colors : "white"; ix : 6; iy : 9 } 110 | 111 | ListElement { name: "(7,1)"; colors : "white"; ix : 7; iy : 1 } 112 | ListElement { name: "(7,2)"; colors : "white"; ix : 7; iy : 2 } 113 | ListElement { name: "(7,3)"; colors : "white"; ix : 7; iy : 3 } 114 | ListElement { name: "(7,4)"; colors : "white"; ix : 7; iy : 4 } 115 | ListElement { name: "(7,5)"; colors : "white"; ix : 7; iy : 5 } 116 | ListElement { name: "(7,6)"; colors : "white"; ix : 7; iy : 6 } 117 | ListElement { name: "(7,7)"; colors : "white"; ix : 7; iy : 7 } 118 | ListElement { name: "(7,8)"; colors : "white"; ix : 7; iy : 8 } 119 | ListElement { name: "(7,9)"; colors : "white"; ix : 7; iy : 9 } 120 | 121 | ListElement { name: "(8,1)"; colors : "white"; ix : 8; iy : 1 } 122 | ListElement { name: "(8,2)"; colors : "white"; ix : 8; iy : 2 } 123 | ListElement { name: "(8,3)"; colors : "white"; ix : 8; iy : 3 } 124 | ListElement { name: "(8,4)"; colors : "white"; ix : 8; iy : 4 } 125 | ListElement { name: "(8,5)"; colors : "white"; ix : 8; iy : 5 } 126 | ListElement { name: "(8,6)"; colors : "white"; ix : 8; iy : 6 } 127 | ListElement { name: "(8,7)"; colors : "white"; ix : 8; iy : 7 } 128 | ListElement { name: "(8,8)"; colors : "white"; ix : 8; iy : 8 } 129 | ListElement { name: "(8,9)"; colors : "white"; ix : 8; iy : 9 } 130 | 131 | ListElement { name: "(9,1)"; colors : "white"; ix : 9; iy : 1 } 132 | ListElement { name: "(9,2)"; colors : "white"; ix : 9; iy : 2 } 133 | ListElement { name: "(9,3)"; colors : "white"; ix : 9; iy : 3 } 134 | ListElement { name: "(9,4)"; colors : "white"; ix : 9; iy : 4 } 135 | ListElement { name: "(9,5)"; colors : "white"; ix : 9; iy : 5 } 136 | ListElement { name: "(9,6)"; colors : "white"; ix : 9; iy : 6 } 137 | ListElement { name: "(9,7)"; colors : "white"; ix : 9; iy : 7 } 138 | ListElement { name: "(9,8)"; colors : "white"; ix : 9; iy : 8 } 139 | ListElement { name: "(9,9)"; colors : "white"; ix : 9; iy : 9 } 140 | } 141 | GridView { 142 | id : mazeview 143 | anchors.fill: parent 144 | cellWidth: 100; cellHeight: 100 145 | focus: true 146 | model: appModel 147 | 148 | highlight: Rectangle { width: 100; height: 100; color: "lightsteelblue" } 149 | 150 | delegate: Rectangle { 151 | width: 100; height: 100 152 | color: "white" 153 | border.color : "grey" 154 | property string id : name 155 | // Image { 156 | // width: 100; height: 100 157 | // source: "image/end" 158 | // } 159 | MouseArea { 160 | anchors.fill: parent 161 | onClicked: { 162 | if (root.mode == 1){ 163 | for (var i = 0; i < mazeview.contentItem.children.length - 1; i++){ 164 | if(mazeview.contentItem.children[i].id == maze.robotstring()) 165 | mazeview.contentItem.children[i].color = "white" 166 | } 167 | maze.setstart(ix ,iy) 168 | root.mode = 0; 169 | parent.color = "red" 170 | } 171 | else if (root.mode == 2){ 172 | for (var i = 0; i < mazeview.contentItem.children.length - 1; i++){ 173 | if(mazeview.contentItem.children[i].id == maze.endstring()) 174 | mazeview.contentItem.children[i].color = "white" 175 | } 176 | maze.setend(ix,iy) 177 | root.mode = 0 178 | parent.color = "green" 179 | } 180 | else if (root.mode == 3){ 181 | maze.setobs(ix,iy) 182 | parent.color = "black" 183 | } 184 | } 185 | } 186 | Timer{ 187 | interval: 20; running: true; repeat: true 188 | onTriggered: { 189 | parent.color = "white" 190 | if(parent.id == maze.robotstring()){ 191 | parent.color = "red" 192 | } 193 | if(parent.id == maze.endstring()){ 194 | parent.color = "green" 195 | } 196 | for (var i = 0; i < maze.obsnum(); i++){ 197 | if(parent.id == maze.obsstring()[i]){ 198 | parent.color = "black" 199 | } 200 | } 201 | if(maze.requestisfinalpath()){ 202 | for (var i = 0; i < maze.finalpathlen(); i++){ 203 | if(parent.id == maze.finalpathlist()[i]){ 204 | parent.color = "red" 205 | break 206 | } 207 | } 208 | } 209 | } 210 | } 211 | } 212 | } 213 | } 214 | 215 | Rectangle{ 216 | id : infoBar 217 | width : parent.width; height : 30 218 | color : activePalette.window 219 | anchors.bottom : root.bottom 220 | Text { 221 | id : episodeText 222 | anchors { right : parent.right; verticalCenter : parent.verticalCenter } 223 | color : activePalette.buttonText 224 | text : maze.requestepisode() 225 | } 226 | Timer{ 227 | interval: 20; running: true; repeat: true 228 | onTriggered: { 229 | episodeText.text = maze.requestepisode() 230 | } 231 | } 232 | } 233 | GroupBox{ 234 | id : toolBar 235 | width : 500; height : 500 236 | anchors { right : root.right; top : root.top } 237 | title : "settings" 238 | 239 | Grid{ 240 | height: 200 241 | id : settingListView; 242 | verticalItemAlignment: Grid.AlignVCenter; 243 | horizontalItemAlignment: Grid.AlignLeft; 244 | anchors.horizontalCenter: parent.horizontalCenter; 245 | columnSpacing: 5; 246 | rowSpacing: 30; 247 | columns:2; 248 | rows:3; 249 | Button { 250 | id : newButton 251 | width : 230 252 | text : "New Path-Planning" 253 | onClicked : { 254 | root.mode = 4 255 | console.log(root.mode) 256 | maze.go() 257 | } 258 | } 259 | Button { 260 | id : setStartButton 261 | width : 230 262 | text : "Set Startpoint" 263 | onClicked : { 264 | root.mode = 1 265 | console.log(root.mode) 266 | } 267 | } 268 | Button { 269 | id : setObsButton 270 | width : 230 271 | text : "Set Obstacles" 272 | onClicked : { 273 | root.mode = 3 274 | console.log(root.mode) 275 | } 276 | } 277 | Button { 278 | id : setEndButton 279 | width : 230 280 | text : "Set Endpoint" 281 | onClicked : { 282 | root.mode = 2 283 | console.log(root.mode) 284 | } 285 | } 286 | Button { 287 | id : resetButton 288 | width : 230 289 | text : "Reset All" 290 | onClicked : { 291 | maze.reset() 292 | } 293 | } 294 | Button { 295 | id : quitButton 296 | width : 230 297 | text : "Quit" 298 | onClicked : { 299 | root.mode = 6 300 | console.log(root.mode) 301 | maze.quit() 302 | } 303 | } 304 | } 305 | Grid{ 306 | id : paramListView; 307 | anchors {top: settingListView.bottom} 308 | verticalItemAlignment: Grid.AlignVCenter; 309 | horizontalItemAlignment: Grid.AlignLeft; 310 | anchors.horizontalCenter: parent.horizontalCenter; 311 | columnSpacing: 5; 312 | rowSpacing: 30; 313 | columns:3; 314 | rows:5; 315 | 316 | Text{ 317 | text : "Max Episode: " 318 | } 319 | Slider { 320 | id : episode 321 | from : 0 322 | to : 200 323 | stepSize: 1 324 | value : 100 325 | onValueChanged: { 326 | maze.maxepisode(value.toFixed(0)) 327 | } 328 | } 329 | Text{ 330 | text : episode.value.toFixed(0) 331 | } 332 | 333 | Text{ 334 | text : "Learning rate: " 335 | } 336 | Slider { 337 | id : learningrate 338 | from : 0 339 | to : 1 340 | stepSize: 0.01 341 | value : 0.01 342 | onValueChanged: { 343 | maze.learningrate(value.toFixed(2)) 344 | } 345 | } 346 | Text{ 347 | text : learningrate.value.toFixed(2) 348 | } 349 | 350 | Text{ 351 | text : "Discount factor: " 352 | } 353 | Slider { 354 | id : discountfactor 355 | from : 0 356 | to : 1 357 | stepSize: 0.01 358 | value : 0.9 359 | onValueChanged: { 360 | maze.discountfactor(value.toFixed(2)) 361 | } 362 | } 363 | Text{ 364 | text : discountfactor.value.toFixed(2) 365 | } 366 | 367 | Text{ 368 | text : "E_greedy: " 369 | } 370 | Slider { 371 | id : egreedy 372 | from : 0 373 | to : 1 374 | stepSize: 0.01 375 | value : 0.9 376 | onValueChanged: { 377 | maze.egreedy(value.toFixed(2)) 378 | } 379 | } 380 | Text{ 381 | text : egreedy.value.toFixed(2) 382 | } 383 | } 384 | } 385 | GroupBox{ 386 | id : qtableview 387 | width : 500; height : 390 388 | anchors { right : root.right; bottom : infoBar.top} 389 | title : "Q table" 390 | clip : true 391 | ScrollView{ 392 | width : parent.width; height : 340 393 | anchors { bottom : parent.bottom } 394 | ScrollBar.vertical.policy: ScrollBar.AlwaysOn 395 | clip : true 396 | Text{ 397 | id : qtabletext 398 | text : "Hello, I'm Q table!" 399 | anchors.fill : parent 400 | } 401 | } 402 | Timer{ 403 | interval: 20; running: true; repeat: true 404 | onTriggered: { 405 | qtabletext.text = maze.requestqtable() 406 | } 407 | } 408 | } 409 | } 410 | -------------------------------------------------------------------------------- /PathPlanning/RRT/README.md: -------------------------------------------------------------------------------- 1 | # RRT 2 | 3 | - Init 4 | - Random Sampling 5 | - Find the nearest node 6 | - expand the tree 7 | -------------------------------------------------------------------------------- /PathPlanning/RRT/RRT.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0aqz0/Robotics-Notebook/f94dfa1f1a76e6148e073cf0b3929975628d6f3c/PathPlanning/RRT/RRT.png -------------------------------------------------------------------------------- /PathPlanning/RRT/RRT.py: -------------------------------------------------------------------------------- 1 | """ 2 | RRT path planning implementation with python 3 | """ 4 | from PathPlanning.utils import * 5 | import random 6 | import math 7 | 8 | class RRTPlanner(PathPlanner): 9 | """ 10 | Path Planner using RRT algorithm. 11 | """ 12 | def __init__(self, map, iterations=1e4, epsilon=0.05, stepSize=5): 13 | PathPlanner.__init__(self) 14 | self.nodeList = [] 15 | self.map = map 16 | self.iterations = iterations 17 | self.epsilon = epsilon 18 | self.stepSize = stepSize 19 | 20 | def plan(self, start, target): 21 | self.nodeList = [] 22 | self.nodeList.append(Node(start)) 23 | for iteration in range(int(self.iterations)): 24 | # random sample 25 | randNode = Node(self.randomSample(self.epsilon, target)) 26 | # find the nearest node 27 | nearestNode = self.findNearestNode(randNode) 28 | # expand the tree 29 | theta = math.atan2(randNode.pos.y - nearestNode.pos.y, randNode.pos.x - nearestNode.pos.x) 30 | newNode = Node(nearestNode.pos + Vector(self.stepSize * math.cos(theta), self.stepSize * math.sin(theta)), nearestNode) 31 | 32 | # outside the map 33 | if self.map.out_of_map(newNode.pos): 34 | continue 35 | # in the node list 36 | if self.inNodeList(newNode): 37 | continue 38 | # meet obstacles 39 | if self.check_obstacle(newNode.pos): 40 | continue 41 | 42 | self.nodeList.append(newNode) 43 | if newNode.pos.dist(target) < self.stepSize: 44 | print("final") 45 | self.finalPath = [] 46 | self.finalPath.append(target) 47 | currentNode = self.nodeList[-1] 48 | self.finalPath.append(currentNode.pos) 49 | while currentNode.parent is not None: 50 | self.finalPath.append(currentNode.parent.pos) 51 | currentNode = currentNode.parent 52 | self.finalPath.reverse() 53 | break 54 | 55 | def findNearestNode(self, node): 56 | minDist = 1e10 57 | nearestNode = None 58 | for candidate in self.nodeList: 59 | if node.pos.dist(candidate.pos) < minDist: 60 | minDist = node.pos.dist(candidate.pos) 61 | nearestNode = candidate 62 | return nearestNode 63 | 64 | def randomSample(self, epsilon, target): 65 | if random.random() >= epsilon: 66 | return Point(self.map.left + self.map.length * random.random(), self.map.down + self.map.width * random.random()) 67 | else: 68 | return target 69 | 70 | def inNodeList(self, node): 71 | for candidate in self.nodeList: 72 | if candidate.pos == node.pos: 73 | return True 74 | return False 75 | 76 | def check_obstacle(self, pos): 77 | for obs in self.map.obstacles: 78 | if obs.check_collision(pos, 10): 79 | return True 80 | return False 81 | -------------------------------------------------------------------------------- /PathPlanning/RRT/__init__.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.RRT.RRT import RRTPlanner -------------------------------------------------------------------------------- /PathPlanning/RRT/run.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.RRT import RRTPlanner 2 | from PathPlanning.utils import * 3 | 4 | def main(): 5 | map = Map(top=480, down=0, left=0, right=640) 6 | map.add_obstacle(CircleObstacle(pos=Point(300, 300), radius=50)) 7 | map.add_obstacle(RectangleObstacle(top=300, down=100, left=500, right=550)) 8 | start = Point(0, 0) 9 | end = Point(640, 480) 10 | rrtPlanner = RRTPlanner(map, epsilon=0.05, stepSize=10) 11 | while map.is_open: 12 | rrtPlanner.plan(start=start, target=end) 13 | map.add_geometry(type='point', pos=start.tuple(), size=30, color=(100, 0, 0)) 14 | map.add_geometry(type='point', pos=end.tuple(), size=30, color=(0, 100, 0)) 15 | for node in rrtPlanner.nodeList: 16 | map.add_geometry(type='point', pos=node.pos.tuple()) 17 | if node.parent is not None: 18 | map.add_geometry(type='line', start=node.parent.pos.tuple(), end=node.pos.tuple()) 19 | for i in range(len(rrtPlanner.finalPath)-1): 20 | map.add_geometry(type='line', start=rrtPlanner.finalPath[i].tuple(), end=rrtPlanner.finalPath[i+1].tuple(), color=(0, 100, 0)) 21 | map.render() 22 | 23 | 24 | if __name__ == '__main__': 25 | main() 26 | -------------------------------------------------------------------------------- /PathPlanning/RRTConnect/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0aqz0/Robotics-Notebook/f94dfa1f1a76e6148e073cf0b3929975628d6f3c/PathPlanning/RRTConnect/README.md -------------------------------------------------------------------------------- /PathPlanning/RRTConnect/RRTConnect.py: -------------------------------------------------------------------------------- 1 | """ 2 | RRTConnect path planning implementation with python 3 | """ 4 | from PathPlanning.utils import * 5 | import random 6 | import math 7 | 8 | class RRTConnectPlanner(PathPlanner): 9 | """ 10 | Path Planner using RRTConnect algorithm. 11 | """ 12 | def __init__(self, map, iterations=1e4, epsilon=0.05, stepSize=5): 13 | PathPlanner.__init__(self) 14 | self.nodeList1 = [] 15 | self.nodeList2 = [] 16 | self.map = map 17 | self.iterations = iterations 18 | self.epsilon = epsilon 19 | self.stepSize = stepSize 20 | 21 | def plan(self, start, target): 22 | self.nodeList1 = [] 23 | self.nodeList1.append(Node(start)) 24 | self.nodeList2 = [] 25 | self.nodeList2.append(Node(target)) 26 | for iteration in range(int(self.iterations)): 27 | if iteration%2 == 0: 28 | expandList = self.nodeList1 29 | otherList = self.nodeList2 30 | else: 31 | expandList = self.nodeList2 32 | otherList = self.nodeList1 33 | # random sample 34 | randNode = Node(self.randomSample(self.epsilon, target)) 35 | # find the nearest node 36 | nearestNode = self.findNearestNode(randNode, expandList) 37 | # expand the tree 38 | theta = math.atan2(randNode.pos.y - nearestNode.pos.y, randNode.pos.x - nearestNode.pos.x) 39 | newNode = Node(nearestNode.pos + Vector(self.stepSize * math.cos(theta), self.stepSize * math.sin(theta)), nearestNode) 40 | 41 | # outside the map 42 | if self.map.out_of_map(newNode.pos): 43 | continue 44 | # in the node list 45 | if self.inNodeList(newNode, expandList): 46 | continue 47 | # meet obstacles 48 | if self.check_obstacle(newNode.pos): 49 | continue 50 | 51 | expandList.append(newNode) 52 | checkNode = self.findNearestNode(newNode, otherList) 53 | if newNode.pos.dist(checkNode.pos) < self.stepSize: 54 | print("final") 55 | # path1 56 | finalPath1 = [] 57 | currentNode = newNode 58 | finalPath1.append(currentNode.pos) 59 | while currentNode.parent is not None: 60 | finalPath1.append(currentNode.parent.pos) 61 | currentNode = currentNode.parent 62 | finalPath1.reverse() 63 | # path2 64 | finalPath2 = [] 65 | currentNode = checkNode 66 | finalPath2.append(currentNode.pos) 67 | while currentNode.parent is not None: 68 | finalPath2.append(currentNode.parent.pos) 69 | currentNode = currentNode.parent 70 | 71 | self.finalPath = finalPath1 + finalPath2 72 | break 73 | 74 | def findNearestNode(self, node, nodelist): 75 | minDist = 1e10 76 | nearestNode = None 77 | for candidate in nodelist: 78 | if node.pos.dist(candidate.pos) < minDist: 79 | minDist = node.pos.dist(candidate.pos) 80 | nearestNode = candidate 81 | return nearestNode 82 | 83 | def randomSample(self, epsilon, target): 84 | if random.random() >= epsilon: 85 | return Point(self.map.left + self.map.length * random.random(), self.map.down + self.map.width * random.random()) 86 | else: 87 | return target 88 | 89 | def inNodeList(self, node, nodelist): 90 | for candidate in nodelist: 91 | if candidate.pos == node.pos: 92 | return True 93 | return False 94 | 95 | def check_obstacle(self, pos): 96 | for obs in self.map.obstacles: 97 | if obs.check_collision(pos, 10): 98 | return True 99 | return False 100 | -------------------------------------------------------------------------------- /PathPlanning/RRTConnect/__init__.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.RRTConnect.RRTConnect import RRTConnectPlanner -------------------------------------------------------------------------------- /PathPlanning/RRTConnect/run.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.RRTConnect import RRTConnectPlanner 2 | from PathPlanning.utils import * 3 | 4 | def main(): 5 | map = Map(top=480, down=0, left=0, right=640) 6 | map.add_obstacle(CircleObstacle(pos=Point(300, 300), radius=50)) 7 | map.add_obstacle(RectangleObstacle(top=300, down=100, left=500, right=550)) 8 | start = Point(0, 0) 9 | end = Point(640, 480) 10 | rrtPlanner = RRTConnectPlanner(map, epsilon=0.05, stepSize=10) 11 | while map.is_open: 12 | rrtPlanner.plan(start=start, target=end) 13 | map.add_geometry(type='point', pos=start.tuple(), size=30, color=(100, 0, 0)) 14 | map.add_geometry(type='point', pos=end.tuple(), size=30, color=(0, 100, 0)) 15 | for node in rrtPlanner.nodeList1: 16 | # map.add_geometry(type='point', pos=node.pos.tuple()) 17 | if node.parent is not None: 18 | map.add_geometry(type='line', start=node.parent.pos.tuple(), end=node.pos.tuple(), color=(100, 0, 0)) 19 | for node in rrtPlanner.nodeList2: 20 | # map.add_geometry(type='point', pos=node.pos.tuple()) 21 | if node.parent is not None: 22 | map.add_geometry(type='line', start=node.parent.pos.tuple(), end=node.pos.tuple(), color=(0, 0, 100)) 23 | for i in range(len(rrtPlanner.finalPath)-1): 24 | map.add_geometry(type='line', start=rrtPlanner.finalPath[i].tuple(), end=rrtPlanner.finalPath[i+1].tuple(), color=(0, 100, 0)) 25 | map.render() 26 | 27 | 28 | if __name__ == '__main__': 29 | main() 30 | -------------------------------------------------------------------------------- /PathPlanning/RRTStar/RRTStar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0aqz0/Robotics-Notebook/f94dfa1f1a76e6148e073cf0b3929975628d6f3c/PathPlanning/RRTStar/RRTStar.png -------------------------------------------------------------------------------- /PathPlanning/RRTStar/RRTStar.py: -------------------------------------------------------------------------------- 1 | """ 2 | RRT* path planning implementation with python 3 | """ 4 | from PathPlanning.utils import * 5 | import random 6 | import math 7 | 8 | class RRTStarPlanner(PathPlanner): 9 | """ 10 | Path Planner using RRT* algorithm. 11 | """ 12 | def __init__(self, map, iterations=1e4, epsilon=0.2, stepSize=10): 13 | PathPlanner.__init__(self) 14 | self.nodeList = [] 15 | self.map = map 16 | self.iterations = iterations 17 | self.epsilon = epsilon 18 | self.stepSize = stepSize 19 | 20 | def plan(self, start, target): 21 | self.nodeList = [] 22 | self.nodeList.append(Node(start)) 23 | for iteration in range(int(self.iterations)): 24 | # random sample 25 | randNode = Node(self.randomSample(self.epsilon, target)) 26 | # find the nearest node 27 | nearestNode = self.findNearestNode(randNode) 28 | # expand the tree 29 | theta = math.atan2(randNode.pos.y - nearestNode.pos.y, randNode.pos.x - nearestNode.pos.x) 30 | newNode = Node(nearestNode.pos + Polar2Vector(self.stepSize, theta), nearestNode, nearestNode.cost+self.stepSize) 31 | 32 | # outside the map 33 | if self.map.out_of_map(newNode.pos): 34 | continue 35 | # in the node list 36 | if self.inNodeList(newNode): 37 | continue 38 | # meet obstacles 39 | if self.check_obstacle(newNode.pos): 40 | continue 41 | 42 | # choose best parent 43 | radius = 100.0*math.sqrt(math.log(len(self.nodeList))/len(self.nodeList)) 44 | self.chooseBestParent(newNode, radius) 45 | # rewire 46 | self.rewire(newNode, radius) 47 | 48 | self.nodeList.append(newNode) 49 | if newNode.pos.dist(target) < self.stepSize: 50 | print("final") 51 | self.finalPath = [] 52 | self.finalPath.append(target) 53 | currentNode = self.nodeList[-1] 54 | self.finalPath.append(currentNode.pos) 55 | while currentNode.parent is not None: 56 | self.finalPath.append(currentNode.parent.pos) 57 | currentNode = currentNode.parent 58 | self.finalPath.reverse() 59 | break 60 | 61 | def rewire(self, newNode, radius): 62 | # find near nodes 63 | nearNodes = [nearNode for nearNode in self.nodeList if newNode.dist(nearNode) < radius] 64 | # rewire 65 | for nearNode in nearNodes: 66 | new_cost = newNode.cost + newNode.dist(nearNode) 67 | if new_cost < nearNode.cost: 68 | nearNode.cost = new_cost 69 | nearNode.parent = newNode 70 | 71 | def chooseBestParent(self, newNode, radius): 72 | # find near nodes 73 | nearNodes = [nearNode for nearNode in self.nodeList if newNode.dist(nearNode) < radius] 74 | # choose best parent 75 | for nearNode in nearNodes: 76 | new_cost = nearNode.cost + newNode.dist(nearNode) 77 | if self.check_line_collision(newNode.pos, nearNode.pos): 78 | continue 79 | if new_cost < newNode.cost: 80 | newNode.cost = new_cost 81 | newNode.parent = nearNode 82 | 83 | def findNearestNode(self, node): 84 | minDist = 1e10 85 | nearestNode = None 86 | for candidate in self.nodeList: 87 | if node.pos.dist(candidate.pos) < minDist: 88 | minDist = node.pos.dist(candidate.pos) 89 | nearestNode = candidate 90 | return nearestNode 91 | 92 | def randomSample(self, epsilon, target): 93 | if random.random() >= epsilon: 94 | return Point(self.map.left + self.map.length * random.random(), self.map.down + self.map.width * random.random()) 95 | else: 96 | return target 97 | 98 | def inNodeList(self, node): 99 | for candidate in self.nodeList: 100 | if candidate.pos == node.pos: 101 | return True 102 | return False 103 | 104 | def check_obstacle(self, pos): 105 | for obs in self.map.obstacles: 106 | if obs.check_collision(pos, 10): 107 | return True 108 | return False 109 | 110 | def check_line_collision(self, pos1, pos2): 111 | testNum = pos1.dist(pos2)/10 112 | for i in range(int(testNum)+1): 113 | testPos = pos1 + Polar2Vector(i*10, pos1.dir(pos2)) 114 | if self.check_obstacle(testPos): 115 | return True 116 | return False -------------------------------------------------------------------------------- /PathPlanning/RRTStar/__init__.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.RRTStar.RRTStar import RRTStarPlanner -------------------------------------------------------------------------------- /PathPlanning/RRTStar/run.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.RRTStar import RRTStarPlanner 2 | from PathPlanning.utils import * 3 | 4 | def main(): 5 | map = Map(480, 0, 0, 640) 6 | map.add_obstacle(CircleObstacle(pos=Point(400, 300), radius=50)) 7 | map.add_obstacle(RectangleObstacle(top=200, down=100, left=100, right=300)) 8 | rrtPlanner = RRTStarPlanner(map, epsilon=0.2, stepSize=10) 9 | start = Point(0, 0) 10 | end = Point(640, 480) 11 | while map.is_open: 12 | rrtPlanner.plan(start=start, target=end) 13 | map.add_geometry(type='point', pos=start.tuple(), size=30, color=(100, 0, 0)) 14 | map.add_geometry(type='point', pos=end.tuple(), size=30, color=(0, 100, 0)) 15 | for node in rrtPlanner.nodeList: 16 | map.add_geometry(type='point', pos=node.pos.tuple()) 17 | if node.parent is not None: 18 | map.add_geometry(type='line', start=node.parent.pos.tuple(), end=node.pos.tuple()) 19 | for i in range(len(rrtPlanner.finalPath)-1): 20 | map.add_geometry(type='line', start=rrtPlanner.finalPath[i].tuple(), end=rrtPlanner.finalPath[i+1].tuple(), color=(0, 100, 0)) 21 | map.render() 22 | 23 | 24 | if __name__ == '__main__': 25 | main() -------------------------------------------------------------------------------- /PathPlanning/RRTStarSmart/RRTStarSmart.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0aqz0/Robotics-Notebook/f94dfa1f1a76e6148e073cf0b3929975628d6f3c/PathPlanning/RRTStarSmart/RRTStarSmart.png -------------------------------------------------------------------------------- /PathPlanning/RRTStarSmart/RRTStarSmart.py: -------------------------------------------------------------------------------- 1 | """ 2 | RRT* Smart Path Planning Algorithm 3 | """ 4 | from PathPlanning.utils import * 5 | import random 6 | 7 | class RRTStarSmartPlanner(PathPlanner): 8 | """ 9 | Path Planner using RRT* Smart algorithm. 10 | """ 11 | def __init__(self, map, iterations=1e3, epsilon=0.2, step_size=10, biasing_radius=50, biasing_ratio=3): 12 | PathPlanner.__init__(self) 13 | self.nodeList = [] 14 | self.map = map 15 | self.iterations = iterations 16 | self.epsilon = epsilon 17 | self.step_size = step_size 18 | self.biasing_radius = biasing_radius 19 | self.biasing_ratio = biasing_ratio 20 | self.beacons = [] 21 | self.n = None 22 | self.cost = float('inf') 23 | 24 | def plan(self, start, target): 25 | self.nodeList = [Node(start)] 26 | # self.beacons = [] 27 | # self.n = None 28 | # self.cost = float('inf') 29 | for iteration in range(int(self.iterations)): 30 | # random sample or intelligent sample 31 | if self.n is not None and (iteration - self.n)%self.biasing_ratio is 0: 32 | randNode = Node(self.intelligent_sample()) 33 | else: 34 | randNode = Node(self.randomSample(self.epsilon, target)) 35 | # find the nearest node 36 | nearestNode = self.findNearestNode(randNode) 37 | # expand the tree 38 | theta = math.atan2(randNode.pos.y - nearestNode.pos.y, randNode.pos.x - nearestNode.pos.x) 39 | newNode = Node(nearestNode.pos + Polar2Vector(self.step_size, theta), nearestNode, 40 | nearestNode.cost + self.step_size) 41 | 42 | # outside the map 43 | if self.map.out_of_map(newNode.pos): 44 | continue 45 | # in the node list 46 | if self.inNodeList(newNode): 47 | continue 48 | # meet obstacles 49 | if self.check_obstacle(newNode.pos): 50 | continue 51 | 52 | # choose best parent 53 | radius = 100.0 * math.sqrt(math.log(len(self.nodeList)) / len(self.nodeList)) 54 | self.chooseBestParent(newNode, radius) 55 | # rewire 56 | self.rewire(newNode, radius) 57 | 58 | self.nodeList.append(newNode) 59 | if newNode.pos.dist(target) < self.step_size: 60 | # generate final path 61 | finalPath = [target] 62 | currentNode = self.nodeList[-1] 63 | finalPath.append(currentNode.pos) 64 | while currentNode.parent is not None: 65 | finalPath.append(currentNode.parent.pos) 66 | currentNode = currentNode.parent 67 | finalPath.reverse() 68 | 69 | # find initial path 70 | if len(self.finalPath) is 0: 71 | self.n = iteration 72 | # path optimization 73 | optimized_path, cost = self.path_optimization(finalPath) 74 | # update beacons 75 | if cost < self.cost: 76 | print("better") 77 | self.cost = cost 78 | self.finalPath = optimized_path 79 | self.beacons = self.finalPath 80 | # start next path planning 81 | self.nodeList = [Node(start)] 82 | print('final') 83 | 84 | 85 | def rewire(self, newNode, radius): 86 | # find near nodes 87 | nearNodes = [nearNode for nearNode in self.nodeList if newNode.dist(nearNode) < radius] 88 | # rewire 89 | for nearNode in nearNodes: 90 | new_cost = newNode.cost + newNode.dist(nearNode) 91 | if new_cost < nearNode.cost: 92 | nearNode.cost = new_cost 93 | nearNode.parent = newNode 94 | 95 | def chooseBestParent(self, newNode, radius): 96 | # find near nodes 97 | nearNodes = [nearNode for nearNode in self.nodeList if newNode.dist(nearNode) < radius] 98 | # choose best parent 99 | for nearNode in nearNodes: 100 | new_cost = nearNode.cost + newNode.dist(nearNode) 101 | if self.check_line_collision(newNode.pos, nearNode.pos): 102 | continue 103 | if new_cost < newNode.cost: 104 | newNode.cost = new_cost 105 | newNode.parent = nearNode 106 | 107 | def findNearestNode(self, node): 108 | minDist = 1e10 109 | nearestNode = None 110 | for candidate in self.nodeList: 111 | if node.pos.dist(candidate.pos) < minDist: 112 | minDist = node.pos.dist(candidate.pos) 113 | nearestNode = candidate 114 | return nearestNode 115 | 116 | def randomSample(self, epsilon, target): 117 | if random.random() >= epsilon: 118 | return Point(self.map.left + self.map.length * random.random(), 119 | self.map.down + self.map.width * random.random()) 120 | else: 121 | return target 122 | 123 | def intelligent_sample(self): 124 | samples = random.sample(self.beacons, 1) 125 | beacon = samples[0] 126 | return beacon + Polar2Vector(self.biasing_radius * random.random(), random.uniform(0, 2 * math.pi)) 127 | 128 | def inNodeList(self, node): 129 | for candidate in self.nodeList: 130 | if candidate.pos == node.pos: 131 | return True 132 | return False 133 | 134 | def check_obstacle(self, pos): 135 | for obs in self.map.obstacles: 136 | if obs.check_collision(pos, 5): 137 | return True 138 | return False 139 | 140 | def check_line_collision(self, pos1, pos2): 141 | testNum = pos1.dist(pos2) / 10 142 | for i in range(int(testNum) + 1): 143 | testPos = pos1 + Polar2Vector(i * 10, pos1.dir(pos2)) 144 | if self.check_obstacle(testPos): 145 | return True 146 | return False 147 | 148 | def path_optimization(self, finalPath): 149 | cost = 0 150 | optimized_path = [finalPath[-1]] 151 | parent_index = (len(finalPath) - 1) - 1 152 | while parent_index >= 0: 153 | if parent_index is 0 or self.check_line_collision(optimized_path[-1], finalPath[parent_index-1]): 154 | cost = cost + optimized_path[-1].dist(finalPath[parent_index]) 155 | optimized_path.append(finalPath[parent_index]) 156 | parent_index = parent_index - 1 157 | optimized_path.reverse() 158 | return optimized_path, cost -------------------------------------------------------------------------------- /PathPlanning/RRTStarSmart/__init__.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.RRTStarSmart.RRTStarSmart import RRTStarSmartPlanner -------------------------------------------------------------------------------- /PathPlanning/RRTStarSmart/run.py: -------------------------------------------------------------------------------- 1 | from PathPlanning.RRTStarSmart import RRTStarSmartPlanner 2 | from PathPlanning.utils import * 3 | 4 | def main(): 5 | map = Map(left=0, right=640, top=480, down=0) 6 | map.add_obstacle(RectangleObstacle(left=450, right=550, top=220, down=200)) 7 | map.add_obstacle(RectangleObstacle(left=100, right=400, top=300, down=250)) 8 | map.add_obstacle(RectangleObstacle(left=400, right=450, top=400, down=50)) 9 | rrtStarSmartPlanner = RRTStarSmartPlanner(map=map, iterations=1e3, epsilon=0.3, step_size=10) 10 | start = Point(550, 350) 11 | end = Point(200, 150) 12 | while map.is_open: 13 | rrtStarSmartPlanner.plan(start=start, target=end) 14 | map.add_geometry(type='point', pos=start.tuple(), size=20, color=(100, 0, 0)) 15 | map.add_geometry(type='point', pos=end.tuple(), size=20, color=(0, 100, 0)) 16 | for i in range(len(rrtStarSmartPlanner.finalPath)-1): 17 | map.add_geometry(type='line', start=rrtStarSmartPlanner.finalPath[i].tuple(), end=rrtStarSmartPlanner.finalPath[i+1].tuple(), color=(0, 100, 0)) 18 | map.render() 19 | 20 | if __name__ == '__main__': 21 | main() -------------------------------------------------------------------------------- /PathPlanning/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0aqz0/Robotics-Notebook/f94dfa1f1a76e6148e073cf0b3929975628d6f3c/PathPlanning/__init__.py -------------------------------------------------------------------------------- /PathPlanning/geometry.py: -------------------------------------------------------------------------------- 1 | """ 2 | geometry elements 3 | """ 4 | import math 5 | 6 | class Point(object): 7 | def __init__(self, x, y): 8 | self.x = x 9 | self.y = y 10 | 11 | def __eq__(self, other): 12 | return self.x == other.x and self.y == other.y 13 | 14 | def __add__(self, other): 15 | return Point(self.x + other.x, self.y + other.y) 16 | 17 | def dist(self, other): 18 | return math.sqrt(pow(self.x - other.x, 2) + pow(self.y - other.y, 2)) 19 | 20 | def dir(self, other): 21 | return math.atan2(other.y - self.y, other.x - self.x) 22 | 23 | def tuple(self): 24 | return self.x, self.y 25 | 26 | 27 | class Vector(object): 28 | def __init__(self, x, y): 29 | self.x = x 30 | self.y = y 31 | 32 | def dir(self): 33 | return math.atan2(self.y, self.x) 34 | 35 | def mod(self): 36 | return math.sqrt(pow(self.x, 2) + pow(self.y, 2)) 37 | 38 | def __mul__(self, other): 39 | return Vector(other*self.x, other*self.y) 40 | 41 | def __add__(self, other): 42 | return Vector(self.x+other.x, self.y+other.y) 43 | 44 | 45 | def Polar2Vector(dist, theta): 46 | return Vector(dist*math.cos(theta), dist*math.sin(theta)) -------------------------------------------------------------------------------- /PathPlanning/icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0aqz0/Robotics-Notebook/f94dfa1f1a76e6148e073cf0b3929975628d6f3c/PathPlanning/icon.png -------------------------------------------------------------------------------- /PathPlanning/render.py: -------------------------------------------------------------------------------- 1 | """ 2 | render tools 3 | """ 4 | try: 5 | import pyglet 6 | except ImportError as err: 7 | raise ImportError(''' 8 | Unable to import pyglet. 9 | Please install pyglet via 'pip install pyglet' 10 | ''') 11 | 12 | try: 13 | from pyglet.gl import * 14 | except ImportError as err: 15 | raise ImportError(''' 16 | Unable to import gl from pyglet. 17 | Please install OpenGL. 18 | ''') 19 | 20 | import math 21 | import os 22 | 23 | class Viewer(object): 24 | def __init__(self, width=640, height=480, caption="Robotics Notebook/Path Planning", icon_file="icon.png"): 25 | self.width = width 26 | self.height = height 27 | platform = pyglet.window.get_platform() 28 | display = platform.get_default_display() 29 | # screen = display.get_default_screen() 30 | config = Config(double_buffer=True) 31 | self.window = pyglet.window.Window(width=int(width), height=int(height), display=display, 32 | config=config, caption=caption) 33 | icon = pyglet.image.load(os.path.dirname(__file__) + '/' + icon_file) 34 | self.window.set_icon(icon) 35 | self.is_open = True 36 | self.window.on_close = self.close_viewer 37 | self.window.on_draw = self.draw 38 | self.geoms = [] 39 | 40 | def render(self): 41 | glClearColor(1,1,1,1) 42 | self.window.clear() 43 | self.window.switch_to() 44 | self.window.dispatch_events() 45 | self.window.dispatch_event('on_draw') 46 | self.window.flip() 47 | 48 | def add_geometry(self, type, **attrs): 49 | if type == 'point': 50 | self.geoms.append(Point(**attrs)) 51 | elif type == 'line': 52 | self.geoms.append(Line(**attrs)) 53 | 54 | def draw(self): 55 | self.draw_point(pos=(400, 300), color=(100, 0, 0), pointSize=3) 56 | self.draw_line(start=(200, 200), end=(400, 400), color=(0, 100, 0), lineWidth=3) 57 | self.draw_circle(pos=(100,350), radius=50, res=50, color=(100, 100, 0)) 58 | self.draw_circle(pos=(100, 50), radius=30, res=10, filled=False, lineWidth=5) 59 | self.draw_polygon(points=((200, 50), (200, 250), (400, 250), (400, 50)), close=True, lineWidth=3, color=(0, 0, 100)) 60 | self.draw_polygon(points=((500, 250), (500, 350), (600, 350), (600, 250)), filled=True, color=(0, 0, 100)) 61 | 62 | def draw_point(self, pos, **attrs): 63 | point = Point(pos=pos) 64 | if 'color' in attrs: 65 | point.set_color(*attrs['color']) 66 | if 'pointSize' in attrs: 67 | point.set_pointSize(attrs['pointSize']) 68 | point.render() 69 | 70 | def draw_line(self, start, end, **attrs): 71 | line = Line(start=start, end=end) 72 | if 'color' in attrs: 73 | line.set_color(*attrs['color']) 74 | if 'lineWidth' in attrs: 75 | line.set_lineWidth(attrs['lineWidth']) 76 | line.render() 77 | 78 | def draw_circle(self, pos, radius, **attrs): 79 | circle = Circle(pos=pos, radius=radius) 80 | if 'color' in attrs: 81 | circle.set_color(*attrs['color']) 82 | if 'res' in attrs: 83 | circle.set_res(attrs['res']) 84 | if 'filled' in attrs: 85 | circle.set_filled(attrs['filled']) 86 | if 'lineWidth' in attrs: 87 | circle.set_lineWidth(attrs['lineWidth']) 88 | circle.render() 89 | 90 | def draw_polygon(self, points, **attrs): 91 | polygon = Polygon(points=points) 92 | if 'color' in attrs: 93 | polygon.set_color(*attrs['color']) 94 | if 'close' in attrs: 95 | polygon.set_close(attrs['close']) 96 | if 'filled' in attrs: 97 | polygon.set_filled(attrs['filled']) 98 | if 'lineWidth' in attrs: 99 | polygon.set_lineWidth(attrs['lineWidth']) 100 | polygon.render() 101 | 102 | def close_viewer(self): 103 | self.is_open = False 104 | 105 | 106 | class Attr(object): 107 | def enable(self): 108 | raise NotImplementedError 109 | def disable(self): 110 | pass 111 | 112 | class Color(Attr): 113 | def __init__(self, *color): 114 | self.color = color 115 | def enable(self): 116 | glColor3b(*self.color) 117 | 118 | class LineWidth(Attr): 119 | def __init__(self, width): 120 | self.width = width 121 | def enable(self): 122 | glLineWidth(self.width) 123 | 124 | class PointSize(Attr): 125 | def __init__(self, size): 126 | self.size = size 127 | def enable(self): 128 | glPointSize(self.size) 129 | 130 | class Geom(object): 131 | def __init__(self): 132 | self._color = Color(0, 0, 0) 133 | self.attrs = [self._color] 134 | def render(self): 135 | for attr in self.attrs: 136 | attr.enable() 137 | self.render1() 138 | for attr in self.attrs: 139 | attr.disable() 140 | def render1(self): 141 | raise NotImplementedError 142 | def add_attr(self, attr): 143 | self.attrs.append(attr) 144 | def set_color(self, r, g, b): 145 | self._color.color = (r, g, b) 146 | 147 | class Point(Geom): 148 | def __init__(self, pos=(0,0), size=3, color=None): 149 | Geom.__init__(self) 150 | self._pos = pos 151 | self._pointSize = PointSize(size=size) 152 | self.add_attr(self._pointSize) 153 | if color is not None: 154 | self.set_color(*color) 155 | def render1(self): 156 | glBegin(GL_POINTS) 157 | glVertex2d(*self._pos) 158 | glEnd() 159 | def set_pointSize(self, size): 160 | self._pointSize.size = size 161 | 162 | class Line(Geom): 163 | def __init__(self, start, end, width=3, color=None): 164 | Geom.__init__(self) 165 | self._start = start 166 | self._end = end 167 | self._lineWidth = LineWidth(width=width) 168 | self.add_attr(self._lineWidth) 169 | if color is not None: 170 | self.set_color(*color) 171 | def render1(self): 172 | glBegin(GL_LINES) 173 | glVertex2d(*self._start) 174 | glVertex2d(*self._end) 175 | glEnd() 176 | def set_lineWidth(self, width): 177 | self._lineWidth.width = width 178 | 179 | class Circle(Geom): 180 | def __init__(self, pos, radius, res=30, filled=True, width=3, color=None): 181 | Geom.__init__(self) 182 | self._pos = pos 183 | self._radius = radius 184 | self._res = res 185 | self._filled = filled 186 | self._lineWidth = LineWidth(width=width) 187 | if color is not None: 188 | self.set_color(*color) 189 | def render1(self): 190 | if self._filled: 191 | glBegin(GL_POLYGON) 192 | else: 193 | glBegin(GL_LINE_LOOP) 194 | for i in range(max(self._res, 5)): 195 | angle = 2*math.pi/self._res*i 196 | glVertex2d(self._pos[0] + self._radius * math.cos(angle), self._pos[1] + self._radius * math.sin(angle)) 197 | glEnd() 198 | def set_pos(self, pos): 199 | self._pos = pos 200 | def set_radius(self, radius): 201 | self._radius = radius 202 | def set_res(self, res): 203 | self._res = res 204 | def set_filled(self, filled): 205 | self._filled = filled 206 | def set_lineWidth(self, width): 207 | self._lineWidth.width = width 208 | 209 | class Polygon(Geom): 210 | def __init__(self, points, close=True, filled=False, width=3, color=None): 211 | Geom.__init__(self) 212 | self._points = points 213 | self._close = close 214 | self._filled = filled 215 | self._lineWidth = LineWidth(width=width) 216 | self.add_attr(self._lineWidth) 217 | if color is not None: 218 | self.set_color(*color) 219 | def render1(self): 220 | if self._filled: 221 | if len(self._points) > 4: 222 | glBegin(GL_POLYGON) 223 | elif len(self._points) == 4: 224 | glBegin(GL_QUADS) 225 | else: 226 | glBegin(GL_TRIANGLES) 227 | else: 228 | glBegin(GL_LINE_LOOP if self._close else GL_LINE_STRIP) 229 | for point in self._points: 230 | glVertex2d(*point) 231 | glEnd() 232 | def set_close(self, close): 233 | self._close = close 234 | def set_filled(self, filled): 235 | self._filled = filled 236 | def set_lineWidth(self, width): 237 | self._lineWidth.width = width 238 | 239 | 240 | if __name__ == '__main__': 241 | viewer = Viewer() 242 | while viewer.is_open: 243 | viewer.render() -------------------------------------------------------------------------------- /PathPlanning/utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | collections of common structures 3 | """ 4 | from PathPlanning.geometry import * 5 | from PathPlanning.render import Viewer 6 | 7 | class Node(object): 8 | def __init__(self, pos, parent=None, cost=0): 9 | self.pos = pos 10 | self.parent = parent 11 | self.cost = cost 12 | 13 | def dist(self, node): 14 | return self.pos.dist(node.pos) 15 | 16 | 17 | class Obstacle(object): 18 | def __init__(self, pos): 19 | self.pos = pos 20 | 21 | def type(self): 22 | raise NotImplementedError 23 | 24 | def dist(self, other): 25 | raise NotImplementedError 26 | 27 | def check_collision(self, other, avoidDist): 28 | raise NotImplementedError 29 | 30 | 31 | class CircleObstacle(Obstacle): 32 | def __init__(self, pos, radius): 33 | Obstacle.__init__(self, pos) 34 | self.radius = radius 35 | 36 | def type(self): 37 | return "circle" 38 | 39 | def dist(self, other): 40 | return max(self.pos.dist(other) - self.radius, 0) 41 | 42 | def check_collision(self, other, avoidDist): 43 | return self.dist(other) <= avoidDist 44 | 45 | 46 | class RectangleObstacle(Obstacle): 47 | def __init__(self, top, down, left, right): 48 | self.top = max(top, down) 49 | self.down = min(top, down) 50 | self.left = min(left, right) 51 | self.right = max(left, right) 52 | self.length = math.fabs(left - right) 53 | self.width = math.fabs(top - down) 54 | Obstacle.__init__(self, Point((left+right)/2, (top+down)/2)) 55 | 56 | def type(self): 57 | return "rectangle" 58 | 59 | def vertex(self): 60 | return (self.left, self.down), (self.left, self.top), (self.right, self.top), (self.right, self.down) 61 | 62 | def dist(self, other): 63 | if other.x < self.left: 64 | if other.y > self.top: 65 | return other.dist(Point(self.left, self.top)) 66 | elif other.y < self.down: 67 | return other.dist(Point(self.left, self.down)) 68 | else: 69 | return math.fabs(other.x - self.left) 70 | elif other.x > self.right: 71 | if other.y > self.top: 72 | return other.dist(Point(self.right, self.top)) 73 | elif other.y < self.down: 74 | return other.dist(Point(self.right, self.down)) 75 | else: 76 | return math.fabs(other.x - self.right) 77 | else: 78 | if other.y > self.top: 79 | return math.fabs(other.y - self.top) 80 | elif other.y < self.down: 81 | return math.fabs(other.y - self.down) 82 | else: 83 | return 0 84 | 85 | def check_collision(self, other, avoidDist): 86 | return self.dist(other) <= avoidDist 87 | 88 | 89 | class Map(Viewer): 90 | def __init__(self, top, down, left, right, refresh=True): 91 | self.top = max(top, down) 92 | self.down = min(top, down) 93 | self.left = min(left, right) 94 | self.right = max(left, right) 95 | self.length = math.fabs(left - right) 96 | self.width = math.fabs(top - down) 97 | self.refresh = refresh 98 | Viewer.__init__(self, width=self.length, height=self.width) 99 | self.obstacles = [] 100 | 101 | def out_of_map(self, pos): 102 | return pos.x < self.left or pos.x > self.right or pos.y < self.down or pos.y > self.top 103 | 104 | def add_obstacle(self, obs): 105 | self.obstacles.append(obs) 106 | 107 | def check_collision(self, other, avoidDist): 108 | for obs in self.obstacles: 109 | if obs.check_collision(other, avoidDist): 110 | return True 111 | return False 112 | 113 | def draw(self): 114 | # draw borders 115 | # self.draw_line(start=(self.left, self.down), end=(self.left, self.top), lineWidth=5) 116 | # self.draw_line(start=(self.left, self.top), end=(self.right, self.top), lineWidth=5) 117 | # self.draw_line(start=(self.right, self.top), end=(self.right, self.down), lineWidth=5) 118 | # self.draw_line(start=(self.right, self.down), end=(self.left, self.down), lineWidth=5) 119 | # draw geoms 120 | for geom in self.geoms: 121 | geom.render() 122 | # draw obstacles 123 | for obs in self.obstacles: 124 | if obs.type() == 'circle': 125 | self.draw_circle(pos=obs.pos.tuple(), radius=obs.radius) 126 | elif obs.type() == 'rectangle': 127 | self.draw_polygon(points=obs.vertex(), filled=True) 128 | if self.refresh: 129 | self.geoms = [] 130 | 131 | 132 | class PathPlanner(object): 133 | """ 134 | superclass for path planning algorithms. 135 | """ 136 | def __init__(self): 137 | self.finalPath = [] 138 | 139 | def plan(self, start, target): 140 | """ 141 | Plans the path. 142 | """ 143 | raise NotImplementedError 144 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robotics-Notebook 2 | 3 | - [Path Planning](#Path-Planning) 4 | - [Dijkstra](#Dijkstra) 5 | - [A*](#AStar) 6 | - [Potential Field](#Potential-Field) 7 | - [RRT](#RRT) 8 | - [RRT*](#RRTStar) 9 | - [RRT* Smart](#RRTStarSmart) 10 | - [Qlearning](#Qlearning) 11 | 12 | ## Path Planning 13 | 14 | ### Dijkstra 15 | 16 | 17 | 18 | *Reference*: 19 | 20 | [Dijkstra's algorithm - Wikipedia](https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm) 21 | 22 | ### A* 23 | 24 | 25 | 26 | *Reference*: 27 | 28 | [A* search algorithm - Wikipedia](https://en.wikipedia.org/wiki/A*_search_algorithm) 29 | 30 | ### Potential Field 31 | 32 | 33 | 34 | *Reference*: 35 | 36 | [Robotic Motion Planning: Potential Functions](https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf) 37 | 38 | ### RRT 39 | 40 | 41 | *Reference*: 42 | 43 | [Rapidly-Exploring Random Trees: A New Tool for Path Planning](http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.35.1853) 44 | 45 | ### RRT* 46 | 47 | 48 | *Reference*: 49 | 50 | [Sampling-based algorithms for optimal motion planning](https://journals.sagepub.com/doi/abs/10.1177/0278364911406761) 51 | 52 | ### RRT* Smart 53 | 54 | 55 | 56 | *Reference*: 57 | 58 | [RRT*-SMART: A Rapid Convergence Implementation of RRT*](https://journals.sagepub.com/doi/pdf/10.5772/56718) 59 | 60 | [A Comparison of RRT, RRT* and RRT*-Smart Path Planning Algorithms](http://paper.ijcsns.org/07_book/201610/20161004.pdf) 61 | 62 | ### Qlearning 63 | 64 | 65 | 66 | *Reference*: 67 | 68 | [**Path Planning with qlearning**]() 69 | 70 | ## Getting Started 71 | 72 | ```bash 73 | $ git clone git@github.com:0aqz0/Robotics-Notebook.git 74 | $ cd Robotics-Notebook 75 | $ virtualenv.exe venv 76 | $ source venv/Scripts/activate 77 | $ pip install -r requirements.txt 78 | $ pip install -e . 79 | $ python PathPlanning/AStar/run.py 80 | ``` 81 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | future==0.17.1 2 | pyglet==1.3.0 3 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | setup(name="Robotics-Notebook", version="0.0.1", packages=find_packages()) --------------------------------------------------------------------------------