├── LICENSE ├── PathPlanning ├── README.md ├── __init__.py ├── maputils.py ├── rrt.py ├── rrt_star.py ├── rrtutils.py └── sampleutils.py ├── Quadrotor ├── .DS_Store ├── __init__.py ├── params.py ├── quadrotor.py ├── quadsim.py ├── quaternion.py └── utils.py ├── README.md ├── TrajGen ├── __init__.py ├── trajGen.py └── trajutils.py ├── _config.yml ├── controller.py ├── google60f4e7ebc14b633d.html ├── imgs ├── .DS_Store ├── ex.gif ├── helixtraj.gif ├── rrt.gif └── rrt2.gif └── runsim.py /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Bharath chandra Reddy Irigireddy 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/README.md: -------------------------------------------------------------------------------- 1 | ## Informed RRT* 2 | 3 | Informed RRT* to plan a collision free path from start to goal position in an N-dimensional Map. 4 | 5 | ### Dependencies 6 | - numpy 7 | - matplotlib 8 | - rtree-linux 9 | 10 | ### Usage example 11 | ```python 12 | import numpy as np 13 | from PathPlanning import RRTStar, Map 14 | np.random.seed(7) 15 | 16 | # 3D boxes lx, ly, lz, hx, hy, hz 17 | obstacles = [[-5, 25, 0, 20, 35, 60], 18 | [30, 25, 0, 55, 35, 100], 19 | [45, 35, 0, 55, 60, 60], 20 | [45, 75, 0, 55, 85, 100], 21 | [-5, 65, 0, 30, 70, 100], 22 | [70, 50, 0, 80, 80, 100]] 23 | 24 | # limits on map dimensions 25 | bounds = np.array([0,100]) 26 | # create map with obstacles 27 | mapobs = Map(obstacles, bounds, dim = 3) 28 | 29 | #plan a path from start to goal 30 | start = np.array([90,60,60]) 31 | goal = np.array([20,40,20]) 32 | 33 | rrt = RRTStar(start = start, goal = goal, 34 | Map = mapobs, max_iter = 500, 35 | goal_sample_rate = 0.1) 36 | 37 | waypoints, min_cost = rrt.plan() 38 | 39 | #plot the waypoints and obstacles 40 | rrt.draw_scene(waypoints, ax) 41 | ``` 42 | ![image](../imgs/ex.gif) 43 | 44 | ### Future Work 45 | 46 | - Improve running time and overall performance 47 | - Implement other Path-Planning algorithms. 48 | 49 | ### References 50 | - [Motion Planning as Search - Russ Tedrake](http://underactuated.csail.mit.edu/planning.html#section2) 51 | 52 | Feel free to contribute to any part of the code. 53 | -------------------------------------------------------------------------------- /PathPlanning/__init__.py: -------------------------------------------------------------------------------- 1 | from .rrt import RRT 2 | from .rrt_star import RRTStar 3 | from .maputils import Map 4 | -------------------------------------------------------------------------------- /PathPlanning/maputils.py: -------------------------------------------------------------------------------- 1 | ''' 2 | map utils 3 | obstacle map with collision checking 4 | 5 | author: Bharath Chandra 6 | email: iambharathchandra@gmail.com 7 | ''' 8 | 9 | from rtree import index 10 | import numpy as np 11 | from matplotlib.pyplot import Rectangle 12 | 13 | #obstacle map 14 | class Map: 15 | def __init__(self,obstacle_list,bounds,path_resolution = 0.5,dim = 3): 16 | self.dim = dim 17 | self.idx = self.get_tree(obstacle_list,dim) 18 | self.len = len(obstacle_list) 19 | self.path_res = path_resolution 20 | self.obstacles = obstacle_list 21 | self.bounds = bounds 22 | 23 | @staticmethod 24 | def get_tree(obstacle_list,dim): 25 | '''initialise map with given obstacle_list''' 26 | p = index.Property() 27 | p.dimension = dim 28 | ls = [(i,(*obj,),None) for i, obj in enumerate(obstacle_list)] 29 | return index.Index(ls, properties=p) 30 | 31 | def add(self,obstacle): 32 | '''add new obstacle''' 33 | self.idx.insert(self.len,obstacle) 34 | self.obstacles.append(obstacle) 35 | self.len += 1 36 | 37 | def collision(self,start,end): 38 | '''find if the ray between start and end collides with obstacles''' 39 | dist = np.linalg.norm(start-end) 40 | n = int(dist/self.path_res) 41 | points = np.linspace(start,end,n) 42 | for p in points: 43 | if self.idx.count((*p,)) != 0 : 44 | return True 45 | return False 46 | 47 | def inbounds(self,p): 48 | '''Check if p lies inside map bounds''' 49 | lower,upper = self.bounds 50 | return (lower <= p).all() and (p <= upper).all() 51 | 52 | def plotobs(self,ax,scale = 1): 53 | '''plot all obstacles''' 54 | obstacles = scale*np.array(self.obstacles) 55 | if self.dim == 2: 56 | for box in obstacles: 57 | l = box[2] - box[0] 58 | w = box[3] - box[1] 59 | box_plt = Rectangle((box[0], box[1]),l,w,color='k',zorder = 1) 60 | ax.add_patch(box_plt) 61 | elif self.dim == 3: 62 | for box in obstacles: 63 | X, Y, Z = cuboid_data(box) 64 | ax.plot_surface(X, Y, Z, rstride=1, cstride=1,color=(0.1, 0.15, 0.3, 0.2),zorder = 1) 65 | else: print('can not plot for given dimensions') 66 | 67 | 68 | #to plot obstacle surfaces 69 | def cuboid_data(box): 70 | l = box[3] - box[0] 71 | w = box[4] - box[1] 72 | h = box[5] - box[2] 73 | x = [[0, l, l, 0, 0], 74 | [0, l, l, 0, 0], 75 | [0, l, l, 0, 0], 76 | [0, l, l, 0, 0]] 77 | y = [[0, 0, w, w, 0], 78 | [0, 0, w, w, 0], 79 | [0, 0, 0, 0, 0], 80 | [w, w, w, w, w]] 81 | z = [[0, 0, 0, 0, 0], 82 | [h, h, h, h, h], 83 | [0, 0, h, h, 0], 84 | [0, 0, h, h, 0]] 85 | return box[0] + np.array(x), box[1] + np.array(y), box[2] + np.array(z) 86 | -------------------------------------------------------------------------------- /PathPlanning/rrt.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import mpl_toolkits.mplot3d.axes3d as Axes3D 4 | from .rrtutils import * 5 | 6 | class RRT: 7 | 8 | def __init__(self, start, goal, Map, 9 | max_extend_length = 5.0, 10 | path_resolution = 0.5, 11 | goal_sample_rate = 0.05, 12 | max_iter = 100 ): 13 | self.start = Node(start) 14 | self.goal = Node(goal) 15 | self.max_extend_length = max_extend_length 16 | self.goal_sample_rate = goal_sample_rate 17 | self.max_iter = max_iter 18 | self.dim = start.shape[0] 19 | self.tree = Rtree(self.dim) 20 | self.map = Map 21 | 22 | def plan(self): 23 | """Plans the path from start to goal while avoiding obstacles""" 24 | self.tree.add(self.start) 25 | for i in range(self.max_iter): 26 | #Generate a random node (rnd_node) 27 | rnd_node = self.get_random_node() 28 | #Get nearest node (nearest_node) 29 | nearest_node = self.tree.nearest(rnd_node) 30 | #Get new node (new_node) by connecting 31 | new_node = self.steer(nearest_node,rnd_node) 32 | #If the path between new_node and the nearest node is not in collision 33 | if not self.map.collision(nearest_node.p,new_node.p): 34 | self.tree.add(new_node) 35 | # If the new_node is very close to the goal, connect it 36 | # directly to the goal and return the final path 37 | if self.dist(new_node,self.goal) <= self.max_extend_length: 38 | if not self.map.collision(new_node.p,self.goal.p): 39 | self.goal.parent = new_node 40 | return self.final_path() 41 | # cannot find path 42 | return None 43 | 44 | @staticmethod 45 | def dist(from_node, to_node): 46 | #euler distance 47 | return np.linalg.norm(from_node.p - to_node.p) 48 | 49 | def steer(self,from_node, to_node): 50 | """Connects from_node to a new_node in the direction of to_node 51 | with maximum distance max_extend_length 52 | """ 53 | dist = self.dist(from_node, to_node) 54 | #Rescale the path to the maximum extend_length 55 | if dist > self.max_extend_length: 56 | diff = from_node.p - to_node.p 57 | to_node.p = from_node.p - diff/dist * self.max_extend_length 58 | to_node.parent = from_node 59 | return to_node 60 | 61 | def sample(self): 62 | # Sample random point inside boundaries 63 | lower,upper = self.map.bounds 64 | return lower + np.random.rand(self.dim)*(upper - lower) 65 | 66 | def get_random_node(self): 67 | """Sample random node inside bounds or sample goal point""" 68 | if np.random.rand() > self.goal_sample_rate: 69 | rnd = self.sample() 70 | else: 71 | rnd = self.goal.p 72 | return Node(rnd) 73 | 74 | def final_path(self): 75 | """Compute the final path from the goal node to the start node""" 76 | path = [] 77 | node = self.goal 78 | if (node.p == node.parent.p).all(): node = node.parent 79 | while node.parent: 80 | path.append(node.p) 81 | node = node.parent 82 | path.append(self.start.p) 83 | return np.array(path[::-1]) 84 | 85 | def draw_graph(self,ax): 86 | '''plot the whole graph''' 87 | for node in self.tree.all(): 88 | if node.parent: 89 | xy = np.c_[node.p,node.parent.p] 90 | ax.plot(*xy, "-g",zorder = 5) 91 | 92 | def draw_path(self,ax,path): 93 | '''draw the path if available''' 94 | if path is None: 95 | print("path not available") 96 | else: 97 | ax.plot(*np.array(path).T, '-', color = (0.9, 0.2, 0.5, 0.8), zorder = 5) 98 | 99 | def draw_scene(self,path = None,ax = None): 100 | '''draw the whole scene''' 101 | if ax is None: 102 | fig = plt.figure() 103 | if self.dim == 3: 104 | ax = Axes3D.Axes3D(fig) 105 | elif self.dim == 2: 106 | ax = plt.axes() 107 | else: 108 | print('cannot plot for current dimensions') 109 | return 110 | self.draw_graph(ax) 111 | self.draw_path(ax,path) 112 | self.map.plotobs(ax) 113 | plt.show() 114 | -------------------------------------------------------------------------------- /PathPlanning/rrt_star.py: -------------------------------------------------------------------------------- 1 | from .rrt import * 2 | from .sampleutils import InformedSampler 3 | from math import ceil 4 | 5 | class RRTStar(RRT): 6 | 7 | def __init__(self, start, goal, Map, 8 | max_extend_length = 10.0, 9 | path_resolution = 0.5, 10 | goal_sample_rate = 0.05, 11 | max_iter = 200 ): 12 | super().__init__(start, goal, Map, max_extend_length, 13 | path_resolution, goal_sample_rate, max_iter) 14 | self.final_nodes = [] 15 | self.Informedsampler = InformedSampler(goal, start) 16 | 17 | def plan(self): 18 | """Plans the path from start to goal while avoiding obstacles""" 19 | self.start.cost = 0 20 | self.tree.add(self.start) 21 | for i in range(self.max_iter): 22 | #Generate a random node (rnd_node) 23 | rnd = self.get_random_node() 24 | # Get nearest node 25 | nearest_node = self.tree.nearest(rnd) 26 | # Get new node by connecting rnd_node and nearest_node 27 | new_node = self.steer(nearest_node, rnd) 28 | # If path between new_node and nearest node is not in collision 29 | if not self.map.collision(nearest_node.p,new_node.p): 30 | #add the node to tree 31 | self.add(new_node) 32 | #Return path if it exists 33 | if not self.goal.parent: path = None 34 | else: path = self.final_path() 35 | return path, self.goal.cost 36 | 37 | def add(self,new_node): 38 | near_nodes = self.near_nodes(new_node) 39 | # Connect the new node to the best parent in near_inds 40 | self.choose_parent(new_node,near_nodes) 41 | #add the new_node to tree 42 | self.tree.add(new_node) 43 | # Rewire the nodes in the proximity of new_node if it improves their costs 44 | self.rewire(new_node,near_nodes) 45 | #check if it is in close proximity to the goal 46 | if self.dist(new_node,self.goal) <= self.max_extend_length: 47 | # Connection between node and goal needs to be collision free 48 | if not self.map.collision(self.goal.p,new_node.p): 49 | #add to final nodes if in goal region 50 | self.final_nodes.append(new_node) 51 | #set best final node and min_cost 52 | self.choose_parent(self.goal,self.final_nodes) 53 | 54 | def choose_parent(self, node, parents): 55 | """Set node.parent to the lowest resulting cost parent in parents and 56 | node.cost to the corresponding minimal cost 57 | """ 58 | # Go through all near nodes and evaluate them as potential parent nodes 59 | for parent in parents: 60 | #checking whether a connection would result in a collision 61 | if not self.map.collision(node.p,parent.p): 62 | #evaluating the cost of the new_node if it had that near node as a parent 63 | cost = self.new_cost(parent, node) 64 | #picking the parent resulting in the lowest cost and updating the cost of the new_node to the minimum cost. 65 | if cost < node.cost: 66 | node.parent = parent 67 | node.cost = cost 68 | 69 | def rewire(self, new_node, near_nodes): 70 | """Rewire near nodes to new_node if this will result in a lower cost""" 71 | #Go through all near nodes and check whether rewiring them to the new_node is useful 72 | for node in near_nodes: 73 | self.choose_parent(node,[new_node]) 74 | self.propagate_cost_to_leaves(new_node) 75 | 76 | def near_nodes(self, node): 77 | """Find the nodes in close proximity to given node""" 78 | nnode = self.tree.len + 1 79 | r = ceil(5.5*np.log(nnode)) 80 | return self.tree.k_nearest(node,r) 81 | 82 | def new_cost(self, from_node, to_node): 83 | """to_node's new cost if from_node were the parent""" 84 | return from_node.cost + self.dist(from_node, to_node) 85 | 86 | def propagate_cost_to_leaves(self, parent_node): 87 | """Recursively update the cost of the nodes""" 88 | for node in self.tree.all(): 89 | if node.parent == parent_node: 90 | node.cost = self.new_cost(parent_node, node) 91 | self.propagate_cost_to_leaves(node) 92 | 93 | def sample(self): 94 | """Sample random node inside the informed region""" 95 | lower,upper = self.map.bounds 96 | if self.goal.parent: 97 | rnd = np.inf 98 | #sample until rnd is inside bounds of the map 99 | while not self.map.inbounds(rnd): 100 | # Sample random point inside ellipsoid 101 | rnd = self.Informedsampler.sample(self.goal.cost) 102 | else: 103 | # Sample random point inside boundaries 104 | rnd = lower + np.random.rand(self.dim)*(upper - lower) 105 | return rnd 106 | -------------------------------------------------------------------------------- /PathPlanning/rrtutils.py: -------------------------------------------------------------------------------- 1 | from rtree import index 2 | import numpy as np 3 | from matplotlib.pyplot import Rectangle 4 | 5 | class Node: 6 | def __init__(self,coords): 7 | self.p = np.array(coords) 8 | self.parent = None 9 | self.cost = np.inf 10 | 11 | def __len__(self): 12 | return len(self.p) 13 | 14 | def __getitem__(self, i): 15 | return self.p[i] 16 | 17 | def __repr__(self): 18 | return 'Node({}, {})'.format(self.p,self.cost) 19 | 20 | #Rtree to store Nodes 21 | class Rtree: 22 | def __init__(self,dim): 23 | self.dim = dim 24 | self.node_list = [] 25 | self.idx = self.get_tree(dim) 26 | self.len = 0 27 | 28 | @staticmethod 29 | def get_tree(dim): 30 | '''Initialise the tree''' 31 | p = index.Property() 32 | p.dimension = dim 33 | p.dat_extension = 'data' 34 | p.idx_extension = 'index' 35 | return index.Index(properties=p) 36 | 37 | def add(self,new_node): 38 | '''add nodes to tree''' 39 | self.node_list.append(new_node) 40 | self.idx.insert(self.len,(*new_node.p,)) 41 | self.len += 1 42 | 43 | def k_nearest(self,node,k): 44 | '''Returns k-nearest nodes to the given node''' 45 | near_ids = self.idx.nearest((*node.p,),k) 46 | for i in near_ids: 47 | yield self.node_list[i] 48 | 49 | def nearest(self,node): 50 | '''Returns nearest node to the given node''' 51 | near_ids = self.idx.nearest((*node.p,),1) 52 | id = list(near_ids)[0] 53 | return self.node_list[id] 54 | 55 | def all(self): 56 | return self.node_list 57 | -------------------------------------------------------------------------------- /PathPlanning/sampleutils.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Sample utils 3 | Uniform Sampling within N-dimensional hypersphere, hyperellipsoid. 4 | InformedSampler for RRT*, BIT* to get random node given goal, start and current minimum cost 5 | 6 | author: Bharath Chandra 7 | email: iambharathchandra@gmail.com 8 | ''' 9 | 10 | import numpy as np 11 | from numpy import linalg as LA 12 | 13 | 14 | def SampleUnitDisc(): 15 | '''uniformly sample a 2D unit Disc''' 16 | r = np.random.uniform(0, 1) 17 | phi = np.random.uniform(0, 2*np.pi) 18 | x = r * np.cos(phi) 19 | y = r * np.sin(phi) 20 | return np.array([x,y]) 21 | 22 | 23 | def SampleUnitNBall(dim = 3,num = 1): 24 | ''' 25 | uniformly sample a N-dimensional unit UnitBall 26 | Reference: 27 | Efficiently sampling vectors and coordinates from the n-sphere and n-ball 28 | http://compneuro.uwaterloo.ca/files/publications/voelker.2017.pdf 29 | 30 | Input: 31 | num - no. of samples 32 | dim - dimensions 33 | 34 | Output: 35 | uniformly sampled points within N-dimensional unit ball 36 | ''' 37 | #Sample on a unit N+1 sphere 38 | u = np.random.normal(0, 1, (num, dim + 2)) 39 | norm = LA.norm(u, axis = -1,keepdims = True) 40 | u = u/norm 41 | #The first N coordinates are uniform in a unit N ball 42 | if num == 1: return u[0,:dim] 43 | return u[:,:dim] 44 | 45 | 46 | def SphereSampler(center, radius, num = 1): 47 | ''' 48 | uniformly sample inside N-dimensional hypersphere 49 | Input: 50 | center - Center of sphere 51 | radius - Radius of Sphere 52 | num - no. of samples 53 | Output: 54 | uniformly sampled points inside the hypersphere 55 | ''' 56 | dim = center.shape[0] 57 | xball = SampleUnitNBall(dim,num) 58 | return radius*xball + center 59 | 60 | 61 | class EllipsoidSampler: 62 | ''' 63 | uniformly sample within a N-dimensional Ellipsoid 64 | Reference: 65 | Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling 66 | of an Admissible Ellipsoidal Heuristic https://arxiv.org/pdf/1404.2334.pdf 67 | ''' 68 | def __init__(self,center,axes = [],rot = []): 69 | ''' 70 | Input: 71 | center - centre of the N-dimensional ellipsoid in the N-dimensional 72 | axes - axes length across each dimension in ellipsoid frame 73 | rot - rotation matrix from ellipsoid frame to world frame 74 | Output: 75 | uniformly sampled points within the hyperellipsoid 76 | ''' 77 | self.dim = center.shape[0] 78 | self.center = center 79 | self.rot = rot 80 | if len(rot) == 0: self.rot = np.eye(self.dim) 81 | if len(axes) == 0: axes = [1]*self.dim 82 | self.L = np.diag(axes) 83 | 84 | def sample(self,num = 1): 85 | xball = SampleUnitNBall(self.dim,num) 86 | #Transform points in UnitBall to ellipsoid 87 | xellip = (self.rot@self.L@xball.T).T + self.center 88 | return xellip 89 | 90 | 91 | class InformedSampler: 92 | ''' 93 | uniformly sample within a N-dimensional Prolate-Hyperspheroid for informed RRT* 94 | with goal and start as focal points 95 | Reference: 96 | Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling 97 | of an Admissible Ellipsoidal Heuristic https://arxiv.org/pdf/1404.2334.pdf 98 | ''' 99 | def __init__(self, goal, start): 100 | self.dim = goal.shape[0] 101 | self.cmin = LA.norm(goal - start) 102 | center = (goal + start)/2 103 | #rotation matrix from ellipsoid frame to world frame 104 | C = self.RotationToWorldFrame(goal, start) 105 | #initialise EllipsoidSampler 106 | self.ellipsampler = EllipsoidSampler(center,rot = C) 107 | 108 | def sample(self, cmax, num = 1): 109 | ''' 110 | Input: 111 | cmax - current best cost 112 | num - no. of samples 113 | Output: 114 | uniformly sampled point within informed region 115 | ''' 116 | #Hyperspheroid axes lengths 117 | r1 = cmax/2 118 | ri = np.sqrt(cmax**2 - self.cmin**2)/2 119 | axes = [r1]+ [ri]*(self.dim - 1) 120 | self.ellipsampler.L = np.diag(axes) 121 | #return sampled point 122 | return self.ellipsampler.sample(num) 123 | 124 | def RotationToWorldFrame(self, goal, start): 125 | ''' 126 | Given two focal points goal and start in N-Dimensions 127 | Returns rotation matrix from the ellipsoid frame to the world frame 128 | ''' 129 | #Transverse axis of the ellipsoid in the world frame 130 | E1 = (goal - start) / self.cmin 131 | #first basis vector of the world frame [1,0,0,...] 132 | W1 = [1]+[0]*(self.dim - 1) 133 | #outer product of E1 and W1 134 | M = np.outer(E1,W1) 135 | #SVD decomposition od outer product 136 | U, S, V = LA.svd(M) 137 | #Calculate the middle diagonal matrix 138 | middleM = np.eye(self.dim) 139 | middleM[-1,-1] = LA.det(U)*LA.det(V) 140 | #calculate the rotation matrix 141 | C = U@middleM@V.T 142 | return C 143 | -------------------------------------------------------------------------------- /Quadrotor/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bharath2/Quadrotor-Simulation/c838fa0adc07da7d52459b228e90897a14dcace4/Quadrotor/.DS_Store -------------------------------------------------------------------------------- /Quadrotor/__init__.py: -------------------------------------------------------------------------------- 1 | from .quadrotor import Quadrotor 2 | from .quadsim import QuadSim 3 | -------------------------------------------------------------------------------- /Quadrotor/params.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | mass = 0.18 # kg 4 | g = 9.81 # m/s^2 5 | arm_length = 0.086 # meter 6 | height = 0.05 7 | 8 | I = np.array([(0.00025, 0, 2.55e-6), 9 | (0, 0.000232, 0), 10 | (2.55e-6, 0, 0.0003738)]); 11 | 12 | invI = np.linalg.inv(I) 13 | 14 | minF = 0.0 15 | maxF = 2.0 * mass * g 16 | 17 | km = 1.5e-9 18 | kf = 6.11e-8 19 | r = km / kf 20 | 21 | L = arm_length 22 | H = height 23 | # [ F ] [ F1 ] 24 | # | M1 | = A * | F2 | 25 | # | M2 | | F3 | 26 | # [ M3 ] [ F4 ] 27 | A = np.array([[ 1, 1, 1, 1], 28 | [ 0, L, 0, -L], 29 | [-L, 0, L, 0], 30 | [ r, -r, r, -r]]) 31 | 32 | invA = np.linalg.inv(A) 33 | 34 | body_frame = np.array([(L, 0, 0, 1), 35 | (0, L, 0, 1), 36 | (-L, 0, 0, 1), 37 | (0, -L, 0, 1), 38 | (0, 0, 0, 1), 39 | (0, 0, H, 1)]) 40 | 41 | B = np.array([[0, L,0, -L], 42 | [-L, 0, L,0]]) 43 | -------------------------------------------------------------------------------- /Quadrotor/quadrotor.py: -------------------------------------------------------------------------------- 1 | """ 2 | author: Peter Huang 3 | email: hbd730@gmail.com 4 | license: BSD 5 | Please feel free to use and modify this, but keep the above information. Thanks! 6 | """ 7 | 8 | import numpy as np 9 | import scipy.integrate as integrate 10 | from collections import namedtuple 11 | from .quaternion import Quaternion 12 | from .utils import RPYToRot, RotToQuat, RotToRPY 13 | from . import params 14 | 15 | State = namedtuple('State', 'pos vel rot omega') 16 | 17 | class Quadrotor: 18 | """ Quadrotor class 19 | 20 | state - 1 dimensional vector but used as 13 x 1. [x, y, z, xd, yd, zd, qw, qx, qy, qz, p, q, r] 21 | where [qw, qx, qy, qz] is quternion and [p, q, r] are angular velocity [roll_dot, pitch_dot, yaw_dot] 22 | F - 1 x 1, thrust output from controller 23 | M - 3 x 1, moments output from controller 24 | params - system parameters struct, arm_length, g, mass, etc. 25 | """ 26 | 27 | def __init__(self, pos, attitude): 28 | """ pos = [x,y,z] attitude = [rool,pitch,yaw] 29 | """ 30 | self.state = np.zeros(13) 31 | roll, pitch, yaw = attitude 32 | rot = RPYToRot(roll, pitch, yaw) 33 | quat = RotToQuat(rot) 34 | self.state[0] = pos[0] 35 | self.state[1] = pos[1] 36 | self.state[2] = pos[2] 37 | self.state[6] = quat[0] 38 | self.state[7] = quat[1] 39 | self.state[8] = quat[2] 40 | self.state[9] = quat[3] 41 | self.ode = integrate.ode(self.state_dot).set_integrator('vode',nsteps=500,method='bdf') 42 | 43 | def world_frame(self): 44 | """ position returns a 3x6 matrix 45 | where row is [x, y, z] column is m1 m2 m3 m4 origin h 46 | """ 47 | origin = self.state[0:3] 48 | rot = self.Rotation().T 49 | wHb = np.r_[np.c_[rot,origin], np.array([[0, 0, 0, 1]])] 50 | quadBodyFrame = params.body_frame.T 51 | quadWorldFrame = wHb.dot(quadBodyFrame) 52 | world_frame = quadWorldFrame[0:3] 53 | return world_frame 54 | 55 | def Rotation(self): 56 | rot = Quaternion(self.state[6:10]).as_rotation_matrix() 57 | return rot 58 | 59 | def get_state(self): 60 | return State(self.state[0:3], 61 | self.state[3:6], 62 | RotToRPY(self.Rotation()), 63 | self.state[10:13]) 64 | 65 | 66 | def state_dot(self, t,state,par): 67 | F,M = par 68 | x, y, z, xdot, ydot, zdot, qw, qx, qy, qz, p, q, r = state 69 | quat = np.array([qw,qx,qy,qz]) 70 | 71 | bRw = Quaternion(quat).as_rotation_matrix() # world to body rotation matrix 72 | wRb = bRw.T # orthogonal matrix inverse = transpose 73 | # acceleration - Newton's second law of motion 74 | accel = 1.0 / params.mass * (wRb.dot(np.array([[0, 0, F]]).T) 75 | - np.array([[0, 0, params.mass * params.g]]).T) 76 | # angular velocity - using quternion 77 | # http://www.euclideanspace.com/physics/kinematics/angularvelocity/ 78 | K_quat = 2.0; # this enforces the magnitude 1 constraint for the quaternion 79 | quaterror = 1.0 - (qw**2 + qx**2 + qy**2 + qz**2) 80 | qdot = (-1.0/2) * np.array([[0, -p, -q, -r], 81 | [p, 0, -r, q], 82 | [q, r, 0, -p], 83 | [r, -q, p, 0]]).dot(quat) + K_quat * quaterror * quat; 84 | 85 | # angular acceleration - Euler's equation of motion 86 | # https://en.wikipedia.org/wiki/Euler%27s_equations_(rigid_body_dynamics) 87 | omega = np.array([p,q,r]) 88 | pqrdot = params.invI.dot( M.flatten() - np.cross(omega, params.I.dot(omega)) ) 89 | state_dot = np.zeros(13) 90 | state_dot[0] = xdot 91 | state_dot[1] = ydot 92 | state_dot[2] = zdot 93 | state_dot[3] = accel[0] 94 | state_dot[4] = accel[1] 95 | state_dot[5] = accel[2] 96 | state_dot[6] = qdot[0] 97 | state_dot[7] = qdot[1] 98 | state_dot[8] = qdot[2] 99 | state_dot[9] = qdot[3] 100 | state_dot[10] = pqrdot[0] 101 | state_dot[11] = pqrdot[1] 102 | state_dot[12] = pqrdot[2] 103 | 104 | return state_dot 105 | 106 | def update(self, dt, F, M): 107 | Mt = M[2] 108 | prop_thrusts = params.invA.dot(np.r_[np.array([[F]]),M]) 109 | prop_thrusts_clamped = np.maximum(np.minimum(prop_thrusts, params.maxF/4), params.minF/4) 110 | # F = np.sum(prop_thrusts_clamped) 111 | M = params.A[1:].dot(prop_thrusts_clamped) 112 | M = np.r_[M[:2],[Mt]] 113 | # print(F) 114 | self.ode.set_initial_value(self.state,0).set_f_params([F,M]) 115 | self.state = self.ode.integrate(self.ode.t + dt) 116 | -------------------------------------------------------------------------------- /Quadrotor/quadsim.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import mpl_toolkits.mplot3d.axes3d as Axes3D 4 | from collections import deque 5 | from .quadrotor import Quadrotor 6 | 7 | class QuadSim: 8 | def __init__(self,controller,des_state,Tmax, 9 | pos = None, attitude = [0,0,0], 10 | animation_frequency = 50, 11 | control_frequency = 200): 12 | 13 | self.t = 0 14 | self.Tmax = Tmax 15 | self.dt = 1/control_frequency 16 | self.animation_rate = 1/animation_frequency 17 | self.control_iterations = int(control_frequency / animation_frequency) 18 | 19 | self.des_state = des_state 20 | self.controller = controller 21 | if pos is None: pos = des_state(0).pos 22 | self.Quadrotor = Quadrotor(pos, attitude) 23 | 24 | self.pos_history = deque(maxlen=100) 25 | 26 | def Step(self): 27 | des_state = self.des_state(self.t) 28 | state = self.Quadrotor.get_state() 29 | if(self.t >= self.Tmax): 30 | U, M = self.controller.run_hover(state, des_state,self.dt) 31 | else: 32 | U, M = self.controller.run(state, des_state) 33 | self.Quadrotor.update(self.dt, U, M) 34 | self.t += self.dt 35 | 36 | def control_loop(self): 37 | for _ in range(self.control_iterations): 38 | self.Step() 39 | return self.Quadrotor.world_frame() 40 | 41 | def run(self,ax = None,save = False): 42 | self.init_plot(ax) 43 | while self.t < self.Tmax + 2: 44 | frame = self.control_loop() 45 | self.update_plot(frame) 46 | plt.pause(self.animation_rate) 47 | 48 | def init_plot(self,ax = None): 49 | if ax is None: 50 | fig = plt.figure() 51 | ax = Axes3D.Axes3D(fig) 52 | ax.set_xlim((0,2)) 53 | ax.set_ylim((0,2)) 54 | ax.set_zlim((0,2)) 55 | ax.plot([], [], [], '-', c='red',zorder = 10) 56 | ax.plot([], [], [], '-', c='blue',zorder = 10) 57 | ax.plot([], [], [], '-', c='green', marker='o', markevery=2,zorder = 10) 58 | ax.plot([], [], [], '.', c='green', markersize=2,zorder = 10) 59 | self.lines = ax.get_lines()[-4:] 60 | 61 | def update_plot(self,frame): 62 | 63 | lines_data = [frame[:,[0,2]], frame[:,[1,3]], frame[:,[4,5]]] 64 | 65 | for line, line_data in zip(self.lines[:3], lines_data): 66 | x, y, z = line_data 67 | line.set_data(x, y) 68 | line.set_3d_properties(z) 69 | 70 | self.pos_history.append(frame[:,4]) 71 | history = np.array(self.pos_history) 72 | self.lines[-1].set_data(history[:,0], history[:,1]) 73 | self.lines[-1].set_3d_properties(history[:,-1]) 74 | -------------------------------------------------------------------------------- /Quadrotor/quaternion.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class Quaternion: 4 | """Quaternions for 3D rotations""" 5 | def __init__(self, x): 6 | self.x = np.asarray(x, dtype=float) 7 | 8 | @classmethod 9 | def from_v_theta(cls, v, theta): 10 | """ Construct quaternion from unit vector v and rotation angle theta""" 11 | theta = np.asarray(theta) 12 | v = np.asarray(v) 13 | 14 | s = np.sin(0.5 * theta) 15 | c = np.cos(0.5 * theta) 16 | vnrm = np.sqrt(np.sum(v * v)) 17 | 18 | q = np.concatenate([[c], s * v / vnrm]) 19 | return cls(q) 20 | 21 | def get_array(self): 22 | return self.x 23 | 24 | def __eq__(self, other): 25 | return np.array_equal(self.x, other.x) 26 | 27 | def __ne__(self, other): 28 | return not (self==other) 29 | 30 | def __repr__(self): 31 | return "Quaternion:\n" + self.x.__repr__() 32 | 33 | def __mul__(self, other): 34 | # multiplication of two quaternions. 35 | prod = self.x[:, None] * other.x 36 | 37 | return self.__class__([(prod[0, 0] - prod[1, 1] 38 | - prod[2, 2] - prod[3, 3]), 39 | (prod[0, 1] + prod[1, 0] 40 | + prod[2, 3] - prod[3, 2]), 41 | (prod[0, 2] - prod[1, 3] 42 | + prod[2, 0] + prod[3, 1]), 43 | (prod[0, 3] + prod[1, 2] 44 | - prod[2, 1] + prod[3, 0])]) 45 | 46 | def as_v_theta(self): 47 | """Return the v, theta equivalent of the (normalized) quaternion""" 48 | # compute theta 49 | norm = np.sqrt((self.x ** 2).sum(0)) 50 | assert(norm != 0) 51 | theta = 2 * np.arccos(self.x[0] / norm) 52 | 53 | # compute the unit vector 54 | v = np.array(self.x[1:], order='F', copy=True) 55 | length = np.sqrt(np.sum(v ** 2, 0)) 56 | if length > 0.0: 57 | v /= length 58 | return v, theta 59 | 60 | def as_rotation_matrix(self): 61 | """Return the rotation matrix of the (normalized) quaternion 62 | https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation 63 | Improving computation speed https://www.ncbi.nlm.nih.gov/pmc/articles/PMC4435132/ 64 | """ 65 | v, theta = self.as_v_theta() 66 | c = np.cos(theta) 67 | s = np.sin(theta) 68 | 69 | return np.array([[v[0] * v[0] * (1. - c) + c, 70 | v[0] * v[1] * (1. - c) - v[2] * s, 71 | v[0] * v[2] * (1. - c) + v[1] * s], 72 | [v[1] * v[0] * (1. - c) + v[2] * s, 73 | v[1] * v[1] * (1. - c) + c, 74 | v[1] * v[2] * (1. - c) - v[0] * s], 75 | [v[2] * v[0] * (1. - c) - v[1] * s, 76 | v[2] * v[1] * (1. - c) + v[0] * s, 77 | v[2] * v[2] * (1. - c) + c]]) 78 | -------------------------------------------------------------------------------- /Quadrotor/utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | author: Peter Huang 3 | email: hbd730@gmail.com 4 | license: BSD 5 | Please feel free to use and modify this, but keep the above information. Thanks! 6 | """ 7 | 8 | import numpy as np 9 | from math import sin, cos, asin, atan2, sqrt 10 | 11 | def vee(S): 12 | return np.array([-S[1,2],S[0,2],-S[0,1]]) 13 | 14 | def RotToRPY(R): 15 | phi = asin(R[1,2]) 16 | theta = atan2(-R[0,2]/cos(phi),R[2,2]/cos(phi)) 17 | psi = atan2(-R[1,0]/cos(phi),R[1,1]/cos(phi)) 18 | return phi, theta, psi 19 | 20 | def RPYToRot(phi, theta, psi): 21 | """ 22 | phi, theta, psi = roll, pitch , yaw 23 | """ 24 | return np.array([[cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta), cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta), -cos(phi)*sin(theta)], 25 | [-cos(phi)*sin(psi), cos(phi)*cos(psi), sin(phi)], 26 | [cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi), sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi), cos(phi)*cos(theta)]]) 27 | 28 | def RotToQuat(R): 29 | """ 30 | ROTTOQUAT Converts a Rotation matrix into a Quaternion 31 | from the following website, deals with the case when tr<0 32 | http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm 33 | takes in W_R_B rotation matrix 34 | """ 35 | 36 | tr = R[0,0] + R[1,1] + R[2,2] 37 | if tr > 0: 38 | S = sqrt(tr+1.0) * 2 # S=4*qw 39 | qw = 0.25 * S 40 | qx = (R[2,1] - R[1,2]) / S 41 | qy = (R[0,2] - R[2,0]) / S 42 | qz = (R[1,0] - R[0,1]) / S 43 | elif (R[0,1] > R[1,1]) and (R[0,0] > R[2,2]): 44 | S = sqrt(1.0 + R[0,0] - R[1,1] - R[2,2]) * 2 # S=4*qx 45 | qw = (R[2,1] - R[1,2]) / S 46 | qx = 0.25 * S 47 | qy = (R[0,1] + R[1,0]) / S 48 | qz = (R[0,2] + R[2,0]) / S 49 | elif R[1,1] > R[2,2]: 50 | S = sqrt(1.0 + R[1,1] - R[0,0] - R[2,2]) * 2 # S=4*qy 51 | qw = (R[0,2] - R[2,0]) / S 52 | qx = (R[0,1] + R[1,0]) / S 53 | qy = 0.25 * S 54 | qz = (R[1,2] + R[2,1]) / S 55 | else: 56 | S = sqrt(1.0 + R[2,2] - R[0,0] - R[1,1]) * 2 # S=4*qz 57 | qw = (R[1,0] - R[0,1]) / S 58 | qx = (R[0,2] + R[2,0]) / S 59 | qy = (R[1,2] + R[2,1]) / S 60 | qz = 0.25 * S 61 | 62 | q = np.sign(qw) * np.array([qw, qx, qy, qz]) 63 | return q 64 | 65 | def writeNpArrayToFile(data): 66 | with open('state.csv','a') as f: 67 | np.savetxt(f, data, newline=",", fmt='%.2f') 68 | f.write('\n') 69 | 70 | def outputTraj(x,y,z): 71 | output = [] 72 | output.append((x,y,z)) 73 | with open('traj.out', 'w') as fp: 74 | fp.write('\n'.join('%s %s %s' % item for item in output)) 75 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Quadrotor simulation 2 | 3 | ### This project contains: 4 | - Informed RRT* to plan a collision free path from start to goal position in an N-dimensional Map. 5 | - A minimum snap trajectory generator. The segment times between consecutive waypoints are also optimised. 6 | - Nonlinear geometric controller for aggressive trajectory tracking and hover controller 7 | - Quadrotor Simulator 8 | 9 | ### Dependencies 10 | - scipy, numpy, matplotlib, rtree-linux 11 | 12 | ### Usage 13 | 14 | #### Trajectory generation and control example 15 | ```python 16 | from TrajGen import trajGenerator, Helix_waypoints 17 | from Quadrotor import QuadSim 18 | import controller 19 | 20 | waypoints = Helix_waypoints(5) 21 | 22 | #Generate trajectory through waypoints 23 | traj = trajGenerator(waypoints,max_vel = 10,gamma = 1e6) 24 | 25 | #initialise simulation with given controller and trajectory 26 | Tmax = traj.TS[-1] 27 | des_state = traj.get_des_state 28 | sim = QuadSim(controller,des_state,Tmax) 29 | 30 | #run simulation 31 | sim.run() 32 | ``` 33 | ![image](./imgs/helixtraj.gif) 34 | 35 | #### With Path Planning 36 | ```python 37 | python3 runsim.py 38 | ``` 39 | ![image](./imgs/rrt2.gif) 40 | 41 | 42 | find out more in [PathPlanning/README.md](./PathPlanning/README.md) 43 | ### Future Work 44 | 45 | - Adding disturbance to quadrotor model 46 | - Collision checking after trajectory generation 47 | 48 | ### References 49 | - [Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments - Charles Richter, Adam Bry, and Nicholas Roy](https://groups.csail.mit.edu/rrg/papers/Richter_ISRR13.pdf) 50 | - [Trajectory Generation and Control for Quadrotors - Daniel Mellinger and Vijay Kumar](https://repository.upenn.edu/cgi/viewcontent.cgi?article=1705&context=edissertations) 51 | - [Motion Planning as Search - Russ Tedrake](http://underactuated.csail.mit.edu/planning.html#section2) 52 | -------------------------------------------------------------------------------- /TrajGen/__init__.py: -------------------------------------------------------------------------------- 1 | from .trajGen import trajGenerator 2 | from .trajutils import Circle_waypoints, Helix_waypoints 3 | -------------------------------------------------------------------------------- /TrajGen/trajGen.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numpy import linalg as LA 3 | from scipy import optimize 4 | from .trajutils import * 5 | 6 | class trajGenerator: 7 | def __init__(self,waypoints,max_vel = 5,gamma = 100): 8 | self.waypoints = waypoints 9 | self.max_vel = max_vel 10 | self.gamma = gamma 11 | self.order = 10 12 | len,dim = waypoints.shape 13 | self.dim = dim 14 | self.len = len 15 | self.TS = np.zeros(self.len) 16 | self.optimize() 17 | self.yaw = 0 18 | self.heading = np.zeros(2) 19 | 20 | def get_cost(self,T): 21 | coeffs,cost = self.MinimizeSnap(T) 22 | cost = cost + self.gamma*np.sum(T) 23 | return cost 24 | 25 | def optimize(self): 26 | diff = self.waypoints[0:-1] - self.waypoints[1:] 27 | Tmin = LA.norm(diff,axis = -1)/self.max_vel 28 | T = optimize.minimize(self.get_cost,Tmin, method="COBYLA",constraints= ({'type': 'ineq', 'fun': lambda T: T-Tmin}))['x'] 29 | 30 | self.TS[1:] = np.cumsum(T) 31 | self.coeffs, self.cost = self.MinimizeSnap(T) 32 | 33 | 34 | def MinimizeSnap(self,T): 35 | unkns = 4*(self.len - 2) 36 | 37 | Q = Hessian(T) 38 | A,B = self.get_constraints(T) 39 | 40 | invA = LA.inv(A) 41 | 42 | if unkns != 0: 43 | R = invA.T@Q@invA 44 | 45 | Rfp = R[:-unkns,-unkns:] 46 | Rpp = R[-unkns:,-unkns:] 47 | 48 | B[-unkns:,] = -LA.inv(Rpp)@Rfp.T@B[:-unkns,] 49 | 50 | P = invA@B 51 | cost = np.trace(P.T@Q@P) 52 | 53 | return P, cost 54 | 55 | def get_constraints(self,T): 56 | n = self.len - 1 57 | o = self.order 58 | 59 | A = np.zeros((self.order*n, self.order*n)) 60 | B = np.zeros((self.order*n, self.dim)) 61 | 62 | B[:n,:] = self.waypoints[ :-1, : ] 63 | B[n:2*n,:] = self.waypoints[1: , : ] 64 | 65 | #waypoints contraints 66 | for i in range(n): 67 | A[i, o*i : o*(i+1)] = polyder(0) 68 | A[i + n, o*i : o*(i+1)] = polyder(T[i]) 69 | 70 | #continuity contraints 71 | for i in range(n-1): 72 | A[2*n + 4*i: 2*n + 4*(i+1), o*i : o*(i+1)] = -polyder(T[i],'all') 73 | A[2*n + 4*i: 2*n + 4*(i+1), o*(i+1) : o*(i+2)] = polyder(0,'all') 74 | 75 | #start and end at rest 76 | A[6*n - 4 : 6*n, : o] = polyder(0,'all') 77 | A[6*n : 6*n + 4, -o : ] = polyder(T[-1],'all') 78 | 79 | #free variables 80 | for i in range(1,n): 81 | A[6*n + 4*i : 6*n + 4*(i+1), o*i : o*(i+1)] = polyder(0,'all') 82 | 83 | return A,B 84 | 85 | def get_des_state(self,t): 86 | 87 | if t > self.TS[-1]: t = self.TS[-1] - 0.001 88 | 89 | i = np.where(t >= self.TS)[0][-1] 90 | 91 | t = t - self.TS[i] 92 | coeff = (self.coeffs.T)[:,self.order*i:self.order*(i+1)] 93 | 94 | pos = coeff@polyder(t) 95 | vel = coeff@polyder(t,1) 96 | accl = coeff@polyder(t,2) 97 | jerk = coeff@polyder(t,3) 98 | 99 | #set yaw in the direction of velocity 100 | yaw, yawdot = self.get_yaw(vel[:2]) 101 | 102 | return DesiredState(pos, vel, accl, jerk, yaw, yawdot) 103 | 104 | def get_yaw(self,vel): 105 | curr_heading = vel/LA.norm(vel) 106 | prev_heading = self.heading 107 | cosine = max(-1,min(np.dot(prev_heading, curr_heading),1)) 108 | dyaw = np.arccos(cosine) 109 | norm_v = np.cross(prev_heading,curr_heading) 110 | self.yaw += np.sign(norm_v)*dyaw 111 | 112 | if self.yaw > np.pi: self.yaw -= 2*np.pi 113 | if self.yaw < -np.pi: self.yaw += 2*np.pi 114 | 115 | self.heading = curr_heading 116 | yawdot = max(-30,min(dyaw/0.005,30)) 117 | return self.yaw,yawdot 118 | 119 | # def collision_optimize(self,Map): 120 | # check = True 121 | # while not check: 122 | # self.optimize() 123 | # check = self.check_collision(Map) 124 | # 125 | # def check_collision(self,Map): 126 | # for t in np.linspace(0,self.TS[-1],1000): 127 | # pos = self.get_des_state(t).pos 128 | # if Map.idx.count((*pos,)) != 0: 129 | # i = np.where(t >= self.TS)[0][-1] 130 | # new_waypoint = (waypoints[i,:]+waypoints[i+1,:])/2 131 | # np.insert(self.waypoints,i,new_waypoint) 132 | # return True 133 | # return False 134 | -------------------------------------------------------------------------------- /TrajGen/trajutils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from collections import namedtuple 3 | 4 | DesiredState = namedtuple('DesiredState', 'pos vel acc jerk yaw yawdot') 5 | 6 | def polyder(t, k = 0, order = 10): 7 | if k == 'all': 8 | terms = np.array([polyder(t,k,order) for k in range(1,5)]) 9 | else: 10 | terms = np.zeros(order) 11 | coeffs = np.polyder([1]*order,k)[::-1] 12 | pows = t**np.arange(0,order-k,1) 13 | terms[k:] = coeffs*pows 14 | return terms 15 | 16 | def Hessian(T,order = 10,opt = 4): 17 | n = len(T) 18 | Q = np.zeros((order*n,order*n)) 19 | for k in range(n): 20 | m = np.arange(0,opt,1) 21 | for i in range(order): 22 | for j in range(order): 23 | if i >= opt and j >= opt: 24 | pow = i+j-2*opt+1 25 | Q[order*k+i,order*k+j] = 2*np.prod((i-m)*(j-m))*T[k]**pow/pow 26 | return Q 27 | 28 | def Circle_waypoints(n,Tmax = 2*np.pi): 29 | t = np.linspace(0,Tmax, n) 30 | x = 1+0.5*np.cos(t) 31 | y = 1+0.5*np.sin(t) 32 | z = 1+0*t 33 | return np.stack((x, y, z), axis=-1) 34 | 35 | def Helix_waypoints(n,Tmax = 2*np.pi): 36 | 37 | t = np.linspace(0, Tmax, n) 38 | x = 1+0.5*np.cos(t) 39 | y = 1+0.5*np.sin(t) 40 | z = t/Tmax*2 41 | 42 | return np.stack((x, y, z), axis=-1) 43 | -------------------------------------------------------------------------------- /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-cayman -------------------------------------------------------------------------------- /controller.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from math import sin, cos 3 | from Quadrotor import params 4 | from Quadrotor.utils import RPYToRot,vee 5 | 6 | kv = np.array([10,10,10]); 7 | kp = np.array([30,30,30]); 8 | 9 | kR = np.array([5,5,5]) 10 | kW = np.array([0.05,0.05,0.1]) 11 | 12 | # Constants 13 | m = params.mass 14 | g = params.g 15 | I = params.I 16 | 17 | eI = np.zeros(3) 18 | 19 | #Nonlinear Controller for trajectory tracking 20 | def run(state, des_state): 21 | yaw_des = des_state.yaw 22 | dyaw_des = des_state.yawdot 23 | 24 | #current Rotation from body frame to world 25 | bRw = RPYToRot(*state.rot).T 26 | #normal vector to body frame 27 | ZB = bRw[:,2] 28 | 29 | #position and velocity errors 30 | ep = des_state.pos - state.pos 31 | ev = des_state.vel - state.vel 32 | 33 | #command acceleration 34 | commd_acc = kp*ep + kv*ev + des_state.acc 35 | # Desired Thrust vector 36 | F_des = m*commd_acc + m*g*np.array([0,0,1]) 37 | # Desired Thrust in ZB direction 38 | U = F_des@ZB 39 | 40 | #current acceleration 41 | curr_acc = U*ZB/m - g*np.array([0,0,1]) 42 | #acceleration error 43 | ea = curr_acc - des_state.acc 44 | 45 | #command jerk 46 | commd_jerk = kp*ev + kv*ea + des_state.jerk 47 | # derivative of desired Thrust vector 48 | dF_des = m*commd_jerk 49 | # derivative of desired Thrust vector in ZB direction 50 | dU = dF_des@ZB 51 | 52 | #desired direction of normal vector 53 | ZB_des = F_des/np.linalg.norm(F_des) 54 | 55 | Xc = np.array([cos(yaw_des),sin(yaw_des),0]) 56 | ZB_Xc = np.cross(ZB_des,Xc) 57 | 58 | YB_des = ZB_Xc/np.linalg.norm(ZB_Xc) 59 | XB_des = np.cross(YB_des,ZB_des) 60 | 61 | #desired Rotation matrix 62 | R_des = np.c_[XB_des.T,YB_des.T,ZB_des.T] 63 | 64 | #Rotation error 65 | eR = 0.5*vee(bRw.T@R_des - R_des.T@bRw) 66 | 67 | #projection of angular velocity on xB − yB plane 68 | hw = (m*commd_jerk - dU*ZB_des)/U 69 | #desired angular velocity 70 | omega_des = np.array([-hw@YB_des, 71 | hw@XB_des, 72 | dyaw_des*ZB_des[2]]) 73 | 74 | #angular velocity error 75 | eW = omega_des - state.omega 76 | 77 | #moment 78 | M = np.array([kR*eR + kW*eW]).T 79 | 80 | return U, M 81 | 82 | #Controller for hover 83 | def run_hover(state, des_state, dt): 84 | yaw_des = des_state.yaw 85 | global eI 86 | 87 | #position and velocity errors 88 | ep = des_state.pos - state.pos 89 | ev = des_state.vel - state.vel 90 | eI += ep*dt 91 | 92 | #command acceleration 93 | commd_acc = kp*ep + kv*ev + kp*eI + des_state.acc 94 | 95 | #Desired Thrust in ZB direction 96 | U = m*commd_acc[2] + m*g 97 | 98 | #Rotation error 99 | phi_des = 1/g*(commd_acc[0]*sin(yaw_des) - commd_acc[1]*cos(yaw_des)) 100 | theta_des = 1/g*(commd_acc[0]*cos(yaw_des) + commd_acc[1]*sin(yaw_des)) 101 | eR = np.array([phi_des,theta_des,yaw_des]) - state.rot 102 | 103 | #desired angular velocity 104 | omega_des = np.array([0,0,0]) 105 | #angular velocity error 106 | eW = omega_des - state.omega; 107 | 108 | #moment 109 | M = np.array([kR*eR + kW*eW]).T 110 | 111 | return U, M 112 | -------------------------------------------------------------------------------- /google60f4e7ebc14b633d.html: -------------------------------------------------------------------------------- 1 | google-site-verification: google60f4e7ebc14b633d.html -------------------------------------------------------------------------------- /imgs/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bharath2/Quadrotor-Simulation/c838fa0adc07da7d52459b228e90897a14dcace4/imgs/.DS_Store -------------------------------------------------------------------------------- /imgs/ex.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bharath2/Quadrotor-Simulation/c838fa0adc07da7d52459b228e90897a14dcace4/imgs/ex.gif -------------------------------------------------------------------------------- /imgs/helixtraj.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bharath2/Quadrotor-Simulation/c838fa0adc07da7d52459b228e90897a14dcace4/imgs/helixtraj.gif -------------------------------------------------------------------------------- /imgs/rrt.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bharath2/Quadrotor-Simulation/c838fa0adc07da7d52459b228e90897a14dcace4/imgs/rrt.gif -------------------------------------------------------------------------------- /imgs/rrt2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bharath2/Quadrotor-Simulation/c838fa0adc07da7d52459b228e90897a14dcace4/imgs/rrt2.gif -------------------------------------------------------------------------------- /runsim.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import mpl_toolkits.mplot3d.axes3d as Axes3D 4 | 5 | from PathPlanning import RRTStar, Map 6 | from TrajGen import trajGenerator, Helix_waypoints, Circle_waypoints 7 | from Quadrotor import QuadSim 8 | import controller 9 | np.random.seed(8) 10 | 11 | # 3D boxes lx, ly, lz, hx, hy, hz 12 | obstacles = [[-5, 25, 0, 20, 35, 60], 13 | [30, 25, 0, 55, 35, 100], 14 | [45, 35, 0, 55, 60, 60], 15 | [45, 75, 0, 55, 85, 100], 16 | [-5, 65, 0, 30, 70, 100], 17 | [70, 50, 0, 80, 80, 100]] 18 | 19 | # limits on map dimensions 20 | bounds = np.array([0,100]) 21 | # create map with obstacles 22 | mapobs = Map(obstacles, bounds, dim = 3) 23 | 24 | #plan a path from start to goal 25 | start = np.array([80,20,10]) 26 | goal = np.array([30,80,80]) 27 | 28 | rrt = RRTStar(start = start, goal = goal, 29 | Map = mapobs, max_iter = 500, 30 | goal_sample_rate = 0.1) 31 | 32 | waypoints, min_cost = rrt.plan() 33 | 34 | 35 | #scale the waypoints to real dimensions 36 | waypoints = 0.02*waypoints 37 | 38 | #Generate trajectory through waypoints 39 | traj = trajGenerator(waypoints, max_vel = 10, gamma = 1e6) 40 | 41 | #initialise simulation with given controller and trajectory 42 | Tmax = traj.TS[-1] 43 | des_state = traj.get_des_state 44 | sim = QuadSim(controller,des_state,Tmax) 45 | 46 | #create a figure 47 | fig = plt.figure() 48 | ax = Axes3D.Axes3D(fig) 49 | ax.set_xlim((0,2)) 50 | ax.set_ylim((0,2)) 51 | ax.set_zlim((0,2)) 52 | 53 | #plot the waypoints and obstacles 54 | rrt.draw_path(ax, waypoints) 55 | mapobs.plotobs(ax, scale = 0.02) 56 | 57 | #run simulation 58 | sim.run(ax) 59 | --------------------------------------------------------------------------------