├── README.md ├── __init__.py ├── config.py ├── .gitignore ├── models ├── gibsonWorld.py ├── randomWorld.py ├── point.py └── racecarv2.py ├── gibsonWorldPlan.py ├── naive_rrt_path.py ├── lqg_planning.py ├── visualize_path.py ├── rrt_motion_density.py ├── rrt_motion_val.py └── rrt_motion_dubin.py /README.md: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /config.py: -------------------------------------------------------------------------------- 1 | # The configuration files for defining the size, and location of obstacles 2 | import numpy as np 3 | 4 | # Box obstacles 5 | xy = [np.r_[2,2], np.r_[2, 8], np.r_[5,5], np.r_[8, 2], np.r_[8, 8]] # Box positions 6 | box_length, box_width = 1., 1. 7 | 8 | # Circle Obstacles 9 | cir_radius = 0.5 10 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Untrack pycache 2 | **/__pycache__/** 3 | 4 | # Untrack package folders 5 | bullet3/** 6 | python-fcl/** 7 | sparse_rrt/** 8 | dccp/** 9 | 10 | # Untrack personal folders 11 | data/** 12 | dataset/** 13 | env_param/** 14 | jupyter_ws/** 15 | .vscode/** 16 | envs/** 17 | assets_core_v2/** 18 | assets/** 19 | param/** 20 | 21 | # Untrack data files 22 | **/*.npy 23 | **/*.p 24 | 25 | # Untrack docker files 26 | docker/** 27 | 28 | # Untrack ipy checkpoints 29 | *.ipynb 30 | **/*.ipynb_checkpoints/ 31 | 32 | # Untrack images 33 | *.png -------------------------------------------------------------------------------- /models/gibsonWorld.py: -------------------------------------------------------------------------------- 1 | '''A script to load a test environment for testing GP models. 2 | ''' 3 | import numpy as np 4 | import GPy 5 | from os import path as osp 6 | from scipy import stats 7 | import pickle 8 | 9 | import time 10 | 11 | folder = '/root/prob_planning/assets/Allensville/{}.urdf' 12 | # folder = '/home/jacoblab/prob_planning/assets/Allensville/{}.urdf' 13 | # Load environment 14 | items = [ 15 | 'coffee_table', 16 | 'side_table', 17 | 'kitchen_counter', 18 | 'chair1', 19 | 'chair2', 20 | 'outerWall2', 21 | 'outerWall3', 22 | 'outerWall4', 23 | 'kitchen2', 24 | ] 25 | 26 | def set_obstacles(client_obj): 27 | ''' 28 | Set up the obstacles in the given client. 29 | :param clinet_obj: 30 | :returns list: A list of obstacles 31 | ''' 32 | obstacles = [client_obj.loadURDF(folder.format(i)) for i in items] 33 | return obstacles 34 | -------------------------------------------------------------------------------- /models/randomWorld.py: -------------------------------------------------------------------------------- 1 | ''' Create a random environment 2 | ''' 3 | 4 | import pybullet_utils.bullet_client as bc 5 | import pybullet as pyb 6 | import pybullet_data 7 | 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | from config import box_length, box_width, cir_radius 11 | 12 | class RandomWorld: 13 | seed = 10 14 | 15 | # Randomly generate boxes 16 | num_boxes = 8 17 | xy = [] 18 | 19 | # Randomly generate circles 20 | num_circles = 5 21 | xy_circle = [] 22 | 23 | def set_obstacles(client_obj, **kwargs): 24 | ''' 25 | A function to set obstacles in the environment 26 | :param client_obj: A pybullet_utils.BulletClient object 27 | :param kwargs: Additional environment parameters. 28 | seed: The random seed used to create environment 29 | num_boxes: Number of square obstacles 30 | num_circles: Number of circular obstacles 31 | ''' 32 | rgba = [0.125, 0.5, 0.5, 1] 33 | geomBox = client_obj.createCollisionShape(pyb.GEOM_BOX, halfExtents=[box_length/2, box_width/2, 0.2]) 34 | visualBox = client_obj.createVisualShape(pyb.GEOM_BOX, halfExtents=[box_length/2, box_width/2, 0.2], rgbaColor=rgba) 35 | geomCircle = client_obj.createCollisionShape(pyb.GEOM_CYLINDER, radius=cir_radius, height = 0.4) 36 | visualCircle = client_obj.createVisualShape(pyb.GEOM_CYLINDER, radius=cir_radius, length = 0.4, rgbaColor=rgba) 37 | 38 | if 'seed' in kwargs.keys(): 39 | np.random.seed(kwargs['seed']) 40 | RandomWorld.seed = kwargs['seed'] 41 | else: 42 | np.random.seed(RandomWorld.seed) 43 | 44 | if 'num_boxes' in kwargs.keys(): 45 | RandomWorld.num_boxes = kwargs['num_boxes'] 46 | 47 | if 'num_circles' in kwargs.keys(): 48 | RandomWorld.num_circles = kwargs['num_circles'] 49 | 50 | RandomWorld.xy = np.random.rand(RandomWorld.num_boxes, 2)*9 + 0.5 51 | RandomWorld.xy_circle = np.random.rand(RandomWorld.num_circles, 2)*9 + 0.5 52 | 53 | obstacles_box = [ 54 | client_obj.createMultiBody( 55 | baseMass=0, 56 | baseCollisionShapeIndex=geomBox, 57 | baseVisualShapeIndex=visualBox, 58 | basePosition=np.r_[xy_i, 0.2] 59 | ) 60 | for xy_i in RandomWorld.xy 61 | ] 62 | 63 | obstacles_circle = [ 64 | client_obj.createMultiBody( 65 | baseMass=0, 66 | baseCollisionShapeIndex=geomCircle, 67 | baseVisualShapeIndex=visualCircle, 68 | basePosition=np.r_[xy_i, 0.2] 69 | ) 70 | for xy_i in RandomWorld.xy_circle 71 | ] 72 | obstacles = obstacles_box + obstacles_circle 73 | return obstacles 74 | 75 | 76 | def plot_env(ax, alpha=1): 77 | ''' 78 | Plots the environment on matplotlib figure 79 | :param ax: The axis on which to plot the path. 80 | ''' 81 | assert len(RandomWorld.xy)>0 or len(RandomWorld.xy_circle)>0, "Call the set_env first" 82 | 83 | # Initialize the position of obstacles 84 | dimensions = [box_length, box_width] 85 | rectangle_corner = np.r_[(-dimensions[0]/2, -dimensions[1]/2)] 86 | 87 | # ax.set_xlim((-0.750, 10.5)) 88 | # ax.set_ylim((-0.750, 10.5)) 89 | ax.set_xlim((-2, 12)) 90 | ax.set_ylim((-2, 12)) 91 | 92 | 93 | for xy_i in RandomWorld.xy_circle: 94 | plt_cir = plt.Circle(xy_i, radius=cir_radius, color='r', alpha=alpha) 95 | ax.add_patch(plt_cir) 96 | 97 | for xy_i in RandomWorld.xy: 98 | plt_box = plt.Rectangle(xy_i+rectangle_corner, dimensions[0], dimensions[1], color='r', alpha=alpha) 99 | ax.add_patch(plt_box) -------------------------------------------------------------------------------- /gibsonWorldPlan.py: -------------------------------------------------------------------------------- 1 | '''A script to plan for the gibsonWorldEnv 2 | ''' 3 | 4 | import pybullet as p 5 | import time 6 | import numpy as np 7 | from scipy import stats 8 | import matplotlib.pyplot as plt 9 | import seaborn as sns 10 | 11 | import pickle 12 | 13 | import GPy 14 | 15 | import os.path as osp 16 | import sys 17 | try: 18 | import ompl.base as ob 19 | import ompl.geometric as og 20 | 21 | class ValidityChecker(ob.StateValidityChecker): 22 | '''A class to check the validity of the state 23 | ''' 24 | def isValid(self, state): 25 | ''' 26 | Check if the given state is valid. 27 | ''' 28 | yaw = state.getYaw() 29 | x_hat = np.r_[[[state.getX(), state.getY(), np.cos(yaw), np.sin(yaw)]]] 30 | mean, var = m.predict(x_hat) 31 | return (mean/np.sqrt(2*var))[0,0] > c 32 | 33 | class ValidityCheckerDistance(ob.StateValidityChecker): 34 | '''A class to check the validity of the state by calculating 35 | the distance to obstacle 36 | ''' 37 | def isValid(self, state): 38 | ''' 39 | Check if the given state is valid. 40 | ''' 41 | collision = racecarv2.check_collision( 42 | np.r_[state.getX(), state.getY(), state.getYaw()], 43 | obstacles, 44 | robot 45 | ) 46 | return not collision 47 | 48 | 49 | # Wheel rotation is kept at np.pi*0.35, R = wheelbase/np.sin(max_steer) 50 | dubinSpace = ob.DubinsStateSpace(0.5) 51 | 52 | # Define SpaceInformation object 53 | si = ob.SpaceInformation(dubinSpace) 54 | 55 | # Collision checker obj 56 | GP_check = True 57 | if GP_check: 58 | ValidityChecker_obj = ValidityChecker(si) 59 | else: 60 | ValidityChecker_obj = ValidityCheckerDistance(si) 61 | si.setStateValidityChecker(ValidityChecker_obj) 62 | 63 | # Set the bounds of the space 64 | bounds = ob.RealVectorBounds(2) 65 | bounds.setLow(0.0) 66 | bounds.setHigh(10) 67 | dubinSpace.setBounds(bounds) 68 | except ImportError: 69 | print("Run code in a the ompl docker") 70 | print("ValidityChecker and ValidityCheckerDistance won't work") 71 | # raise ImportError("Run in a docker with ompl") 72 | 73 | from models import racecarv2 74 | 75 | obstacles, robot = racecarv2.set_env(name="Gibson") 76 | 77 | def SE2State2Tuple(state): 78 | ''' 79 | convert SE2 state object to tuple 80 | :param state: An SE2 object 81 | :return tuple: (x,y, theta) 82 | ''' 83 | return (state.getX(), state.getY(), state.getYaw()) 84 | 85 | 86 | # Set up planning threshold limits 87 | thresh = 0.01 88 | N = stats.norm(scale=np.sqrt(1/2)) 89 | c = N.ppf(1-thresh) 90 | 91 | m = racecarv2.get_model_KF(robot, obstacles) 92 | 93 | 94 | def get_path(start, goal): 95 | ''' 96 | Get the RRT* for SE2 space for a given start and goal. 97 | :param start: og.State object. 98 | :param goal: og.State object. 99 | returns (np.array, np.array, success): A tuple of numpy arrays of a valid path, 100 | interpolated path and whether the plan was successful or not. 101 | ''' 102 | success = False 103 | ss = og.SimpleSetup(si) 104 | ss.setStartAndGoalStates(start, goal, 0.1) 105 | 106 | # planner = og.RRT(si) 107 | planner = og.RRTstar(si) 108 | ss.setPlanner(planner) 109 | 110 | time = 60.0 111 | solved = ss.solve(time) 112 | 113 | while not ss.haveExactSolutionPath(): 114 | solved = ss.solve(30.0) 115 | time += 30 116 | if time>1500: 117 | break 118 | 119 | if ss.haveExactSolutionPath(): 120 | ss.simplifySolution() 121 | success = True 122 | print("Found Solution") 123 | path = [ 124 | SE2State2Tuple(ss.getSolutionPath().getState(i)) 125 | for i in range(ss.getSolutionPath().getStateCount()) 126 | ] 127 | ss.getSolutionPath().interpolate(5000) 128 | path_obj = ss.getSolutionPath() 129 | path_interpolated = [ 130 | SE2State2Tuple(path_obj.getState(i)) 131 | for i in range(path_obj.getStateCount()) 132 | ] 133 | else: 134 | path = [SE2State2Tuple(start()), SE2State2Tuple(goal())] 135 | path_interpolated = [] 136 | 137 | return path, path_interpolated, success 138 | 139 | 140 | def plan_path(): 141 | start = ob.State(dubinSpace) 142 | start[0] = 6.5 143 | start[1] = 6.5 144 | start[2] = -3*np.pi/4 145 | 146 | goal = ob.State(dubinSpace) 147 | goal[0] = 4 #2.4 148 | goal[1] = 2 #2.5 149 | goal[2] = -np.pi #3*np.pi/4 150 | path, path_interpolated, success = get_path(start, goal) 151 | path_param = {'path':path, 'path_interpolated':path_interpolated, 'success': success} 152 | 153 | if GP_check: 154 | exp = 'ccgp' 155 | else: 156 | exp = 'rrt_star' 157 | pickle.dump(path_param, open('/root/data/gibson_path_{}.p'.format(exp), 'wb')) 158 | 159 | 160 | steering = [0, 2] 161 | 162 | def control_robot(): 163 | ''' 164 | Intialize the control of the robot. 165 | ''' 166 | targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -50, 50, 0) 167 | maxForceSlider = p.addUserDebugParameter("maxForce", 0, 50, 20) 168 | steeringSlider = p.addUserDebugParameter("steering", -1, 1, 0) 169 | while True: 170 | maxForce = p.readUserDebugParameter(maxForceSlider) 171 | targetVelocity = p.readUserDebugParameter(targetVelocitySlider) 172 | steeringAngle = p.readUserDebugParameter(steeringSlider) 173 | #print(targetVelocity) 174 | 175 | for wheel in racecarv2.wheels: 176 | p.setJointMotorControl2(robot, 177 | wheel, 178 | p.VELOCITY_CONTROL, 179 | targetVelocity=targetVelocity, 180 | force=maxForce) 181 | 182 | for steer in steering: 183 | p.setJointMotorControl2(robot, steer, p.POSITION_CONTROL, targetPosition=-steeringAngle) 184 | 185 | steering 186 | print(racecarv2.get_distance(obstacles, robot)) 187 | p.stepSimulation() 188 | time.sleep(0.01) 189 | 190 | if __name__ == "__main__": 191 | plan_path() 192 | # control_robot() 193 | # pass 194 | 195 | 196 | 197 | -------------------------------------------------------------------------------- /naive_rrt_path.py: -------------------------------------------------------------------------------- 1 | '''Planning according to naive RRT. 2 | ''' 3 | import numpy as np 4 | import pybullet as p 5 | from scipy import stats, optimize, special 6 | 7 | import matplotlib.pyplot as plt 8 | import seaborn as sns 9 | 10 | try: 11 | from ompl import base as ob 12 | from ompl import geometric as og 13 | except ImportError: 14 | print("Could not find OMPL") 15 | raise ImportError("Run inside docker!!") 16 | 17 | # Import project specific files 18 | from models import point 19 | from config import box_width, box_length, xy, cir_radius 20 | 21 | obstacles, robot = point.set_env() 22 | 23 | # Define LTI system 24 | A, B, M, N= point.get_dyn() 25 | 26 | # Noise model 27 | N_n = stats.multivariate_normal(cov=N) # Observation Noise 28 | M_n = stats.multivariate_normal(cov=M) # Motion Noise 29 | 30 | # Define gamma function for evaluating path 31 | S = lambda x: special.gammainc(1, x) 32 | 33 | # Threshold for each point. 34 | thresh = 0.1 35 | 36 | fig, ax = plt.subplots() 37 | sns.set() 38 | 39 | ax.set_xlim((-0.2, 10.2)) 40 | ax.set_ylim((-0.2, 10.2)) 41 | 42 | # Initialize the position of obstacles 43 | dimensions = [box_length, box_width] 44 | rectangle_corner = np.r_[(-dimensions[0]/2, -dimensions[1]/2)] 45 | 46 | for xy_i in point.xy_circle: 47 | plt_cir = plt.Circle(xy_i, radius=cir_radius, color='r') 48 | ax.add_patch(plt_cir) 49 | 50 | for xy_i in point.xy: 51 | plt_box = plt.Rectangle(xy_i+rectangle_corner, dimensions[0], dimensions[1], color='r') 52 | ax.add_patch(plt_box) 53 | 54 | 55 | class ValidityCheckerDistance(ob.StateValidityChecker): 56 | '''A class to check the validity of the state, by checking distance function 57 | ''' 58 | defaultOrientation = p.getQuaternionFromEuler([0., 0., 0.]) 59 | 60 | def isValid(self, state): 61 | ''' 62 | Check if the given state is valid. 63 | :param state: An ob.STate object to be checked. 64 | :return bool: True if the state is valid. 65 | ''' 66 | return self.getDistance(state)>0 67 | 68 | def getDistance(self, state): 69 | ''' 70 | Get the shortest distance from robot to obstacle. 71 | :param x: A numpy array of state x. 72 | :returns float: The closest distance between the robot and obstacle. 73 | ''' 74 | p.resetBasePositionAndOrientation(robot, np.r_[state.getX(), state.getY(), 0.1], self.defaultOrientation) 75 | return point.get_distance(obstacles, robot) 76 | 77 | 78 | # Define the space 79 | space = ob.SE2StateSpace() 80 | 81 | # Set the bounds 82 | bounds = ob.RealVectorBounds(2) 83 | bounds.setLow(-0.2) 84 | bounds.setHigh(10) 85 | space.setBounds(bounds) 86 | 87 | 88 | # Define the start and goal location 89 | start = ob.State(space) 90 | # start.random() 91 | # while not ValidityChecker_obj.isValid(start()): 92 | # start.random() 93 | # start[0] = 1.0 94 | # start[1] = 8.0 95 | goal = ob.State(space) 96 | goal[0] = 8.5 97 | goal[1] = 8.5 98 | 99 | # Function to get the distribution of path. 100 | A = A[:2, :2] 101 | B = B[:2, :2] 102 | # LQG parameter: 103 | C = np.eye(2) 104 | D = np.eye(2) 105 | 106 | Q = np.block([ 107 | [M, np.zeros((2,2))], 108 | [np.zeros((2,2)), N] 109 | ]) 110 | 111 | # try: 112 | # path = np.load("path_lqg.npy") 113 | # path_interpolated = np.load("path_interpolated_lqg.npy") 114 | # # start[0] = path[0,0] 115 | # # start[1] = path[0,1] 116 | # except FileNotFoundError: 117 | 118 | def get_path(start, goal): 119 | ''' 120 | Get a RRT path from start and goal. 121 | :param start: og.State object. 122 | :param goal: og.State object. 123 | returns (np.array, np.array): A tuple of numpy arrays of a valid path and 124 | interpolated path. 125 | ''' 126 | # Define the SpaceInformation object. 127 | si = ob.SpaceInformation(space) 128 | 129 | ValidityChecker_obj = ValidityCheckerDistance(si) 130 | 131 | si.setStateValidityChecker(ValidityChecker_obj) 132 | 133 | # Create a simple setup 134 | ss = og.SimpleSetup(si) 135 | 136 | # Set the start and goal states: 137 | ss.setStartAndGoalStates(start, goal) 138 | 139 | # define the planner 140 | planner = og.RRT(si) 141 | ss.setPlanner(planner) 142 | 143 | # Attempt to solve within the given time 144 | solved = ss.solve(10.0) 145 | if solved: 146 | print("Found solution") 147 | path = [ 148 | [ss.getSolutionPath().getState(i).getX(), ss.getSolutionPath().getState(i).getY()] 149 | for i in range(ss.getSolutionPath().getStateCount()) 150 | ] 151 | ss.getSolutionPath().interpolate(100) 152 | path_obj = ss.getSolutionPath() 153 | path_interpolated = np.array([ 154 | [path_obj.getState(i).getX(), path_obj.getState(i).getY()] 155 | for i in range(path_obj.getStateCount()) 156 | ]) 157 | else: 158 | path = [] 159 | path_interpolated = [] 160 | return np.array(path), np.array(path_interpolated) 161 | 162 | import os 163 | import pickle 164 | 165 | def start_experiment(): 166 | ''' 167 | Run the LQG experiment for the start and goal points for the same RRT-paths 168 | ''' 169 | # Define path 170 | folder_loc = '/root/data' 171 | 172 | # for file_i in os.listdir(folder_loc): 173 | for i in range(350, 400): 174 | # if '.p' in file_i: 175 | file_i = 'path_{}.p'.format(i) 176 | data = pickle.load(open(os.path.join(folder_loc, file_i), 'rb')) 177 | start_array = data['path'][0] 178 | goal_array = data['path'][-1] 179 | start = ob.State(space) 180 | start[0] = start_array[0] 181 | start[1] = start_array[1] 182 | goal = ob.State(space) 183 | goal[0] = goal_array[0] 184 | goal[1] = goal_array[1] 185 | 186 | path_param = {} 187 | path, path_interpolated = get_path(start, goal) 188 | path_param['path'] = path 189 | path_param['path_interpolated'] = path_interpolated 190 | # Evaluate 100 paths 191 | accuracy = 0 192 | si_check = ob.SpaceInformation(space) 193 | ValidityChecker_dis_obj = ValidityCheckerDistance(si_check) 194 | si_check.setStateValidityChecker(ValidityChecker_dis_obj) 195 | for _ in range(100): 196 | _, _, done = point.execute_path(path_interpolated, C, D, si_check) 197 | if done: 198 | accuracy += 1 199 | path_param['accuracy'] = accuracy 200 | 201 | pickle.dump(path_param, open(os.path.join(folder_loc, 'rrt', file_i), 'wb')) 202 | 203 | if __name__ == "__main__": 204 | 205 | start_experiment() 206 | 207 | if False: 208 | ax.scatter(start[0], start[1], color='r', marker='x') 209 | ax.scatter(goal[0], goal[1], color='g', marker='o') 210 | 211 | if path!=[]: 212 | ax.plot(path[:,0], path[:,1], color='b', alpha=0.5, label='RRT path') 213 | ax.scatter(path[:,0], path[:,1], color='b', label='RRT path') 214 | 215 | # Evaluate 10 paths 216 | successfull = 0 217 | si_check = ob.SpaceInformation(space) 218 | ValidityChecker_obj = ValidityCheckerDistance(si_check) 219 | si_check.setStateValidityChecker(ValidityChecker_obj) 220 | for _ in range(10): 221 | path_est, path_noisy, done = point.execute_path(path_interpolated, C, D, si_check) 222 | # if not done: 223 | path_est = np.array(path_est) 224 | # ax.plot(path_est[:,0], path_est[:,1], color='r', label='estimated state') 225 | path_noisy = np.array(path_noisy) 226 | ax.plot(path_noisy[:,0], path_noisy[:,1], '--',color='g', alpha=0.5, label='real state') 227 | # else: 228 | # successfull+=1 229 | print("Total succesful paths {}".format(successfull)) -------------------------------------------------------------------------------- /lqg_planning.py: -------------------------------------------------------------------------------- 1 | '''Planning according to LQG-MP. 2 | ''' 3 | import numpy as np 4 | import pybullet as p 5 | from scipy import stats, optimize, special 6 | 7 | import matplotlib.pyplot as plt 8 | import seaborn as sns 9 | 10 | try: 11 | from ompl import base as ob 12 | from ompl import geometric as og 13 | except ImportError: 14 | print("Could not find OMPL") 15 | raise ImportError("Run inside docker!!") 16 | 17 | # Import project specific files 18 | from models import point 19 | from config import box_width, box_length, xy, cir_radius 20 | 21 | obstacles, robot = point.set_env() 22 | 23 | # Define LTI system 24 | A, B, M, N= point.get_dyn() 25 | 26 | # Noise model 27 | N_n = stats.multivariate_normal(cov=N) # Observation Noise 28 | M_n = stats.multivariate_normal(cov=M) # Motion Noise 29 | 30 | # Define gamma function for evaluating path 31 | S = lambda x: special.gammainc(1, x) 32 | 33 | # Threshold for each point. 34 | thresh = 0.1 35 | 36 | fig, ax = plt.subplots() 37 | sns.set() 38 | 39 | ax.set_xlim((-0.2, 10.2)) 40 | ax.set_ylim((-0.2, 10.2)) 41 | 42 | # Initialize the position of obstacles 43 | dimensions = [box_length, box_width] 44 | rectangle_corner = np.r_[(-dimensions[0]/2, -dimensions[1]/2)] 45 | 46 | for xy_i in point.xy_circle: 47 | plt_cir = plt.Circle(xy_i, radius=cir_radius, color='r') 48 | ax.add_patch(plt_cir) 49 | 50 | for xy_i in point.xy: 51 | plt_box = plt.Rectangle(xy_i+rectangle_corner, dimensions[0], dimensions[1], color='r') 52 | ax.add_patch(plt_box) 53 | 54 | 55 | class ValidityCheckerDistance(ob.StateValidityChecker): 56 | '''A class to check the validity of the state, by checking distance function 57 | ''' 58 | defaultOrientation = p.getQuaternionFromEuler([0., 0., 0.]) 59 | 60 | def isValid(self, state): 61 | ''' 62 | Check if the given state is valid. 63 | :param state: An ob.STate object to be checked. 64 | :return bool: True if the state is valid. 65 | ''' 66 | return self.getDistance(state)>0 67 | 68 | def getDistance(self, state): 69 | ''' 70 | Get the shortest distance from robot to obstacle. 71 | :param x: A numpy array of state x. 72 | :returns float: The closest distance between the robot and obstacle. 73 | ''' 74 | p.resetBasePositionAndOrientation(robot, np.r_[state[0], state[1], 0.1], self.defaultOrientation) 75 | return point.get_distance(obstacles, robot) 76 | 77 | 78 | # Define the space 79 | space = ob.RealVectorStateSpace(2) 80 | 81 | # Set the bounds 82 | bounds = ob.RealVectorBounds(2) 83 | bounds.setLow(-0.2) 84 | bounds.setHigh(10) 85 | space.setBounds(bounds) 86 | 87 | 88 | # Define the start and goal location 89 | start = ob.State(space) 90 | # start.random() 91 | # while not ValidityChecker_obj.isValid(start()): 92 | # start.random() 93 | # start[0] = 1.0 94 | # start[1] = 8.0 95 | goal = ob.State(space) 96 | goal[0] = 8.5 97 | goal[1] = 8.5 98 | 99 | # Function to get the distribution of path. 100 | A = A[:2, :2] 101 | B = B[:2, :2] 102 | # LQG parameter: 103 | C = np.eye(2) 104 | D = np.eye(2) 105 | 106 | Q = np.block([ 107 | [M, np.zeros((2,2))], 108 | [np.zeros((2,2)), N] 109 | ]) 110 | 111 | # try: 112 | # path = np.load("path_lqg.npy") 113 | # path_interpolated = np.load("path_interpolated_lqg.npy") 114 | # # start[0] = path[0,0] 115 | # # start[1] = path[0,1] 116 | # except FileNotFoundError: 117 | 118 | def get_path(start, goal): 119 | ''' 120 | Get a RRT path from start and goal. 121 | :param start: og.State object. 122 | :param goal: og.State object. 123 | returns (np.array, np.array): A tuple of numpy arrays of a valid path and 124 | interpolated path. 125 | ''' 126 | 127 | # Define the SpaceInformation object. 128 | si = ob.SpaceInformation(space) 129 | 130 | ValidityChecker_obj = ValidityCheckerDistance(si) 131 | 132 | si.setStateValidityChecker(ValidityChecker_obj) 133 | 134 | # Create a simple setup 135 | ss = og.SimpleSetup(si) 136 | 137 | # Set the start and goal states: 138 | ss.setStartAndGoalStates(start, goal) 139 | 140 | # define the planner 141 | planner = og.RRT(si) 142 | ss.setPlanner(planner) 143 | 144 | # Attempt to solve within the given time 145 | solved = ss.solve(10.0) 146 | if solved: 147 | print("Found solution") 148 | path = [ 149 | [ss.getSolutionPath().getState(i)[0], ss.getSolutionPath().getState(i)[1]] 150 | for i in range(ss.getSolutionPath().getStateCount()) 151 | ] 152 | ss.getSolutionPath().interpolate(100) 153 | path_obj = ss.getSolutionPath() 154 | path_interpolated = np.array([ 155 | [path_obj.getState(i)[0], path_obj.getState(i)[1]] 156 | for i in range(path_obj.getStateCount()) 157 | ]) 158 | else: 159 | path = [] 160 | path_interpolated = [] 161 | return np.array(path), np.array(path_interpolated) 162 | 163 | 164 | def lqg_mp(start, goal): 165 | ''' 166 | Get the lqg_mp plan for a the given start and goal position. 167 | :param start: A np.array representing the start position. 168 | :param goal: A np.array representing the goal positon. 169 | :return (path, path_interpolated): Returns a tuple of path and interpolated path. 170 | ''' 171 | 172 | best_path = [] 173 | best_cost = 0.0 174 | best_path_interpolated = [] 175 | best_pi = [] 176 | for _ in range(100): 177 | path, path_interpolated = get_path(start, goal) 178 | 179 | # Define initial parameters: 180 | P = np.eye(2)*1e-1 181 | R = np.block([ 182 | [P, np.zeros((2,2))], 183 | [np.zeros((2,2)), np.zeros((2,2))] 184 | ]) 185 | 186 | L_list = point.get_lqr(path_interpolated, C, D) 187 | 188 | V_list = [P] 189 | for p_i,L in zip(path_interpolated[:-1], reversed(L_list)): 190 | # Kalman-updates - forward 191 | P_bar = A@P@A.T + M 192 | K = P_bar@np.linalg.inv(P_bar+N) 193 | P = (np.eye(2)-K)@P_bar 194 | 195 | # Distribution 196 | F = np.block([ 197 | [A, B@L], 198 | [K@A, A+B@L-K@A] 199 | ]) 200 | G = np.block([ 201 | [np.eye(2), np.zeros((2,2))], 202 | [K, K] 203 | ]) 204 | 205 | R = F@R@F.T + G@Q@G.T 206 | lam = np.block([ 207 | [np.eye(2), np.zeros((2,2))], 208 | [np.zeros((2,2)), L] 209 | ]) 210 | V = lam@R@lam.T 211 | V_list.append(V) 212 | 213 | # Calculate cost: 214 | cost = 1 215 | p_list = [] 216 | for i, p_i in enumerate(path_interpolated): 217 | p.resetBasePositionAndOrientation(robot, np.r_[p_i, 0.1], (0.0, 0.0, 0.0, 0.1)) 218 | d = point.get_distance(obstacles, robot) 219 | c_i = d/np.sqrt(V_list[i][0,0]) 220 | p_i = S((c_i**2)/2) 221 | cost *=S((c_i**2)/2) 222 | p_list.append(p_i) 223 | 224 | # Doing a filter of 225 | if cost>best_cost :#and all(p_i>(1-thresh) for p_i in p_list): 226 | best_path = path 227 | best_path_interpolated = path_interpolated 228 | best_cost = cost 229 | best_pi = p_list 230 | 231 | return best_path, best_path_interpolated, best_pi 232 | 233 | import os 234 | import pickle 235 | 236 | def start_experiment(start, samples): 237 | ''' 238 | Run the LQG experiment for the start and goal points for the same RRT-paths 239 | ''' 240 | # Define path 241 | folder_loc = '/root/data/point/' 242 | exp = 3 243 | for i in range(start, start+samples): 244 | file_i = os.path.join(folder_loc, 'ccgp-mp-star', 'exp6', 'path_{}.p'.format(i)) 245 | data = pickle.load(open(file_i, 'rb')) 246 | start_array = data['path'][0] 247 | goal_array = data['path'][-1] 248 | start = ob.State(space) 249 | start[0] = start_array[0] 250 | start[1] = start_array[1] 251 | goal = ob.State(space) 252 | goal[0] = goal_array[0] 253 | goal[1] = goal_array[1] 254 | 255 | path_param = {} 256 | path, path_interpolated, p_list = lqg_mp(start, goal) 257 | path_param['path'] = path 258 | path_param['path_interpolated'] = path_interpolated 259 | path_param['p_list'] = p_list 260 | 261 | file_i_lqg = os.path.join(folder_loc, 'lqg_mp', 'exp{}'.format(exp), 'path_{}.p'.format(i)) 262 | pickle.dump(path_param, open(file_i_lqg, 'wb')) 263 | 264 | 265 | import sys 266 | if __name__ == "__main__": 267 | start, samples = int(sys.argv[1]), int(sys.argv[2]) 268 | 269 | start_experiment(start, samples) 270 | 271 | if False: 272 | ax.scatter(start[0], start[1], color='r', marker='x') 273 | ax.scatter(goal[0], goal[1], color='g', marker='o') 274 | 275 | if path!=[]: 276 | ax.plot(path[:,0], path[:,1], color='b', alpha=0.5, label='RRT path') 277 | ax.scatter(path[:,0], path[:,1], color='b', label='RRT path') 278 | 279 | # Evaluate 10 paths 280 | successfull = 0 281 | si_check = ob.SpaceInformation(space) 282 | ValidityChecker_obj = ValidityCheckerDistance(si_check) 283 | si_check.setStateValidityChecker(ValidityChecker_obj) 284 | for _ in range(10): 285 | path_est, path_noisy, done = point.execute_path(path_interpolated, C, D, si_check) 286 | # if not done: 287 | path_est = np.array(path_est) 288 | # ax.plot(path_est[:,0], path_est[:,1], color='r', label='estimated state') 289 | path_noisy = np.array(path_noisy) 290 | ax.plot(path_noisy[:,0], path_noisy[:,1], '--',color='g', alpha=0.5, label='real state') 291 | # else: 292 | # successfull+=1 293 | print("Total succesful paths {}".format(successfull)) -------------------------------------------------------------------------------- /visualize_path.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | # Code from - https://github.com/StanfordVL/GibsonEnv/blob/30b7e7c1f352ffadc67f60abdf3bdcb2fe52f7d7/gibson/data/visualize_path.py 4 | import bpy 5 | import argparse 6 | import numpy as np 7 | import json 8 | from mathutils import Matrix, Vector, Euler 9 | import json 10 | import random 11 | 12 | from os import path as osp 13 | 14 | def import_obj(file_loc): 15 | imported_object = bpy.ops.import_scene.obj(filepath=file_loc) 16 | obj_object = bpy.context.selected_objects[0] ####<--Fix 17 | print('Imported name: ', obj_object.name) 18 | model = bpy.context.object 19 | return model 20 | 21 | 22 | def line_distance(pt1, pt2): 23 | return np.linalg.norm(pt1 - pt2) 24 | 25 | def visualize(path): 26 | d1 = makeMaterial('Orange',(1, 0.25, 0, 1),(1,1,1),1) 27 | d2 = makeMaterial('Green',(0, 1, 0, 1),(1,1,1),1) 28 | d3 = makeMaterial('Blue',(0, 0, 1, 1),(1,1,1),1) 29 | 30 | start = path[0] 31 | end = path[-1] 32 | 33 | start_loc = [start[0], start[1], start[2]] 34 | end_loc = [end[0], end[1], end[2]] 35 | end_obj = bpy.ops.mesh.primitive_uv_sphere_add(location=end_loc, radius=0.2) 36 | setMaterial("Sphere", d2) 37 | start_obj = bpy.ops.mesh.primitive_uv_sphere_add(location=start_loc, radius=0.1) 38 | '''setMaterial("Sphere", d2) 39 | setMaterial("Sphere", d3)''' 40 | # for loc in path[1:-1]: 41 | # y_up_loc = [loc[0], loc[1], loc[2]] 42 | # bpy.ops.mesh.primitive_uv_sphere_add(location=y_up_loc, radius=0.05) 43 | setMaterial("Sphere", d1) 44 | 45 | def visualize_traj(traj): 46 | ''' 47 | Plots the entire trajectory 48 | :param traj: The trajectory to plot 49 | ''' 50 | d3 = makeMaterial('Blue',(0,0,1, 1),(1,1,1),1) 51 | 52 | for loc in traj[1:-1]: 53 | y_up_loc = [loc[0], loc[1], loc[2]] 54 | bpy.ops.mesh.primitive_uv_sphere_add(location=y_up_loc, radius=0.05) 55 | setMaterial("Sphere", d3) 56 | 57 | 58 | def visualize_curve(traj, color): 59 | ''' 60 | Visualize the trajectory using path curve 61 | :param traj: The trajectory to plot 62 | :parma color: The color of the curve 63 | ''' 64 | crv = bpy.data.curves.new('crv', 'CURVE') 65 | crv.dimensions = '3D' 66 | crv.bevel_depth=0.01 67 | 68 | spline = crv.splines.new(type='NURBS') 69 | spline.points.add(len(traj)-1) 70 | 71 | for p, new_co in zip(spline.points, traj): 72 | p.co = (new_co + [1.0]) 73 | 74 | # Setting the color 75 | material = bpy.data.materials.new('crv_material') 76 | material.diffuse_color = color 77 | crv.materials.append(material) 78 | 79 | obj = bpy.data.objects.new('trajectory', crv) 80 | bpy.context.scene.collection.objects.link(obj) 81 | 82 | 83 | def makeMaterial(name, diffuse, specular, alpha): 84 | mat = bpy.data.materials.new(name) 85 | mat.diffuse_color = diffuse 86 | # mat.diffuse_shader = 'LAMBERT' 87 | # mat.diffuse_intensity = 1.0 88 | # mat.specular_color = specular 89 | # mat.specular_shader = 'COOKTORR' 90 | # mat.specular_intensity = 0.5 91 | # mat.alpha = alpha 92 | # mat.ambient = 1 93 | return mat 94 | 95 | def setMaterial(name, mat): 96 | for ob in bpy.data.objects: 97 | if name in ob.name: 98 | me = ob.data 99 | me.materials.append(mat) 100 | ob.name = "PathPoint" 101 | 102 | 103 | def duplicateObject(scene, name, copyobj): 104 | # Create new mesh 105 | mesh = bpy.data.meshes.new(name) 106 | # Create new object associated with the mesh 107 | ob_new = bpy.data.objects.new(name, mesh) 108 | # Copy data block from the old object into the new object 109 | ob_new.data = copyobj.data.copy() 110 | ob_new.scale = copyobj.scale 111 | ob_new.location = copyobj.location 112 | # Link new object to the given scene and select it 113 | scene.collection.objects.link(ob_new) 114 | ob_new.select_set(True) 115 | ob_new.location = Vector((0, 0, 0)) 116 | return ob_new 117 | 118 | def moveFromCenter(obj, dx=2000, dy=2000, dz=2000): 119 | obj.location = Vector((dx, dy, dz)) 120 | 121 | 122 | def prepare(): 123 | bpy.ops.object.select_all(action='DESELECT') 124 | if 'Cube' in bpy.data.objects.keys(): 125 | mesh = bpy.data.meshes["Cube"] 126 | bpy.data.meshes.remove(mesh) 127 | bpy.ops.object.delete() 128 | # lamp = bpy.data.lights.new(name='Lamp', type='SUN') 129 | lamp = bpy.data.lights['Light'] 130 | lamp.energy = 1.0 # 10 is the max value for energy 131 | lamp.type = 'SUN' # in ['POINT', 'SUN', 'SPOT', 'HEMI', 'AREA'] 132 | lamp.distance = 100 133 | 134 | def install_lamp(obj_lamp, loc_lamp, loc_target): 135 | direction = loc_target - loc_lamp 136 | rot_quat = direction.to_track_quat('-Z', 'Y') 137 | mat_loc = Matrix.Translation(loc_lamp) 138 | mat_rot = rot_quat.to_matrix().to_4x4() 139 | mat_comb = mat_loc * mat_rot 140 | obj_lamp.matrix_world = mat_comb 141 | 142 | def look_at(obj_camera, loc_camera, loc_target): 143 | '''Set camera to look at loc_target from loc_camera 144 | Camera default y is up 145 | ''' 146 | direction = loc_target - loc_camera 147 | rot_quat = direction.to_track_quat('-Z', 'Y') 148 | mat_loc = Matrix.Translation(loc_camera) 149 | mat_rot = rot_quat.to_matrix().to_4x4() 150 | mat_comb = mat_loc @ mat_rot 151 | obj_camera.matrix_world = mat_comb 152 | 153 | 154 | def get_model_camera_vals(filepath): 155 | all_x, all_y, all_z = [], [], [] 156 | with open(filepath, "r") as f: 157 | for line in f: 158 | vals = line.split(",") 159 | all_x.append(float(vals[1])) 160 | all_y.append(float(vals[2])) 161 | all_z.append(float(vals[3])) 162 | max_x, min_x = (max(all_x), min(all_x)) 163 | max_y, min_y = (max(all_y), min(all_y)) 164 | max_z, min_z = (max(all_z), min(all_z)) 165 | center = Vector(((max_x + min_x)/2, (max_y + min_y)/2, (max_z + min_z)/2)) 166 | return (max_x, min_x), (max_y, min_y), (max_z, min_z), center 167 | 168 | def join_objects(): 169 | scene = bpy.context.scene 170 | obs = [] 171 | for ob in scene.objects: 172 | if ob.type == 'MESH': 173 | obs.append(ob) 174 | ctx = bpy.context.copy() 175 | ctx['active_object'] = obs[0] 176 | ctx['selected_objects'] = obs 177 | ctx['selected_editable_bases'] = obs #[scene.object_bases[ob.name] for ob in obs] 178 | bpy.ops.object.join(ctx) 179 | 180 | 181 | def deleteObject(obj): 182 | '''for ob in bpy.data.objects: 183 | print(ob) 184 | ob.select = False''' 185 | bpy.ops.object.mode_set(mode='OBJECT') 186 | bpy.ops.object.select_all(action='DESELECT') 187 | if type(obj) == str: 188 | bpy.data.objects[obj].select_set(True) 189 | else: 190 | obj.select_set(True) 191 | for name in bpy.data.objects.keys(): 192 | if "PathPoint" in name: 193 | bpy.data.objects[name].select_set(True) 194 | bpy.ops.object.delete() 195 | 196 | def deleteSpheres(): 197 | bpy.ops.object.mode_set(mode='OBJECT') 198 | bpy.ops.object.select_all(action='DESELECT') 199 | for name in bpy.data.objects.keys(): 200 | if "Sphere" in name: 201 | bpy.data.objects[name].select_set(True) 202 | bpy.ops.object.delete() 203 | 204 | 205 | def deleteCube(): 206 | for name, obj in bpy.data.objects.items(): 207 | if "Cube" in name: 208 | bpy.data.objects.remove(obj, True) 209 | 210 | 211 | def capture_top(dst_dir, model_id, obj_model, focus_center, path, distance, exp): 212 | def set_render_resolution(x=2560, y=2560): 213 | bpy.context.scene.render.resolution_x = x 214 | bpy.context.scene.render.resolution_y = y 215 | set_render_resolution() 216 | camera_pos = focus_center + Vector((0, 0, distance)) 217 | lamp_pos = camera_pos 218 | obj_camera = bpy.data.objects["Camera"] 219 | obj_camera.location = camera_pos 220 | obj_lamp = bpy.data.objects["Light"] 221 | obj_lamp.location = camera_pos 222 | look_at(obj_camera, camera_pos, focus_center) 223 | install_lamp(obj_lamp, lamp_pos, focus_center) 224 | slicename="slice" 225 | cut_height = np.mean([loc[2] for loc in path]) 226 | cobj=duplicateObject(bpy.context.scene, slicename, obj_model) 227 | bpy.ops.object.select_all(action='DESELECT') 228 | bpy.context.view_layer.objects.active = bpy.data.objects[slicename] 229 | bpy.ops.object.mode_set(mode='EDIT') 230 | bpy.ops.mesh.select_all(action='SELECT') 231 | bpy.ops.mesh.bisect(plane_co=(0, 0, cut_height + 0.7),plane_no=(0,0,1), clear_outer=True,clear_inner=False) 232 | bpy.ops.object.mode_set(mode='OBJECT') 233 | bpy.ops.object.select_all(action='DESELECT') 234 | bpy.data.scenes['Scene'].render.filepath = osp.join(dst_dir, 'compare_path.png') 235 | bpy.ops.render.render( write_still=True, scene='Scene') 236 | # deleteObject(slicename) 237 | 238 | # def parse_local_args( args ): 239 | # local_args = args[ args.index( '--' ) + 1: ] 240 | # return parser.parse_known_args( local_args ) 241 | 242 | # parser = argparse.ArgumentParser() 243 | # parser.add_argument('--filepath', required=True, help='trajectory file path', type=str) 244 | # parser.add_argument('--datapath', required=True, help='gibson dataset path', type=str) 245 | # parser.add_argument('--renderpath', help='visualization output path', default=None, type=str) 246 | # parser.add_argument('--model', required=True, type=str) 247 | # parser.add_argument('--idx' , default=0, type=int) 248 | 249 | import pickle 250 | def main(): 251 | # global args, logger 252 | # opt, remaining_args = parse_local_args( sys.argv ) 253 | 254 | trajectories = {} 255 | # json_path = os.path.join(opt.filepath, "{}.json".format(opt.model)) 256 | # with open(json_path, "r") as f: 257 | # trajectories = json.load(f) 258 | exp = 'rrt_star' 259 | path_param = pickle.load(open('/home/jacoblab/prob_planning_data/gibson_path_{}.p'.format(exp), 'rb')) 260 | waypoints = [[p[0], p[1], 0.5] for p in path_param['path']] 261 | traj_rrt = [[p[0], p[1], 0.5] for p in path_param['path_interpolated']] 262 | exp = 'ccgp' 263 | path_param = pickle.load(open('/home/jacoblab/prob_planning_data/gibson_path_{}_5.p'.format(exp), 'rb')) 264 | traj_ccgp = [[p[0], p[1], 0.5] for p in path_param['path_interpolated']] 265 | 266 | 267 | prepare() 268 | datapath = '/home/jacoblab/prob_planning/assets' 269 | model = 'Allensville' 270 | import_obj(osp.join(datapath, model, "mesh_z_up.obj")) 271 | camera_pose = osp.join(datapath, model, "camera_poses.csv") 272 | 273 | join_objects() 274 | obj_model, cobj = bpy.data.objects[2], None 275 | moveFromCenter(obj_model) 276 | (max_x, min_x), (max_y, min_y), (max_z, min_z), _ = get_model_camera_vals(camera_pose) 277 | dist = max(((max_x - min_x), (max_y - min_y), (max_z - min_z))) / (2*np.tan(np.pi/10)) 278 | cent = Vector(((max_x + min_x)/2, (max_y + min_y)/2, (max_z + min_z)/2)) 279 | 280 | renderpath = datapath #if opt.renderpath is None else opt.renderpath 281 | visualize(waypoints) 282 | visualize_curve(traj_rrt, (1, 0, 0, 1)) 283 | visualize_curve(traj_ccgp, (0, 0, 1, 1)) 284 | 285 | capture_top(renderpath, model, obj_model, cent, waypoints, dist, exp) 286 | 287 | if __name__ == '__main__': 288 | main() -------------------------------------------------------------------------------- /rrt_motion_density.py: -------------------------------------------------------------------------------- 1 | ''' Script to generate random environments with different density and path RRT 2 | paths 3 | ''' 4 | 5 | import numpy as np 6 | import pickle 7 | import json 8 | from os import path as osp 9 | import os 10 | from scipy import stats, optimize 11 | import time 12 | import networkx as nx 13 | import matplotlib.pyplot as plt 14 | try: 15 | from ompl import base as ob 16 | from ompl import geometric as og 17 | except ImportError: 18 | print("Could not find OMPL") 19 | raise ImportError("Run inside docker!!") 20 | 21 | # Import project specific files 22 | from models import point 23 | from models.randomWorld import plot_env 24 | import json 25 | import pybullet as pyb 26 | import argparse 27 | 28 | parser = argparse.ArgumentParser() 29 | parser.add_argument('--envNum', required=True, type=int) 30 | parser.add_argument('--numObstacles', required=True, type=int) 31 | parser.add_argument('--seed', required=True, type=int) 32 | 33 | args = parser.parse_args() 34 | 35 | dataFolder = f'/root/data/density/env{args.envNum}' 36 | 37 | if not osp.isdir(dataFolder): 38 | os.mkdir(dataFolder) 39 | 40 | def plan_again(count, planPath=True, evaluatePath=False): 41 | paramFile = osp.join(dataFolder, 'envParam.json') 42 | if osp.exists(paramFile): 43 | envParam = json.load(open(paramFile, 'r')) 44 | else: 45 | envParam = dict( 46 | seed=args.seed+count, 47 | num_boxes=args.numObstacles, 48 | num_circles=args.numObstacles 49 | ) 50 | 51 | json.dump(envParam, open(paramFile, 'w')) 52 | 53 | # Set up environment details 54 | thresh = 0.01 55 | N = stats.norm(scale=np.sqrt(1/2)) 56 | c = N.ppf(1-thresh) 57 | 58 | # Define environment 59 | obstacles, robot = point.set_env( 60 | **envParam 61 | ) 62 | # Define the space 63 | space = ob.RealVectorStateSpace(2) 64 | 65 | # Set the bounds 66 | bounds = ob.RealVectorBounds(2) 67 | bounds.setLow(-2.) 68 | bounds.setHigh(12.0) 69 | space.setBounds(bounds) 70 | 71 | if planPath: 72 | # Get GP_model 73 | m = point.get_model_KF(robot, obstacles, samples=2500, dataFolder=dataFolder) 74 | # m = point.get_model_KF_sparse(robot, obstacles, samples=2500, dataFolder=dataFolder) 75 | # TODO: Initialize model with GP_model 76 | def get_GP_G(start, goal): 77 | ''' 78 | The guassian function from x_start and x_goal.Return the function for line 79 | :param start: A np.array representing the start position 80 | :param goal: A np.array representing the goal position 81 | :return function: Return the function of F along the line 82 | ''' 83 | def G(alpha, *args): 84 | ''' 85 | The function G 86 | ''' 87 | if alpha<0: 88 | alpha=0 89 | elif alpha>1: 90 | alpha = 1 91 | x_hat = (1-alpha)*start + alpha*goal 92 | mean, var = m.predict(x_hat) 93 | return (mean/np.sqrt(2*var)) 94 | return G 95 | 96 | class ValidityChecker(ob.StateValidityChecker): 97 | '''A class to check the validity of the state 98 | ''' 99 | def isValid(self, state): 100 | ''' 101 | Check if the given state is valid. 102 | :param state: An ob.State object to be checked. 103 | :returns bool: True if the state is valid. 104 | ''' 105 | x_hat = np.c_[state[0], state[1]] 106 | return self.getZscore(x_hat)>c 107 | 108 | def getZscore(self, x): 109 | ''' 110 | The ratio of E[f(x)]/sqrt(2*var(f(x))). 111 | :param x: a numpy array of state x. 112 | :return float: The value of E[f(x)]/sqrt(2*var(f(x))) 113 | ''' 114 | mean, var = m.predict(x) 115 | return (mean/np.sqrt(2*var))[0,0] 116 | 117 | class check_motion(ob.MotionValidator): 118 | '''A motion validator check using chance constrained. 119 | ''' 120 | def __init__(self, spaceInformation): 121 | ''' 122 | Initialize the motion validator. 123 | :param spaceInformation: A ob.SpaceInformation object. 124 | ''' 125 | super(check_motion, self).__init__(spaceInformation) 126 | 127 | def checkMotion(self, start, goal): 128 | ''' 129 | Check if there exists a valid motion between start and state2, that 130 | satisfies the given constraints. 131 | :param start: an object of type og.State ,representing the start state 132 | :param goal: an object of type og.State , representing the goal state. 133 | :returns bool: True if there exists a path between start and goal. 134 | ''' 135 | # assert isinstance(start, ob.State), "Start has to be of ob.State" 136 | # assert isinstance(goal, ob.State), "Goal has t obe of ob.State" 137 | G = get_GP_G(np.c_[start[0], start[1]], np.c_[goal[0], goal[1]]) 138 | sol = optimize.shgo(G, bounds=[(0, 1)], iters=10, n=10, sampling_method='sobol') 139 | if sol.success and sol.fun>c: 140 | return True 141 | return False 142 | 143 | # Define the SpaceInformation object. 144 | si = ob.SpaceInformation(space) 145 | print("Using GP model for collision check") 146 | # Set the StateValidator 147 | ValidityChecker_obj = ValidityChecker(si) 148 | 149 | # Set the MotionValidator 150 | MotionValidator_obj = check_motion(si) 151 | si.setMotionValidator(MotionValidator_obj) 152 | 153 | si.setStateValidityChecker(ValidityChecker_obj) 154 | for pathNum in range(25, 30): 155 | # Define start and goal 156 | posN = stats.norm(scale=0.5) 157 | startMean = np.r_[-0.25, 10.0] 158 | startSample = startMean + posN.rvs() 159 | start = ob.State(space) 160 | start[0] = startSample[0] 161 | start[1] = startSample[1] 162 | mean, var = m.predict(np.c_[start[0], start[1]]) 163 | c_start = (mean/np.sqrt(2*var))[0,0] 164 | print(f"Start Zscore :{c_start, c}") 165 | goalMean = np.r_[11.0, 0.0] 166 | goalSample = goalMean + posN.rvs() 167 | goal = ob.State(space) 168 | goal[0] = goalSample[0] 169 | goal[1] = goalSample[1] 170 | mean, var = m.predict(np.c_[goal[0], goal[1]]) 171 | c_goal = (mean/np.sqrt(2*var))[0,0] 172 | print(f"Goal Zscore :{c_goal, c}") 173 | # Plan Path 174 | success = False 175 | # Create a simple setup 176 | ss = og.SimpleSetup(si) 177 | 178 | # Set the start and goal states: 179 | ss.setStartAndGoalStates(start, goal, 0.1) 180 | 181 | # Use RRT 182 | planner = og.RRT(si) 183 | # planner = og.LazyRRT(si) 184 | # planner.setRange(0.5) 185 | 186 | ss.setPlanner(planner) 187 | 188 | # fig, ax = plt.subplots() 189 | # ax.scatter(start[0], start[1], color='g') 190 | # ax.scatter(goal[0], goal[1], color='r') 191 | def plot_graph(ax, pd): 192 | ''' 193 | Plot planner data on the graph. 194 | :param ax: The axis on which to plot the graph. 195 | :param pd: A ompl.base.PlannerData object 196 | ''' 197 | G = nx.parse_graphml(pd.printGraphML()) 198 | pos = nx.get_node_attributes(G, 'coords') 199 | new_pos = {key:np.array(value.split(','), dtype=np.float) for key, value in pos.items()} 200 | nx.draw_networkx(G, new_pos, ax = ax, alpha=0.5) 201 | plot_env(ax, alpha=1) 202 | 203 | # Attempt to solve within the given time 204 | planTime = 60 205 | startTime = time.time() 206 | solved = ss.solve(planTime) 207 | # planner_data = ob.PlannerData(si) 208 | # ss.getPlannerData(planner_data) 209 | # plot_graph(ax, planner_data) 210 | # plt.show(block=False) 211 | # plt.pause(5) 212 | 213 | while not ss.haveExactSolutionPath(): 214 | solved = ss.solve(30.0) 215 | # ss.getPlannerData(planner_data) 216 | # plot_graph(ax, planner_data) 217 | # plt.show(block=False) 218 | # plt.pause(1) 219 | planTime +=30 220 | if planTime>600: 221 | break 222 | totalTime = time.time()-startTime 223 | # plt.show() 224 | # Visualize search tree 225 | 226 | if ss.haveExactSolutionPath(): 227 | ss.simplifySolution() 228 | success = True 229 | print("Found solution") 230 | path = [ 231 | [ss.getSolutionPath().getState(i)[0], ss.getSolutionPath().getState(i)[1]] 232 | for i in range(ss.getSolutionPath().getStateCount()) 233 | ] 234 | # Define path 235 | ss.getSolutionPath().interpolate(100) 236 | path_obj = ss.getSolutionPath() 237 | path_interpolated = np.array([ 238 | [path_obj.getState(i)[0], path_obj.getState(i)[1]] 239 | for i in range(path_obj.getStateCount()) 240 | ]) 241 | else: 242 | path = [[start[0], start[1]], [goal[0], goal[1]]] 243 | path_interpolated = [] 244 | 245 | # Save path parameters 246 | pathParam = dict( 247 | path=path, 248 | path_interpolated=path_interpolated, 249 | success=success, 250 | totalTime=totalTime 251 | ) 252 | pickle.dump(pathParam, open(osp.join(dataFolder, f'path_{pathNum}.p'), 'wb')) 253 | 254 | if evaluatePath: 255 | # LQG parameter: 256 | C = np.eye(2)*10 257 | D = np.eye(2)*0.1 258 | 259 | class ValidityCheckerDistance(ob.StateValidityChecker): 260 | '''A class to check the validity of the state, by checking distance function 261 | ''' 262 | defaultOrientation = pyb.getQuaternionFromEuler([0., 0., 0.]) 263 | 264 | def isValid(self, state): 265 | ''' 266 | Check if the given state is valid. 267 | :param state: An ob.STate object to be checked. 268 | :return bool: True if the state is valid. 269 | ''' 270 | return self.getDistance(state)>0 271 | 272 | def getDistance(self, state): 273 | ''' 274 | Get the shortest distance from robot to obstacle. 275 | :param x: A numpy array of state x. 276 | :returns float: The closest distance between the robot and obstacle. 277 | ''' 278 | pyb.resetBasePositionAndOrientation(robot, np.r_[state[0], state[1], 0.1], self.defaultOrientation) 279 | return point.get_distance(obstacles, robot) 280 | 281 | 282 | si_check = ob.SpaceInformation(space) 283 | ValidityChecker_dis_obj = ValidityCheckerDistance(si_check) 284 | si_check.setStateValidityChecker(ValidityChecker_dis_obj) 285 | 286 | for pathNum in range(0, 30): 287 | path_param = pickle.load(open(osp.join(dataFolder, f'path_{pathNum}.p'), 'rb')) 288 | accuracy = 0 289 | if path_param['success']: 290 | ompl_traj = og.PathGeometric(si_check) 291 | state_temp = ob.State(si_check.getStateSpace()) 292 | for path_i in path_param['path']: 293 | state_temp[0], state_temp[1] = path_i[0], path_i[1] 294 | ompl_traj.append(state_temp()) 295 | ompl_traj.interpolate(1000) 296 | traj = np.array([[ompl_traj.getState(i)[0], ompl_traj.getState(i)[1]] for i in range(ompl_traj.getStateCount())]) 297 | for _ in range(100): 298 | _, _, done = point.execute_path(traj, C, D, si_check) 299 | if done: 300 | accuracy += 1 301 | path_param['accuracy'] = accuracy 302 | pickle.dump(path_param, open(osp.join(dataFolder, f'path_{pathNum}.p'), 'wb')) 303 | 304 | 305 | plan_again(0, planPath=False, evaluatePath=True) 306 | # plan_again(0) 307 | 308 | # TODO: Save Evaluation results -------------------------------------------------------------------------------- /models/point.py: -------------------------------------------------------------------------------- 1 | '''Define a LTI point system 2 | ''' 3 | from os import path as osp 4 | import pybullet_utils.bullet_client as bc 5 | import pybullet as pyb 6 | import pybullet_data 7 | import numpy as np 8 | 9 | import GPy 10 | 11 | from models.randomWorld import set_obstacles, RandomWorld 12 | 13 | try: 14 | from ompl import base as ob 15 | from ompl import geometric as og 16 | except ImportError: 17 | print("Could not find OMPL") 18 | print("execute_path() will not work!!") 19 | 20 | from scipy import stats 21 | 22 | from config import box_length, box_width, cir_radius 23 | 24 | p = bc.BulletClient(connection_mode=pyb.DIRECT) 25 | # p = bc.BulletClient(connection_mode=pyb.GUI) 26 | 27 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally 28 | p.setGravity(0,0,-9.8) 29 | planeId = p.loadURDF("plane.urdf") 30 | 31 | p.resetDebugVisualizerCamera( 32 | cameraDistance=10, 33 | cameraYaw=0, 34 | cameraPitch=-89.9, 35 | cameraTargetPosition=[5,5,0]) 36 | 37 | geomRobot = p.createCollisionShape(p.GEOM_CYLINDER, radius=0.1, height=0.2) 38 | 39 | def set_env(**kwargs): 40 | ''' 41 | Set the environment up with the obstacles and robot. 42 | :param kwargs: Environment parameters 43 | :return tuple: The pybullet ID of obstacles and robot 44 | ''' 45 | obstacles = set_obstacles(p, **kwargs) 46 | robot = p.createMultiBody( 47 | baseMass=0, 48 | baseCollisionShapeIndex=geomRobot, 49 | basePosition=[0.0, 0.0, 0.1]) 50 | return obstacles, robot 51 | 52 | 53 | def get_observations(robot): 54 | ''' 55 | Returns the sensor observations, similar to lidar data. 56 | :param robot: The robot ID 57 | ''' 58 | num_rays = 8 59 | ray_length = 1 60 | robot_position, _ = p.getBasePositionAndOrientation(robot) 61 | ray_from = [robot_position]*num_rays 62 | ray_to = [ 63 | (robot_position[0]+ray_length*np.cos(theta), robot_position[1]+ray_length*np.sin(theta), 0.1) 64 | for theta in np.linspace(0,2*np.pi, num_rays) 65 | ] 66 | results = p.rayTestBatch(ray_from, ray_to) 67 | # return the distance to obstacle for each ray 68 | return [result[2]*ray_length for result in results] 69 | 70 | 71 | def get_distance(obstacles, robot): 72 | ''' 73 | Get the shortest distance between the obstacles and robot, for the given 74 | setup. 75 | :param obs: A list of all obstacle ID's 76 | :param robot: The robot ID 77 | :return float: The penetration distance between the robot and obstacles, If the 78 | distance is negative, object is in collision. 79 | ''' 80 | assert isinstance(obstacles, list), "Obstacles has to be a list" 81 | distance = min( 82 | ( 83 | p.getClosestPoints(bodyA=obs, bodyB=robot, distance=100)[0][8] 84 | for obs in obstacles 85 | ) 86 | ) 87 | return distance 88 | 89 | # Define Robot model 90 | A = np.eye(2) 91 | B = np.eye(2) 92 | 93 | M = np.eye(2)*1e-1 # Motion Noise 94 | N = np.eye(2)*1e-2 # Observation Noise 95 | def get_dyn(): 96 | ''' 97 | Return the dynamics of the LTI system represented by: 98 | x_t+1 = A@x_t + B@u_t + m_t, m_t ~ N(0, M) 99 | z_t+1 = x_t + n_t, n_t ~ N(0,N) 100 | :returns (A,B,M,N): where the parameters correspond to the above equation. 101 | ''' 102 | return A, B, M, N 103 | 104 | # KF state-estimator 105 | def x_hat( x_est, control, obs, P): 106 | ''' 107 | :param x_est: The previous state estimate 108 | :param control: The current control 109 | :param obs: The current observatoin 110 | :returns tuple: A tuple of the current state estimate and Covariance matrix 111 | ''' 112 | x_bar = A@x_est + B@control 113 | P_bar = A@P@A.T + M 114 | K = P_bar@np.linalg.inv(P_bar+N) 115 | x_est = x_bar + K@(obs-x_bar) 116 | P = (np.eye(2)-K)@P_bar 117 | return x_est, P 118 | 119 | 120 | def get_lqr(traj, C, D): 121 | ''' 122 | Returns the LQR gains for a finite horizon system. 123 | :param traj: The trajectory to be followed. 124 | :param C : The cost for state. 125 | :param D : The cost for control. 126 | :return list: A list of LQR gains 127 | ''' 128 | # Get LQR gains 129 | SN = C 130 | L_list = [] 131 | # Evaluate L from the end 132 | for _ in traj[:-1]: 133 | L = -np.linalg.inv(B.T@SN@B+D)@B.T@SN@A 134 | L_list.append(L) 135 | SN = C + A.T@SN@A + A.T@SN@B@L 136 | return L_list 137 | 138 | # Using infinite horizon lqr gains 139 | def solve_DARE(A, B, Q, R): 140 | """ 141 | solve a discrete time_Algebraic Riccati equation (DARE) 142 | """ 143 | X = Q 144 | maxiter = 150 145 | eps = 0.01 146 | 147 | for i in range(maxiter): 148 | Xn = A.T @ X @ A - A.T @ X @ B @ \ 149 | np.linalg.inv(R + B.T @ X @ B) @ B.T @ X @ A + Q 150 | if (abs(Xn - X)).max() < eps: 151 | break 152 | X = Xn 153 | 154 | return Xn 155 | 156 | 157 | def dlqr(A, B, Q, R): 158 | """Solve the discrete time lqr controller. 159 | x[k+1] = A x[k] + B u[k] 160 | cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] 161 | # ref Bertsekas, p.151 162 | """ 163 | 164 | # first, try to solve the ricatti equation 165 | X = solve_DARE(A, B, Q, R) 166 | 167 | # compute the LQR gain 168 | K = np.linalg.inv(B.T @ X @ B + R) @ (B.T @ X @ A) 169 | 170 | eigVals, eigVecs = np.linalg.eig(A - B @ K) 171 | 172 | return K, X, eigVals 173 | 174 | 175 | def calc_nearest_index(state, traj): 176 | ''' 177 | Calculate the closest index of trajectory path. 178 | ''' 179 | d = np.linalg.norm(state[:2] - traj[:, :2], axis=1) 180 | ind = np.argmin(d) 181 | 182 | return ind 183 | 184 | 185 | # Noise model 186 | N_n = stats.multivariate_normal(cov=N) 187 | M_n = stats.multivariate_normal(cov=M) 188 | def execute_path(traj, C, D, si_check): 189 | ''' 190 | Execute the path trajectory using LQR controller. Assumes error of P. 191 | :param traj: The path to follow. 192 | :param C, D: The LQR parameters 193 | :param si_check: An ompl.base.SpaceInformation with a valid collision checker. 194 | :returns (estimated state, real state, valid): A tuple containing the estimated state 195 | and real state of the robot, and whether the path was completed or not. 196 | ''' 197 | K, _ , eigvals = dlqr(A, B, C, D) 198 | 199 | # Define the parameters for the path. 200 | ompl_path_obj = og.PathGeometric(si_check) 201 | state_temp = ob.State(si_check.getStateSpace()) 202 | start = traj[0] 203 | goal = traj[-1] 204 | 205 | P = np.eye(2)*0 206 | # L_list = get_lqr(traj, C, D) 207 | x = np.r_[start[0], start[1]] #+ stats.multivariate_normal(cov = P).rvs() 208 | x_real = x 209 | state_temp[0], state_temp[1] = x[0], x[1] 210 | ompl_path_obj.append(state_temp()) 211 | path_est = [x] 212 | path_noisy = [x] 213 | done = False 214 | count = 0 215 | ind = 0 216 | # for i,T in enumerate(zip(traj[:-1], reversed(L_list))): 217 | while not done and count<1500: 218 | count += 1 219 | # x_t, L = T 220 | temp_ind = calc_nearest_index(x, traj) 221 | if temp_ind>ind: 222 | ind = temp_ind 223 | x_t = traj[ind] 224 | 225 | c = K@(x_t-x) 226 | c = np.clip(c, -0.75, 0.75) 227 | 228 | # delta_c = L@(x_real-x_t) 229 | # c = np.linalg.inv(B)@(traj[i+1]-A@x_t) + delta_c 230 | 231 | x_real = A@x_real + B@c + M_n.rvs() 232 | state_temp[0], state_temp[1] = x_real[0], x_real[1] 233 | ompl_path_obj.append(state_temp()) 234 | 235 | z_real = x_real + N_n.rvs() 236 | x, P = x_hat(x, c, z_real, P) 237 | path_est.append(x) 238 | path_noisy.append(x_real) 239 | # Check if goal is reached 240 | if np.linalg.norm(x-np.r_[goal[0], goal[1]])<0.5: 241 | done = True 242 | break 243 | if not ompl_path_obj.check(): 244 | done = False 245 | break 246 | 247 | return path_est, path_noisy, done 248 | 249 | 250 | def execute_traj_ignoreCol(traj): 251 | ''' 252 | execute the trajectory by ignoring collison. 253 | :param traj: 254 | return tuple: A tuple of estimated path and the target trajectory 255 | ''' 256 | 257 | # Define the parameters for the path. 258 | start = traj[0] 259 | goal = traj[-1] 260 | ind = 1 261 | P = np.eye(2)*0 262 | x = np.r_[start[0], start[1]] #+ stats.multivariate_normal(cov = P).rvs() 263 | x_real = x 264 | C = np.eye(2)*10 265 | D = np.eye(2)*1e-1 266 | K, _ , eigvals = dlqr(A, B, C, D) 267 | path_est = [x] 268 | path_target = [x] 269 | 270 | done = False 271 | ind = 0 272 | count = 0 273 | while not done and count<1000: 274 | count +=1 275 | temp_ind = calc_nearest_index(x, traj) 276 | if temp_ind>ind: 277 | ind = temp_ind 278 | x_t = traj[ind] 279 | path_target.append(x_t) 280 | 281 | # Asymptotic Gains 282 | c = K@(x_t-x) 283 | c = np.clip(c, -0.75, 0.75) 284 | 285 | # Take a step 286 | x_real = A@x_real + B@c + M_n.rvs() 287 | z_real = x_real + N_n.rvs() 288 | 289 | x, P = x_hat(x, c, z_real, P) 290 | path_est.append(x) 291 | 292 | # Check if goal is reached 293 | if np.linalg.norm(x-np.r_[goal[0], goal[1]])<0.5: 294 | done = True 295 | break 296 | 297 | return path_est, path_target 298 | 299 | 300 | def get_model_KF(robot, obstacles, samples, dataFolder=None): 301 | ''' 302 | Getting the GP model using the KF. Generating points by executing random trajectories 303 | using controller. 304 | :param robot: pybullet.MultiBody object representing the robot 305 | :param obstacles: pybullet.MultiBody object representing the obstacles in the environment 306 | :param samples: The number of samples to use for the model 307 | :param dataFolder: The folder where the data and modelParams are stored 308 | :returns GPy.models.GPRegression model representing the obstacle space 309 | ''' 310 | if dataFolder is None: 311 | dataFolder = 'param' 312 | seed = RandomWorld.seed 313 | try: 314 | print("Loading data for map") 315 | X = np.load(osp.join(dataFolder, f'X_{seed}.npy')) 316 | Y = np.load(osp.join(dataFolder, f'Y_{seed}.npy')) 317 | except FileNotFoundError: 318 | print("Could not find data, gathering data") 319 | X = np.array([[0.0, 0.0]]) 320 | Y = np.array([0.0]) 321 | robotOrientation = (0.0, 0.0, 0.0, 1.0) 322 | count = 0 323 | while count1: 74 | alpha = 1 75 | x_hat = (1-alpha)*start + alpha*goal 76 | mean, var = m.predict(x_hat) 77 | return (mean/np.sqrt(2*var)) 78 | return G 79 | 80 | class ValidityChecker(ob.StateValidityChecker): 81 | '''A class to check the validity of the state 82 | ''' 83 | def isValid(self, state): 84 | ''' 85 | Check if the given state is valid. 86 | :param state: An ob.State object to be checked. 87 | :returns bool: True if the state is valid. 88 | ''' 89 | x_hat = np.c_[state[0], state[1]] 90 | return self.getZscore(x_hat)>c 91 | 92 | def getZscore(self, x): 93 | ''' 94 | The ratio of E[f(x)]/sqrt(2*var(f(x))). 95 | :param x: a numpy array of state x. 96 | :return float: The value of E[f(x)]/sqrt(2*var(f(x))) 97 | ''' 98 | mean, var = m.predict(x) 99 | return (mean/np.sqrt(2*var))[0,0] 100 | 101 | 102 | class ValidityCheckerDistance(ob.StateValidityChecker): 103 | '''A class to check the validity of the state, by checking distance function 104 | ''' 105 | defaultOrientation = p.getQuaternionFromEuler([0., 0., 0.]) 106 | 107 | def isValid(self, state): 108 | ''' 109 | Check if the given state is valid. 110 | :param state: An ob.STate object to be checked. 111 | :return bool: True if the state is valid. 112 | ''' 113 | return self.getDistance(state)>0 114 | 115 | def getDistance(self, state): 116 | ''' 117 | Get the shortest distance from robot to obstacle. 118 | :param x: A numpy array of state x. 119 | :returns float: The closest distance between the robot and obstacle. 120 | ''' 121 | p.resetBasePositionAndOrientation(robot, np.r_[state[0], state[1], 0.1], self.defaultOrientation) 122 | return point.get_distance(obstacles, robot) 123 | 124 | class check_motion(ob.MotionValidator): 125 | '''A motion validator check using chance constrained. 126 | ''' 127 | def __init__(self, spaceInformation): 128 | ''' 129 | Initialize the motion validator. 130 | :param spaceInformation: A ob.SpaceInformation object. 131 | ''' 132 | super(check_motion, self).__init__(spaceInformation) 133 | 134 | def checkMotion(self, start, goal): 135 | ''' 136 | Check if there exists a valid motion between start and state2, that 137 | satisfies the given constraints. 138 | :param start: an object of type og.State ,representing the start state 139 | :param goal: an object of type og.State , representing the goal state. 140 | :returns bool: True if there exists a path between start and goal. 141 | ''' 142 | # assert isinstance(start, ob.State), "Start has to be of ob.State" 143 | # assert isinstance(goal, ob.State), "Goal has t obe of ob.State" 144 | G = get_GP_G(np.c_[start[0], start[1]], np.c_[goal[0], goal[1]]) 145 | sol = optimize.shgo(G, bounds=[(0, 1)], iters=10) 146 | if sol.success and sol.fun>c: 147 | return True 148 | return False 149 | 150 | 151 | # Define the space 152 | space = ob.RealVectorStateSpace(2) 153 | 154 | # Set the bounds 155 | bounds = ob.RealVectorBounds(2) 156 | bounds.setLow(-0.2) 157 | bounds.setHigh(10) 158 | space.setBounds(bounds) 159 | 160 | # Define the SpaceInformation object. 161 | si = ob.SpaceInformation(space) 162 | 163 | GP_check = True 164 | if GP_check: 165 | print("Using GP model for collision check") 166 | # Set the StateValidator 167 | ValidityChecker_obj = ValidityChecker(si) 168 | 169 | # Set the MotionValidator 170 | MotionValidator_obj = check_motion(si) 171 | si.setMotionValidator(MotionValidator_obj) 172 | else: 173 | print("Using noisy distance function") 174 | ValidityChecker_obj = ValidityCheckerDistance(si) 175 | 176 | si.setStateValidityChecker(ValidityChecker_obj) 177 | 178 | def get_path(start, goal): 179 | ''' 180 | Get a RRT path from start and goal. 181 | :param start: og.State object. 182 | :param goal: og.State object. 183 | returns (np.array, np.array, success): A tuple of numpy arrays of a valid path, 184 | interpolated path and whether the plan was successful or not. 185 | ''' 186 | success = False 187 | # Create a simple setup 188 | ss = og.SimpleSetup(si) 189 | 190 | # Set the start and goal states: 191 | ss.setStartAndGoalStates(start, goal, 0.1) 192 | 193 | # Use RRT 194 | planner = og.RRT(si) 195 | # planner.setRange(0.5) 196 | # # Use RRT* 197 | # planner = og.RRTstar(si) 198 | 199 | ss.setPlanner(planner) 200 | 201 | # Attempt to solve within the given time 202 | time = 60 203 | solved = ss.solve(60.0) 204 | while not ss.haveExactSolutionPath(): 205 | solved = ss.solve(30.0) 206 | time +=30 207 | if time>600: 208 | break 209 | if ss.haveExactSolutionPath(): 210 | success = True 211 | print("Found solution") 212 | path = [ 213 | [ss.getSolutionPath().getState(i)[0], ss.getSolutionPath().getState(i)[1]] 214 | for i in range(ss.getSolutionPath().getStateCount()) 215 | ] 216 | # Define path 217 | ss.getSolutionPath().interpolate(100) 218 | path_obj = ss.getSolutionPath() 219 | path_interpolated = np.array([ 220 | [path_obj.getState(i)[0], path_obj.getState(i)[1]] 221 | for i in range(path_obj.getStateCount()) 222 | ]) 223 | else: 224 | path = [[start[0], start[1]], [goal[0], goal[1]]] 225 | path_interpolated = [] 226 | 227 | return np.array(path), np.array(path_interpolated), success 228 | 229 | def start_experiment(start, samples): 230 | ''' 231 | Run the ccgp-mp experiment for random start and goal points. 232 | :param start: The starting index. 233 | :param samples: The number of samples to collect 234 | ''' 235 | if not GP_check: 236 | raise NameError("Toggle GP_check to True") 237 | 238 | exp = 'ccgp-mp-star' 239 | exp_num = 8 240 | 241 | for i in range(start, start+samples): 242 | path_param = {} 243 | 244 | data = pickle.load(open('/root/data/point/ccgp-mp-star/exp6/path_{}.p'.format(i), 'rb')) 245 | 246 | start = ob.State(space) 247 | start[0] = data['path'][0,0] 248 | start[1] = data['path'][0,1] 249 | 250 | goal = ob.State(space) 251 | goal[0] = data['path'][-1, 0] 252 | goal[1] = data['path'][-1, 1] 253 | 254 | # # Define the start and goal location 255 | # start = ob.State(space) 256 | # start.random() 257 | # while not ValidityChecker_obj.isValid(start()): 258 | # start.random() 259 | # goal = ob.State(space) 260 | # goal.random() 261 | # while not ValidityChecker_obj.isValid(goal()): 262 | # goal.random() 263 | 264 | path, path_interpolated, success = get_path(start, goal) 265 | path_param['path'] = path 266 | path_param['path_interpolated'] = path_interpolated 267 | path_param['success'] = success 268 | pickle.dump(path_param, open('/root/data/point/{}/exp{}/path_{}.p'.format(exp, exp_num, i), 'wb')) 269 | 270 | 271 | def start_experiment_rrt(start, samples): 272 | ''' 273 | Run the ccgp-mp experiment for random start and goal points. 274 | ''' 275 | if GP_check: 276 | raise NameError("Toggle GP_check to False") 277 | 278 | exp = 1 279 | 280 | for i in range(start, start+samples): 281 | path_param = {} 282 | 283 | data = pickle.load(open('/root/data/point/ccgp-mp-star/exp6/path_{}.p'.format(i), 'rb')) 284 | 285 | start = ob.State(space) 286 | start[0] = data['path'][0,0] 287 | start[1] = data['path'][0,1] 288 | 289 | goal = ob.State(space) 290 | goal[0] = data['path'][-1, 0] 291 | goal[1] = data['path'][-1, 1] 292 | 293 | path, path_interpolated, success = get_path(start, goal) 294 | path_param['path'] = path 295 | path_param['path_interpolated'] = path_interpolated 296 | path_param['success'] = success 297 | 298 | pickle.dump(path_param, open('/root/data/point/rrt-star/exp{}/path_{}.p'.format(exp, i), 'wb')) 299 | 300 | 301 | # LQG parameter: 302 | C = np.eye(2)*10 303 | D = np.eye(2)*0.1 304 | 305 | def evaluate_path(start, samples): 306 | ''' 307 | Evalute the path by executing it multiple times. 308 | :param start: The start index of the experiment 309 | :param samples: Number of samples in this experiment 310 | ''' 311 | exp = 'ccgp-mp-star' 312 | exp_num = 8 313 | 314 | si_check = ob.SpaceInformation(space) 315 | ValidityChecker_dis_obj = ValidityCheckerDistance(si_check) 316 | si_check.setStateValidityChecker(ValidityChecker_dis_obj) 317 | 318 | for i in range(start, start+samples): 319 | root_file = '/root/data/point/{}/exp{}/path_{}.p'.format(exp, exp_num, i) 320 | path_param = pickle.load(open(root_file, 'rb')) 321 | accuracy = 0 322 | # if path_param['success']: 323 | if len(path_param['path_interpolated'])>0: 324 | ompl_traj = og.PathGeometric(si_check) 325 | state_temp = ob.State(si_check.getStateSpace()) 326 | for path_i in path_param['path']: 327 | state_temp[0], state_temp[1] = path_i[0], path_i[1] 328 | ompl_traj.append(state_temp()) 329 | ompl_traj.interpolate(1000) 330 | traj = np.array([[ompl_traj.getState(i)[0], ompl_traj.getState(i)[1]] for i in range(ompl_traj.getStateCount())]) 331 | for _ in range(100): 332 | _, _, done = point.execute_path(traj, C, D, si_check) 333 | if done: 334 | accuracy += 1 335 | path_param['accuracy'] = accuracy 336 | print("Accuracy for path {} : {}".format(i, accuracy)) 337 | pickle.dump(path_param, open(root_file.format(i), 'wb')) 338 | 339 | 340 | if __name__ == "__main__": 341 | start, samples = int(sys.argv[1]), int(sys.argv[2]) 342 | 343 | # start_experiment(start, samples) 344 | # start_experiment_rrt(start, samples) 345 | evaluate_path(start, samples) 346 | 347 | # Visualize the planning network 348 | visualize_network = False 349 | if visualize_network: 350 | num = 0 351 | # data = pickle.load(open('/root/data/path_{}.p'.format(num), 'rb')) 352 | 353 | # start = ob.State(space) 354 | # start[0] = data['path'][0,0] 355 | # start[1] = data['path'][0,1] 356 | 357 | # goal = ob.State(space) 358 | # goal[0] = data['path'][-1, 0] 359 | # goal[1] = data['path'][-1, 1] 360 | 361 | start = ob.State(space) 362 | start.random() 363 | while not ValidityChecker_obj.isValid(start()): 364 | start.random() 365 | goal = ob.State(space) 366 | goal.random() 367 | while not ValidityChecker_obj.isValid(goal()): 368 | goal.random() 369 | 370 | fig, ax = plt.subplots() 371 | sns.set() 372 | plot_env(ax) 373 | 374 | ax.scatter(start[0], start[1], color='g') 375 | ax.scatter(goal[0], goal[1], color='r') 376 | success = False 377 | # Create a simple setup 378 | ss = og.SimpleSetup(si) 379 | 380 | # Set the start and goal states: 381 | ss.setStartAndGoalStates(start, goal, 0.1) 382 | 383 | # # Use RRT 384 | # planner = og.RRT(si) 385 | 386 | # Use RRT* 387 | planner = og.RRTstar(si) 388 | 389 | ss.setPlanner(planner) 390 | 391 | # Attempt to solve within the given time 392 | time = 60 393 | solved = ss.solve(60.0) 394 | while not ss.haveExactSolutionPath(): 395 | solved = ss.solve(30.0) 396 | time +=30 397 | if time>600: 398 | break 399 | if ss.haveExactSolutionPath(): 400 | success = True 401 | print("Found solution") 402 | 403 | import networkx as nx 404 | 405 | planner_data = ob.PlannerData(si) 406 | ss.getPlannerData(planner_data) 407 | G = nx.parse_graphml(planner_data.printGraphML()) 408 | pos = nx.get_node_attributes(G, 'coords') 409 | new_pos = {key:np.array(value.split(','), dtype=np.float) for key, value in pos.items()} 410 | 411 | nx.draw_networkx(G, new_pos, ax = ax, alpha=0.5) 412 | fig.show() 413 | 414 | visualize_paths = False 415 | if visualize_paths: 416 | fig, ax = plt.subplots() 417 | sns.set() 418 | 419 | plot_env(ax) 420 | 421 | total_success = 0 422 | for i in range(40): 423 | data = pickle.load(open('/root/data/path_{}.p'.format(i),'rb')) 424 | path = data['path'] 425 | path_interpolated = data['path_interpolated'] 426 | total_success += int(data['success']) 427 | 428 | ax.scatter(path[0, 0], path[0, 1], color='r', marker='x') 429 | ax.scatter(path[-1, 0], path[-1, 1], color='g', marker='o') 430 | 431 | if path!=[]: 432 | ax.plot(path[:,0], path[:,1], color='b', alpha=0.5) 433 | ax.scatter(path[:,0], path[:,1], color='b', alpha=0.5) 434 | temp = path.shape[0]-1 -------------------------------------------------------------------------------- /rrt_motion_dubin.py: -------------------------------------------------------------------------------- 1 | import pybullet as p 2 | import time 3 | import numpy as np 4 | from scipy import stats 5 | import matplotlib.pyplot as plt 6 | import seaborn as sns 7 | 8 | import pickle 9 | 10 | import GPy 11 | 12 | import os 13 | from os import path as osp 14 | import sys 15 | try: 16 | import ompl.base as ob 17 | import ompl.geometric as og 18 | except ImportError: 19 | print("Run code in a the ompl docker") 20 | print("ValidityChecker and ValidityCheckerDistance won't work") 21 | raise ImportError("Run in a docker with ompl") 22 | 23 | from models import racecarv2 24 | from config import box_width, box_length, xy, cir_radius 25 | 26 | sns.set() 27 | obstacles, robot = racecarv2.set_env( 28 | seed=10, 29 | num_boxes=8, 30 | num_circles=5 31 | ) 32 | 33 | def SE2State2Tuple(state): 34 | ''' 35 | convert SE2 state object to tuple 36 | :param state: An SE2 object 37 | :return tuple: (x,y, theta) 38 | ''' 39 | return (state.getX(), state.getY(), state.getYaw()) 40 | 41 | 42 | # Set up planning threshold limits 43 | thresh = 0.05 44 | N = stats.norm(scale=np.sqrt(1/2)) 45 | c = N.ppf(1-thresh) 46 | def reset_threshold(thresh): 47 | ''' 48 | A function to reset the threshold for planning environments: 49 | :param thresh: A value between [0, 1] to set as threshold for planning. 50 | ''' 51 | global c 52 | c = N.ppf(1-thresh) 53 | print("New threshold set as {}".format(thresh)) 54 | 55 | m = racecarv2.get_model_KF(robot, obstacles) 56 | 57 | class ValidityChecker(ob.StateValidityChecker): 58 | '''A class to check the validity of the state 59 | ''' 60 | def isValid(self, state): 61 | ''' 62 | Check if the given state is valid. 63 | ''' 64 | yaw = state.getYaw() 65 | x_hat = np.r_[[[state.getX(), state.getY(), np.cos(yaw), np.sin(yaw)]]] 66 | mean, var = m.predict(x_hat) 67 | return (mean/np.sqrt(2*var))[0,0] > c 68 | 69 | class ValidityCheckerDistance(ob.StateValidityChecker): 70 | '''A class to check the validity of the state by calculating 71 | the distance to obstacle 72 | ''' 73 | def isValid(self, state): 74 | ''' 75 | Check if the given state is valid. 76 | ''' 77 | collision = racecarv2.check_collision( 78 | np.r_[state.getX(), state.getY(), state.getYaw()], 79 | obstacles, 80 | robot 81 | ) 82 | return not collision 83 | 84 | # Wheel rotation is kept at np.pi*0.35, R = wheelbase/np.sin(max_steer) 85 | dubinSpace = ob.DubinsStateSpace(0.5) 86 | 87 | # Define SpaceInformation object 88 | si = ob.SpaceInformation(dubinSpace) 89 | 90 | # Collision checker obj 91 | GP_check = False 92 | if GP_check: 93 | ValidityChecker_obj = ValidityChecker(si) 94 | else: 95 | ValidityChecker_obj = ValidityCheckerDistance(si) 96 | si.setStateValidityChecker(ValidityChecker_obj) 97 | 98 | # Set the bounds of the space 99 | bounds = ob.RealVectorBounds(2) 100 | bounds.setLow(0.0) 101 | bounds.setHigh(10) 102 | dubinSpace.setBounds(bounds) 103 | 104 | 105 | def get_path(start, goal): 106 | ''' 107 | Get the RRT* for SE2 space for a given start and goal. 108 | :param start: og.State object. 109 | :param goal: og.State object. 110 | returns (np.array, np.array, success): A tuple of numpy arrays of a valid path, 111 | interpolated path and whether the plan was successful or not. 112 | ''' 113 | success = False 114 | ss = og.SimpleSetup(si) 115 | ss.setStartAndGoalStates(start, goal, 0.1) 116 | 117 | # planner = og.RRT(si) 118 | planner = og.RRTstar(si) 119 | ss.setPlanner(planner) 120 | 121 | time = 60.0 122 | solved = ss.solve(time) 123 | 124 | while not ss.haveExactSolutionPath(): 125 | solved = ss.solve(30.0) 126 | time += 30 127 | if time>1200: 128 | break 129 | 130 | if ss.haveExactSolutionPath(): 131 | ss.simplifySolution() 132 | success = True 133 | print("Found Solution") 134 | path = [ 135 | SE2State2Tuple(ss.getSolutionPath().getState(i)) 136 | for i in range(ss.getSolutionPath().getStateCount()) 137 | ] 138 | ss.getSolutionPath().interpolate(5000) 139 | path_obj = ss.getSolutionPath() 140 | path_interpolated = [ 141 | SE2State2Tuple(path_obj.getState(i)) 142 | for i in range(path_obj.getStateCount()) 143 | ] 144 | else: 145 | path = [SE2State2Tuple(start()), SE2State2Tuple(goal())] 146 | path_interpolated = [] 147 | 148 | return np.array(path), np.array(path_interpolated), success 149 | 150 | def start_experiment(start, samples, expFolder, randomSample, compExpFolder=None, ccgpFlag=False): 151 | ''' 152 | Save the RRT* experiments for a given range of (start, start+samples) with or without 153 | CCGP. 154 | :param start: The start index 155 | :param samples: The number of samples to collect. 156 | :param expFolder: Location to save the data. 157 | :param RandomSample: If True, randomly sample valid start and goal pairs. 158 | :param compExpFolder: The experiment Folder to select start and goal pairs from. 159 | :param ccgpFlag: If True, plan RRT* using CCGP 160 | ''' 161 | assert osp.isdir(expFolder), f"The file directory, {expFolder}, does not exits" 162 | 163 | if not randomSample and compExpFolder is not None: 164 | assert osp.isdir(compExpFolder), "No folder found to compare" 165 | 166 | if ccgpFlag: 167 | assert GP_check, "Turn on GP_check and rerun experiment" 168 | else: 169 | assert (not GP_check), "Turn off GP_check and rerun experiment" 170 | 171 | for i in range(start, start+samples): 172 | path_param = {} 173 | if randomSample: 174 | # Define random start and goal locations 175 | start = ob.State(dubinSpace) 176 | start.random() 177 | while not ValidityChecker_obj.isValid(start()): 178 | start.random() 179 | 180 | goal = ob.State(dubinSpace) 181 | goal.random() 182 | while not ValidityChecker_obj.isValid(goal()): 183 | goal.random() 184 | else: 185 | data = pickle.load(open(osp.join(compExpFolder, f'path_{i}.p'), 'rb')) 186 | 187 | start = ob.State(dubinSpace) 188 | start[0] = data['path'][0, 0] 189 | start[1] = data['path'][0, 1] 190 | start[2] = data['path'][0, 2] 191 | 192 | goal = ob.State(dubinSpace) 193 | goal[0] = data['path'][-1, 0] 194 | goal[1] = data['path'][-1, 1] 195 | goal[2] = data['path'][-1, 2] 196 | 197 | path, path_interpolated, success = get_path(start, goal) 198 | 199 | path_param['path'] = path 200 | path_param['path_interpolated'] = path_interpolated 201 | path_param['success'] = success 202 | 203 | pickle.dump(path_param, open(osp.join(expFolder, f'path_{i}.p'), 'wb')) 204 | 205 | 206 | def evaluate_path(start, samples, expFolder): 207 | ''' 208 | Evaluate the path of the trajectory for 100 trials and save the stats back 209 | in the pickle file. 210 | :param start: The start index 211 | :param samples: The number of samples to be collected 212 | :param expFolder: The folder with the paths 213 | ''' 214 | for i in range(start, start+samples): 215 | root_file = osp.join(expFolder, f'path_{i}.p') 216 | path_param = pickle.load(open(root_file, 'rb')) 217 | accuracy = 0 218 | if path_param['success']: 219 | for _ in range(100): 220 | _, done = racecarv2.execute_path(robot, path_param['path_interpolated'], obstacles) 221 | if done: 222 | accuracy += 1 223 | path_param['accuracy'] = accuracy 224 | print("Accuracy for path {} : {}".format(i, accuracy)) 225 | pickle.dump(path_param, open(root_file, 'wb')) 226 | 227 | 228 | def collect_data(num): 229 | ''' 230 | The function to collect data for fig:diff_thresh_paths for the paper. 231 | ''' 232 | root_folder = '/root/data/dubins/' 233 | file_loc = osp.join(root_folder, 'CCGP-MP', 'exp16', 'path_{}.p'.format(num)) 234 | data = {} 235 | path_param = pickle.load(open(file_loc, 'rb')) 236 | # Set the start and goal locations 237 | start_array = path_param['path'][0] 238 | goal_array = path_param['path'][-1] 239 | # Define start and goal locations from file 240 | start = ob.State(dubinSpace) 241 | start().setX(start_array[0]) 242 | start().setY(start_array[1]) 243 | start().setYaw(start_array[2]) 244 | 245 | goal = ob.State(dubinSpace) 246 | goal().setX(goal_array[0]) 247 | goal().setY(goal_array[1]) 248 | goal().setYaw(goal_array[2]) 249 | # Evaluate the trajectory for these thresholds 250 | if path_param['success']: 251 | path_param['true_traj'] = [] 252 | path_param['est_traj'] = [] 253 | for _ in range(20): 254 | done, true_traj, est_traj = racecar.execute_path_LQR_data( 255 | robot, 256 | path_param['path_interpolated'], 257 | obstacles, 258 | get_log=True 259 | ) 260 | path_param['true_traj'].append(np.array(true_traj)) 261 | path_param['est_traj'].append(np.array(est_traj)) 262 | data['0.01'] = path_param 263 | 264 | thresh_list = [0.05, 0.1] 265 | # Plan the trajectory for different thresholds 266 | for thresh in thresh_list: 267 | reset_threshold(thresh) 268 | path_param = {} 269 | path, path_interpolated, success = get_path(start, goal) 270 | path_param['path'] = path 271 | path_param['path_interpolated'] = path_interpolated 272 | path_param['success'] = success 273 | 274 | if success: 275 | # Evaluate the trajectory for these thresholds 276 | path_param['true_traj'] = [] 277 | path_param['est_traj'] = [] 278 | for _ in range(20): 279 | done, true_traj, est_traj = racecar.execute_path_LQR_data( 280 | robot, 281 | path_param['path_interpolated'], 282 | obstacles, 283 | get_log=True 284 | ) 285 | path_param['true_traj'].append(np.array(true_traj)) 286 | path_param['est_traj'].append(np.array(est_traj)) 287 | data['{}'.format(thresh)] = path_param 288 | 289 | save_file_loc = osp.join(root_folder,'fig_data','fig3','path_{}.p'.format(num)) 290 | pickle.dump(data, open(save_file_loc, 'wb')) 291 | 292 | def collect_data_rrt(num): 293 | ''' 294 | The function to collect data for fig:diff_thresh_paths 295 | :param num: The path number 296 | ''' 297 | root_folder = '/root/data/dubins/' 298 | file_loc = osp.join(root_folder, 'CCGP-MP', 'exp16', 'path_{}.p'.format(num)) 299 | path_param = pickle.load(open(file_loc, 'rb')) 300 | # Set the start and goal locations 301 | start_array = path_param['path'][0] 302 | goal_array = path_param['path'][-1] 303 | # Define start and goal locations from file 304 | start = ob.State(dubinSpace) 305 | start().setX(start_array[0]) 306 | start().setY(start_array[1]) 307 | start().setYaw(start_array[2]) 308 | 309 | goal = ob.State(dubinSpace) 310 | goal().setX(goal_array[0]) 311 | goal().setY(goal_array[1]) 312 | goal().setYaw(goal_array[2]) 313 | # Evaluate the trajectory for these thresholds 314 | path, path_interpolated, success = get_path(start, goal) 315 | path_param['path'] = path 316 | path_param['path_interpolated'] = path_interpolated 317 | path_param['success'] = success 318 | 319 | data_file = osp.join(root_folder, 'fig_data', 'fig3', 'path_{}.p'.format(num)) 320 | data = pickle.load(open(data_file, 'rb')) 321 | 322 | if success: 323 | # Evaluate the trajectory for these thresholds 324 | path_param['true_traj'] = [] 325 | path_param['est_traj'] = [] 326 | for _ in range(20): 327 | done, true_traj, est_traj = racecar.execute_path_LQR_data( 328 | robot, 329 | path_param['path_interpolated'], 330 | obstacles, 331 | get_log=True 332 | ) 333 | path_param['true_traj'].append(np.array(true_traj)) 334 | path_param['est_traj'].append(np.array(est_traj)) 335 | data['rrt'] = path_param 336 | pickle.dump(data, open(data_file, 'wb')) 337 | 338 | import argparse 339 | 340 | if __name__=="__main__": 341 | parser = argparse.ArgumentParser(description="Collect RRT/RRT* paths using w/ without CCGP-MP") 342 | parser.add_argument('--start', help='The start index of the experiment', default=0, type=int) 343 | parser.add_argument('--samples', help='Number of paths to collect', default=1, type=int) 344 | parser.add_argument('--expFolder', help='Location to save the collected data') 345 | parser.add_argument('--compExpFolder', help='Location to save the collected data', default=None) 346 | parser.add_argument('-r', '--randomSample', help='Sample random start and goal location', action="store_true") 347 | parser.add_argument('--CCGP', help='Plan with CCGP enabled', action="store_true") 348 | 349 | args = parser.parse_args() 350 | 351 | start_experiment(args.start, args.samples, args.expFolder, args.randomSample, compExpFolder=args.compExpFolder) 352 | evaluate_path(args.start, args.samples, args.expFolder) 353 | 354 | # path_param = pickle.load(open('/root/data/dubins/path_0.p', 'rb')) 355 | # done = racecar.execute_path(car, path_param['path_interpolated'], obstacles) 356 | 357 | # fig, ax = plt.subplots() 358 | 359 | # ax.set_xlim((-0.2, 10.2)) 360 | # ax.set_ylim((-0.2, 10.2)) 361 | 362 | # # Initialize the position of obstacles 363 | # dimensions = [box_length, box_width] 364 | # rectangle_corner = np.r_[(-dimensions[0]/2, -dimensions[1]/2)] 365 | 366 | # for xy_i in racecar.xy_circle: 367 | # plt_cir = plt.Circle(xy_i, radius=cir_radius, color='r', alpha=0.5) 368 | # ax.add_patch(plt_cir) 369 | 370 | # for xy_i in racecar.xy: 371 | # plt_box = plt.Rectangle(xy_i+rectangle_corner, dimensions[0], dimensions[1], color='r', alpha=0.5) 372 | # ax.add_patch(plt_box) 373 | 374 | if False: 375 | # Define random start and goal locations 376 | start = ob.State(dubinSpace) 377 | start.random() 378 | while not ValidityChecker_obj.isValid(start()): 379 | start.random() 380 | 381 | goal = ob.State(dubinSpace) 382 | goal.random() 383 | while not ValidityChecker_obj.isValid(goal()): 384 | goal.random() 385 | path, path_interpolated, success = get_path(start, goal) 386 | 387 | ax.scatter(start().getX(), start().getY(), color='g') 388 | ax.scatter(goal().getX(), goal().getY(), color='r') 389 | 390 | if success: 391 | for path_i in path: 392 | ax.arrow(path_i[0], path_i[1], 0.1*np.cos(path_i[2]), 0.1*np.sin(path_i[2]), color='k') 393 | 394 | 395 | for path_i in path_interpolated: 396 | ax.arrow(path_i[0], path_i[1], 0.1*np.cos(path_i[2]), 0.1*np.sin(path_i[2]), color='b', width=0.01) 397 | 398 | # fig.show() 399 | # fig, ax = plt.subplots() 400 | if False: 401 | plt_objects = m.plot(ax = ax, visible_dims=np.array([0, 1]), plot_data=True, cmap='Greys') 402 | # Get the contour plot objects 403 | contour = plt_objects['gpmean'][0] 404 | ax.clabel(contour, contour.levels, inline=True, fontsize=10) 405 | # plt.colorbar(ax) -------------------------------------------------------------------------------- /models/racecarv2.py: -------------------------------------------------------------------------------- 1 | ''' A car model from Probabilistic Robotics. 2 | ''' 3 | import pybullet_utils.bullet_client as bc 4 | import pybullet as pyb 5 | import pybullet_data 6 | 7 | import numpy as np 8 | from scipy import stats 9 | 10 | import GPy 11 | 12 | # Define the environment 13 | from config import box_length, box_width, cir_radius 14 | 15 | # Assuming the wheelbase of the car is the same used in MIT racecar 16 | wheelbase = 0.325 17 | # Initialize few model parameters 18 | steering = [0, 2] 19 | wheels = [8, 15] 20 | 21 | useRealTimeSim = 0 22 | # p = bc.BulletClient(connection_mode=pyb.GUI) 23 | p = bc.BulletClient(connection_mode=pyb.DIRECT) 24 | 25 | # Model parameters 26 | delta_t = 1/20 27 | class NoiseParams: 28 | # Motion Noise 29 | M = None 30 | M_t = None 31 | gamma_t = None 32 | # Observation Noise 33 | N = None 34 | N_t = None 35 | modelName= None 36 | 37 | def set_simulation_env(client_obj): 38 | ''' 39 | Set environment for the given client_object. 40 | :param client_obj: A pybullet_utils.BulletClient object 41 | ''' 42 | 43 | client_obj.setAdditionalSearchPath(pybullet_data.getDataPath()) 44 | client_obj.resetSimulation() 45 | client_obj.setGravity(0, 0, -9.8) 46 | 47 | #for video recording (works best on Mac and Linux, not well on Windows) 48 | #client_obj.startStateLogging(client_obj.STATE_LOGGING_VIDEO_MP4, "racecar.mp4") 49 | client_obj.setRealTimeSimulation(useRealTimeSim) # either this 50 | client_obj.loadURDF("plane.urdf") 51 | 52 | client_obj.resetDebugVisualizerCamera( 53 | cameraDistance=10, 54 | cameraYaw=0, 55 | cameraPitch=-89.9, 56 | cameraTargetPosition=[5,5,0]) 57 | 58 | def initialize_constraints(client_obj, car): 59 | ''' 60 | Initialize constraints for the car model. 61 | :param car: The pybullet id of the car model. 62 | ''' 63 | for wheel in range(client_obj.getNumJoints(car)): 64 | client_obj.setJointMotorControl2(car, wheel, pyb.VELOCITY_CONTROL, targetVelocity=0, force=0) 65 | client_obj.getJointInfo(car, wheel) 66 | 67 | print("----------------") 68 | 69 | #client_obj.setJointMotorControl2(car,10,client_obj.VELOCITY_CONTROL,targetVelocity=1,force=10) 70 | c = client_obj.createConstraint(car, 71 | 9, 72 | car, 73 | 11, 74 | jointType=pyb.JOINT_GEAR, 75 | jointAxis=[0, 1, 0], 76 | parentFramePosition=[0, 0, 0], 77 | childFramePosition=[0, 0, 0]) 78 | client_obj.changeConstraint(c, gearRatio=1, maxForce=10000) 79 | 80 | c = client_obj.createConstraint(car, 81 | 10, 82 | car, 83 | 13, 84 | jointType=pyb.JOINT_GEAR, 85 | jointAxis=[0, 1, 0], 86 | parentFramePosition=[0, 0, 0], 87 | childFramePosition=[0, 0, 0]) 88 | client_obj.changeConstraint(c, gearRatio=-1, maxForce=10000) 89 | 90 | c = client_obj.createConstraint(car, 91 | 9, 92 | car, 93 | 13, 94 | jointType=pyb.JOINT_GEAR, 95 | jointAxis=[0, 1, 0], 96 | parentFramePosition=[0, 0, 0], 97 | childFramePosition=[0, 0, 0]) 98 | client_obj.changeConstraint(c, gearRatio=-1, maxForce=10000) 99 | 100 | c = client_obj.createConstraint(car, 101 | 16, 102 | car, 103 | 18, 104 | jointType=pyb.JOINT_GEAR, 105 | jointAxis=[0, 1, 0], 106 | parentFramePosition=[0, 0, 0], 107 | childFramePosition=[0, 0, 0]) 108 | client_obj.changeConstraint(c, gearRatio=1, maxForce=10000) 109 | 110 | c = client_obj.createConstraint(car, 111 | 16, 112 | car, 113 | 19, 114 | jointType=pyb.JOINT_GEAR, 115 | jointAxis=[0, 1, 0], 116 | parentFramePosition=[0, 0, 0], 117 | childFramePosition=[0, 0, 0]) 118 | client_obj.changeConstraint(c, gearRatio=-1, maxForce=10000) 119 | 120 | c = client_obj.createConstraint(car, 121 | 17, 122 | car, 123 | 19, 124 | jointType=pyb.JOINT_GEAR, 125 | jointAxis=[0, 1, 0], 126 | parentFramePosition=[0, 0, 0], 127 | childFramePosition=[0, 0, 0]) 128 | client_obj.changeConstraint(c, gearRatio=-1, maxForce=10000) 129 | 130 | c = client_obj.createConstraint(car, 131 | 1, 132 | car, 133 | 18, 134 | jointType=pyb.JOINT_GEAR, 135 | jointAxis=[0, 1, 0], 136 | parentFramePosition=[0, 0, 0], 137 | childFramePosition=[0, 0, 0]) 138 | client_obj.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000) 139 | c = client_obj.createConstraint(car, 140 | 3, 141 | car, 142 | 19, 143 | jointType=pyb.JOINT_GEAR, 144 | jointAxis=[0, 1, 0], 145 | parentFramePosition=[0, 0, 0], 146 | childFramePosition=[0, 0, 0]) 147 | client_obj.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000) 148 | 149 | 150 | 151 | def set_env(name='Random', **kwargs): 152 | ''' 153 | Set the environment as either Random or Gibson 154 | :param name: Either has to be 'Random' or 'Gibon' 155 | :param kwargs: Other environment parameters 156 | :return tuple: The pybullet ID of obstacles and robot 157 | ''' 158 | assert name in ['Random', 'Gibson'], 'Only two environments possible Gibson/Random' 159 | print('------------------------------------') 160 | set_simulation_env(p) 161 | # Load car on simulator 162 | car = p.loadURDF("racecar/racecar_differential.urdf", [0.0, 0.0, 0.05074242991633105]) #, [0,0,2],useFixedBase=True) #, [0,0,2],useFixedBase=True) 163 | initialize_constraints(client_obj=p, car=car) 164 | if name=='Random': 165 | from models.randomWorld import set_obstacles 166 | # Motion Noise 167 | NoiseParams.M = np.eye(3)*1e-1 168 | NoiseParams.M[2, 2] = np.deg2rad(10)**2 169 | NoiseParams.M_t = stats.multivariate_normal(cov=NoiseParams.M[:2, :2]) 170 | NoiseParams.gamma_t = stats.norm(scale=np.sqrt(NoiseParams.M[2, 2])) 171 | # Observation Noise 172 | NoiseParams.N = np.eye(3)*1e-1 173 | NoiseParams.N[2, 2] = np.deg2rad(5)**2 174 | NoiseParams.N_t = stats.multivariate_normal(cov=NoiseParams.N) 175 | NoiseParams.modelName = f'random_{seed}' 176 | else: 177 | from models.gibsonWorld import set_obstacles 178 | # Motion Noise 179 | NoiseParams.M = np.eye(3)*5e-2 180 | NoiseParams.M[2, 2] = np.deg2rad(10)**2 181 | NoiseParams.M_t = stats.multivariate_normal(cov=NoiseParams.M[:2, :2]) 182 | NoiseParams.gamma_t = stats.norm(scale=np.sqrt(NoiseParams.M[2, 2])) 183 | # Observation Noise 184 | NoiseParams.N = np.eye(3)*5e-2 185 | NoiseParams.N[2, 2] = np.deg2rad(5)**2 186 | NoiseParams.N_t = stats.multivariate_normal(cov=NoiseParams.N) 187 | NoiseParams.modelName = f'gibson' 188 | 189 | obstacles = set_obstacles(p, **kwargs) 190 | return obstacles, car 191 | 192 | def check_collision(x, obstacles, robot): 193 | ''' 194 | Check if the robot is in collision at x with obstacles. 195 | :param x: The state of the robot. 196 | :param obstacles: A list of all obstacles ID's 197 | :param robot: The robot ID 198 | :return bool: True if the x is in collision. 199 | ''' 200 | robotOrientation = p.getQuaternionFromEuler((0, 0, x[2])) 201 | p.resetBasePositionAndOrientation(robot, np.r_[x[:2], 0.05074242991633105], robotOrientation) 202 | return get_distance(obstacles, robot)<0 203 | 204 | 205 | def get_distance(obstacles ,robot): 206 | ''' 207 | Get the shortest distance between the obstacles and robot, for the given setup. 208 | :param obstacles: A list of all obstacle ID's 209 | :param robot: The robot ID 210 | :return float : The penetration distance between the robot and obstacles, If the distance is 211 | negative, object is in collision. 212 | ''' 213 | assert isinstance(obstacles, list), "Obstacles has to be a list" 214 | try: 215 | distance = min( 216 | ( 217 | min(p.getClosestPoints(bodyA=obs, bodyB=robot, distance=100), key=lambda x:x[8])[8] 218 | for obs in obstacles 219 | ) 220 | ) 221 | except ValueError: 222 | import pdb;pdb.set_trace() 223 | return distance 224 | 225 | def pi_2_pi(angle): 226 | ''' 227 | Convert angle to -pi/pi. 228 | :param angle:The angle to convert 229 | :return float: The angle between -pi/pi. 230 | ''' 231 | return (angle + np.pi) % (2 * np.pi) - np.pi 232 | 233 | 234 | def step(x, u): 235 | ''' 236 | Setting up the velocity model as proposed in Probablistic Planning textbook. 237 | :param u : The input control 238 | :param x : The current state of the robot. 239 | :return np.array: The next state of the robot. 240 | ''' 241 | assert u.shape[0] == 2 242 | assert x.shape[0] == 3 243 | 244 | u = u + NoiseParams.M_t.rvs() 245 | theta = x[2] + NoiseParams.gamma_t.rvs()*delta_t 246 | x = get_forward(x, u) 247 | return x 248 | 249 | 250 | def get_forward(x, u): 251 | ''' 252 | Get the forward motion of the racecar without noise. 253 | :param x: The current state of the robot 254 | :param u: The input control 255 | :return np.array: The next state of the robot 256 | ''' 257 | 258 | if np.isclose(u[1], 0): 259 | delta_x = np.r_[ 260 | u[0]*delta_t*np.cos(x[2]), 261 | u[0]*delta_t*np.sin(x[2]), 262 | 0.0 263 | ] 264 | else: 265 | K = u[0]/u[1] 266 | delta_x = np.r_[ 267 | K*(-np.sin(x[2]) + np.sin(x[2]+u[1]*delta_t)), 268 | K*(np.cos(x[2]) - np.cos(x[2]+u[1]*delta_t)), 269 | u[1]*delta_t 270 | ] 271 | x = x + delta_x 272 | # x[2] = pi_2_pi(x[2]) 273 | return x 274 | 275 | 276 | def get_state_obs(x, u): 277 | ''' 278 | Return the observation and next state of the robot. 279 | :param x: The current state of the robot 280 | :param u: The control input of the robot. 281 | :return (np.array, np.array): The next state and current observation 282 | of the robot 283 | ''' 284 | x = step(x, u) 285 | z = x + NoiseParams.N_t.rvs() 286 | return x, z 287 | 288 | 289 | def get_dyn(x, u): 290 | ''' 291 | Getting the next state wsing the EKF model. 292 | :param x: The cwrrent state. 293 | :param w: The cwrrent control inpwt. 294 | ''' 295 | assert u.shape[0] == 2 296 | assert x.shape[0] == 3 297 | w = u[1] 298 | if np.isclose(w, 0): 299 | w = 1e-6 300 | 301 | V = u[0]/w 302 | A = np.array([ 303 | [1, 0, V*(-np.cos(x[2]) + np.cos(x[2]+w*delta_t))], 304 | [0, 1, V*(-np.sin(x[2]) + np.sin(x[2]+w*delta_t))], 305 | [0, 0, 1] 306 | ]) 307 | B = np.array([ 308 | [(V/w)*(np.sin(x[2]) - np.sin(x[2]+w*delta_t)) + V*np.cos(x[2]+w*delta_t)*delta_t], 309 | [(V/w)*(-np.cos(x[2]) + np.cos(x[2]+w*delta_t)) + V*np.sin(x[2]+w*delta_t)*delta_t], 310 | [delta_t] 311 | ]) 312 | 313 | V = np.array([ 314 | [(1/w)*(-np.sin(x[2]) + np.sin(x[2]+w*delta_t)), (V/w)*(np.sin(x[2]) - np.sin(x[2]+delta_t*w)) + V*np.cos(x[2]+w*delta_t)*delta_t, 0], 315 | [(1/w)*(np.cos(x[2]) - np.cos(x[2]+w*delta_t)), (V/w)*(-np.cos(x[2]) + np.cos(x[2]+delta_t*w)) + V*np.sin(x[2]+w*delta_t)*delta_t, 0], 316 | [0, delta_t, delta_t] 317 | ]) 318 | return A, B, V 319 | 320 | 321 | def ekf(x, Sigma, u, z): 322 | ''' 323 | Estimate the target location and distribution. 324 | :param x: The current state of the robot 325 | :param Sigma : The variance of current state. 326 | :param u: The input control 327 | :param z: The current observation. 328 | :return (np.array, np.array): The mean and variance of the next state. 329 | ''' 330 | 331 | # Get model parameters 332 | A, B, V = get_dyn(x, u) 333 | 334 | x_bar = get_forward(x, u) 335 | Sigma_bar = A@Sigma@A.T + V@NoiseParams.M@V.T 336 | 337 | K = Sigma_bar@np.linalg.inv(Sigma_bar+NoiseParams.N) 338 | 339 | x = x_bar + K@(z-x_bar) 340 | Sigma = (np.eye(3)-K)@Sigma_bar 341 | 342 | return x, Sigma 343 | 344 | def get_starting_position(robot, obstacles): 345 | ''' 346 | Get the random position of the robot. 347 | :param robot: 348 | :param obstacles: 349 | :returns np.array: A viable starting position. 350 | ''' 351 | valid_start = False 352 | while not valid_start: 353 | start = np.random.rand(2)*10 354 | theta = np.random.rand()*np.pi*2 355 | x_state = np.r_[start, theta] 356 | robotOrientation = p.getQuaternionFromEuler((0, 0, theta)) 357 | p.resetBasePositionAndOrientation(robot, np.r_[start, 0.05074242991633105], robotOrientation) 358 | valid_start = get_distance(obstacles, robot)>0 359 | return x_state 360 | 361 | 362 | def get_path(samples, robot, obstacles): 363 | ''' 364 | Generate a random trajectory 365 | :param samples: The number of the samples to generate 366 | :param robot: 367 | :param obstacles: 368 | :returns np.array: A trajectory and distance measure for a 369 | random sample. 370 | ''' 371 | x = get_starting_position(robot, obstacles) 372 | x_est = x 373 | Sigma = np.eye(3)*0 374 | 375 | # Get a random path 376 | true_path = [x] 377 | est_path = [x] 378 | for _ in range(samples): 379 | u = np.r_[1, np.random.uniform(low=-1, high=1)] 380 | x, z = get_state_obs(x, u) 381 | x_est, Sigma = ekf(x_est, Sigma, u, z) 382 | if any(x[:2]>10) or any(x[:2]<0): 383 | break 384 | true_path.append(x) 385 | est_path.append(x_est) 386 | 387 | # Get the distance 388 | d = [] 389 | for x_est in est_path: 390 | robotOrientation = p.getQuaternionFromEuler((0, 0, x_est[2])) 391 | p.resetBasePositionAndOrientation(robot, np.r_[x_est[:2], 0.05074242991633105], robotOrientation) 392 | d.append(get_distance(obstacles, robot)) 393 | 394 | # Return the true path, random path, and distance. 395 | return np.array(true_path), np.array(est_path), np.array(d) 396 | 397 | 398 | def get_model_KF(robot, obstacles): 399 | ''' 400 | Navigate the model around the environment, and collect 401 | distance to collision. Using the data, construct a GP model to 402 | estimate the distance to collision. 403 | :param robot: pybullet.MultiBody object representing the robot 404 | :param obstacles: pybullet.MultiBody object representing the obstacles in the environment 405 | :returns GPy.models.GPRegression model representing the obstacle space 406 | ''' 407 | try: 408 | print("Loading data for map") 409 | X = np.load(f'param/X_{NoiseParams.modelName}_dubins.npy') 410 | Y = np.load(f'param/Y_{NoiseParams.modelName}_dubins.npy') 411 | except FileNotFoundError: 412 | print("Did not find file, Generating data ....") 413 | X = np.array([[0.0, 0.0, 0.0, 0.0]]) 414 | Y = np.array([0.0]) 415 | count = 0 416 | samples = 2000 417 | skip = 5 418 | while countind: 540 | ind = temp_ind 541 | A, B, V = get_dyn(x, u) 542 | K, _, eigvals = dlqr(A, B, Q, R) 543 | 544 | delta_x = np.zeros((3, 1)) 545 | th_e = pi_2_pi(x_est[2] - traj_orien[ind, 2]) 546 | delta_x[0, 0] = x_est[0]-traj_orien[ind, 0] 547 | delta_x[1, 0] = x_est[1]-traj_orien[ind, 1] 548 | delta_x[2, 0] = th_e 549 | fb = (K @ delta_x)[0, 0] 550 | delta = np.clip(fb, -1, 1) 551 | u = np.r_[0.5, -delta] 552 | x, z = get_state_obs(x, u) 553 | path = np.r_[path, x[None, :]] 554 | # Check for collisions 555 | if check_collision(x, obstacles, robot): 556 | break 557 | x_est, Sigma = ekf(x_est, Sigma, u, z) 558 | if np.linalg.norm(x[:2]-goal.T)<0.25: 559 | goal_reached = True 560 | num +=1 561 | 562 | return path, goal_reached 563 | --------------------------------------------------------------------------------