├── config_space ├── config │ ├── .gitignore │ ├── bar_robot.csv │ ├── rrt_sample_world.yaml │ └── block_world.yaml ├── plot_tools │ ├── __init__.py │ └── surf_rotation_animation.py ├── fig │ └── .gitignore ├── nearest_neighbour_speedtest.py ├── config_space_plot.py ├── potential_field.py ├── rrt_simple.py ├── robot_tools.py ├── arm_config_space.py └── polygon_tools.py ├── poincare_example ├── vid │ └── .gitignore ├── poincare_map.py └── poincare_1d.py ├── .gitignore ├── requirements.txt ├── LICENSE.md ├── random_walk └── gaussian_random_walk.py └── README.md /config_space/config/.gitignore: -------------------------------------------------------------------------------- 1 | * -------------------------------------------------------------------------------- /config_space/plot_tools/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /poincare_example/vid/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | -------------------------------------------------------------------------------- /config_space/fig/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | !.gitignore 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.py[cod] 2 | */__pycache__/ 3 | *.egg-info* -------------------------------------------------------------------------------- /config_space/config/bar_robot.csv: -------------------------------------------------------------------------------- 1 | 0.1, 0.01 2 | -0.1, 0.01 3 | -0.1, -0.01 4 | 0.1, -0.01 -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | matplotlib 3 | argparse 4 | tqdm 5 | pyyaml 6 | scipy 7 | scikit-image>=0.13 -------------------------------------------------------------------------------- /config_space/config/rrt_sample_world.yaml: -------------------------------------------------------------------------------- 1 | workspace: 2 | type: Workspace2D 3 | parameters: 4 | limits: 5 | - [0.0, 10.0] 6 | - [0.0, 10.0] 7 | 8 | obstacles: 9 | - type: Rectangle 10 | parameters: 11 | xlim: [2.3, 3.4] 12 | ylim: [7, 9.8] 13 | 14 | - type: Rectangle 15 | parameters: 16 | xlim: [7.3, 8.5] 17 | ylim: [7.6, 10] 18 | 19 | - type: Rectangle 20 | parameters: 21 | xlim: [1.3, 3] 22 | ylim: [2.8, 3.8] 23 | 24 | - type: Polygon 25 | parameters: 26 | p_in: 27 | - [5, 0.9] 28 | - [7.1, 0.9] 29 | - [7.1, 3.7] 30 | 31 | robot: 32 | type: Robot2D 33 | parameters: 34 | pos: [5.0, 5.0] 35 | footprint: [[0.1, 0.01], [-0.1, 0.01], [-0.1, -0.01], [0.1, -0.01]] 36 | -------------------------------------------------------------------------------- /config_space/config/block_world.yaml: -------------------------------------------------------------------------------- 1 | robot: 2 | type: RobotArm2D 3 | parameters: 4 | base_position: [5.0, 5.0] 5 | link_lengths: [2.1, 2.1] 6 | 7 | workspace: 8 | type: Workspace2D 9 | parameters: 10 | limits: [[0, 10.0], [0, 10.0]] 11 | 12 | obstacles: 13 | - type: Rectangle 14 | parameters: 15 | xlim: [2.3, 3.4] 16 | ylim: [7, 9.8] 17 | 18 | - type: Rectangle 19 | parameters: 20 | xlim: [7.3, 8.5] 21 | ylim: [7.6, 10] 22 | 23 | - type: Rectangle 24 | parameters: 25 | xlim: [1.3, 3] 26 | ylim: [2.8, 3.8] 27 | 28 | # This is equivalent to a Rectangle, just an example to show how other polygons could be defined by their vertices 29 | - type: Polygon 30 | parameters: 31 | p_in: 32 | - [5, 0.9] 33 | - [7.1, 0.9] 34 | - [7.1, 3.7] 35 | - [5, 3.7] 36 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 ETHZ ASL 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 | -------------------------------------------------------------------------------- /poincare_example/poincare_map.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from matplotlib import rc 4 | from poincare_1d import DynamicFunction, linear_damper, linear_exact, logistic_periodic 5 | 6 | rc('text', usetex=True) 7 | 8 | 9 | def calculate_poincare_map(dyn_func, x0, nt=100): 10 | # Calculate the Poincar\'e map for a given dynamic function 11 | # If exact solution known, solve directly. If not, Euler integration with nt steps 12 | if dyn_func.xt is not None: # Have exact solution 13 | Px = dyn_func.xt(dyn_func.period, x0) 14 | else: 15 | Px = np.array(x0).copy() 16 | dt = dyn_func.period/nt 17 | tt = np.arange(0.0, dyn_func.period, dt) 18 | # TODO: Use a better integration scheme (RK4) 19 | for t in tt: 20 | dxdt = dyn_func.dx_dt(Px, t) 21 | Px = Px + dxdt * dt 22 | 23 | return Px 24 | 25 | 26 | def follow_stability(x, Px, x0, n_jumps=5): 27 | xx = [x0] 28 | for i in range(n_jumps): 29 | xx.append(np.interp(xx[-1], Px, x)) 30 | return xx 31 | 32 | 33 | x_0 = np.linspace(-3.0, 5.0, 201) 34 | 35 | linear = DynamicFunction(linear_damper, xt=linear_exact, fname='$\dot{x} = -x + 2\cos(t)$') 36 | logistic = DynamicFunction(logistic_periodic, fname='$\dot{x} = -x(x+1) + 2\cos(2 \pi t)$', period=1.0) 37 | 38 | test_fun = linear 39 | Px = calculate_poincare_map(test_fun, x_0) 40 | 41 | with plt.style.context('ggplot'): 42 | fh, ah = plt.subplots() 43 | ah.set_xlabel('$x_0$') 44 | ah.set_ylabel('$P(x_0)$') 45 | hP, = ah.plot(x_0, Px) 46 | h0, = ah.plot([x_0[0], x_0[-1]], [x_0[0], x_0[-1]], '--') 47 | ah.set_title(r"Poincar\'e map for {0}".format(test_fun.fname)) 48 | ah.legend([hP, h0], ['$P(x_0)$', '$P(x)=x$']) 49 | plt.show() 50 | -------------------------------------------------------------------------------- /config_space/nearest_neighbour_speedtest.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial import KDTree 3 | from scipy.spatial.distance import euclidean, cdist 4 | import timeit 5 | import time 6 | 7 | def kdtree_nn(point_set, target_point): 8 | tree = KDTree(point_set) 9 | dist, ind = tree.query(target_point, k=1) 10 | return dist, ind 11 | 12 | def bruteforce_nn(point_set, target_point): 13 | d_best = euclidean(point_set[0], target_point) 14 | i_best = 0 15 | for i, p in enumerate(point_set[1:]): 16 | d = euclidean(p, target_point) 17 | if d < d_best: 18 | d_best = d 19 | i_best = i+1 20 | return d_best, i_best 21 | 22 | def cdist_nn(point_set, target_point): 23 | d_full = cdist(point_set, [target_point]).flatten() 24 | i_best = np.argmin(d_full) 25 | return d_full[i_best], i_best 26 | 27 | def wrapper(func, x_rand): 28 | def wrapped(): 29 | func(x_rand[1:], x_rand[0]) 30 | return wrapped 31 | 32 | # Value test 33 | x_rand = np.random.uniform([0, 0], [1, 1], size=(1000 + 1, 2)) 34 | for func in [kdtree_nn, cdist_nn, bruteforce_nn]: 35 | d, i = func(x_rand[1:], x_rand[0]) 36 | print('{0}: d={1}, i={2}'.format(func.__name__, d, i)) 37 | 38 | for n_points in [10, 1000, 100000]: 39 | for func in [kdtree_nn, cdist_nn, bruteforce_nn]: 40 | np.random.seed(1) 41 | x_rand = np.random.uniform([0, 0], [1, 1], size=(n_points + 1, 2)) 42 | wrapped = wrapper(func, x_rand) 43 | number = int(1000000/n_points) 44 | t = timeit.timeit(wrapped, number=number) 45 | print('{0} (n={1}): {2}'.format(func.__name__, n_points, t)) 46 | 47 | # cdist seems insanely fast... just do a second sanity check 48 | def time_test(func, n_points): 49 | x_rand = np.random.uniform([0, 0], [1, 1], size=(n_points + 1, 2)) 50 | return func(x_rand[1:], x_rand[0]) 51 | 52 | for func in [kdtree_nn, cdist_nn, bruteforce_nn]: 53 | n_points = 10000 54 | t0 = time.time() 55 | for i in range(1000): 56 | time_test(func, n_points) 57 | print('{0} (n={1}): {2}'.format(func.__name__, n_points, time.time()-t0)) -------------------------------------------------------------------------------- /config_space/plot_tools/surf_rotation_animation.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from matplotlib import cm 3 | from mpl_toolkits.mplot3d.art3d import Poly3DCollection 4 | 5 | class RotationAnimator(object): 6 | h_surf = None 7 | h_goal = None 8 | 9 | def __init__(self, x, y, z, goal, start_azim=-60.0, delta_angle=10.0, start_elev=30.0): 10 | self.f = plt.figure() 11 | self.a = self.f.add_subplot(111, projection='3d') 12 | self.x = x 13 | self.y = y 14 | self.z = z 15 | self.goal = goal 16 | self.start_azim = start_azim 17 | self.start_elev = start_elev 18 | self.delta_angle = delta_angle 19 | self.init() 20 | 21 | def init(self): 22 | self.a.cla() 23 | self.h_surf = self.a.plot_surface(self.x, self.y, self.z.T, cmap=cm.coolwarm) 24 | # self.h_start = a3.plot([self.start[0]], [self.start[1]], [self.z[self.start[0], self.start[1]]], 'g^') 25 | # self.h_path = a3.plot(self.path[:,0], self.path[:,1]) 26 | self.h_goal, = self.a.plot([self.goal[0]], [self.goal[1]], [self.z[int(self.goal[0]), int(self.goal[1])]], 'yo') 27 | self.a.azim = self.start_azim 28 | self.a.elev = self.start_elev 29 | return [self.h_surf, self.h_goal] 30 | 31 | def update(self, n): 32 | self.a.view_init(elev=self.start_elev, azim=self.start_azim+n*self.delta_angle) 33 | return [self.h_surf, self.h_goal] 34 | 35 | 36 | class TrisurfRotationAnimator(object): 37 | h_surf = None 38 | 39 | def __init__(self, vertices, faces, ax_lims=None, start_azim=-60.0, delta_angle=10.0, start_elev=30.0, 40 | x_label=r'$x$', y_label=r'$y$', z_label=r"$z$"): 41 | self.f = plt.figure() 42 | self.a = self.f.add_subplot(111, projection='3d') 43 | self.vertices = vertices 44 | self.faces = faces 45 | self.start_azim = start_azim 46 | self.start_elev = start_elev 47 | self.delta_angle = delta_angle 48 | self.ax_lims = ax_lims 49 | self.x_label = x_label 50 | self.y_label = y_label 51 | self.z_label = z_label 52 | self.init() 53 | 54 | def init(self): 55 | self.a.cla() 56 | self.a.set_xlabel(self.x_label) 57 | self.a.set_ylabel(self.y_label) 58 | self.a.set_zlabel(self.z_label) 59 | 60 | self.h_surf = self.a.plot_trisurf(self.vertices[:, 0], self.vertices[:, 1], self.faces, self.vertices[:, 2], 61 | cmap='Spectral', lw=1) 62 | if self.ax_lims is not None: 63 | self.a.set_xlim(self.ax_lims[0]) 64 | self.a.set_ylim(self.ax_lims[1]) 65 | self.a.set_zlim(self.ax_lims[2]) 66 | 67 | self.a.azim = self.start_azim 68 | self.a.elev = self.start_elev 69 | return [self.h_surf] 70 | 71 | def update(self, n): 72 | self.a.view_init(elev=self.start_elev, azim=self.start_azim+n*self.delta_angle) 73 | return [self.h_surf] 74 | -------------------------------------------------------------------------------- /random_walk/gaussian_random_walk.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import matplotlib.animation as animation 4 | import matplotlib.cm as cm 5 | import argparse 6 | 7 | """ 8 | 9 | Basic random walk visualisation 10 | 11 | Requires: numpy, matplotlib, argparse 12 | 13 | Author: Nicholas Lawrance (nicholas.lawrance@mavt.ethz.ch) 14 | 15 | """ 16 | 17 | plt.rc('font', **{'family': 'serif', 'sans-serif': ['Computer Modern Roman']}) 18 | plt.rc('text', usetex=True) 19 | cmap = cm.viridis 20 | 21 | parser = argparse.ArgumentParser(description='Basic random walk') 22 | parser.add_argument('-n', type=int, default=50, help='Number of total trajectories') 23 | parser.add_argument('-t', type=int, default=100, help='Number of timesteps') 24 | parser.add_argument('--sigma', type=float, default=1.0, help='Standard deviation') 25 | parser.add_argument('--fps', type=int, default=50, help='FPS') 26 | parser.add_argument('--n_show', type=int, default=5, help='Number of single paths to show') 27 | parser.add_argument('-vf', '--video-file', type=str, default='', help='Save animation to file') 28 | args = parser.parse_args() 29 | 30 | class WalkAnimator(object): 31 | 32 | def __init__(self, n=50, t=100, n_show=10, sigma=1.0, end_append=100): 33 | assert n_show <= n 34 | self.fig, self.ax = plt.subplots() 35 | self.fig.set_size_inches([4.8, 2.7]) # 1920*1080 at 200 dpi 36 | self.n = n 37 | self.t = t 38 | self.tt = np.arange(self.t) 39 | self.n_show = n_show 40 | self.sigma = sigma 41 | self.end_append = end_append # Extra frames on the end (freeze) 42 | 43 | self.paths = np.zeros((self.n, self.t)) 44 | self.paths[:, 1:] = np.random.normal(0, self.sigma**2, (self.n, self.t-1)).cumsum(axis=1) 45 | 46 | self.max_frames = self.t*self.n_show + self.t + self.end_append 47 | self.h_lines = [] 48 | 49 | 50 | def init_fig(self): 51 | self.ax.cla() 52 | 53 | self.ax.set_xlim(0, self.t) 54 | self.ax.set_ylim(self.paths.min(), self.paths.max()) 55 | self.ax.grid() 56 | 57 | 58 | self.ax.set_xlabel(r'$t$') 59 | self.ax.set_ylabel(r'$x(t)$') 60 | self.ax.set_title(r'Gaussian random walk, $x_{n+1} = x_n + z$, $z \sim \mathcal{N}(0, '+'{0:0.1f})$'.format(self.sigma)) 61 | self.fig.tight_layout() 62 | 63 | self.h_lines = [] 64 | for p in self.paths: 65 | self.h_lines.append(self.ax.plot([0], p[:1])[0]) 66 | 67 | return self.h_lines 68 | 69 | def animate(self, i): 70 | l_n = i // self.t 71 | k = i % self.t 72 | 73 | if l_n < self.n_show: 74 | k = i % self.t 75 | self.h_lines[l_n].set_data(self.tt[:k+1], self.paths[l_n, :k+1]) 76 | 77 | elif i < self.max_frames - self.end_append: 78 | for p, h in zip(self.paths[self.n_show:], self.h_lines[self.n_show:]): 79 | h.set_data(self.tt[:k+1], p[:k+1]) 80 | 81 | return self.h_lines 82 | 83 | walker = WalkAnimator(args.n, args.t, n_show=args.n_show, sigma=args.sigma) 84 | anim = animation.FuncAnimation(walker.fig, walker.animate, init_func=walker.init_fig, frames=walker.max_frames, 85 | interval=1000.0/args.fps, blit=True) 86 | 87 | if args.video_file: 88 | # animation.save('fig/arm_config_space_video.gif', writer='imagemagick', fps=1000.0/delta_t) 89 | anim.save(args.video_file, writer='ffmpeg', codec='libx264', fps=args.fps, dpi=200, 90 | extra_args=["-crf", "18", "-profile:v", "main", "-tune", "animation", "-pix_fmt", "yuv420p"]) 91 | plt.show() -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # AMR Visualisations 2 | Simple python animated examples for the planning and dynamics segments of the Autonomous Mobile Robots lecture slides. Primarily for students of the [course](https://asl.ethz.ch/education/lectures/autonomous_mobile_robots.html) but open to anyone interested. 3 | 4 | 5 | ## Basic installation and requirements 6 | Clone the repo, install dependencies 7 | ``` 8 | git clone https://github.com/ethz-asl/amr_visualisations.git 9 | cd amr_visualisations 10 | pip install -r requirements.txt 11 | ``` 12 | 13 | ## Configuration space examples 14 | 15 | These are some basic examples of generating configuration spaces and illustrating them. 16 | 17 | First, enter the directory `cd config_space` 18 | 19 | ### 3D (x,y,θ) robot configuration space 20 | This simple script uses a basic polygon collision checking library and sampling to show the configuration space for a 2D robot. 21 | Obstacles are generated by sampling random points and creating a convex hull. 22 | 23 | A basic example can be generated with: 24 | ``` 25 | python config_space_plot.py 26 | ``` 27 | ![config_space_slices](https://user-images.githubusercontent.com/10678827/81062882-16cf0400-8ed7-11ea-9d36-697450a56593.png) 28 | 29 | 30 | Help for the flags etc. are available with: 31 | ``` 32 | python config_space_plot.py -h 33 | ``` 34 | 35 | Some things you might like to change include: 36 | - The robot footprint is defined with a csv file ([example](config_space/robots/bar_robot.csv)) and set with the `--robot-footprint` flag 37 | - You can change the sampling density with the `-nx` flag 38 | - Generate the 3D rotation animation with the `--animation` flag 39 | 40 | ![config_space_rotation](https://user-images.githubusercontent.com/10678827/81062839-0585f780-8ed7-11ea-8619-5da014477b18.gif) 41 | 42 | ### Robot arm configuration space 43 | This script shows the configuration space for a basic multi-jointed robot arm. 44 | A basic example can be generated with: 45 | ``` 46 | python arm_config_space.py 47 | ``` 48 | 49 | Again, basic help can be found with 50 | ``` 51 | python arm_config_space.py -h 52 | ``` 53 | The robot parameters and workspace obstacles are defined with a basic yaml file. 54 | The default configuration can be found in the `config/block_world.yaml` file. 55 | To set a new workspace, create a copy of the `block_world.yaml` file, edit the robot and/or obstacles, and use the `-w` flag to specify your new world file: 56 | ``` 57 | python arm_config_space.py -w config/my_new_world.yaml 58 | ``` 59 | Currently, only a basic jointed robot arm and polygonal obstacles are implemented. 60 | 61 | ![arm_config_space_video](https://user-images.githubusercontent.com/10678827/81062807-eedfa080-8ed6-11ea-8d94-a39898cf47cd.gif) 62 | 63 | ### Rapidly-exploring Random Tree (RRT) 64 | Basic example of an RRT search showing the Voronoi regions to demonstrate how an RRT demonstrates a 'space-filling' effect. 65 | A basic example can be generated with: 66 | ``` 67 | python rrt_simple.py 68 | ``` 69 | The workspace definition can be changed to another file using the `--world` argument, number of iterations with `-i` and other options (try `python rrt_simple.py -h` for more help). 70 | 71 | ![rrt_video](https://user-images.githubusercontent.com/10678827/127008176-1b9e58d1-330a-45af-a9b2-ebdbfe8938de.gif) 72 | 73 | ### Potential field example 74 | ``` 75 | python potential_field.py 76 | ``` 77 | ![rotating_potentialfieldobs](https://user-images.githubusercontent.com/10678827/81062961-3e25d100-8ed7-11ea-917e-fd2a75dafec1.gif) 78 | 79 | ## Poincaré example 80 | ``` 81 | cd poincare_example 82 | python poincare_1d.py 83 | ``` 84 | ![poincare_example_ffm](https://user-images.githubusercontent.com/10678827/81069466-2ef85080-8ee2-11ea-9317-88f10991f86b.gif) 85 | 86 | ## Feedback 87 | Comments, feedback and PRs for changes are welcome (nicholas.lawrance AT mavt.ethz.ch). 88 | -------------------------------------------------------------------------------- /config_space/config_space_plot.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import polygon_tools as poly 5 | import robot_tools 6 | import shutil 7 | from matplotlib.patches import Polygon as PlotPolygon 8 | from matplotlib.collections import PatchCollection 9 | import matplotlib.animation as animation 10 | from skimage import measure 11 | from mpl_toolkits.mplot3d.art3d import Poly3DCollection 12 | import copy 13 | import argparse 14 | from plot_tools.surf_rotation_animation import TrisurfRotationAnimator 15 | 16 | """ 17 | 18 | Plot an example of config space for Autonomous Mobile Robots lecture notes 19 | 20 | Requires: numpy, matplotlib, argparse, scikit-image (>=0.13, for marching cubes) 21 | 22 | Author: Nicholas Lawrance (nicholas.lawrance@mavt.ethz.ch) 23 | 24 | """ 25 | 26 | if shutil.which('latex'): 27 | plt.rc('font', **{'family': 'serif', 'sans-serif': ['Computer Modern Roman']}) 28 | plt.rc('text', usetex=True) 29 | 30 | parser = argparse.ArgumentParser(description='Basic visualisation of configuration space for mobile robot') 31 | parser.add_argument('-nx', type=int, default=61, help='Resolution (n points in each dimension') 32 | parser.add_argument('-rf', '--robot-footprint', default='config/bar_robot.csv', help='Robot footprint csv file') 33 | parser.add_argument('-no', '--n-obstacles', type=int, default=5, help='Number of obstacles') 34 | parser.add_argument('-ns', '--n-samples', type=int, default=5, help='Number of sample locations for testing') 35 | parser.add_argument('-ss', '--std-samples', type=float, default=0.1, help='Sample standard deviation') 36 | parser.add_argument('--seed', type=int, default=5, help='Numpy random seed') 37 | parser.add_argument('--animation', action='store_true', help='Generate animation') 38 | args = parser.parse_args() 39 | 40 | nx = args.nx 41 | num_obstacles = args.n_obstacles 42 | n_obs_samples = args.n_samples 43 | obs_std = args.std_samples 44 | np.random.seed(args.seed) 45 | 46 | # Generate obstacles (random points then convex hull) 47 | obs_centres = [poly.Point(*np.random.uniform(size=2)) for i in range(num_obstacles)] 48 | obstacles = [] 49 | for pc in obs_centres: 50 | px, py = np.random.normal(pc, obs_std, size=(n_obs_samples, 2)).T 51 | px, py = np.clip(px, 0.0, 1.0), np.clip(py, 0.0, 1.0) 52 | p = poly.PointList([poly.Point(x, y) for x, y in zip(px, py)]) 53 | p = poly.convex_hull(p) 54 | obstacles.append(p) 55 | 56 | # Get some random points and see if they're in the obstacles: 57 | in_obs, out_obs = poly.PointList([]), poly.PointList([]) 58 | for i in range(200): 59 | p = poly.Point(*np.random.uniform(size=2)) 60 | collision = False 61 | for o in obstacles: 62 | if o.point_inside(p): 63 | collision = True 64 | break 65 | if collision: 66 | in_obs.append(p) 67 | else: 68 | out_obs.append(p) 69 | 70 | f1, a1 = plt.subplots() 71 | h_obs = [] 72 | for o in obstacles: 73 | h_obs.append(PlotPolygon(o, color='lightgrey', zorder=1)) 74 | c_obs = PatchCollection(h_obs) 75 | a1.add_collection(c_obs) 76 | a1.scatter(*zip(*in_obs), color='r', marker='x') 77 | a1.scatter(*zip(*out_obs), color='g', marker='.') 78 | print("Intersect: {0}".format(obstacles[0].intersect(obstacles[1]))) 79 | 80 | # Load the robot shape 81 | robo = robot_tools.Robot2D(footprint_file=args.robot_footprint) 82 | 83 | # Now try robot poses: 84 | a1.add_artist(PlotPolygon(robo.get_current_polygon(), facecolor='r')) 85 | 86 | robo.set_position((0.25, 0.38)) 87 | robo.get_current_polygon().intersect(obstacles[-1]) 88 | 89 | x, y, h = np.linspace(0, 1, nx), np.linspace(0, 1, nx), np.linspace(0, np.pi, nx) 90 | v = np.zeros((len(x), len(y), len(h))) 91 | for i,xi in enumerate(x): 92 | for j, yj in enumerate(y): 93 | robo.set_position((xi, yj)) 94 | for k, hk in enumerate(h): 95 | in_obs = 0.0 96 | robo.set_heading(hk) 97 | fp = robo.get_current_polygon() 98 | for o in obstacles: 99 | if fp.intersect(o): 100 | in_obs = 1.0 101 | break 102 | v[i, j, k] = in_obs 103 | 104 | if hasattr(measure, 'marching_cubes_lewiner'): 105 | verts, faces, normals, values = measure.marching_cubes_lewiner(v, spacing=(x[1]-x[0], y[1]-y[0], (h[1]-h[0])*180/np.pi)) 106 | else: 107 | verts, faces, normals, values = measure.marching_cubes(v, spacing=(x[1]-x[0], y[1]-y[0], (h[1]-h[0])*180/np.pi), ) 108 | ax_lims = [[0, x[-1]], [0, y[-1]], [0, h[-1]*180/np.pi]] 109 | 110 | fig = plt.figure(figsize=(10, 10)) 111 | ax = fig.add_subplot(111, projection='3d') 112 | ax.plot_trisurf(verts[:, 0], verts[:, 1], faces, verts[:, 2], cmap='Spectral', lw=1) 113 | ax.set_xlim(ax_lims[0]) 114 | ax.set_ylim(ax_lims[1]) 115 | ax.set_zlim(ax_lims[2]) 116 | ax.set_xlabel(r'$x_c$') 117 | ax.set_ylabel(r'$y_c$') 118 | ax.set_zlabel(r"$\theta (^{\circ})$") 119 | 120 | robo.set_position([0.1, 0.1]) 121 | f2, a2 = plt.subplots(2, 2) 122 | for i, ax in enumerate(a2.flat): 123 | dex = int(i*0.25*(len(h)-1)) 124 | ax.matshow(v[:, :, dex].transpose(), origin='lower', extent=[0, 1, 0, 1], cmap='Greys') 125 | ax.add_collection(PatchCollection(copy.copy(h_obs))) 126 | robo.set_heading(h[dex]) 127 | ax.add_artist(PlotPolygon(robo.get_current_polygon(), facecolor='r')) 128 | ax.plot(*robo.position, color='g', marker='x') 129 | ax.set_title(r"$\theta = {0:0.1f}$".format(h[dex]*180/np.pi)) 130 | ax.tick_params(top=0, left=0) 131 | 132 | if args.animation: 133 | rotator = TrisurfRotationAnimator(verts, faces, ax_lims=ax_lims, delta_angle=5.0, 134 | x_label=r'$x_c$', y_label=r'$y_c$', z_label=r"$\theta (^{\circ})$") 135 | ani = animation.FuncAnimation(rotator.f, rotator.update, 72, init_func=rotator.init, interval=10, blit=False) 136 | # ani.save('fig/config_space_rotation.gif', writer='imagemagick', fps=15) 137 | ani.save('fig/config_space_rotation.mp4', writer='ffmpeg', fps=int(15), 138 | extra_args=["-crf", "18", "-profile:v", "main", "-tune", "animation", "-pix_fmt", "yuv420p"]) 139 | 140 | plt.show() 141 | -------------------------------------------------------------------------------- /poincare_example/poincare_1d.py: -------------------------------------------------------------------------------- 1 | # Primarily taken from: https://www.math.wisc.edu/~angenent/519.2016s/notes/poincare-map.html 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | from matplotlib import cm 6 | from matplotlib.animation import FuncAnimation 7 | from tqdm import tqdm 8 | 9 | 10 | class PoincarePlotter(object): 11 | # Class for plotting time evolution of one-dimensional periodic dynamic functions (technically any 1D dynamics) 12 | t = 0.0 13 | x = [] 14 | artists = [] 15 | y_lim = [-1.0, 1.0] 16 | legend_entries = [] 17 | 18 | def __init__(self, dynamic_function, ax, x0=[0.0], frames=1000, delta_t=1e-2, n_max=1000, graph_max=10.0, fskip=1): 19 | assert isinstance(dynamic_function, DynamicFunction) 20 | self.f = dynamic_function 21 | self.ax = ax[0] 22 | self.ax2 = ax[1] 23 | self.delta_t = delta_t 24 | self.n_max = n_max 25 | self.x0 = np.array(x0) 26 | self.graph_max = graph_max 27 | self.frames = frames 28 | self.frame_skip = fskip 29 | 30 | self.colours = cm.get_cmap()(np.linspace(0, 1.0, len(self.x0)+2))[1:-1] 31 | 32 | # Solve equations: 33 | self.x = np.zeros((self.frames + 1, len(self.x0)), dtype=float) 34 | self.dx_dt = self.x.copy() 35 | self.x[0] = self.x0 36 | 37 | self.t = np.arange(0.0, (self.frames + 1) * self.delta_t, self.delta_t) 38 | 39 | for i in tqdm(range(self.frames + 1), desc='Solving dynamics: '): 40 | 41 | if i is 0: 42 | self.dx_dt[i] = self.f.dx_dt(self.x[i], self.t[i]) 43 | continue 44 | 45 | if self.f.xt is not None: 46 | self.x[i] = self.f.xt(self.t[i], self.x0) 47 | else: 48 | # TODO: Change to better integration scheme (RK4) 49 | # Euler integration 50 | self.x[i] = self.x[i-1] + self.delta_t * self.dx_dt[i-1] 51 | self.dx_dt[i] = self.f.dx_dt(self.x[i], self.t[i]) 52 | 53 | self.init() 54 | 55 | def init(self): 56 | self.ax.cla() 57 | self.ax.set_xlim(0, 2.0) 58 | self.y_lim = [-1.0, 1.0] 59 | self.ax2.cla() 60 | 61 | self.artists = [] 62 | self.legend_entries = [] 63 | for x, dx_dt, c in zip(self.x[0], self.dx_dt[0], self.colours): 64 | l, = self.ax.plot(self.t[0], x, c=c, lw=2.0) 65 | p, = self.ax.plot(self.t[0], x, 'o', c=l.get_color()) 66 | l2, = self.ax2.plot(x, dx_dt, c=c, lw=2.0) 67 | p2, = self.ax2.plot(x, dx_dt, 'o', c=l.get_color()) 68 | self.artists.extend([l, p, l2, p2]) 69 | self.legend_entries.append('$x_0={0:0.2f}$'.format(x)) 70 | self.ax.grid(True) 71 | self.ax2.grid(True) 72 | self.ax.set_xlabel('$t$') 73 | self.ax.set_ylabel('$x$') 74 | self.ax.set_title(self.f.fname) 75 | self.ax2.set_xlabel('$x$') 76 | self.ax2.set_ylabel('$\partial x / \partial t$') 77 | self.ax2.set_title(self.f.fname) 78 | 79 | if len(self.x0) <= 11: 80 | self.ax.legend(self.artists[0:-1:4], self.legend_entries, loc=4) 81 | return self.artists 82 | 83 | def animate(self, i): 84 | assert i <= self.frames 85 | i = i*self.frame_skip 86 | 87 | for j in range(self.x.shape[1]): 88 | self.artists[4*j].set_data(self.t[:i+1], self.x[:i+1, j]) # Lines 89 | self.artists[4*j+1].set_data(self.t[i], self.x[i, j]) # Points 90 | self.artists[4*j+2].set_data(self.x[:i+1, j], self.dx_dt[:i+1, j]) # Lines 91 | self.artists[4*j+3].set_data(self.x[i, j], self.dx_dt[i, j]) # Points 92 | 93 | self.y_lim[0] = max(min(self.y_lim[0], self.x[i].min()-0.1), -self.graph_max) 94 | self.y_lim[1] = min(max(self.y_lim[1], self.x[i].max()+0.1), self.graph_max) 95 | self.ax.set_ylim(self.y_lim) 96 | self.ax.set_xlim(0, max(2.0, self.t[i])) 97 | 98 | y_lim2 = self.ax2.get_ylim() 99 | x_lim2 = self.ax2.get_xlim() 100 | x_lim2 = [max(min(x_lim2[0], self.x[i].min()), -self.graph_max), 101 | min(max(x_lim2[1], self.x[i].max()), self.graph_max)] 102 | y_lim2 = [max(min(y_lim2[0], self.dx_dt[i].min()), -self.graph_max), 103 | min(max(y_lim2[1], self.dx_dt[i].max()), self.graph_max)] 104 | self.ax2.set_xlim(x_lim2) 105 | self.ax2.set_ylim(y_lim2) 106 | 107 | return self.artists 108 | 109 | 110 | class DynamicFunction(object): 111 | def __init__(self, dx_dt, xt=None, fname='', period=2*np.pi): 112 | self.dx_dt = dx_dt 113 | self.xt = xt # Exact solution for x(t) (if known) 114 | self.fname = fname 115 | self.period = period 116 | 117 | 118 | def linear_damper(x, t): 119 | return - x + 2*np.cos(t) 120 | 121 | 122 | def linear_exact(t, x0=0): 123 | return np.exp(-t)*(x0-1) + np.cos(t) + np.sin(t) 124 | 125 | 126 | def logistic_periodic(x, t): 127 | return -x*(1+x) + 2*np.cos(2*np.pi*t) 128 | 129 | 130 | if __name__ == '__main__': 131 | plt.rc('text', usetex=True) 132 | 133 | linear = DynamicFunction(linear_damper, xt=linear_exact, fname='$\dot{x} = -x + 2\cos(t)$') 134 | logistic = DynamicFunction(logistic_periodic, fname='$\dot{x} = -x(x+1) + 2\cos(2 \pi t)$', period=1.0) 135 | 136 | delta_t = 0.015 137 | fskip = 1 138 | 139 | # x_0 = [1.0] 140 | # x_0 = [0.9, 1.0, 1.1] 141 | # x_0 = [-9.0, -4.0, -1.0, 0.5, 1.0, 1.5, 3.0, 11.0] 142 | # x_0 = [-5.0, 0.0, 1.0, 2.0, 7.0] 143 | # x_0 = np.linspace(-2.0, 2.0, 5) 144 | # x_0 = [0.0] 145 | # x_0 = [-1.0, 0.0, 1.0, 2.0] 146 | x_0 = np.linspace(-1.1, -1.0, 11) 147 | # x_0 = np.append(x_0, [0.0, 1.0, 5.0, 10.0]) 148 | 149 | with plt.style.context('ggplot'): 150 | fh, ah = plt.subplots(1, 2) 151 | fh.set_size_inches([13.5, 6]) #[8.5, 4]) # 152 | 153 | animator = PoincarePlotter(logistic, ah, x0=x_0, delta_t=delta_t, fskip=fskip) 154 | 155 | animation = FuncAnimation(fh, animator.animate, init_func=animator.init, frames=int(1000/fskip), interval=20*fskip, blit=True) 156 | # animation.save('vid/poincare_example.mp4', writer='ffmpeg', dpi=200, fps=1000.0/(20*fskip), 157 | # extra_args=["-crf", "18", "-profile:v", "main", "-tune", "animation", "-pix_fmt", "yuv420p"]) 158 | # animation.save('vid/poincare_example.gif', writer='imagemagick', fps=1000.0/(20*fskip)) 159 | plt.show() 160 | -------------------------------------------------------------------------------- /config_space/potential_field.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from mpl_toolkits.mplot3d.art3d import Poly3DCollection 4 | from matplotlib import cm 5 | from matplotlib.patches import Circle 6 | import matplotlib.animation as animation 7 | from plot_tools.surf_rotation_animation import RotationAnimator 8 | 9 | # Windows stuff if running on Windows (requires pointer to ImageMagick convert for animated gif) 10 | # import os, sys 11 | # imgk_path = os.path.join('C:/', 'Users', 'USER', 'Downloads', 'ImageMagick', 'convert.exe') 12 | # plt.rcParams['animation.convert_path'] = imgk_path 13 | # if imgk_path not in sys.path: sys.path.append(imgk_path) 14 | 15 | class World(object): 16 | def __init__(self, width, height, max_cost=1.0): 17 | self.width = width 18 | self.height = height 19 | self.cost_map = np.zeros((height, width), dtype='float') 20 | self.obs_map = np.zeros((height, width), dtype='bool') 21 | self.obs = [] 22 | self.global_cost = [] 23 | self.max_cost = max_cost 24 | 25 | def reset(self): 26 | self.obs = [] 27 | self.global_cost = [] 28 | self.cost_map *= 0 29 | self.obs_map *= 0 30 | 31 | def add_obstacles(self, centres, radii, Q_star, eta): 32 | for c, r in zip(centres, radii): 33 | new_obstacle = RoundObstacle(c, r, Q_star, eta) 34 | self.obs.append(new_obstacle) 35 | 36 | # Probably a bit silly, but we can cheat because we have circular objects: 37 | for i in range(self.width): 38 | for j in range(self.height): 39 | # Get distances to all obstacle boundaries 40 | d_obs = np.linalg.norm(centres - np.array([i, j]), axis=1) - radii 41 | nearest_obs = np.argmin(d_obs) 42 | d_obs = d_obs[nearest_obs] 43 | if d_obs <= 0: 44 | self.obs_map[i, j] = 1 45 | self.cost_map[i, j] = self.max_cost 46 | else: 47 | self.cost_map[i, j] += min(self.max_cost, self.obs[nearest_obs].cost(d_obs)) 48 | 49 | def add_global_cost(self, global_cost): 50 | self.global_cost = global_cost 51 | for i in range(self.width): 52 | for j in range(self.height): 53 | self.cost_map[i,j] += self.global_cost.get_cost([i, j]) 54 | self.cost_map[i,j] = min(self.cost_map[i,j], self.max_cost) 55 | 56 | 57 | class RoundObstacle(object): 58 | def __init__(self, pos, r, Q_star, eta): 59 | self.pos = pos 60 | self.r = r 61 | self.Q_star = Q_star 62 | self.half_eta = 0.5*eta 63 | self.inv_Qstar = 1.0/Q_star 64 | 65 | def cost(self, d): 66 | if d <= 0 or d > self.Q_star: 67 | u = 0.0 68 | else: 69 | u = self.half_eta*(1.0/d - self.inv_Qstar)**2 70 | return u 71 | 72 | def get_cost(self, p): 73 | d = np.linalg.norm(self.pos - p ) - self.r 74 | return self.cost(d) 75 | 76 | 77 | class CombinedGlobalPotential(object): 78 | def __init__(self, goal, zeta, d_star): 79 | self.goal = goal 80 | self.zeta = zeta 81 | self.d_star = d_star 82 | self.offset = 0.5*self.zeta*self.d_star**2 83 | 84 | def get_cost(self, p): 85 | d = np.linalg.norm(p-self.goal) 86 | if d <= self.d_star: 87 | u = 0.5*self.zeta*d**2 88 | else: 89 | u = self.d_star*self.zeta*d -self.offset 90 | return u 91 | 92 | 93 | def gradient_path(dx, dy, start, goal, step=0.1, goal_range = 0.5): 94 | path = [start.copy()] 95 | cp = start.copy() 96 | while np.linalg.norm(goal-cp) > goal_range: 97 | d_int = np.rint(cp).astype(int) 98 | ddx, ddy = dx[d_int[0], d_int[1]], dy[d_int[0], d_int[1]] 99 | ddd = np.sqrt(ddx**2 + ddy**2) 100 | cp[0] = cp[0] - step*ddx/ddd 101 | cp[1] = cp[1] - step*ddy/ddd 102 | path.append(cp.copy()) 103 | return np.array(path) 104 | 105 | w = 100 106 | h = 100 107 | n_obs = 5 108 | goal = np.array([5.0, 5.0]) 109 | start = np.array([70.0, 90.0]) 110 | 111 | my_world = World(w, h) 112 | 113 | # centres = np.zeros((n_obs, 2)) 114 | # centres[:, 0] = np.random.uniform(0, w, n_obs) 115 | # centres[:, 1] = np.random.uniform(0, h, n_obs) 116 | # ranges = np.random.uniform(5.0, 15.0, n_obs) 117 | centres = np.array([[20, 30], [80, 10], [10, 78.8], [90.5, 75]]) 118 | ranges = np.array([5.0, 15.0, 12.0, 11.0]) 119 | 120 | my_world.add_obstacles(centres, ranges, Q_star=15.0, eta=15.0) 121 | 122 | fo, ho = plt.subplots() 123 | ho.matshow(my_world.cost_map.T, origin='lower') #, extent=[0,w,0,h]) 124 | 125 | fg, hg = plt.subplots(1, 2) #, sharex=True, sharey=True, subplot_kw={'aspect':'equal'}) 126 | fg.set_size_inches([9.6, 3.5]) 127 | my_world.add_global_cost(CombinedGlobalPotential(goal, zeta=1.0e-4, d_star=50.0)) 128 | h_potential = hg[0].matshow(my_world.cost_map.T, origin='lower', cmap=cm.coolwarm) # , extent=[0,w,0,h] 129 | hg[0].xaxis.set_ticks_position('bottom') 130 | hg[0].plot(goal[0], goal[1], 'yo') 131 | 132 | X, Y = np.meshgrid(np.arange(w), np.arange(h)) 133 | dx, dy = np.gradient(my_world.cost_map) 134 | magD = np.sqrt(dx**2 + dy**2) 135 | magD[magD == 0] =1.0 136 | U = -(dx/magD).T 137 | V = -(dy/magD).T 138 | harr = hg[0].quiver(X[::4,::4], Y[::4,::4], U[::4,::4], V[::4,::4]) 139 | h_contour = hg[1].contour(my_world.cost_map.T, np.linspace(0, 0.5, 11), cmap=cm.coolwarm) 140 | plt.colorbar(h_potential, ax=hg[0]) 141 | for o in my_world.obs: 142 | hg[0].add_artist(Circle( o.pos, o.r+o.Q_star, fc='None', ec='k')) 143 | for hh in hg: 144 | hh.set_xlim(-0.5, w-0.5) 145 | hh.set_ylim(-0.5, h-0.5) 146 | hh.set_aspect('equal') 147 | 148 | path = gradient_path(dx, dy, start, goal, step=0.1, goal_range=1.0) 149 | h_path = hg[0].plot(path[:, 0], path[:, 1], 'r-') 150 | z_path = np.zeros(path.shape[0]) 151 | for i, (xp, yp) in enumerate(path): 152 | z_path[i] = my_world.cost_map[int(xp), int(yp)] 153 | 154 | 155 | f3 = plt.figure() 156 | a3 = f3.add_subplot(111, projection='3d') 157 | h_path3 = a3.plot(path[:,0], path[:,1], z_path, 'r-') 158 | h_surf = a3.plot_surface(X, Y, my_world.cost_map.T, cmap=cm.coolwarm) 159 | a3.plot([goal[0]], [goal[1]], [my_world.cost_map[int(goal[0]), int(goal[1])]], 'yo') 160 | 161 | angles = np.linspace(-60, 300, 51)[:-1] # A list of 20 angles between 0 and 360 162 | 163 | rotator = RotationAnimator(X, Y, my_world.cost_map, goal, start_azim=-60, delta_angle=5.0) 164 | ani = animation.FuncAnimation(rotator.f, rotator.update, 72, init_func=rotator.init, interval=10, blit=False) 165 | # ani.save('fig/animation.gif', writer='imagemagick', fps=30) 166 | 167 | plt.show() 168 | -------------------------------------------------------------------------------- /config_space/rrt_simple.py: -------------------------------------------------------------------------------- 1 | import time 2 | import logging 3 | import argparse 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | import robot_tools 7 | from matplotlib.collections import LineCollection 8 | from scipy.spatial import KDTree, Voronoi, voronoi_plot_2d, distance 9 | 10 | 11 | """ 12 | 13 | Simple RRT implementation for visualising RRTs with Voronoi regions 14 | 15 | Requires: numpy, scipy, matplotlib, argparse, pyyaml 16 | 17 | Author: Nicholas Lawrance (nicholas.lawrance@mavt.ethz.ch) 18 | 19 | """ 20 | 21 | 22 | class RRTree(object): 23 | _start = None 24 | _goal = None 25 | _kdtree = None 26 | _kd = False 27 | vertices = [] 28 | edges = [] 29 | 30 | def __init__(self, world, start=None, goal=None, delta=1.0, 31 | collision_step=0.01, nearest='cdist'): 32 | self.world = robot_tools.PlanningProblem(world) 33 | logging.info('Loaded RRT search problem from {0}'.format(world)) 34 | self._delta = delta 35 | if nearest == 'kdtree': 36 | self._get_nearest = self._get_nearest_kd 37 | self._kd = True 38 | logging.info('Using kd tree nearest-neighbour search') 39 | else: 40 | self._get_nearest = self._get_nearest_cdist 41 | logging.info('Using scipy cdist nearest-neighbour search') 42 | 43 | if start is not None: 44 | self.set_start(start) 45 | if goal is not None: 46 | self.set_goal(goal) 47 | self._collision_step_dist = collision_step*distance.euclidean(self.world.workspace.limits[:,0], self.world.workspace.limits[:,1]) 48 | 49 | def set_start(self, start): 50 | # Slightly hacky because we are assuming planning in workspace, not config space 51 | assert len(start) == len(self.world.workspace.limits) 52 | self._start = np.array(start) 53 | 54 | def set_goal(self, goal): 55 | # Must be a poly.Polygon type 56 | self._goal = goal 57 | 58 | def search(self, imax = 1000): 59 | self.vertices = [self._start] 60 | self.edges = [[]] 61 | if self._kd: 62 | self._kdtree = KDTree(self.vertices) 63 | 64 | for i in range(0, imax): 65 | self._search_step() 66 | 67 | def _search_step(self): 68 | x_rand = self._sample_freespace() 69 | i_near = self._get_nearest(x_rand) 70 | x_near = self.vertices[i_near] 71 | x_new = self._steer(x_near, x_rand) 72 | if self._valid_pose(x_new) and self._collision_free(x_near, x_new): 73 | self.vertices.append(x_new) 74 | self.edges.append([]) 75 | self.edges[i_near].append(len(self.vertices) - 1) 76 | if self._kd: 77 | self._kdtree = KDTree(self.vertices) 78 | 79 | # def animated_search(self, imax=100, show_voronoi = True): 80 | 81 | 82 | def _valid_pose(self, x): 83 | self.world.robot.set_position(x) 84 | return not self.world.workspace.in_collision_poly(self.world.robot.get_current_polygon()) 85 | 86 | def _sample_freespace(self, max_iter=1000): 87 | # Assume uniform sampling 88 | valid = False 89 | i = 0 90 | while (not valid) and i < max_iter: 91 | x_rand = np.random.uniform(self.world.workspace.limits[:, 0], self.world.workspace.limits[:, 1]) 92 | valid = self._valid_pose(x_rand) 93 | i += 1 94 | 95 | if i == max_iter: 96 | raise Exception('Max iterations reached - no valid sample found') 97 | 98 | return x_rand 99 | 100 | def _get_nearest_kd(self, point): 101 | # Lazy KDTRee from scipy - deprecated (cdist is much faster) 102 | dist, ind = self._kdtree.query(point, k=1) 103 | return ind 104 | 105 | def _get_nearest_cdist(self, point): 106 | d_full = distance.cdist(self.vertices, [point]).flatten() 107 | i_best = np.argmin(d_full) 108 | return i_best 109 | 110 | def _steer(self, x_near, x_rand): 111 | dd = distance.euclidean(x_near, x_rand) 112 | if dd < self._delta: 113 | return x_rand 114 | return x_near + (x_rand-x_near)/dd*self._delta 115 | 116 | def _collision_free(self, x_near, x_new, dist=None): 117 | # Lazy collision walker 118 | valid = True 119 | d_full = distance.euclidean(x_new, x_near) 120 | v_hat = (x_new - x_near)/d_full 121 | for i in range(1, int(d_full/self._collision_step_dist)+1): 122 | c_pos = x_near + i*self._collision_step_dist*v_hat 123 | valid = self._valid_pose(c_pos) 124 | if not valid: 125 | break 126 | return valid 127 | 128 | def plot_tree(self, ax): 129 | ax.plot([self._start[0]], [self._start[1]], 'go') 130 | h_verts = plt.plot([v[0] for v in self.vertices], [v[1] for v in self.vertices], 'k.') 131 | 132 | lines = [] 133 | for i_start in range(len(self.edges)): 134 | for i_stop in self.edges[i_start]: 135 | lines.append([self.vertices[i_start], self.vertices[i_stop]]) 136 | lc = LineCollection(lines, linewidths=2, colors='firebrick') 137 | ax.add_collection(lc) 138 | return h_verts, lc 139 | 140 | def plot_voronoi(self, ax, **kwargs): 141 | vor = Voronoi(self.vertices) 142 | voronoi_plot_2d(vor, ax=ax, **kwargs) 143 | 144 | 145 | if __name__ == "__main__": 146 | plt.rc('font', **{'family': 'serif', 'sans-serif': ['Computer Modern Roman']}) 147 | plt.rc('text', usetex=True) 148 | 149 | parser = argparse.ArgumentParser(description='Basic RRT example for plotting') 150 | parser.add_argument('-w', '--world', default='config/rrt_sample_world.yaml', help='World definition (obstacles)') 151 | parser.add_argument('-i', '--iterations', type=int, default=100, help='Max RRT iterations') 152 | parser.add_argument('--seed', type=int, default=1, help='Random seed') 153 | parser.add_argument('--save-fig', action='store_true', help='Save figure location') 154 | parser.add_argument('-kd', action='store_true', help='Use kd_tree (it is outright slower though!)') 155 | parser.add_argument('--loglevel', default='INFO', type=str, 156 | help='World definition (obstacles)') 157 | args = parser.parse_args() 158 | np.random.seed(args.seed) 159 | 160 | numeric_level = getattr(logging, args.loglevel.upper(), None) 161 | if not isinstance(numeric_level, int): 162 | raise ValueError('Invalid log level: %s' % args.loglevel) 163 | logging.basicConfig(level=numeric_level) 164 | 165 | if args.kd: 166 | nearest = 'kdtree' 167 | else: 168 | nearest = 'cdist' 169 | rrt = RRTree(args.world, nearest=nearest) 170 | 171 | start_pos = [5.0, 5.0] 172 | rrt.set_start(start_pos) 173 | t0 = time.time() 174 | logging.info('Running RRT search with max iterations n={0}'.format(args.iterations)) 175 | rrt.search(imax=args.iterations) 176 | logging.info('RRT search complete in {0}s ({1})'.format(time.time()-t0, nearest)) 177 | f = plt.figure(figsize=(6, 6)) 178 | ax = f.add_subplot(1, 1, 1) 179 | if len(rrt.vertices) >= 4: 180 | rrt.plot_voronoi(ax, show_points=False, show_vertices=False, line_colors='grey') 181 | rrt.world.workspace.plot(ax) 182 | rrt.plot_tree(ax) 183 | 184 | if args.save_fig: 185 | f.savefig('fig/rrt/{0:04}.png'.format(args.iterations), bbox_inches='tight') 186 | else: 187 | plt.show() 188 | 189 | 190 | -------------------------------------------------------------------------------- /config_space/robot_tools.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import polygon_tools as poly 3 | import csv 4 | import yaml 5 | import sys 6 | import matplotlib as plt 7 | from matplotlib.patches import Polygon as PlotPolygon 8 | from matplotlib.collections import PatchCollection 9 | import matplotlib.cm as cm 10 | import logging 11 | 12 | 13 | def robot_builder(robot): 14 | # Note that the robot type must be implemented in this module, so the example robot: 15 | # {type: RobotArm2D, parameters: {base_position: [5.0, 5.0], link_lengths: [2.1, 2.1]} 16 | # would call as constructor: robot_tools.RobotArm2D(base_position=[5.0, 5.0], link_lengths=[2.1, 2.1]) 17 | return getattr(sys.modules[__name__], robot['type'])(**robot['parameters']) 18 | 19 | 20 | def workspace_builder(workspace): 21 | # Note that the workspace type must be implemented in this module, so the example workspace: 22 | # {type: Workspace2D, parameters: {limits=[[0,1.0],[0,1.0]], obstacles=[]} 23 | # would call as constructor: robot_tools.Workspace2D(limits=[[0,1.0],[0,1.0]], obstacles=[]) 24 | return getattr(sys.modules[__name__], workspace['type'])(**workspace['parameters']) 25 | 26 | 27 | class Workspace2D(object): 28 | def __init__(self, limits=[[0,1.0],[0,1.0]], obstacles=[]): 29 | self.limits = np.array(limits) 30 | assert self.limits.shape == (2,2), 'Currently only implemented for 2D workspaces' 31 | self.obstacles = [] 32 | for ob in obstacles: 33 | # Add each obstacle (must be a Polygon or derived class like Rectangle from poly_tools) 34 | self.obstacles.append(getattr(poly, ob['type'])(**ob['parameters'])) 35 | 36 | def in_collision_point(self, point): 37 | p = poly.Point(*point) 38 | collision = False 39 | for o in self.obstacles: 40 | if o.point_inside(p): 41 | collision = True 42 | break 43 | return collision 44 | 45 | def in_collision_poly(self, polygon): 46 | collision = False 47 | for o in self.obstacles: 48 | if polygon.intersect(o): 49 | collision = True 50 | break 51 | return collision 52 | 53 | def plot(self, hax=None, cmap=cm.viridis): 54 | if hax is None: 55 | f, hax = plt.subplots(1) 56 | h_obs = [] 57 | for o in self.obstacles: 58 | h_obs.append(PlotPolygon(o, zorder=1)) 59 | c_obs = PatchCollection(h_obs, cmap=cmap) 60 | # This sets colors for some reason (command in Polygon does not) 61 | c_obs.set_array(np.linspace(0, 1.0, len(self.obstacles) + 1)[1:]) 62 | hax.add_collection(c_obs) 63 | 64 | hax.set_aspect('equal') 65 | 66 | hax.set_xlabel(r'$x$') 67 | hax.set_ylabel(r'$y$') 68 | hax.set_xlim(self.limits[0]) 69 | hax.set_ylim(self.limits[1]) 70 | 71 | 72 | class Robot2D(object): 73 | _c_poly = None 74 | 75 | def __init__(self, pos=[0.0, 0.0], heading=0.0, footprint=[(0.0, 0.0)], footprint_file=None): 76 | self.R = np.eye(2) 77 | 78 | if footprint_file is not None: 79 | with open(footprint_file, mode='r') as fh: 80 | csv_reader = csv.reader(fh) 81 | footprint = [] 82 | for row in csv_reader: 83 | assert len(row) == 2, 'Row {0} does not have 2 elements'.format(len(row)+1) 84 | footprint.append([float(row[0]), float(row[1])]) 85 | logging.info('Loaded robot footprint file {0} with {1} points'.format(footprint_file, len(footprint))) 86 | 87 | self.position = poly.Point(pos[0], pos[1]) 88 | self.footprint = poly.PointList(footprint) 89 | self.heading = heading 90 | self._set_heading_transformation() 91 | self._update_poly() 92 | 93 | 94 | def _set_heading_transformation(self): 95 | ct, st = np.cos(self.heading), np.sin(self.heading) 96 | self.R = np.array([[ct, -st], [st, ct]]) 97 | 98 | def set_heading(self, heading): 99 | self.heading = heading 100 | self._set_heading_transformation() 101 | self._update_poly() 102 | 103 | def set_position(self, pos): 104 | self.position = pos 105 | self._update_poly() 106 | 107 | def set_footprint(self, footprint): 108 | self.footprint = footprint 109 | self._update_poly() 110 | 111 | def _update_poly(self): 112 | self._c_poly = poly.Polygon([poly.Point(*(np.matmul(self.R, p)+self.position)) for p in self.footprint]) 113 | 114 | def get_current_polygon(self): 115 | return self._c_poly 116 | 117 | 118 | class RobotArm2D(object): 119 | _spine_pts = None 120 | 121 | def __init__(self, base_position=[0.0, 0.0], link_lengths=[1.0, 1.0], link_angles=[0.0, 0.0]): 122 | # Assume arm angles are relative (can be summed) 123 | 124 | self._base_position = poly.Point(base_position[0], base_position[1]) 125 | 126 | assert len(link_lengths) == len(link_angles) 127 | self._link_lengths = np.array(link_lengths) 128 | self._link_angles = np.array(link_angles) 129 | 130 | self._R = [np.eye(2) for i in self._link_angles] 131 | self._set_rotation_transforms() 132 | 133 | def set_link_angles(self, link_angles): 134 | self._link_angles = np.array(link_angles) 135 | self._set_rotation_transforms() 136 | 137 | def _set_rotation_transforms(self): 138 | sum_angles = self._link_angles.cumsum() 139 | for i, theta in enumerate(sum_angles): 140 | ct, st = np.cos(theta), np.sin(theta) 141 | self._R[i] = np.array([[ct, -st], [st, ct]]) 142 | self._set_spine_points() 143 | 144 | def get_current_polygon(self): 145 | # Run backwards through the points to make a polygon 146 | return poly.Polygon(self._spine_pts + self._spine_pts[-2:0:-1]) 147 | 148 | def _set_spine_points(self): 149 | self._spine_pts = [self._base_position] 150 | for R, ll in zip(self._R, self._link_lengths): 151 | self._spine_pts.append(poly.Point(*(np.matmul(R, [ll, 0])+self._spine_pts[-1]))) 152 | 153 | def get_spine_points(self): 154 | return [p.x for p in self._spine_pts], [p.y for p in self._spine_pts] 155 | 156 | def get_end_effector_position(self): 157 | return self._spine_pts[-1] 158 | 159 | def end_effector_path(self, config_path): 160 | c_pose = self._link_angles.copy() 161 | ee_path = [] 162 | for pose in config_path: 163 | self.set_link_angles(pose) 164 | ee_path.append(self.get_end_effector_position()) 165 | self.set_link_angles(c_pose) 166 | return np.array(ee_path) 167 | 168 | 169 | class PlanningProblem(object): 170 | 171 | def __init__(self, world_file): 172 | 173 | # Load world 174 | with open(world_file, 'r') as fh: 175 | world = yaml.safe_load(fh) 176 | 177 | self.workspace = workspace_builder(world['workspace']) 178 | # Note that the robot type must be implemented in the robot_tools module, so the example robot: 179 | # {type: RobotArm2D, parameters: {base_position: [5.0, 5.0], link_lengths: [2.1, 2.1]} 180 | # would call as constructor: robot_tools.RobotArm2D(base_position=[5.0, 5.0], link_lengths=[2.1, 2.1]) 181 | self.robot = robot_builder(world['robot']) 182 | 183 | 184 | def construct_config_space(self, nx=101): 185 | # TODO: This should be more general (number of dimensions, wraparound etc. in the robot class) 186 | theta1, theta2 = np.linspace(0, 2.0 * np.pi, nx), np.linspace(0, 2.0 * np.pi, nx) 187 | v = np.zeros((len(theta1), len(theta2)), dtype=int) 188 | 189 | for i, t1 in enumerate(theta1): 190 | for j, t2 in enumerate(theta2): 191 | self.robot.set_link_angles([t1, t2]) 192 | in_obs = 0 193 | fp = self.robot.get_current_polygon() 194 | for o_num, o in enumerate(self.workspace.obstacles): 195 | if fp.intersect(o): 196 | in_obs = o_num + 1 197 | break 198 | v[i, j] = in_obs 199 | 200 | return [theta1, theta2], v -------------------------------------------------------------------------------- /config_space/arm_config_space.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import polygon_tools as poly 4 | import robot_tools 5 | from matplotlib.patches import Polygon as PlotPolygon 6 | from matplotlib.collections import PatchCollection 7 | import matplotlib.animation as animation 8 | import matplotlib.cm as cm 9 | import matplotlib.colors 10 | import argparse 11 | import shutil 12 | from matplotlib.ticker import MultipleLocator 13 | 14 | """ 15 | 16 | Plot the config space from Introduction to Autonomous Mobile Robots Fig 6.1 17 | 18 | Requires: numpy, matplotlib, argparse 19 | 20 | Author: Nicholas Lawrance (nicholas.lawrance@mavt.ethz.ch) 21 | 22 | """ 23 | 24 | if shutil.which('latex'): 25 | plt.rc('font', **{'family': 'serif', 'sans-serif': ['Computer Modern Roman']}) 26 | plt.rc('text', usetex=True) 27 | 28 | # colour_map = cm.viridis.copy() 29 | colour_map = matplotlib.colors.ListedColormap([[0.4, 0.4, 0.4, 1.0]]) 30 | 31 | 32 | def angle_wrap(angles): 33 | return angles % (2 * np.pi) 34 | 35 | 36 | def linear_path(points, n=50): 37 | c_point = points[0] 38 | path = np.array([c_point]) 39 | 40 | for end_point in points[1:]: 41 | new_path = np.linspace(path[-1], end_point, n) # Requires numpy > 1.16.0 42 | path = np.concatenate((path, new_path[1:])) 43 | 44 | return path 45 | 46 | 47 | def plot_config_space(ax, obstacles, arm, cspace_array, col_map, xlim, ylim, theta1_lim, theta2_lim): 48 | h_obs = [] 49 | for o in obstacles: 50 | h_obs.append(PlotPolygon(o, zorder=1)) 51 | c_obs = PatchCollection(h_obs, cmap=col_map) 52 | # This sets colors for some reason (command in Polygon does not) 53 | c_obs.set_array(np.linspace(0, 1.0, len(obstacles) + 1)[1:]) 54 | ax[0].add_collection(c_obs) 55 | 56 | h_arm, = ax[0].plot(*arm.get_spine_points(), c='black', lw=3.0) 57 | 58 | for a in ax: 59 | a.set_aspect('equal') 60 | 61 | ax[0].set_xlabel(r'$x$') 62 | ax[0].set_ylabel(r'$y$') 63 | ax[0].set_xlim(xlim) 64 | ax[0].set_ylim(ylim) 65 | 66 | ax[1].set_xlabel(r'$\theta_1$') 67 | ax[1].set_ylabel(r'$\theta_2$') 68 | ax[1].set_xlim(theta1_lim[0], theta1_lim[-1]) 69 | ax[1].set_ylim(theta2_lim[0], theta2_lim[-1]) 70 | 71 | # This is a bit dumb, should probably just assume [0, 2pi) everywhere, but meh 72 | ax[0].xaxis.set_major_locator(MultipleLocator(2.0)) 73 | ax[0].xaxis.set_minor_locator(MultipleLocator(1.0)) 74 | ax[0].yaxis.set_major_locator(MultipleLocator(2.0)) 75 | ax[0].yaxis.set_minor_locator(MultipleLocator(1.0)) 76 | ax[1].set_xticks([0, np.pi / 2, np.pi, 3 * np.pi / 2, 2 * np.pi]) 77 | ax[1].set_yticks([0, np.pi/2, np.pi, 3*np.pi/2, 2*np.pi]) 78 | ax[1].xaxis.set_minor_locator(MultipleLocator(np.pi/4)) 79 | ax[1].yaxis.set_minor_locator(MultipleLocator(np.pi/4)) 80 | 81 | ax[1].set_xticklabels([r'$0$', r'$\pi/2$', r'$\pi$', r'3$\pi/2$', r'$2\pi$']) 82 | ax[1].set_yticklabels([r'$0$', r'$\pi/2$', r'$\pi$', r'3$\pi/2$', r'$2\pi$']) 83 | for a in ax: 84 | a.grid(which='both', axis='both') 85 | a.set_axisbelow(True) 86 | 87 | cspace_array = np.ma.masked_where(cspace_array == 0.0, cspace_array) 88 | col_map.set_bad(color='white') 89 | ax[1].imshow(cspace_array.transpose(), origin='lower', cmap=col_map, 90 | extent=[theta1_lim[0], theta1_lim[1], theta2_lim[0], theta2_lim[1]]) 91 | return h_arm 92 | 93 | 94 | class ArmAnimator(object): 95 | h_arm = None 96 | plot_artists = [] 97 | 98 | def __init__(self, arm, obstacles, cspace_array, path, x_lim, y_lim, t1_lim, t2_lim, col_map=cm.viridis.copy(), 99 | shadow_skip=0): 100 | 101 | self.fig, self.ax = plt.subplots(1, 2) 102 | self.fig.set_size_inches([9.6, 5.4]) # 1920*1080 at 200 dpi 103 | self.arm = arm 104 | self.obstacles = obstacles 105 | self.cspace_array = cspace_array 106 | self.path = path 107 | self.cmap = col_map 108 | self.x_lim = x_lim 109 | self.y_lim = y_lim 110 | self.t1lim = t1_lim 111 | self.t2lim = t2_lim 112 | self.max_frames = self.path.shape[0] 113 | self.end_effector_path = poly.PointList([]) 114 | self._shadow_skip = shadow_skip 115 | 116 | def init_fig(self): 117 | for a in self.ax: 118 | a.cla() 119 | 120 | self.arm.set_link_angles(self.path[0]) 121 | self.h_arm = plot_config_space(self.ax, self.obstacles, self.arm, self.cspace_array, self.cmap, self.x_lim, 122 | self.y_lim, self.t1lim, self.t2lim) 123 | 124 | self.last_break = 0 125 | self.h_path, = self.ax[1].plot(self.path[:1, 0], self.path[:1, 1], 'r--') 126 | self.h_pathend, = self.ax[1].plot(self.path[0, 0], self.path[0, 1], 'ro') 127 | 128 | self.end_effector_path = poly.PointList([self.arm.get_end_effector_position()]) 129 | self.h_ee_path, = self.ax[0].plot([self.end_effector_path[0].x], [self.end_effector_path[0].y], 'r--') 130 | self.h_ee_pathend, = self.ax[0].plot([self.end_effector_path[0].x], [self.end_effector_path[0].y], 'ro') 131 | self.plot_artists = [self.h_arm, self.h_path, self.h_pathend, self.h_ee_path, self.h_ee_pathend] 132 | 133 | return self.plot_artists 134 | 135 | def animate(self, i): 136 | 137 | # If plotting extra arm shadows, add them to the plot_artists 138 | if self._shadow_skip != 0 and i % self._shadow_skip == 0: 139 | gv = 0.9-float(i)/self.max_frames*0.9 140 | h_arm_shadow = self.ax[0].plot(*self.arm.get_spine_points(), c=[gv, gv, gv], lw=1.0) 141 | h_arm_shadow.extend(self.plot_artists) 142 | self.plot_artists = h_arm_shadow 143 | 144 | self.arm.set_link_angles(self.path[i]) 145 | self.h_arm.set_data(*self.arm.get_spine_points()) 146 | 147 | # If the path crosses one of the boundaries, break it and add a new path 148 | if any(abs(self.path[i] - self.path[i-1]) > np.pi): 149 | old_path, = self.ax[1].plot(self.path[self.last_break:i, 0], self.path[self.last_break:i, 1], 'r--') 150 | self.plot_artists.append(old_path) 151 | self.last_break = i 152 | 153 | self.h_path.set_data(self.path[self.last_break:(i+1), 0], self.path[self.last_break:(i+1), 1]) 154 | self.h_pathend.set_data(self.path[i, 0], self.path[i, 1]) 155 | 156 | self.end_effector_path.append(self.arm.get_end_effector_position()) 157 | self.h_ee_path.set_data(*self.end_effector_path.get_xy()) 158 | self.h_ee_pathend.set_data([self.end_effector_path[-1].x], [self.end_effector_path[-1].y]) 159 | 160 | return self.plot_artists 161 | 162 | 163 | 164 | if __name__ == "__main__": 165 | parser = argparse.ArgumentParser(description='Plot the config space from Fig 6.1 in Intro to AMR textbook') 166 | parser.add_argument('-nx', type=int, default=201, help='Resolution (n points in each dimension)') 167 | parser.add_argument('-w', '--world', default='config/block_world.yaml', help='World definition (obstacles)') 168 | parser.add_argument('-sa', '--save-animation', default=None, help='Save animation to file') 169 | parser.add_argument('-na', '--no-animation', action='store_true', help='Don\'t animate') 170 | parser.add_argument('--arm-shadows', type=int, default=0, 171 | help='Plot shadows of arm position every n steps (0 for off)') 172 | args = parser.parse_args() 173 | 174 | 175 | arm_problem = robot_tools.PlanningProblem(args.world) 176 | theta, c_space = arm_problem.construct_config_space(args.nx) 177 | 178 | # Example path from textbook 179 | path_fit = np.polyfit([0.3, 1.6, 4.3, 5.9], [1.2, 0.8, 3.3, 3.2], 3) 180 | path_theta2 = np.linspace(0.3, 5.9, 300) 181 | path_theta1 = np.polyval(path_fit, path_theta2) 182 | path_full = angle_wrap(np.array([path_theta1, path_theta2]).T) 183 | 184 | # Piecewise linear path 185 | # path_full = angle_wrap(linear_path([[1.2, 0.3], [2.5, -2.4], [4, -2.4], [3.2, -0.2]], 100)) 186 | 187 | ee_path = arm_problem.robot.end_effector_path(path_full) 188 | 189 | f1, a1 = plt.subplots(1, 2) 190 | f1.set_size_inches([9.6, 4.8]) 191 | # f2, a2 = plt.subplots(1, 1) 192 | # f2.set_size_inches([4.8, 4.8]) 193 | # a1 = [a1, a2] 194 | arm_problem.robot.set_link_angles(path_full[0]) 195 | try: 196 | map_lims = arm_problem.workspace.limits 197 | except KeyError: 198 | map_lims = [[0, 10], [0, 10]] 199 | plot_config_space(a1, arm_problem.workspace.obstacles, arm_problem.robot, c_space, colour_map, map_lims[0], map_lims[1], 200 | theta[0][[0, -1]], theta[1][[0, -1]]) 201 | 202 | tt = np.linspace(0, 2*np.pi, 101) 203 | a1[0].plot(arm_problem.robot._base_position.x + arm_problem.robot._link_lengths[0]*np.cos(tt), 204 | arm_problem.robot._base_position.y + arm_problem.robot._link_lengths[0]*np.sin(tt), 205 | '--', 206 | color='grey', 207 | lw=0.5) 208 | # a1[0].plot(ee_path[:, 0], ee_path[:, 1], 'r--') 209 | # a1[1].plot(path_full[:, 0], path_full[:, 1], 'r--') 210 | 211 | if not args.no_animation: 212 | # Animation 213 | animation_length = 10.0 214 | arm_anim = ArmAnimator(arm_problem.robot, arm_problem.workspace.obstacles, c_space, path_full, map_lims[0], map_lims[1], theta[0][[0, -1]], theta[1][[0, -1]], 215 | shadow_skip=args.arm_shadows) 216 | delta_t = (animation_length * 1000.0 / arm_anim.max_frames) 217 | arm_animation = animation.FuncAnimation(arm_anim.fig, arm_anim.animate, init_func=arm_anim.init_fig, frames=arm_anim.max_frames, 218 | interval=delta_t, blit=True) 219 | 220 | if args.save_animation is not None: 221 | # animation.save('fig/arm_config_space_video.gif', writer='imagemagick', fps=1000.0/delta_t) 222 | # animation.save('fig/arm_config/%03d.png', writer='imagemagick') 223 | arm_animation.save(args.save_animation, writer='ffmpeg', fps=int(1000.0/delta_t), dpi=200, 224 | extra_args=["-crf", "18", "-profile:v", "main", "-tune", "animation", "-pix_fmt", "yuv420p"]) 225 | # # Final plot frame 226 | # arm_anim.fig.savefig('fig/arm_config_space_final.pdf') 227 | # arm_anim.fig.savefig('fig/arm_config_space_final.png') 228 | plt.show() 229 | -------------------------------------------------------------------------------- /config_space/polygon_tools.py: -------------------------------------------------------------------------------- 1 | # Some basic tools for polygonal geometry 2 | 3 | # Convex hull part sourced from: 4 | # https://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain#Python 5 | # Partially adapted from C++ code Dan Sunday, at http://geomalgorithms.com/a09-_intersect-3.html 6 | # // Copyright 2001 softSurfer, 2012 Dan Sunday 7 | # // This code may be freely used and modified for any purpose 8 | # // providing that this copyright notice is included with it. 9 | # // SoftSurfer makes no warranty for this code, and cannot be held 10 | # // liable for any real or imagined damage resulting from its use. 11 | # // Users of this code must verify correctness for their application. 12 | # // 13 | # // Assume that classes are already given for the objects: 14 | # // Point with 2D coordinates {float x, y;} 15 | # // Polygon with n vertices {int n; Point *V;} with V[n]=V[0] 16 | # // Tnode is a node element structure for a BBT 17 | # // BBT is a class for a Balanced Binary Tree 18 | # // such as an AVL, a 2-3, or a red-black tree 19 | # // with methods given by the placeholder code: 20 | 21 | from collections import namedtuple 22 | 23 | # Points should be 2-tuple (x,y) 24 | Point = namedtuple('Point', ('x', 'y')) 25 | OrderedEdge = namedtuple('OrderedEdge', ('lp', 'rp')) 26 | 27 | 28 | class PointList(list): 29 | bounds = None 30 | 31 | def __init__(self, p_in): 32 | p_out = [] 33 | for p in p_in: 34 | if not isinstance(p, Point): 35 | assert len(p) == 2 36 | p = Point(p[0], p[1]) 37 | p_out.append(p) 38 | super(PointList, self).__init__(p_out) 39 | 40 | def min_yx_index(self): 41 | im = 0 42 | for i, pi in enumerate(self): 43 | if pi.y < self[im].y: 44 | im = i 45 | elif pi.y == self[im].y and pi.x < self[im].x: 46 | im = i 47 | return im 48 | 49 | def swap(self, i, j): 50 | self[i], self[j] = self[j], self[i] 51 | 52 | def get_bounds(self): 53 | # returns [minx, maxx, miny, maxy] 54 | if self.bounds is None: 55 | self.bounds = [min(self, key=lambda t:t[0])[0], 56 | max(self, key=lambda t:t[0])[0], 57 | min(self, key=lambda t:t[1])[1], 58 | max(self, key=lambda t:t[1])[1]] 59 | return self.bounds 60 | 61 | def get_xy(self): 62 | x, y = zip(*self) 63 | return x, y 64 | 65 | 66 | class Polygon(PointList): 67 | # List of points (with closure), use method edges() to get iterator over edges 68 | # This automatically closes the polygon, so that Polygon[n] = Polygon[0], but length (n) is still number of verts 69 | 70 | # def __getitem__(self, key): 71 | # if key == len(self): 72 | # return super(Polygon, self).__getitem__(0) 73 | # else: 74 | # return super(Polygon, self).__getitem__(key) 75 | # 76 | # def __iter__(self): 77 | # for p in super(Polygon, self).__iter__(): 78 | # yield p 79 | # yield self[0] 80 | # 81 | # def vertices(self): 82 | # return self[:len(self)] 83 | 84 | def edges(self): 85 | for i in range(len(self)-1): 86 | yield self[i], self[i+1] 87 | yield self[-1], self[0] 88 | 89 | def get_edge(self, i): 90 | return self[i], self[(i+1) % len(self)] 91 | 92 | def point_inside_cn(self, p): 93 | # crossing_number_poly(): crossing number test for a point in a polygon 94 | # Input: P = a point, 95 | # V[] = vertex points of a polygon V[n+1] with V[n]=V[0] 96 | # Return: 0 = outside, 1 = inside 97 | # This code is patterned after [Franklin, 2000] (normally just use winding method) 98 | cn = 0 # crossing number counter 99 | 100 | # loop through all edges of the polygon 101 | for v0, v1 in self.edges(): # edge from V[i] to V[i+1] 102 | if ((v0.y <= p.y) and (v1.y > p.y)) or ((v0.y > p.y) and (v1.y <= p.y)): # a downward crossing 103 | # Compute the actual edge-ray intersect x-coordinate 104 | vt = (p.y - v0.y) / (v1.y - v0.y) 105 | if p.x < v0.x + vt * (v1.x - v0.x): # P.x < intersect 106 | cn += 1 # a valid crossing of y=P.y right of P.x 107 | 108 | return bool(cn % 2) # 0 if even (out), and 1 if odd (in) 109 | 110 | def point_inside(self, p): 111 | # point_inside(): winding number test for a point in a polygon 112 | # Input: p = a point, 113 | # Return: wn = the winding number (=0 only when P is outside) 114 | 115 | wn = 0 # the winding number counter 116 | 117 | # loop through all edges of the polygon 118 | for v0, v1 in self.edges(): # edge from V[i] to V[i+1] 119 | if v0.y <= p.y: # start y <= P.y 120 | if v1.y > p.y: # an upward crossing 121 | if is_left(v0, v1, p) > 0: # P left of edge 122 | wn += 1 # have a valid up intersect 123 | 124 | else: # start y > P.y (no test needed) 125 | if v1.y <= p.y: # a downward crossing 126 | if is_left(v0, v1, p) < 0: # P right of edge 127 | wn -= 1 # have a valid down intersect 128 | return wn 129 | 130 | def intersect(self, poly2): 131 | assert isinstance(poly2, Polygon) 132 | bounds1 = self.get_bounds() 133 | bounds2 = poly2.get_bounds() 134 | 135 | if (bounds2[1] <= bounds1[0] or bounds2[0] >= bounds1[1] or 136 | bounds2[3] <= bounds1[2] or bounds2[2] >= bounds1[3]): 137 | return False 138 | 139 | for p in poly2: 140 | if self.point_inside(p): 141 | return True 142 | for p in self: 143 | if poly2.point_inside(p): 144 | return True 145 | 146 | all_edges = [] 147 | def add_ordered_edges(edge_list, new_edges): 148 | for p0, p1 in new_edges: 149 | if p0 < p1: 150 | edge_list.append(OrderedEdge(p0, p1)) 151 | else: 152 | edge_list.append(OrderedEdge(p1, p0)) 153 | 154 | my_edges = [] 155 | add_ordered_edges(my_edges, self.edges()) 156 | your_edges = [] 157 | add_ordered_edges(your_edges, poly2.edges()) 158 | for e1 in my_edges: 159 | for e2 in your_edges: 160 | if line_intersect(e1, e2): 161 | return True 162 | return False 163 | 164 | """ Something (occasionally) wrong with my edge ordering in Shamos-Hoey booooooo~~ """ 165 | # add_ordered_edges(all_edges, self.edges()) 166 | # add_ordered_edges(all_edges, poly2.edges()) 167 | # 168 | # event_queue = EventQueue(all_edges) 169 | # segment_list = [] 170 | # def find_list_index(seglist, new_edge): 171 | # i = -1 172 | # for i, edge_index in enumerate(seglist): 173 | # if is_left(new_edge.vertex, *all_edges[edge_index]): 174 | # return i 175 | # return i+1 176 | # 177 | # for e in event_queue.events: 178 | # 179 | # if e.is_left_end: 180 | # # We're adding it to the segment list 181 | # # If it's a left end (new segment), check who is above, put in the list 182 | # i = find_list_index(segment_list, e) 183 | # segment_list.insert(i, e.edge_id) 184 | # if i > 0 and line_intersect(all_edges[e.edge_id], all_edges[segment_list[i-1]]): 185 | # return True 186 | # if i < (len(segment_list)-1) and line_intersect(all_edges[e.edge_id], all_edges[segment_list[i+1]]): 187 | # return True 188 | # 189 | # else: 190 | # # It's a right end and we need to pull it out of the queue 191 | # i = segment_list.index(e.edge_id) 192 | # if 0 < i < (len(segment_list)-1) and line_intersect(all_edges[segment_list[i-1]], all_edges[segment_list[i+1]]): 193 | # return True 194 | # segment_list.pop(i) 195 | # 196 | # return False 197 | 198 | 199 | class Rectangle(Polygon): 200 | def __init__(self, xlim, ylim): 201 | super(Rectangle, self).__init__([[xlim[0], ylim[0]], [xlim[1], ylim[0]], [xlim[1], ylim[1]], [xlim[0], ylim[1]]]) 202 | 203 | 204 | def line_intersect(l0, l1): 205 | # Assume ordered lines (OrderedEdge objects) 206 | lsign = is_left(l0.lp, l0.rp, l1.lp) # l1 left point sign 207 | rsign = is_left(l0.lp, l0.rp, l1.rp) # l1 right point sign 208 | if (lsign * rsign >= 0): # l1 endpoints have same sign relative to l0 209 | return False # => on same side => no intersect is possible 210 | lsign = is_left(l1.lp, l1.rp, l0.lp) # l0 left point sign 211 | rsign = is_left(l1.lp, l1.rp, l0.rp) # l0 right point sign 212 | if (lsign * rsign >= 0): # l0 endpoints have same sign relative to l1 213 | return False # => on same side => no intersect is possible 214 | return True # => an intersect exists 215 | 216 | 217 | def convex_hull(points, return_copy=False): 218 | """Computes the convex hull of a set of 2D points. 219 | 220 | Input: an iterable sequence of (x, y) pairs representing the points. 221 | Output: a list of vertices of the convex hull in counter-clockwise order, 222 | starting from the vertex with the lexicographically smallest coordinates. 223 | Implements Andrew's monotone chain algorithm. O(n log n) complexity. 224 | """ 225 | if return_copy: 226 | raise NotImplementedError 227 | 228 | # Sort the points lexicographically (tuples are compared lexicographically). 229 | # Remove duplicates to detect the case we have just one unique point. 230 | points = sorted(set(points)) 231 | 232 | # Boring case: no points or a single point, possibly repeated multiple times. 233 | if len(points) <= 1: 234 | return points 235 | 236 | def ccw(p1, p2, p3): 237 | # Three points are a counter-clockwise turn if ccw > 0, clockwise if 238 | # ccw < 0, and collinear if ccw = 0 because ccw is a determinant that 239 | # gives twice the signed area of the triangle formed by p1, p2 and p3 240 | return (p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x) 241 | 242 | # Build lower hull 243 | lower = PointList([]) 244 | for p in points: 245 | while len(lower) >= 2 and ccw(lower[-2], lower[-1], p) <= 0: 246 | lower.pop() 247 | lower.append(p) 248 | 249 | # Build upper hull 250 | upper = PointList([]) 251 | for p in reversed(points): 252 | while len(upper) >= 2 and ccw(upper[-2], upper[-1], p) <= 0: 253 | upper.pop() 254 | upper.append(p) 255 | 256 | # Concatenation of the lower and upper hulls gives the convex hull. 257 | # Last point of each list is omitted because it is repeated at the beginning of the other list. 258 | return Polygon(lower[:-1] + upper[:-1]) 259 | 260 | 261 | def xy_order(p1, p2): 262 | # Determines the xy lexicographical order of two points 263 | # Returns: (+1) if p1 > p2; (-1) if p1 < p2; and 0 if equal 264 | if p1.x > p2.x: 265 | return 1 266 | if p1.x < p2.x: 267 | return -1 268 | # tiebreak with y 269 | if p1.y > p2.y: 270 | return 1 271 | if p1.y < p2.y: 272 | return -1 273 | # otherwise same point 274 | return 0 275 | 276 | 277 | def is_left(p0, p1, p2): 278 | # tests if point P2 is Left|On|Right of the line P0 to P1. 279 | # returns: >0 for left, 0 for on, and <0 for right of the line. 280 | return (p1.x - p0.x) * (p2.y - p0.y) - (p2.x - p0.x) * (p1.y - p0.y) 281 | 282 | # =================================================================== 283 | 284 | 285 | class Event: 286 | def __init__(self, edge_id, is_left_end, vertex): 287 | self.edge_id = edge_id 288 | self.is_left_end = is_left_end 289 | self.vertex = vertex 290 | 291 | 292 | def event_compare(event0, event1): 293 | # Compare (order) two events 294 | return xy_order(event0.vertex, event1.vertex) 295 | 296 | 297 | class EventQueue(object): 298 | 299 | def __init__(self, edge_list): 300 | self.events =[] 301 | for i, (v0, v1) in enumerate(edge_list): 302 | left_first = (v0 <= v1) # (xy_order(v0, v1) >= 0) 303 | self.events.append(Event(i, left_first, v0)) 304 | self.events.append(Event(i, (not left_first), v1)) 305 | 306 | self.events = sorted(self.events, cmp=event_compare) 307 | 308 | def pop(self, index=-1): 309 | return self.events.pop(index) 310 | 311 | 312 | 313 | class SweepLineSegment(object): 314 | 315 | def __init__(self, edge_id, p_left, p_right, above=None, below=None): 316 | self.edge_id = edge_id 317 | self.p_left = p_left 318 | self.p_right = p_right 319 | self.above = above 320 | self.below = below 321 | 322 | def set_above(self, above): 323 | self.above = above 324 | 325 | def set_below(self, below): 326 | self.below = below 327 | 328 | class SweepLine: 329 | 330 | def __init__(self, poly): 331 | self.nv = len(poly) 332 | self.poly = poly 333 | self.tree = {} 334 | 335 | 336 | def add(self, event): 337 | v0, v1 = self.poly.get_edge(event.edge_id) 338 | if v0 < v1: 339 | new_seg = SweepLineSegment(event.edge_id, p_left=v0, p_right=v1) 340 | else: 341 | new_seg = SweepLineSegment(event.edge_id, p_left=v1, p_right=v0) 342 | self.tree[event.edge_id] = new_seg 343 | --------------------------------------------------------------------------------