├── .DS_Store ├── Project Report.pdf ├── README.md ├── dubins ├── dubin_1 .png ├── dubin_2.png ├── dubin_3.png ├── dubin_4.png ├── dubins.py └── gazebo_dubin.py ├── rrt_dubins ├── Minkowski.py ├── gazebo_rrt_dubins.py ├── rrt_dubin.py ├── rrt_dubin_2.png ├── rrt_dubin_2_2.png ├── rrt_dubin_4.png └── rrt_dubin_4_1.png └── rrt_star_dubins ├── .DS_Store ├── Figure1.png ├── Figure2.png ├── rrt_star.png ├── rrt_star_dubins.py ├── rrt_star_dubins_minimized_euclidean.py ├── rrt_star_dubins_minimzed_dubins_paths.py ├── rrt_star_with_random_samplings.png ├── rrt_star_with_random_samplings2.png ├── rrtstar_minimized_dubins_paths.png └── rrtstar_minimized_dubins_paths2.png /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/.DS_Store -------------------------------------------------------------------------------- /Project Report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/Project Report.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Mobile Robots Autonomous Navigation 2 | 3 | Implemented Dubins Curves and RRT(Rapidly Exploring Random Trees) along with the Optimal RRT version. 4 | 5 | Simulated an environment with obstacles with a Pioneer P3DX robot in Gazebo using ROS and applied these path planning algorithms. 6 | 7 | Refer the Project Report for more information. 8 | 9 | # Visuals 10 | 11 | 12 |

RRT Star + Dubins (Sub-Optimal) with minimized Euclidean distance between nodes, 15 |

17 | 18 |

RRT Star + Dubins with random samples, 21 |

23 | 24 |

RRT Star + Dubins (Optimal) with minimized Dubins Paths between nodes, 27 |

29 | 30 | 31 |
32 | -------------------------------------------------------------------------------- /dubins/dubin_1 .png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/dubins/dubin_1 .png -------------------------------------------------------------------------------- /dubins/dubin_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/dubins/dubin_2.png -------------------------------------------------------------------------------- /dubins/dubin_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/dubins/dubin_3.png -------------------------------------------------------------------------------- /dubins/dubin_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/dubins/dubin_4.png -------------------------------------------------------------------------------- /dubins/dubins.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import math 4 | import matplotlib.patches as patches 5 | import sys 6 | 7 | current_x=0 8 | current_y=0 9 | current_th=0 #current angle in Degrees 10 | 11 | PI=3.14 12 | r_min=2 13 | 14 | goal_x=4 15 | goal_y=4 16 | goal_th=0 #goal angle in Degrees 17 | 18 | # fig, ax = plt.subplots() 19 | 20 | 21 | 22 | 23 | def LSL(): 24 | plt.ylim(-5,15) 25 | plt.xlim(-5,15) 26 | c1_x=current_x-r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 27 | c1_y=current_y-r_min*math.sin(math.radians(current_th)-(PI/2)) 28 | 29 | c2_x=goal_x-r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 30 | c2_y=goal_y-r_min*math.sin(math.radians(goal_th)-(PI/2)) 31 | #print c1_x,c1_y,c2_x,c2_y, c2_y-c1_y, c2_x-c1_x 32 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 33 | 34 | tan_pt1_x=c1_x+r_min*math.cos(theta) #tangent point in first circle of min_turning radius 35 | tan_pt1_y=c1_y-r_min*math.sin(theta) 36 | 37 | tan_pt2_x=c2_x+r_min*math.cos(theta) #tangent point in second circle of min_turning radius 38 | tan_pt2_y=c2_y-r_min*math.sin(theta) 39 | 40 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 41 | # print S,theta 42 | # print (tan_pt1_x,tan_pt1_y) 43 | # print (tan_pt2_x,tan_pt2_y) 44 | # print (c1_x,c1_y,c2_x,c2_y) 45 | 46 | 47 | 48 | Ti = np.arctan2(current_x-c1_x,current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 49 | 50 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(goal_x - c2_x,goal_y- c2_y) #Final Turning angle 51 | 52 | if Ti<0: 53 | Ti+=2*np.pi 54 | if Tf<0: 55 | Tf+=2*np.pi 56 | 57 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 58 | 59 | Ti= Ti*180/np.pi 60 | if Ti<0: 61 | Ti+=360 62 | Tf= Tf*180/np.pi 63 | if Tf<0: 64 | Tf+=360 65 | 66 | # print Ti,Tf, total_dist,S 67 | ans=[Ti,Tf, total_dist,S] 68 | 69 | start_angle1 = np.arctan2(current_y - c1_y, current_x - c1_x) * 180 / np.pi 70 | end_angle1 = np.arctan2(tan_pt1_y - c1_y, tan_pt1_x - c1_x) * 180 / np.pi 71 | 72 | arc1 = patches.Arc((c1_x, c1_y), 2*r_min,2*r_min,theta1=start_angle1, theta2=end_angle1) 73 | plt.gca().add_patch(arc1) 74 | 75 | start_angle2 = np.arctan2(tan_pt2_y - c2_y, tan_pt2_x - c2_x) * 180 / np.pi 76 | end_angle2 = np.arctan2(goal_y - c2_y, goal_x - c2_x) * 180 / np.pi 77 | 78 | arc2 = patches.Arc((c2_x, c2_y), 2*r_min,2*r_min,theta1=start_angle2, theta2=end_angle2) 79 | plt.gca().add_patch(arc2) 80 | 81 | 82 | plt.scatter(current_x,current_y) 83 | plt.scatter(goal_x,goal_y) 84 | plt.scatter(c1_x,c1_y) 85 | plt.plot([tan_pt1_x,tan_pt2_x],[tan_pt1_y,tan_pt2_y],'ro-') 86 | plt.scatter(c2_x,c2_y) 87 | plt.scatter(tan_pt1_x,tan_pt1_y) 88 | plt.scatter(tan_pt2_x,tan_pt2_y) 89 | 90 | # x = np.linspace(-10, 20, 100) 91 | # y = np.linspace(-10, 20, 100) 92 | # X, Y = np.meshgrid(x,y) 93 | # F = (X-c1_x)**2 + (Y-c1_y)**2 - r_min**2 94 | # F1 = (X-c2_x)**2 + (Y-c2_y)**2 - r_min**2 95 | # # plt.contour(X,Y,F,[0]) 96 | # plt.contour(X,Y,F1,[0]) 97 | plt.gca().set_aspect('equal') 98 | plt.show() 99 | # ax.relim() 100 | return ans 101 | 102 | def RSR(): 103 | plt.ylim(-5,15) 104 | plt.xlim(-5,15) 105 | c1_x=current_x+r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 106 | c1_y=current_y+r_min*math.sin(math.radians(current_th)-(PI/2)) 107 | 108 | c2_x=goal_x+r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 109 | c2_y=goal_y+r_min*math.sin(math.radians(goal_th)-(PI/2)) 110 | 111 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 112 | 113 | tan_pt1_x=c1_x-r_min*math.cos(theta) #tangent point in first circle of min_turning radius 114 | tan_pt1_y=c1_y+r_min*math.sin(theta) 115 | 116 | tan_pt2_x=c2_x-r_min*math.cos(theta) #tangent point in second circle of min_turning radius 117 | tan_pt2_y=c2_y+r_min*math.sin(theta) 118 | 119 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 120 | 121 | # print (tan_pt1_x,tan_pt1_y) 122 | # print (tan_pt2_x,tan_pt2_y) 123 | # print (c2_x,c2_y) 124 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(current_x-c1_x,current_y-c1_y) #Initial Turning angle 125 | 126 | Tf = np.arctan2(goal_x - c2_x,goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 127 | 128 | if Ti<0: 129 | Ti+=2*np.pi 130 | if Tf<0: 131 | Tf+=2*np.pi 132 | 133 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 134 | 135 | Ti= Ti*180/np.pi 136 | if Ti<0: 137 | Ti+=360 138 | Tf= Tf*180/np.pi 139 | if Tf<0: 140 | Tf+=360 141 | 142 | # print Ti,Tf,total_dist,S 143 | 144 | start_angle1 = np.arctan2(current_y - c1_y, current_x - c1_x) * 180 / np.pi 145 | end_angle1 = np.arctan2(tan_pt1_y - c1_y, tan_pt1_x - c1_x) * 180 / np.pi 146 | 147 | arc1 = patches.Arc((c1_x, c1_y), 2*r_min,2*r_min,theta1=end_angle1, theta2=start_angle1) 148 | plt.gca().add_patch(arc1) 149 | 150 | start_angle2 = np.arctan2(tan_pt2_y - c2_y, tan_pt2_x - c2_x) * 180 / np.pi 151 | end_angle2 = np.arctan2(goal_y - c2_y, goal_x - c2_x) * 180 / np.pi 152 | 153 | arc2 = patches.Arc((c2_x, c2_y), 2*r_min,2*r_min,theta1=end_angle2, theta2=start_angle2) 154 | plt.gca().add_patch(arc2) 155 | 156 | print start_angle1,end_angle1,start_angle2,end_angle2 157 | 158 | plt.scatter(current_x,current_y) 159 | plt.scatter(goal_x,goal_y) 160 | plt.scatter(c1_x,c1_y) 161 | plt.scatter(c2_x,c2_y) 162 | plt.scatter(tan_pt1_x,tan_pt1_y) 163 | plt.scatter(tan_pt2_x,tan_pt2_y) 164 | plt.plot([tan_pt1_x,tan_pt2_x],[tan_pt1_y,tan_pt2_y],'ro-') 165 | 166 | # x = np.linspace(-10, 20, 100) 167 | # y = np.linspace(-10, 20, 100) 168 | # X, Y = np.meshgrid(x,y) 169 | # F = (X-c1_x)**2 + (Y-c1_y)**2 - r_min**2 170 | # F1 = (X-c2_x)**2 + (Y-c2_y)**2 - r_min**2 171 | # plt.contour(X,Y,F,[0]) 172 | # plt.contour(X,Y,F1,[0]) 173 | plt.gca().set_aspect('equal') 174 | plt.show() 175 | 176 | ans=[Ti,Tf, total_dist,S] 177 | return ans 178 | 179 | def LSR(): 180 | plt.ylim(-5,15) 181 | plt.xlim(-5,15) 182 | c1_x=current_x-r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 183 | c1_y=current_y-r_min*math.sin(math.radians(current_th)-(PI/2)) 184 | 185 | c2_x=goal_x+r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 186 | c2_y=goal_y+r_min*math.sin(math.radians(goal_th)-(PI/2)) 187 | 188 | # if c2_x-c1_x<0: 189 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*r_min)) 190 | # elif c2_x-c1_x>0: 191 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*r_min)) 192 | 193 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 194 | 195 | S = 2*((-r_min**2 + (S1**2)/4.0 )**0.5) 196 | 197 | theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y)) - np.arctan2(r_min,S/2.0) 198 | 199 | 200 | tan_pt1_x=c1_x+r_min*math.cos(theta) #tangent point in first circle of min_turning radius 201 | tan_pt1_y=c1_y-r_min*math.sin(theta) 202 | 203 | tan_pt2_x=c2_x-r_min*math.cos(theta) #tangent point in second circle of min_turning radius 204 | tan_pt2_y=c2_y+r_min*math.sin(theta) 205 | 206 | start_angle1 = np.arctan2(current_y - c1_y, current_x - c1_x) * 180 / np.pi 207 | end_angle1 = np.arctan2(tan_pt1_y - c1_y, tan_pt1_x - c1_x) * 180 / np.pi 208 | 209 | arc1 = patches.Arc((c1_x, c1_y), 2*r_min,2*r_min,theta1=start_angle1, theta2=end_angle1) 210 | plt.gca().add_patch(arc1) 211 | 212 | start_angle2 = np.arctan2(tan_pt2_y - c2_y, tan_pt2_x - c2_x) * 180 / np.pi 213 | end_angle2 = np.arctan2(goal_y - c2_y, goal_x - c2_x) * 180 / np.pi 214 | 215 | arc2 = patches.Arc((c2_x, c2_y), 2*r_min,2*r_min,theta1=end_angle2, theta2=start_angle2) 216 | plt.gca().add_patch(arc2) 217 | 218 | plt.scatter(current_x,current_y) 219 | plt.scatter(goal_x,goal_y) 220 | plt.scatter(c1_x,c1_y) 221 | plt.scatter(c2_x,c2_y) 222 | plt.scatter(tan_pt1_x,tan_pt1_y) 223 | plt.scatter(tan_pt2_x,tan_pt2_y) 224 | plt.plot([tan_pt1_x,tan_pt2_x],[tan_pt1_y,tan_pt2_y],'ro-') 225 | 226 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 227 | 228 | S = 2*((-r_min**2 + (S1**2)/4.0 )**0.5) 229 | print (tan_pt1_x,tan_pt1_y) 230 | print (tan_pt2_x,tan_pt2_y) 231 | print (c1_x,c1_y,c2_x,c2_y) 232 | 233 | Ti = np.arctan2(current_x-c1_x,current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 234 | 235 | Tf = np.arctan2(goal_x - c2_x,goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 236 | 237 | if Ti<0: 238 | Ti+=2*np.pi 239 | if Tf<0: 240 | Tf+=2*np.pi 241 | 242 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 243 | 244 | Ti= Ti*180/np.pi 245 | if Ti<0: 246 | Ti+=360 247 | Tf= Tf*180/np.pi 248 | if Tf<0: 249 | Tf+=360 250 | 251 | print Ti,Tf,total_dist,S,"yooo" 252 | 253 | # x = np.linspace(-10, 20, 100) 254 | # y = np.linspace(-10, 20, 100) 255 | # X, Y = np.meshgrid(x,y) 256 | # F = (X-c1_x)**2 + (Y-c1_y)**2 - r_min**2 257 | # F1 = (X-c2_x)**2 + (Y-c2_y)**2 - r_min**2 258 | # plt.contour(X,Y,F,[0]) 259 | # plt.contour(X,Y,F1,[0]) 260 | plt.gca().set_aspect('equal') 261 | plt.show() 262 | 263 | ans=[Ti,Tf, total_dist,S] 264 | return ans 265 | 266 | def RSL(): 267 | plt.ylim(-5,15) 268 | plt.xlim(-5,15) 269 | c1_x=current_x+r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 270 | c1_y=current_y+r_min*math.sin(math.radians(current_th)-(PI/2)) 271 | 272 | c2_x=goal_x-r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 273 | c2_y=goal_y-r_min*math.sin(math.radians(goal_th)-(PI/2)) 274 | 275 | # if c2_x-c1_x<0: 276 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*r_min)) 277 | # elif c2_x-c1_x>0: 278 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*r_min)) 279 | 280 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 281 | 282 | S = 2*((-r_min**2 + (S1**2)/4.0 )**0.5) 283 | 284 | theta = + (np.arctan2((c2_x-c1_x), c2_y-c1_y)) + np.arctan2(r_min,S/2.0) 285 | 286 | 287 | tan_pt1_x=c1_x-r_min*math.cos(theta) #tangent point in first circle of min_turning radius 288 | tan_pt1_y=c1_y+r_min*math.sin(theta) 289 | 290 | tan_pt2_x=c2_x+r_min*math.cos(theta) #tangent point in second circle of min_turning radius 291 | tan_pt2_y=c2_y-r_min*math.sin(theta) 292 | 293 | plt.scatter(current_x,current_y) 294 | plt.scatter(goal_x,goal_y) 295 | plt.scatter(c1_x,c1_y) 296 | plt.scatter(c2_x,c2_y) 297 | plt.scatter(tan_pt1_x,tan_pt1_y) 298 | plt.scatter(tan_pt2_x,tan_pt2_y) 299 | plt.plot([tan_pt1_x,tan_pt2_x],[tan_pt1_y,tan_pt2_y],'ro-') 300 | 301 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 302 | print (tan_pt1_x,tan_pt1_y) 303 | print (tan_pt2_x,tan_pt2_y) 304 | print (c1_x,c1_y,c2_x,c2_y) 305 | 306 | S = 2*((-r_min**2 + (S1**2)/4.0 )**0.5) 307 | 308 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(current_x-c1_x,current_y-c1_y) #Initial Turning angle 309 | 310 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(goal_x - c2_x,goal_y- c2_y) #Final Turning angle 311 | 312 | if Ti<0: 313 | Ti+=2*np.pi 314 | if Tf<0: 315 | Tf+=2*np.pi 316 | 317 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 318 | 319 | Ti= Ti*180/np.pi 320 | if Ti<0: 321 | Ti+=360 322 | Tf= Tf*180/np.pi 323 | if Tf<0: 324 | Tf+=360 325 | 326 | # print Ti,Tf,total_dist,S 327 | 328 | start_angle1 = np.arctan2(current_y - c1_y, current_x - c1_x) * 180 / np.pi 329 | end_angle1 = np.arctan2(tan_pt1_y - c1_y, tan_pt1_x - c1_x) * 180 / np.pi 330 | 331 | arc1 = patches.Arc((c1_x, c1_y), 2*r_min,2*r_min,theta1=end_angle1, theta2=start_angle1) 332 | plt.gca().add_patch(arc1) 333 | 334 | start_angle2 = np.arctan2(tan_pt2_y - c2_y, tan_pt2_x - c2_x) * 180 / np.pi 335 | end_angle2 = np.arctan2(goal_y - c2_y, goal_x - c2_x) * 180 / np.pi 336 | 337 | arc2 = patches.Arc((c2_x, c2_y), 2*r_min,2*r_min,theta1=start_angle2, theta2=end_angle2) 338 | plt.gca().add_patch(arc2) 339 | 340 | # x = np.linspace(-10, 20, 100) 341 | # y = np.linspace(-10, 20, 100) 342 | # X, Y = np.meshgrid(x,y) 343 | # F = (X-c1_x)**2 + (Y-c1_y)**2 - r_min**2 344 | # F1 = (X-c2_x)**2 + (Y-c2_y)**2 - r_min**2 345 | # plt.contour(X,Y,F,[0]) 346 | # plt.contour(X,Y,F1,[0]) 347 | plt.gca().set_aspect('equal') 348 | plt.show() 349 | 350 | ans=[Ti,Tf, total_dist,S] 351 | return ans 352 | 353 | 354 | paths=[LSL,RSR,LSR,RSL] 355 | distance=sys.maxint 356 | ans=[] 357 | count=0 358 | ans_count=1 359 | path="" 360 | for i in paths: 361 | path_ans=i() 362 | count+=1 363 | if distance>path_ans[-2]: 364 | ans=path_ans 365 | distance=path_ans[-2] 366 | ans_count=count 367 | 368 | # print "Optimal Path: " ,paths[ans_count-1], "Total distance to cover: ",distance 369 | # LSL() 370 | # RSR() 371 | # LSR() 372 | # RSL() 373 | 374 | -------------------------------------------------------------------------------- /dubins/gazebo_dubin.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import getch 3 | import roslib; roslib.load_manifest('p3dx_mover') 4 | import rospy 5 | import numpy as np 6 | import math 7 | import sys 8 | 9 | from nav_msgs.msg import Odometry 10 | 11 | from geometry_msgs.msg import Twist 12 | 13 | class A(): 14 | def __init__(self): 15 | rospy.init_node('odometry') 16 | odom_sub=rospy.Subscriber('/odom',Odometry, self.callback) 17 | self.rate = rospy.Rate(10) 18 | self.PI=np.pi 19 | 20 | def callback(self, msg): 21 | self.current_x = round(msg.pose.pose.position.x, 4) 22 | self.current_y = round(msg.pose.pose.position.y, 4) 23 | self.q_w = round(msg.pose.pose.orientation.w ,4) 24 | self.q_x = round(msg.pose.pose.orientation.x ,4) 25 | self.q_y = round(msg.pose.pose.orientation.y ,4) 26 | self.q_z = round(msg.pose.pose.orientation.z ,4) 27 | siny = +2.0 * (self.q_w * self.q_z + self.q_x * self.q_y); 28 | cosy = +1.0 - 2.0 * (self.q_y * self.q_y + self.q_z * self.q_z); 29 | yaw = math.atan2(siny, cosy); 30 | self.current_th = yaw 31 | 32 | def LSL(self): 33 | c1_x=self.current_x-self.r_min*math.cos((self.current_th)-(self.PI/2)) #center of first circle of min_turning radius 34 | c1_y=self.current_y-self.r_min*math.sin((self.current_th)-(self.PI/2)) 35 | 36 | c2_x=self.goal_x-self.r_min*math.cos(math.radians(self.goal_th)-(self.PI/2)) #center of second circle of min_turning radius 37 | c2_y=self.goal_y-self.r_min*math.sin(math.radians(self.goal_th)-(self.PI/2)) 38 | #print c1_x,c1_y,c2_x,c2_y, c2_y-c1_y, c2_x-c1_x 39 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 40 | 41 | tan_pt1_x=c1_x+self.r_min*math.cos(theta) #tangent point in first circle of min_turning radius 42 | tan_pt1_y=c1_y-self.r_min*math.sin(theta) 43 | 44 | tan_pt2_x=c2_x+self.r_min*math.cos(theta) #tangent point in second circle of min_turning radius 45 | tan_pt2_y=c2_y-self.r_min*math.sin(theta) 46 | 47 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 48 | 49 | Ti = np.arctan2(self.current_x-c1_x,self.current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 50 | 51 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(self.goal_x - c2_x,self.goal_y- c2_y) #Final Turning angle 52 | 53 | if Ti<0: 54 | Ti+=2*np.pi 55 | if Tf<0: 56 | Tf+=2*np.pi 57 | 58 | total_dist = Ti*self.r_min + Tf*self.r_min + S #Total Distance from current to Goal 59 | 60 | #Ti= Ti*180/np.pi 61 | ans=[Ti,Tf, c1_x,c2_x,c1_y,c2_y,tan_pt1_x,tan_pt2_x,tan_pt1_y,tan_pt2_y,1,1,total_dist,S] 62 | 63 | return ans 64 | 65 | def RSR(self): 66 | c1_x=self.current_x+self.r_min*math.cos((self.current_th)-(self.PI/2)) #center of first circle of min_turning radius 67 | c1_y=self.current_y+self.r_min*math.sin((self.current_th)-(self.PI/2)) 68 | 69 | c2_x=self.goal_x+self.r_min*math.cos(math.radians(self.goal_th)-(self.PI/2)) #center of second circle of min_turning radius 70 | c2_y=self.goal_y+self.r_min*math.sin(math.radians(self.goal_th)-(self.PI/2)) 71 | 72 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 73 | 74 | tan_pt1_x=c1_x-self.r_min*math.cos(theta) #tangent point in first circle of min_turning radius 75 | tan_pt1_y=c1_y+self.r_min*math.sin(theta) 76 | 77 | tan_pt2_x=c2_x-self.r_min*math.cos(theta) #tangent point in second circle of min_turning radius 78 | tan_pt2_y=c2_y+self.r_min*math.sin(theta) 79 | 80 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 81 | 82 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(self.current_x-c1_x,self.current_y-c1_y) #Initial Turning angle 83 | 84 | Tf = np.arctan2(self.goal_x - c2_x,self.goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 85 | 86 | if Ti<0: 87 | Ti+=2*np.pi 88 | if Tf<0: 89 | Tf+=2*np.pi 90 | 91 | total_dist = Ti*self.r_min + Tf*self.r_min + S #Total Distance from current to Goal 92 | 93 | #Ti= Ti*180/np.pi 94 | 95 | ans=[Ti,Tf, c1_x,c2_x,c1_y,c2_y,tan_pt1_x,tan_pt2_x,tan_pt1_y,tan_pt2_y,-1,-1,total_dist,S] 96 | return ans 97 | 98 | def LSR(self): 99 | c1_x=self.current_x-self.r_min*math.cos((self.current_th)-(self.PI/2)) #center of first circle of min_turning radius 100 | c1_y=self.current_y-self.r_min*math.sin((self.current_th)-(self.PI/2)) 101 | 102 | c2_x=self.goal_x+self.r_min*math.cos(math.radians(self.goal_th)-(self.PI/2)) #center of second circle of min_turning radius 103 | c2_y=self.goal_y+self.r_min*math.sin(math.radians(self.goal_th)-(self.PI/2)) 104 | 105 | if c2_x-c1_x<0: 106 | theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*self.r_min)) 107 | elif c2_x-c1_x>0: 108 | theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*self.r_min)) 109 | 110 | tan_pt1_x=c1_x+self.r_min*math.cos(theta) #tangent point in first circle of min_turning radius 111 | tan_pt1_y=c1_y-self.r_min*math.sin(theta) 112 | 113 | tan_pt2_x=c2_x-self.r_min*math.cos(theta) #tangent point in second circle of min_turning radius 114 | tan_pt2_y=c2_y+self.r_min*math.sin(theta) 115 | 116 | 117 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 118 | 119 | S = 2*((-self.r_min**2 + (S1**2)/4.0 )**0.5) 120 | 121 | Ti = np.arctan2(self.current_x-c1_x,self.current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 122 | 123 | Tf = np.arctan2(self.goal_x - c2_x,self.goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 124 | 125 | if Ti<0: 126 | Ti+=2*np.pi 127 | if Tf<0: 128 | Tf+=2*np.pi 129 | 130 | total_dist = Ti*self.r_min + Tf*self.r_min + S #Total Distance from current to Goal 131 | 132 | #Ti= Ti*180/np.pi 133 | 134 | ans=[Ti,Tf, c1_x,c2_x,c1_y,c2_y,tan_pt1_x,tan_pt2_x,tan_pt1_y,tan_pt2_y,1,-1,total_dist,S] 135 | return ans 136 | 137 | def RSL(self): 138 | c1_x=self.current_x+self.r_min*math.cos((self.current_th)-(self.PI/2)) #center of first circle of min_turning radius 139 | c1_y=self.current_y+self.r_min*math.sin((self.current_th)-(self.PI/2)) 140 | 141 | c2_x=self.goal_x-self.r_min*math.cos(math.radians(self.goal_th)-(self.PI/2)) #center of second circle of min_turning radius 142 | c2_y=self.goal_y-self.r_min*math.sin(math.radians(self.goal_th)-(self.PI/2)) 143 | 144 | if c2_x-c1_x<0: 145 | theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*self.r_min)) 146 | elif c2_x-c1_x>0: 147 | theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*self.r_min)) 148 | 149 | tan_pt1_x=c1_x-self.r_min*math.cos(theta) #tangent point in first circle of min_turning radius 150 | tan_pt1_y=c1_y+self.r_min*math.sin(theta) 151 | 152 | tan_pt2_x=c2_x+self.r_min*math.cos(theta) #tangent point in second circle of min_turning radius 153 | tan_pt2_y=c2_y-self.r_min*math.sin(theta) 154 | 155 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 156 | 157 | S = 2*((-self.r_min**2 + (S1**2)/4.0 )**0.5) 158 | 159 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(self.current_x-c1_x,self.current_y-c1_y) #Initial Turning angle 160 | 161 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(self.goal_x - c2_x,self.goal_y- c2_y) #Final Turning angle 162 | 163 | if Ti<0: 164 | Ti+=2*np.pi 165 | if Tf<0: 166 | Tf+=2*np.pi 167 | 168 | total_dist = Ti*self.r_min + Tf*self.r_min + S #Total Distance from current to Goal 169 | 170 | #Ti= Ti*180/np.pi 171 | 172 | ans=[Ti,Tf, c1_x,c2_x,c1_y,c2_y,tan_pt1_x,tan_pt2_x,tan_pt1_y,tan_pt2_y,-1,1,total_dist,S] 173 | return ans 174 | def run(self): 175 | self.goal_x = input("x goal:") 176 | self.goal_y = input("y goal:") 177 | self.goal_th = input("theta goal: (Input in Degrees )") 178 | 179 | pub = rospy.Publisher('cmd_vel', Twist, queue_size=10) 180 | #rospy.init_node('p3dx_mover') 181 | 182 | twist = Twist() 183 | 184 | self.r_min = 2 185 | 186 | #samp=self.LSL() 187 | 188 | 189 | 190 | paths=[self.LSL,self.RSR,self.LSR,self.RSL] 191 | distance=sys.maxint 192 | ans=[] 193 | count=0 194 | ans_count=1 195 | path="" 196 | for i in paths: 197 | path_ans=i() 198 | count+=1 199 | if distance>path_ans[-2]: 200 | ans=path_ans 201 | distance=path_ans[-2] 202 | ans_count=count 203 | 204 | #print "Optimal Path: " ,paths[ans_count-1], "Total distance to cover: ",distance 205 | ans=self.LSL() #SELECT THE PATH HERE 206 | Ti=ans[0] 207 | Tf=ans[1] 208 | c1_x=ans[2] 209 | c2_x=ans[3] 210 | c1_y=ans[4] 211 | c2_y=ans[5] 212 | tan_pt1_x=ans[6] 213 | tan_pt2_x=ans[7] 214 | tan_pt1_y=ans[8] 215 | tan_pt2_y=ans[9] 216 | LoR1=ans[10] 217 | LoR2=ans[11] 218 | # c1_x=self.current_x-r_min*cos(self.current_th-self.PI/2) 219 | # c1_y=self.current_y-r_min*sin(self.current_th-self.PI/2) 220 | 221 | # c2_x=goal_x-r_min*cos(goal_th-self.PI/2) 222 | # c2_y=goal_y-r_min*sin(goal_th-self.PI/2) 223 | 224 | # theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 225 | 226 | # tan_pt1_x=c1_x+r_min*cos(theta) 227 | # tan_pt1_y=c1_y-r_min*sin(theta) 228 | 229 | # tan_pt2_x=c2_x+r_min*cos(theta) 230 | # tan_pt2_y=c2_y-r_min*sin(theta) 231 | 232 | 233 | # Ti = np.arctan2(self.current_x-c1_x,self.current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) 234 | 235 | # Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(goal_x - c2_x,goal_y- c2_y) 236 | 237 | # if Ti<0: 238 | # Ti+=2*np.pi 239 | # if Tf<0: 240 | # Tf+=2*np.pi 241 | 242 | # print c1_x,c1_y,tan_pt1_x,tan_pt1_y, tan_pt2_x,tan_pt2_y,Ti,Tf 243 | print ans 244 | dele=input("checkpoint (Enter any Number to continue)"); 245 | print "initial configuration: ", self.current_x,self.current_y,self.current_th*180/self.PI 246 | 247 | linear_speed=0.2 248 | angular_speed=0.2/(self.r_min/0.808) 249 | 250 | twist.linear.x = 0 251 | twist.linear.y =0 252 | twist.linear.z =0 253 | twist.angular.x = 0 254 | twist.angular.y = 0 255 | twist.angular.z = 0 256 | 257 | twist.linear.x = abs(linear_speed) 258 | if LoR1==1: 259 | twist.angular.z = abs(angular_speed) 260 | elif LoR1==-1: 261 | twist.angular.z = -abs(angular_speed) 262 | 263 | # print ang_total 264 | 265 | err = math.sqrt( math.pow((tan_pt1_x-self.current_x),2) + math.pow((tan_pt1_y-self.current_y),2) ) 266 | 267 | """while (err>0.01): 268 | pub.publish(twist) 269 | print self.current_x,",",self.current_y, "error", err 270 | err = sqrt( pow((tan_pt1_x-self.current_x),2) + pow((tan_pt1_y-self.current_y),2) ) 271 | self.rate.sleep()""" 272 | errorf= math.sqrt( math.pow((tan_pt1_x-self.current_x),2) + math.pow((tan_pt1_y-self.current_y),2) ) 273 | curr_x = self.current_x 274 | curr_y = self.current_y 275 | curr_yaw=self.current_th 276 | error_angle=(curr_yaw+Ti)*180/self.PI 277 | t1=rospy.get_time() 278 | while (errorf>0.1): 279 | pub.publish(twist) 280 | curr_x = self.current_x 281 | curr_y = self.current_y 282 | error_angle-=self.current_th 283 | print curr_x,",",curr_y, "error", errorf, error_angle 284 | errorf= math.sqrt( math.pow((tan_pt1_x-curr_x),2) + math.pow((tan_pt1_y-curr_y),2) ) 285 | self.rate.sleep() 286 | 287 | twist.linear.x =0 288 | twist.angular.z = 0 289 | pub.publish(twist) 290 | 291 | print "configuration after first curve: ", self.current_x,self.current_y,self.current_th 292 | dele=input("checkpoint (Enter any Number to continue)"); 293 | #exit() 294 | 295 | twist.linear.x = linear_speed 296 | 297 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 298 | distance=S*0.808#7.107#sqrt( pow((tan_pt2_x-tan_pt1_x),2) + pow((tan_pt2_y-tan_pt1_y),2) ) 299 | distance_covered=0 300 | t1=rospy.get_time() 301 | while (distance_covered0.1): 330 | pub.publish(twist) 331 | err_goal = math.sqrt( math.pow((self.goal_x-self.current_x),2) + math.pow((self.goal_y-self.current_y),2) ) 332 | if err_goal<0.5: 333 | break 334 | print self.current_x,",",self.current_y, "error", err_goal,errorf 335 | errorf= math.sqrt( math.pow((tan_pt2_x-self.current_x),2) + math.pow((tan_pt2_x-self.current_y),2) ) 336 | self.rate.sleep() 337 | 338 | twist.linear.x =0 339 | twist.angular.z = 0 340 | pub.publish(twist) 341 | 342 | print "final configuration: ", self.current_x,self.current_y,self.current_th 343 | 344 | 345 | a=A() 346 | a.run() -------------------------------------------------------------------------------- /rrt_dubins/Minkowski.py: -------------------------------------------------------------------------------- 1 | def minkowski_sum_circle(robot_radius,obstacle_radius): 2 | obstacle_radius+=2*robot_radius 3 | return obstacle_radius 4 | 5 | 6 | def minkowski_sum_square(robot_radius,obstacle_width): 7 | obstacle_width+=4*robot_radius 8 | return obstacle_width -------------------------------------------------------------------------------- /rrt_dubins/gazebo_rrt_dubins.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import getch 3 | import roslib; roslib.load_manifest('p3dx_mover') 4 | import rospy 5 | import numpy as np 6 | import math 7 | import random 8 | import sys 9 | import copy 10 | 11 | from nav_msgs.msg import Odometry 12 | 13 | from geometry_msgs.msg import Twist 14 | 15 | class Node(): 16 | def __init__(self,x,y): 17 | self.x=x 18 | self.y=y 19 | self.p=None 20 | self.tan_pt1_x=None 21 | self.tan_pt1_y=None 22 | self.tan_pt2_x=None 23 | self.tan_pt2_y=None 24 | self.Ti=None 25 | self.S=None 26 | 27 | 28 | class A(): 29 | def __init__(self): 30 | rospy.init_node('odometry') 31 | odom_sub=rospy.Subscriber('/odom',Odometry, self.callback) 32 | self.rate = rospy.Rate(10) 33 | self.PI=np.pi 34 | 35 | def callback(self, msg): 36 | self.current_x = round(msg.pose.pose.position.x, 4) 37 | self.current_y = round(msg.pose.pose.position.y, 4) 38 | self.q_w = round(msg.pose.pose.orientation.w ,4) 39 | self.q_x = round(msg.pose.pose.orientation.x ,4) 40 | self.q_y = round(msg.pose.pose.orientation.y ,4) 41 | self.q_z = round(msg.pose.pose.orientation.z ,4) 42 | siny = +2.0 * (self.q_w * self.q_z + self.q_x * self.q_y); 43 | cosy = +1.0 - 2.0 * (self.q_y * self.q_y + self.q_z * self.q_z); 44 | yaw = math.atan2(siny, cosy); 45 | self.current_th = yaw 46 | 47 | def LSL(self,current_x,current_y,current_th,goal_x,goal_y,goal_th): 48 | c1_x=current_x-self.r_min*math.cos((current_th)-(self.PI/2)) #center of first circle of min_turning radius 49 | c1_y=current_y-self.r_min*math.sin((current_th)-(self.PI/2)) 50 | 51 | c2_x=goal_x-self.r_min*math.cos(math.radians(goal_th)-(self.PI/2)) #center of second circle of min_turning radius 52 | c2_y=goal_y-self.r_min*math.sin(math.radians(goal_th)-(self.PI/2)) 53 | #print c1_x,c1_y,c2_x,c2_y, c2_y-c1_y, c2_x-c1_x 54 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 55 | 56 | tan_pt1_x=c1_x+self.r_min*math.cos(theta) #tangent point in first circle of min_turning radius 57 | tan_pt1_y=c1_y-self.r_min*math.sin(theta) 58 | 59 | tan_pt2_x=c2_x+self.r_min*math.cos(theta) #tangent point in second circle of min_turning radius 60 | tan_pt2_y=c2_y-self.r_min*math.sin(theta) 61 | 62 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 63 | 64 | Ti = np.arctan2(current_x-c1_x,current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 65 | 66 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(goal_x - c2_x,goal_y- c2_y) #Final Turning angle 67 | 68 | if Ti<0: 69 | Ti+=2*np.pi 70 | if Tf<0: 71 | Tf+=2*np.pi 72 | 73 | total_dist = Ti*self.r_min + Tf*self.r_min + S #Total Distance from current to Goal 74 | 75 | #Ti= Ti*180/np.pi 76 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"LL"] 77 | 78 | return ans 79 | 80 | def RSR(self,current_x,current_y,current_th,goal_x,goal_y,goal_th): 81 | c1_x=current_x+self.r_min*math.cos((current_th)-(self.PI/2)) #center of first circle of min_turning radius 82 | c1_y=current_y+self.r_min*math.sin((current_th)-(self.PI/2)) 83 | 84 | c2_x=goal_x+self.r_min*math.cos(math.radians(goal_th)-(self.PI/2)) #center of second circle of min_turning radius 85 | c2_y=goal_y+self.r_min*math.sin(math.radians(goal_th)-(self.PI/2)) 86 | 87 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 88 | 89 | tan_pt1_x=c1_x-self.r_min*math.cos(theta) #tangent point in first circle of min_turning radius 90 | tan_pt1_y=c1_y+self.r_min*math.sin(theta) 91 | 92 | tan_pt2_x=c2_x-self.r_min*math.cos(theta) #tangent point in second circle of min_turning radius 93 | tan_pt2_y=c2_y+self.r_min*math.sin(theta) 94 | 95 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 96 | 97 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(current_x-c1_x,current_y-c1_y) #Initial Turning angle 98 | 99 | Tf = np.arctan2(goal_x - c2_x,goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 100 | 101 | if Ti<0: 102 | Ti+=2*np.pi 103 | if Tf<0: 104 | Tf+=2*np.pi 105 | 106 | total_dist = Ti*self.r_min + Tf*self.r_min + S #Total Distance from current to Goal 107 | 108 | #Ti= Ti*180/np.pi 109 | 110 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"RR"] 111 | return ans 112 | 113 | def LSR(self,current_x,current_y,current_th,goal_x,goal_y,goal_th): 114 | c1_x=current_x-self.r_min*math.cos((current_th)-(self.PI/2)) #center of first circle of min_turning radius 115 | c1_y=current_y-self.r_min*math.sin((current_th)-(self.PI/2)) 116 | 117 | c2_x=goal_x+self.r_min*math.cos(math.radians(goal_th)-(self.PI/2)) #center of second circle of min_turning radius 118 | c2_y=goal_y+self.r_min*math.sin(math.radians(goal_th)-(self.PI/2)) 119 | 120 | if c2_x-c1_x<0: 121 | theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*self.r_min)) 122 | elif c2_x-c1_x>0: 123 | theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*self.r_min)) 124 | 125 | tan_pt1_x=c1_x+self.r_min*math.cos(theta) #tangent point in first circle of min_turning radius 126 | tan_pt1_y=c1_y-self.r_min*math.sin(theta) 127 | 128 | tan_pt2_x=c2_x-self.r_min*math.cos(theta) #tangent point in second circle of min_turning radius 129 | tan_pt2_y=c2_y+self.r_min*math.sin(theta) 130 | 131 | 132 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 133 | 134 | if(-self.r_min**2 + (S1**2)/4.0 )<0: 135 | # print"Holaaaaaa" 136 | return None 137 | 138 | S = 2*((-self.r_min**2 + (S1**2)/4.0 )**0.5) 139 | 140 | Ti = np.arctan2(current_x-c1_x,current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 141 | 142 | Tf = np.arctan2(goal_x - c2_x,goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 143 | 144 | if Ti<0: 145 | Ti+=2*np.pi 146 | if Tf<0: 147 | Tf+=2*np.pi 148 | 149 | total_dist = Ti*self.r_min + Tf*self.r_min + S #Total Distance from current to Goal 150 | 151 | #Ti= Ti*180/np.pi 152 | 153 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"LR"] 154 | return ans 155 | 156 | def RSL(self,current_x,current_y,current_th,goal_x,goal_y,goal_th): 157 | c1_x=current_x+self.r_min*math.cos((current_th)-(self.PI/2)) #center of first circle of min_turning radius 158 | c1_y=current_y+self.r_min*math.sin((current_th)-(self.PI/2)) 159 | 160 | c2_x=goal_x-self.r_min*math.cos(math.radians(goal_th)-(self.PI/2)) #center of second circle of min_turning radius 161 | c2_y=goal_y-self.r_min*math.sin(math.radians(goal_th)-(self.PI/2)) 162 | 163 | if c2_x-c1_x<0: 164 | theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*self.r_min)) 165 | elif c2_x-c1_x>0: 166 | theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*self.r_min)) 167 | 168 | tan_pt1_x=c1_x-self.r_min*math.cos(theta) #tangent point in first circle of min_turning radius 169 | tan_pt1_y=c1_y+self.r_min*math.sin(theta) 170 | 171 | tan_pt2_x=c2_x+self.r_min*math.cos(theta) #tangent point in second circle of min_turning radius 172 | tan_pt2_y=c2_y-self.r_min*math.sin(theta) 173 | 174 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 175 | 176 | if(-self.r_min**2 + (S1**2)/4.0 )<0: 177 | # print"Holaaaaaa" 178 | return None 179 | 180 | S = 2*((-self.r_min**2 + (S1**2)/4.0 )**0.5) 181 | 182 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(current_x-c1_x,current_y-c1_y) #Initial Turning angle 183 | 184 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(goal_x - c2_x,goal_y- c2_y) #Final Turning angle 185 | 186 | if Ti<0: 187 | Ti+=2*np.pi 188 | if Tf<0: 189 | Tf+=2*np.pi 190 | 191 | total_dist = Ti*self.r_min + Tf*self.r_min + S #Total Distance from current to Goal 192 | 193 | #Ti= Ti*180/np.pi 194 | 195 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"RL"] 196 | # 0. 1. 2 3. 4. 5. 6. 7. 8. 9. 10. 11 12 197 | return ans 198 | 199 | def findNearest(self,sample,tree): 200 | min_d = sys.maxint 201 | ind=0 202 | min_node=tree[0] 203 | for indd,node in enumerate(tree): 204 | dis = ((node.x-sample.x)**2 + (node.y-sample.y)**2)**0.5 205 | if min_d>dis: 206 | min_d=dis 207 | min_node=node 208 | ind=indd 209 | return min_node,ind 210 | 211 | def run(self): 212 | self.goal_x = input("x goal:") 213 | self.goal_y = input("y goal:") 214 | self.goal_th = input("theta goal: (Input in Degrees )") 215 | 216 | current_x=0 217 | current_y=0 218 | current_th=0 219 | 220 | pub = rospy.Publisher('cmd_vel', Twist, queue_size=10) 221 | #rospy.init_node('p3dx_mover') 222 | 223 | step=5 224 | 225 | twist = Twist() 226 | 227 | self.r_min = 1 228 | 229 | #samp=self.LSL() 230 | 231 | tree = [Node(current_x,current_y)] 232 | 233 | initial=[] 234 | 235 | j=0 236 | while True: 237 | j+=1 238 | sample = Node(random.uniform(0,11),random.uniform(0,11)) 239 | nearestNode,ind = self.findNearest(sample,tree) 240 | newNode = copy.deepcopy(nearestNode) 241 | 242 | theta = (np.arctan2(sample.y-nearestNode.y, sample.x-nearestNode.x)) 243 | newNode.x+=step*math.cos(theta) 244 | newNode.y+=step*math.sin(theta) 245 | 246 | theta_start = theta 247 | theta_end = (np.arctan2(newNode.y-self.goal_y, newNode.x-self.goal_x)) 248 | 249 | 250 | paths=[self.LSL,self.RSR,self.LSR,self.RSL] 251 | minS=sys.maxint 252 | ans=[] 253 | count=0 254 | curve="" 255 | ans_count=1 256 | for i in paths: 257 | path_ans=i(nearestNode.x,nearestNode.y,theta_start,newNode.x,newNode.y,theta_end) 258 | count+=1 259 | if path_ans: 260 | if minS>path_ans[3]: 261 | ans=path_ans 262 | minS=path_ans[3] 263 | ans_count=count 264 | curve=path_ans[12] 265 | 266 | newNode.p=ind 267 | nearestNode.tan_pt1_x=ans[4] 268 | nearestNode.tan_pt1_y=ans[5] 269 | nearestNode.tan_pt2_x=ans[6] 270 | nearestNode.tan_pt2_y=ans[7] 271 | nearestNode.S=ans[3] 272 | nearestNode.curve=curve 273 | nearestNode.Ti=ans[0] 274 | 275 | if j==1: 276 | initial.append([ans[4],ans[5],ans[6],ans[7],ans[3],curve]) 277 | 278 | # if ((newNode.x-3)**2 + (newNode.y-3)**2)**0.5 < 1: 279 | # continue 280 | # if ((newNode.x-6)**2 + (newNode.y-5)**2)**0.5 < 1: 281 | # continue 282 | # if newNode.x>10 or newNode.y>10 or newNode.x<0 or newNode.y<0: 283 | # continue 284 | tree.append(newNode) 285 | if ((newNode.x-self.goal_x)**2 + (newNode.y-self.goal_y)**2)**0.5 < step: 286 | break 287 | 288 | paths=[self.LSL,self.RSR,self.LSR,self.RSL] 289 | minS=sys.maxint 290 | ans=[] 291 | count=0 292 | ans_count=1 293 | for i in paths: 294 | path_ans=i(newNode.x,newNode.y,0,self.goal_x,self.goal_y,self.goal_th) 295 | count+=1 296 | if path_ans: 297 | if minS>path_ans[3]: 298 | ans=path_ans 299 | minS=path_ans[3] 300 | ans_count=count 301 | 302 | newNode.tan_pt1_x=ans[4] 303 | newNode.tan_pt1_y=ans[5] 304 | newNode.tan_pt2_x=ans[6] 305 | newNode.tan_pt2_y=ans[7] 306 | newNode.S=ans[3] 307 | newNode.curve=curve 308 | newNode.Ti=ans[0] 309 | 310 | 311 | # path=[[newNode.x,newNode.y]] 312 | # while True: 313 | # if not newNode.p: 314 | # break 315 | # path.append([tree[newNode.p].x,tree[newNode.p].y]) 316 | # newNode=tree[newNode.p] 317 | 318 | path_w_angles=[[newNode.tan_pt1_x,newNode.tan_pt1_y,newNode.tan_pt2_x,newNode.tan_pt2_y,newNode.S,newNode.curve,newNode.Ti]] 319 | while True: 320 | if not newNode.p: 321 | break 322 | path_w_angles.append([tree[newNode.p].tan_pt1_x,tree[newNode.p].tan_pt1_y,tree[newNode.p].tan_pt2_x,tree[newNode.p].tan_pt2_y,tree[newNode.p].S,tree[newNode.p].curve,tree[newNode.p].Ti]) 323 | newNode=tree[newNode.p] 324 | 325 | path_w_angles.append(initial[0]) 326 | 327 | 328 | # x=[self.goal_x] 329 | # y=[self.goal_y] 330 | # for i in path: 331 | # x.append(i[0]) 332 | # y.append(i[1]) 333 | # x.append(current_x) 334 | # y.append(current_y) 335 | # print x,y 336 | 337 | 338 | for ans in path_w_angles: 339 | 340 | tan_pt1_x=ans[0] 341 | tan_pt1_y=ans[1] 342 | tan_pt2_x=ans[2] 343 | tan_pt2_y=ans[3] 344 | S = ans[4] 345 | LoR1=ans[5][0] 346 | LoR2=ans[5][1] 347 | 348 | dele=input("checkpoint"); 349 | print "initial configuration: ", self.current_x,self.current_y,self.current_th*180/self.PI 350 | 351 | linear_speed=0.2 352 | angular_speed=0.2/(self.r_min/0.808) 353 | 354 | twist.linear.x = 0 355 | twist.linear.y =0 356 | twist.linear.z =0 357 | twist.angular.x = 0 358 | twist.angular.y = 0 359 | twist.angular.z = 0 360 | 361 | twist.linear.x = abs(linear_speed) 362 | if LoR1=='L': 363 | twist.angular.z = abs(angular_speed) 364 | elif LoR1=='R': 365 | twist.angular.z = -abs(angular_speed) 366 | 367 | 368 | errorf= math.sqrt( math.pow((tan_pt1_x-self.current_x),2) + math.pow((tan_pt1_y-self.current_y),2) ) 369 | curr_x = self.current_x 370 | curr_y = self.current_y 371 | curr_yaw=self.current_th 372 | 373 | t1=rospy.get_time() 374 | while (errorf>0.1): 375 | pub.publish(twist) 376 | curr_x = self.current_x 377 | curr_y = self.current_y 378 | print curr_x,",",curr_y, "error", errorf 379 | errorf= math.sqrt( math.pow((tan_pt1_x-curr_x),2) + math.pow((tan_pt1_y-curr_y),2) ) 380 | self.rate.sleep() 381 | 382 | twist.linear.x =0 383 | twist.angular.z = 0 384 | pub.publish(twist) 385 | 386 | print "configuration after first curve: ", self.current_x,self.current_y,self.current_th 387 | dele=input("checkpoint"); 388 | 389 | twist.linear.x = linear_speed 390 | 391 | distance=S*0.808#7.107#sqrt( pow((tan_pt2_x-tan_pt1_x),2) + pow((tan_pt2_y-tan_pt1_y),2) ) 392 | distance_covered=0 393 | t1=rospy.get_time() 394 | while (distance_covered0.1): 423 | pub.publish(twist) 424 | err_goal = math.sqrt( math.pow((self.goal_x-self.current_x),2) + math.pow((self.goal_y-self.current_y),2) ) 425 | if err_goal<0.5: 426 | break 427 | print self.current_x,",",self.current_y, "error", err_goal,errorf 428 | errorf= math.sqrt( math.pow((tan_pt2_x-self.current_x),2) + math.pow((tan_pt2_x-self.current_y),2) ) 429 | self.rate.sleep() 430 | 431 | twist.linear.x =0 432 | twist.angular.z = 0 433 | pub.publish(twist) 434 | 435 | print "final configuration: ", self.current_x,self.current_y,self.current_th 436 | 437 | 438 | a=A() 439 | a.run() -------------------------------------------------------------------------------- /rrt_dubins/rrt_dubin.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import matplotlib.patches as patches 4 | import math 5 | import Minkowski 6 | import random 7 | from matplotlib.patches import Arc 8 | import sys 9 | # import dubins 10 | import copy 11 | 12 | class Node(): 13 | def __init__(self,x,y): 14 | self.x=x 15 | self.y=y 16 | self.p=None 17 | self.tan_pt1_x=None 18 | self.tan_pt1_y=None 19 | self.tan_pt2_x=None 20 | self.tan_pt2_y=None 21 | self.Ti=None 22 | self.S=None 23 | 24 | plt.axis([-20, 20, -20, 20]) 25 | # rectangle = plt.Rectangle((0.2, 0.2), 0.2, 0.2) 26 | # plt.gca().add_patch(rectangle) 27 | 28 | obs = plt.Circle((3, 3), radius=1, fc='r') 29 | plt.gca().add_patch(obs) 30 | 31 | obs = plt.Circle((6, 5), radius=1, fc='r') 32 | plt.gca().add_patch(obs) 33 | 34 | # obs = plt.Circle((7, 3), radius=1, fc='r') 35 | # plt.gca().add_patch(obs) 36 | 37 | # obs = plt.Circle((6, 5), radius=1, fc='r') 38 | # plt.gca().add_patch(obs) 39 | 40 | rectangle = plt.Rectangle((0, 0), 1, 1,fill=False) 41 | plt.gca().add_patch(rectangle) 42 | 43 | 44 | current_x=0 45 | current_y=0 46 | current_th=0 #current angle in Degrees 47 | 48 | PI=3.14 49 | r_min=0.2 50 | step=2 51 | 52 | goal_x=8 53 | goal_y=8 54 | goal_th=0 #goal angle in Degrees 55 | 56 | goal_circle = plt.Circle((8, 8), radius=1, fc='y') 57 | plt.gca().add_patch(goal_circle) 58 | 59 | plt.axis('scaled') 60 | # goal_circle = 61 | #plt.show() 62 | 63 | tree = [Node(current_x,current_y)] 64 | 65 | def check_obstacle(x,y,obs_x,obs_y): 66 | obstacle_radius = Minkowski.minkowski_sum_circle(0.2,0.6) 67 | if ((x-obs_x)**2 + (y-obs_y)**2)**0.5 < obstacle_radius+0.5: 68 | return True 69 | return False 70 | def LSL(current_x,current_y,current_th,goal_x,goal_y,goal_th): 71 | c1_x=current_x-r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 72 | c1_y=current_y-r_min*math.sin(math.radians(current_th)-(PI/2)) 73 | 74 | c2_x=goal_x-r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 75 | c2_y=goal_y-r_min*math.sin(math.radians(goal_th)-(PI/2)) 76 | #print c1_x,c1_y,c2_x,c2_y, c2_y-c1_y, c2_x-c1_x 77 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 78 | 79 | tan_pt1_x=c1_x+r_min*math.cos(theta) #tangent point in first circle of min_turning radius 80 | tan_pt1_y=c1_y-r_min*math.sin(theta) 81 | 82 | tan_pt2_x=c2_x+r_min*math.cos(theta) #tangent point in second circle of min_turning radius 83 | tan_pt2_y=c2_y-r_min*math.sin(theta) 84 | 85 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 86 | # print S,theta 87 | # print (tan_pt1_x,tan_pt1_y) 88 | # print (tan_pt2_x,tan_pt2_y) 89 | # print (c1_x,c1_y,c2_x,c2_y) 90 | 91 | Ti = np.arctan2(current_x-c1_x,current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 92 | 93 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(goal_x - c2_x,goal_y- c2_y) #Final Turning angle 94 | 95 | if Ti<0: 96 | Ti+=2*np.pi 97 | if Tf<0: 98 | Tf+=2*np.pi 99 | 100 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 101 | 102 | # Ti= Ti*180/np.pi 103 | 104 | # print Ti,Tf, total_dist,S 105 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"LL"] 106 | 107 | return ans 108 | 109 | def RSR(current_x,current_y,current_th,goal_x,goal_y,goal_th): 110 | c1_x=current_x+r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 111 | c1_y=current_y+r_min*math.sin(math.radians(current_th)-(PI/2)) 112 | 113 | c2_x=goal_x+r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 114 | c2_y=goal_y+r_min*math.sin(math.radians(goal_th)-(PI/2)) 115 | 116 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 117 | 118 | tan_pt1_x=c1_x-r_min*math.cos(theta) #tangent point in first circle of min_turning radius 119 | tan_pt1_y=c1_y+r_min*math.sin(theta) 120 | 121 | tan_pt2_x=c2_x-r_min*math.cos(theta) #tangent point in second circle of min_turning radius 122 | tan_pt2_y=c2_y+r_min*math.sin(theta) 123 | 124 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 125 | 126 | # print (tan_pt1_x,tan_pt1_y) 127 | # print (tan_pt2_x,tan_pt2_y) 128 | # print (c2_x,c2_y) 129 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(current_x-c1_x,current_y-c1_y) #Initial Turning angle 130 | 131 | Tf = np.arctan2(goal_x - c2_x,goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 132 | 133 | if Ti<0: 134 | Ti+=2*np.pi 135 | if Tf<0: 136 | Tf+=2*np.pi 137 | 138 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 139 | 140 | Ti= Ti*180/np.pi 141 | 142 | 143 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"RR"] 144 | return ans 145 | 146 | def LSR(current_x,current_y,current_th,goal_x,goal_y,goal_th): 147 | c1_x=current_x-r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 148 | c1_y=current_y-r_min*math.sin(math.radians(current_th)-(PI/2)) 149 | 150 | c2_x=goal_x+r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 151 | c2_y=goal_y+r_min*math.sin(math.radians(goal_th)-(PI/2)) 152 | 153 | # if c2_x-c1_x<0: 154 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*r_min)) 155 | # elif c2_x-c1_x>0: 156 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*r_min)) 157 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 158 | 159 | if(-r_min**2 + (S1**2)/4.0 )<0: 160 | # print"Holaaaaaa" 161 | return None 162 | 163 | S = 2*((-r_min**2 + (S1**2)/4.0 )**0.5) 164 | theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y)) - np.arctan2(r_min,S/2.0) 165 | 166 | 167 | tan_pt1_x=c1_x+r_min*math.cos(theta) #tangent point in first circle of min_turning radius 168 | tan_pt1_y=c1_y-r_min*math.sin(theta) 169 | 170 | tan_pt2_x=c2_x-r_min*math.cos(theta) #tangent point in second circle of min_turning radius 171 | tan_pt2_y=c2_y+r_min*math.sin(theta) 172 | 173 | 174 | Ti = np.arctan2(current_x-c1_x,current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 175 | 176 | Tf = np.arctan2(goal_x - c2_x,goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 177 | 178 | if Ti<0: 179 | Ti+=2*np.pi 180 | if Tf<0: 181 | Tf+=2*np.pi 182 | 183 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 184 | 185 | Ti= Ti*180/np.pi 186 | print Ti, current_x,current_y,current_th,goal_x,goal_y,goal_th 187 | 188 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"LR"] 189 | return ans 190 | 191 | def RSL(current_x,current_y,current_th,goal_x,goal_y,goal_th): 192 | c1_x=current_x+r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 193 | c1_y=current_y+r_min*math.sin(math.radians(current_th)-(PI/2)) 194 | 195 | c2_x=goal_x-r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 196 | c2_y=goal_y-r_min*math.sin(math.radians(goal_th)-(PI/2)) 197 | 198 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 199 | # print (tan_pt1_x,tan_pt1_y) 200 | # print (tan_pt2_x,tan_pt2_y) 201 | if(-r_min**2 + (S1**2)/4.0 )<0: 202 | # print"Holaaaaaa" 203 | return None 204 | S = 2*((-r_min**2 + (S1**2)/4.0 )**0.5) 205 | 206 | # if c2_x-c1_x<0: 207 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*r_min)) 208 | # elif c2_x-c1_x>0: 209 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*r_min)) 210 | 211 | theta = + (np.arctan2((c2_x-c1_x), c2_y-c1_y)) + np.arctan2(r_min,S/2.0) 212 | 213 | print "THETA ", theta, "c1_x =",c1_x, c1_y,c2_x,c2_y,"S=",S 214 | 215 | tan_pt1_x=c1_x-r_min*math.cos(theta) #tangent point in first circle of min_turning radius 216 | tan_pt1_y=c1_y+r_min*math.sin(theta) 217 | 218 | tan_pt2_x=c2_x+r_min*math.cos(theta) #tangent point in second circle of min_turning radius 219 | tan_pt2_y=c2_y-r_min*math.sin(theta) 220 | 221 | 222 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(current_x-c1_x,current_y-c1_y) #Initial Turning angle 223 | 224 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(goal_x - c2_x,goal_y- c2_y) #Final Turning angle 225 | 226 | if Ti<0: 227 | Ti+=2*np.pi 228 | if Tf<0: 229 | Tf+=2*np.pi 230 | 231 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 232 | 233 | Ti= Ti*180/np.pi 234 | print Ti, current_x,current_y,current_th,goal_x,goal_y,goal_th 235 | 236 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"RL"] 237 | # 0 1. 2. 3. 4. 5. 6. 7. 8. 9. 10. 11 238 | return ans 239 | 240 | 241 | def findNearest(sample): 242 | min_d = sys.maxint 243 | ind=0 244 | min_node=tree[0] 245 | for indd,node in enumerate(tree): 246 | dis = ((node.x-sample.x)**2 + (node.y-sample.y)**2)**0.5 247 | if min_d>dis: 248 | min_d=dis 249 | min_node=node 250 | ind=indd 251 | return min_node,ind 252 | 253 | def RRT(): 254 | j=0 255 | initial=[] 256 | theta_end=None 257 | while True: 258 | j+=1 259 | # if j>5: 260 | # break 261 | sample = Node(random.uniform(0,11),random.uniform(0,11)) 262 | nearestNode,ind = findNearest(sample) 263 | newNode = copy.deepcopy(nearestNode) 264 | 265 | theta = (np.arctan2(sample.y-nearestNode.y, sample.x-nearestNode.x)) 266 | newNode.x+=step*math.cos(theta) 267 | newNode.y+=step*math.sin(theta) 268 | 269 | # ansLSL=LSL(nearestNode.x,nearestNode.y,0,newNode.x,newNode.y,0) 270 | # if 271 | 272 | theta_start = theta 273 | theta_end = (np.arctan2(newNode.y-goal_y, newNode.x-goal_x)) 274 | 275 | 276 | paths=[LSL,RSR,LSR,RSL] 277 | minS=sys.maxint 278 | ans=[] 279 | count=0 280 | curve="" 281 | ans_count=1 282 | for i in paths: 283 | path_ans=i(nearestNode.x,nearestNode.y,theta_start,newNode.x,newNode.y,theta_end) 284 | count+=1 285 | if path_ans: 286 | if minS>path_ans[3]: 287 | ans=path_ans 288 | minS=path_ans[3] 289 | ans_count=count 290 | curve=path_ans[12] 291 | 292 | plt.plot([ans[4],ans[6]],[ans[5],ans[7]]) 293 | 294 | start_angle1 = np.arctan2(nearestNode.y - ans[9], nearestNode.x - ans[8]) * 180 / np.pi 295 | end_angle1 = np.arctan2(ans[5] - ans[9], ans[4] - ans[8]) * 180 / np.pi 296 | 297 | if curve[0]=="L": 298 | arc1 = patches.Arc((ans[8], ans[9]), 2*r_min,2*r_min,theta1=start_angle1, theta2=end_angle1) 299 | else: 300 | arc1 = patches.Arc((ans[8], ans[9]), 2*r_min,2*r_min,theta1=end_angle1, theta2=start_angle1) 301 | plt.gca().add_patch(arc1) 302 | 303 | start_angle2 = np.arctan2(ans[7] - ans[11], ans[6] - ans[10]) * 180 / np.pi 304 | end_angle2 = np.arctan2(newNode.y - ans[11], newNode.x - ans[10]) * 180 / np.pi 305 | 306 | if curve[1]=="L": 307 | arc2 = patches.Arc((ans[10], ans[11]), 2*r_min,2*r_min,theta1=start_angle2, theta2=end_angle2) 308 | else: 309 | arc2 = patches.Arc((ans[10], ans[11]), 2*r_min,2*r_min,theta1=end_angle2, theta2=start_angle2) 310 | 311 | plt.gca().add_patch(arc2) 312 | 313 | plt.gca().set_aspect('equal') 314 | 315 | # ans = LSL(nearestNode.x,nearestNode.y,0,newNode.x,newNode.y,0) 316 | 317 | newNode.p=ind 318 | nearestNode.tan_pt1_x=ans[4] 319 | nearestNode.tan_pt1_y=ans[5] 320 | nearestNode.tan_pt2_x=ans[6] 321 | nearestNode.tan_pt2_y=ans[7] 322 | nearestNode.S=ans[3] 323 | nearestNode.curve=curve 324 | nearestNode.Ti=ans[0] 325 | if j==1: 326 | initial.append([ans[4],ans[5],ans[6],ans[7],ans[3],curve,ans[0]]) 327 | 328 | # if ((newNode.x-3)**2 + (newNode.y-3)**2)**0.5 < 1: 329 | # continue 330 | # if ((newNode.x-6)**2 + (newNode.y-5)**2)**0.5 < 1: 331 | # continue 332 | if check_obstacle(newNode.x,newNode.y,3,3): 333 | continue 334 | if check_obstacle(newNode.x,newNode.y,6,5): 335 | continue 336 | # if check_obstacle(newNode.x,newNode.y,3,6): 337 | # continue 338 | # if check_obstacle(newNode.x,newNode.y,6,5): 339 | # continue 340 | if newNode.x>10 or newNode.y>10 or newNode.x<0 or newNode.y<0: 341 | continue 342 | tree.append(newNode) 343 | if ((newNode.x-goal_x)**2 + (newNode.y-goal_y)**2)**0.5 < step: 344 | break 345 | 346 | # print tree[0].Ti 347 | 348 | paths=[LSL,RSR,LSR,RSL] 349 | minS=sys.maxint 350 | ans=[] 351 | count=0 352 | ans_count=1 353 | for i in paths: 354 | path_ans=i(newNode.x,newNode.y,theta_end,goal_x,goal_y,goal_th) 355 | count+=1 356 | if path_ans: 357 | if minS>path_ans[3]: 358 | ans=path_ans 359 | minS=path_ans[3] 360 | ans_count=count 361 | curve=path_ans[12] 362 | 363 | newNode.tan_pt1_x=ans[4] 364 | newNode.tan_pt1_y=ans[5] 365 | newNode.tan_pt2_x=ans[6] 366 | newNode.tan_pt2_y=ans[7] 367 | newNode.S=ans[3] 368 | newNode.curve=curve 369 | newNode.Ti=ans[0] 370 | 371 | plt.plot([ans[4],ans[6]],[ans[5],ans[7]]) 372 | 373 | start_angle1 = np.arctan2(newNode.y - ans[9], newNode.x - ans[8]) * 180 / np.pi 374 | end_angle1 = np.arctan2(ans[5] - ans[9], ans[4] - ans[8]) * 180 / np.pi 375 | 376 | if curve[0]=="L": 377 | arc1 = patches.Arc((ans[8], ans[9]), 2*r_min,2*r_min,theta1=start_angle1, theta2=end_angle1) 378 | else: 379 | arc1 = patches.Arc((ans[8], ans[9]), 2*r_min,2*r_min,theta1=end_angle1, theta2=start_angle1) 380 | plt.gca().add_patch(arc1) 381 | 382 | start_angle2 = np.arctan2(ans[7] - ans[11], ans[6] - ans[10]) * 180 / np.pi 383 | end_angle2 = np.arctan2(goal_y - ans[11], goal_x - ans[10]) * 180 / np.pi 384 | 385 | if curve[1]=="L": 386 | arc2 = patches.Arc((ans[10], ans[11]), 2*r_min,2*r_min,theta1=start_angle2, theta2=end_angle2) 387 | else: 388 | arc2 = patches.Arc((ans[10], ans[11]), 2*r_min,2*r_min,theta1=end_angle2, theta2=start_angle2) 389 | 390 | plt.gca().add_patch(arc2) 391 | 392 | plt.gca().set_aspect('equal') 393 | 394 | newNode1 = copy.deepcopy(newNode) 395 | 396 | path=[[newNode.x,newNode.y]] 397 | while True: 398 | if not newNode.p: 399 | break 400 | #plt.plot([newNode.x,newNode.y],[tree[newNode.p].x,tree[newNode.p].y],'ro-') 401 | path.append([tree[newNode.p].x,tree[newNode.p].y]) 402 | newNode=tree[newNode.p] 403 | 404 | path_w_angles=[[newNode1.tan_pt1_x,newNode1.tan_pt1_y,newNode1.tan_pt2_x,newNode1.tan_pt2_y,newNode1.S,newNode1.curve,newNode1.Ti]] 405 | while True: 406 | if not newNode1.p: 407 | break 408 | path_w_angles.append([tree[newNode1.p].tan_pt1_x,tree[newNode1.p].tan_pt1_y,tree[newNode1.p].tan_pt2_x,tree[newNode1.p].tan_pt2_y,tree[newNode1.p].S,tree[newNode1.p].curve,tree[newNode1.p].Ti]) 409 | newNode1=tree[newNode1.p] 410 | path_w_angles.append(initial[0]) 411 | 412 | path_w_angles=path_w_angles[::-1] 413 | #path_w_angles=[[0.11382975330055782, 0.03707606850741252, 1.550462272816677, 1.0601573107627982, 1.76369164662819, 'LR', 34.829836137384575], [1.8537386028505958, 1.3667442952423527, 1.3133630638948597, 2.820859157341914, 1.5512755252631716, 'LR', 108.68731498468217], [1.7125078645071232, 3.2591611478524074, 1.802594876855204, 3.8311177818676017, 0.579007824634343, 'LR', 83.07015853891144]] 414 | 415 | x=[goal_x] 416 | y=[goal_y] 417 | for i in path: 418 | x.append(i[0]) 419 | y.append(i[1]) 420 | x.append(current_x) 421 | y.append(current_y) 422 | # print x,y 423 | plt.plot(x, y, '-ro',alpha=0.2) 424 | print path_w_angles, len(path_w_angles), len(path) 425 | plt.show() 426 | RRT() 427 | # RSL(0.151237017246, 1.99427364336, 1.20720741966, 0.862498625581, 3.86352668255, -2.74749039623) -------------------------------------------------------------------------------- /rrt_dubins/rrt_dubin_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/rrt_dubins/rrt_dubin_2.png -------------------------------------------------------------------------------- /rrt_dubins/rrt_dubin_2_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/rrt_dubins/rrt_dubin_2_2.png -------------------------------------------------------------------------------- /rrt_dubins/rrt_dubin_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/rrt_dubins/rrt_dubin_4.png -------------------------------------------------------------------------------- /rrt_dubins/rrt_dubin_4_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/rrt_dubins/rrt_dubin_4_1.png -------------------------------------------------------------------------------- /rrt_star_dubins/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/rrt_star_dubins/.DS_Store -------------------------------------------------------------------------------- /rrt_star_dubins/Figure1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/rrt_star_dubins/Figure1.png -------------------------------------------------------------------------------- /rrt_star_dubins/Figure2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/rrt_star_dubins/Figure2.png -------------------------------------------------------------------------------- /rrt_star_dubins/rrt_star.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/rrt_star_dubins/rrt_star.png -------------------------------------------------------------------------------- /rrt_star_dubins/rrt_star_dubins.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import matplotlib.patches as patches 4 | import math 5 | import random 6 | import sys 7 | import copy 8 | 9 | class Node(): 10 | def __init__(self,x,y): 11 | self.x=x 12 | self.y=y 13 | self.p=None 14 | self.cost=0 15 | 16 | plt.axis([-0.1, 1.1, -0.1, 1.1]) 17 | # rectangle = plt.Rectangle((0.2, 0.2), 0.2, 0.2) 18 | # plt.gca().add_patch(rectangle) 19 | 20 | obs = plt.Circle((0.3, 0.3), radius=0.1, fc='r') 21 | plt.gca().add_patch(obs) 22 | 23 | obs = plt.Circle((0.6, 0.5), radius=0.2, fc='r') 24 | plt.gca().add_patch(obs) 25 | 26 | obs = plt.Circle((0.3, 0.5), radius=0.1, fc='r') 27 | plt.gca().add_patch(obs) 28 | 29 | obstacles = [(0.3,0.3,0.1),(0.6,0.5,0.2),(0.3,0.5,0.1)] 30 | 31 | rectangle = plt.Rectangle((0, 0), 1, 1,fill=False) 32 | plt.gca().add_patch(rectangle) 33 | 34 | 35 | current_x=0 36 | current_y=0 37 | current_th=0 #current angle in Degrees 38 | 39 | PI=3.14 40 | r_min=0.02 41 | step=0.3 42 | 43 | goal_x=0.8 44 | goal_y=0.8 45 | goal_th=0 #goal angle in Degrees 46 | 47 | goal_circle = plt.Circle((0.8, 0.8), radius=0.1, fc='b') 48 | plt.gca().add_patch(goal_circle) 49 | 50 | plt.axis('scaled') 51 | # goal_circle = 52 | #plt.show() 53 | 54 | def LSL(current_x,current_y,current_th,goal_x,goal_y,goal_th): 55 | c1_x=current_x-r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 56 | c1_y=current_y-r_min*math.sin(math.radians(current_th)-(PI/2)) 57 | 58 | c2_x=goal_x-r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 59 | c2_y=goal_y-r_min*math.sin(math.radians(goal_th)-(PI/2)) 60 | #print c1_x,c1_y,c2_x,c2_y, c2_y-c1_y, c2_x-c1_x 61 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 62 | 63 | tan_pt1_x=c1_x+r_min*math.cos(theta) #tangent point in first circle of min_turning radius 64 | tan_pt1_y=c1_y-r_min*math.sin(theta) 65 | 66 | tan_pt2_x=c2_x+r_min*math.cos(theta) #tangent point in second circle of min_turning radius 67 | tan_pt2_y=c2_y-r_min*math.sin(theta) 68 | 69 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 70 | # print S,theta 71 | # print (tan_pt1_x,tan_pt1_y) 72 | # print (tan_pt2_x,tan_pt2_y) 73 | # print (c1_x,c1_y,c2_x,c2_y) 74 | 75 | Ti = np.arctan2(current_x-c1_x,current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 76 | 77 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(goal_x - c2_x,goal_y- c2_y) #Final Turning angle 78 | 79 | if Ti<0: 80 | Ti+=2*np.pi 81 | if Tf<0: 82 | Tf+=2*np.pi 83 | 84 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 85 | 86 | # Ti= Ti*180/np.pi 87 | 88 | # print Ti,Tf, total_dist,S 89 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"LL"] 90 | 91 | return ans 92 | 93 | def RSR(current_x,current_y,current_th,goal_x,goal_y,goal_th): 94 | c1_x=current_x+r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 95 | c1_y=current_y+r_min*math.sin(math.radians(current_th)-(PI/2)) 96 | 97 | c2_x=goal_x+r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 98 | c2_y=goal_y+r_min*math.sin(math.radians(goal_th)-(PI/2)) 99 | 100 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 101 | 102 | tan_pt1_x=c1_x-r_min*math.cos(theta) #tangent point in first circle of min_turning radius 103 | tan_pt1_y=c1_y+r_min*math.sin(theta) 104 | 105 | tan_pt2_x=c2_x-r_min*math.cos(theta) #tangent point in second circle of min_turning radius 106 | tan_pt2_y=c2_y+r_min*math.sin(theta) 107 | 108 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 109 | 110 | # print (tan_pt1_x,tan_pt1_y) 111 | # print (tan_pt2_x,tan_pt2_y) 112 | # print (c2_x,c2_y) 113 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(current_x-c1_x,current_y-c1_y) #Initial Turning angle 114 | 115 | Tf = np.arctan2(goal_x - c2_x,goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 116 | 117 | if Ti<0: 118 | Ti+=2*np.pi 119 | if Tf<0: 120 | Tf+=2*np.pi 121 | 122 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 123 | 124 | Ti= Ti*180/np.pi 125 | 126 | 127 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"RR"] 128 | return ans 129 | 130 | def LSR(current_x,current_y,current_th,goal_x,goal_y,goal_th): 131 | c1_x=current_x-r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 132 | c1_y=current_y-r_min*math.sin(math.radians(current_th)-(PI/2)) 133 | 134 | c2_x=goal_x+r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 135 | c2_y=goal_y+r_min*math.sin(math.radians(goal_th)-(PI/2)) 136 | 137 | # if c2_x-c1_x<0: 138 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*r_min)) 139 | # elif c2_x-c1_x>0: 140 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*r_min)) 141 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 142 | 143 | if(-r_min**2 + (S1**2)/4.0 )<0: 144 | # print"Holaaaaaa" 145 | return None 146 | 147 | S = 2*((-r_min**2 + (S1**2)/4.0 )**0.5) 148 | theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y)) - np.arctan2(r_min,S/2.0) 149 | 150 | 151 | tan_pt1_x=c1_x+r_min*math.cos(theta) #tangent point in first circle of min_turning radius 152 | tan_pt1_y=c1_y-r_min*math.sin(theta) 153 | 154 | tan_pt2_x=c2_x-r_min*math.cos(theta) #tangent point in second circle of min_turning radius 155 | tan_pt2_y=c2_y+r_min*math.sin(theta) 156 | 157 | 158 | Ti = np.arctan2(current_x-c1_x,current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 159 | 160 | Tf = np.arctan2(goal_x - c2_x,goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 161 | 162 | if Ti<0: 163 | Ti+=2*np.pi 164 | if Tf<0: 165 | Tf+=2*np.pi 166 | 167 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 168 | 169 | Ti= Ti*180/np.pi 170 | # print Ti, current_x,current_y,current_th,goal_x,goal_y,goal_th 171 | 172 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"LR"] 173 | return ans 174 | 175 | def RSL(current_x,current_y,current_th,goal_x,goal_y,goal_th): 176 | c1_x=current_x+r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 177 | c1_y=current_y+r_min*math.sin(math.radians(current_th)-(PI/2)) 178 | 179 | c2_x=goal_x-r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 180 | c2_y=goal_y-r_min*math.sin(math.radians(goal_th)-(PI/2)) 181 | 182 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 183 | # print (tan_pt1_x,tan_pt1_y) 184 | # print (tan_pt2_x,tan_pt2_y) 185 | if(-r_min**2 + (S1**2)/4.0 )<0: 186 | # print"Holaaaaaa" 187 | return None 188 | S = 2*((-r_min**2 + (S1**2)/4.0 )**0.5) 189 | 190 | # if c2_x-c1_x<0: 191 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*r_min)) 192 | # elif c2_x-c1_x>0: 193 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*r_min)) 194 | 195 | theta = + (np.arctan2((c2_x-c1_x), c2_y-c1_y)) + np.arctan2(r_min,S/2.0) 196 | 197 | # print "THETA ", theta, "c1_x =",c1_x, c1_y,c2_x,c2_y,"S=",S 198 | 199 | tan_pt1_x=c1_x-r_min*math.cos(theta) #tangent point in first circle of min_turning radius 200 | tan_pt1_y=c1_y+r_min*math.sin(theta) 201 | 202 | tan_pt2_x=c2_x+r_min*math.cos(theta) #tangent point in second circle of min_turning radius 203 | tan_pt2_y=c2_y-r_min*math.sin(theta) 204 | 205 | 206 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(current_x-c1_x,current_y-c1_y) #Initial Turning angle 207 | 208 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(goal_x - c2_x,goal_y- c2_y) #Final Turning angle 209 | 210 | if Ti<0: 211 | Ti+=2*np.pi 212 | if Tf<0: 213 | Tf+=2*np.pi 214 | 215 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 216 | 217 | Ti= Ti*180/np.pi 218 | # print Ti, current_x,current_y,current_th,goal_x,goal_y,goal_th 219 | 220 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"RL"] 221 | # 0 1. 2. 3. 4. 5. 6. 7. 8. 9. 10. 11 222 | return ans 223 | 224 | tree = [Node(current_x,current_y)] 225 | 226 | def check_for_obstacles(x,y): 227 | for ox,oy,orad in obstacles: 228 | if ((x-ox)**2 + (y-oy)**2)**0.5 < orad*2: 229 | return False 230 | return True 231 | 232 | def findNearest(sample): 233 | min_d = sys.maxint 234 | ind=0 235 | min_node=tree[0] 236 | for indd,node in enumerate(tree): 237 | dis = ((node.x-sample.x)**2 + (node.y-sample.y)**2)**0.5 238 | if min_d>dis: 239 | min_d=dis 240 | min_node=node 241 | ind=indd 242 | return min_node,ind 243 | 244 | def find_nearby_nodes(newNode): 245 | radius = step*2 246 | dist_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2 for node in tree] 247 | nearinds = [dist_list.index(i) for i in dist_list if i <= (radius ** 2)] 248 | return nearinds 249 | 250 | def check_collision_extend(node, theta, d): 251 | 252 | temp_node = copy.deepcopy(node) 253 | 254 | for i in range(int(d / step)): 255 | temp_node.x += step * math.cos(theta) 256 | temp_node.y += step * math.sin(theta) 257 | if not check_for_obstacles(temp_node.x,temp_node.y): 258 | return False 259 | if temp_node.x>1 or temp_node.y>1 or temp_node.x<0 or temp_node.y<0: 260 | return False 261 | return True 262 | 263 | def rewire(node, nearinds): 264 | for i in nearinds: 265 | nearNode = tree[i] 266 | dx = node.x - nearNode.x 267 | dy = node.y - nearNode.y 268 | d = math.sqrt(dx ** 2 + dy ** 2) 269 | 270 | if nearNode.cost>node.cost+d: 271 | theta = np.arctan2(dy, dx) 272 | if check_collision_extend(tree[i], theta, d): 273 | nearNode.p=tree.index(node) 274 | nearNode.cost=node.cost+d 275 | 276 | def choose_parent(newNode, nearinds): 277 | if not nearinds: 278 | return newNode 279 | dist_list = [] 280 | for i in nearinds: 281 | dx = newNode.x - tree[i].x 282 | dy = newNode.y - tree[i].y 283 | d = math.sqrt(dx ** 2 + dy ** 2) 284 | theta = np.arctan2(dy, dx) 285 | if check_collision_extend(tree[i], theta, d): 286 | dist_list.append(tree[i].cost + d) 287 | 288 | if not dist_list: 289 | return newNode 290 | 291 | mincost = min(dist_list) 292 | minind = nearinds[dist_list.index(mincost)] 293 | 294 | newNode.cost = mincost 295 | newNode.p = minind 296 | 297 | return newNode 298 | 299 | def RRT(): 300 | while True: 301 | sample = Node(random.uniform(0,1),random.uniform(0,1)) 302 | 303 | nearestNode,ind = findNearest(sample) 304 | newNode = copy.deepcopy(nearestNode) 305 | theta = (np.arctan2(sample.y-nearestNode.y, sample.x-nearestNode.x)) 306 | newNode.x+=step*math.cos(theta) 307 | newNode.y+=step*math.sin(theta) 308 | newNode.p=ind 309 | newNode.cost+=step 310 | # plt.plot(newNode.x,newNode.y,'r+') 311 | # plt.plot(nearestNode.x,nearestNode.y,'bo') 312 | # plt.pause(2) 313 | # plt.plot(nearestNode.x,nearestNode.y,'wo') 314 | # plt.pause(2) 315 | 316 | # if ((newNode.x-0.3)**2 + (newNode.y-0.3)**2)**0.5 < 0.2: 317 | # # plt.plot(newNode.x,newNode.y,'w+') 318 | # # plt.pause(2) 319 | # continue 320 | # if ((newNode.x-0.6)**2 + (newNode.y-0.5)**2)**0.5 < 0.2: 321 | # # plt.plot(newNode.x,newNode.y,'w+') 322 | # # plt.pause(2) 323 | # continue 324 | # if ((newNode.x-0.3)**2 + (newNode.y-0.5)**2)**0.5 < 0.2: 325 | # # plt.plot(newNode.x,newNode.y,'w+') 326 | # # plt.pause(2) 327 | # continue 328 | if not check_for_obstacles(newNode.x,newNode.y): 329 | continue 330 | if newNode.x>1 or newNode.y>1 or newNode.x<0 or newNode.y<0: 331 | # plt.plot(newNode.x,newNode.y,'w+') 332 | # plt.pause(2) 333 | continue 334 | nearbyNodes = find_nearby_nodes(newNode) 335 | newNode = choose_parent(newNode, nearbyNodes) 336 | # plt.plot(newNode.x,newNode.y,'bo') 337 | # plt.pause(2) 338 | # plt.plot(newNode.x,newNode.y,'wo') 339 | # plt.pause(2) 340 | tree.append(newNode) 341 | rewire(newNode,nearbyNodes) 342 | 343 | if ((newNode.x-goal_x)**2 + (newNode.y-goal_y)**2)**0.5 < 0.1: 344 | break 345 | # DrawGraph(sample) 346 | path=[[newNode.x,newNode.y]] 347 | while True: 348 | if not newNode.p: 349 | break 350 | #plt.plot([newNode.x,newNode.y],[tree[newNode.p].x,tree[newNode.p].y],'ro-') 351 | path.append([tree[newNode.p].x,tree[newNode.p].y]) 352 | newNode=tree[newNode.p] 353 | x=[goal_x] 354 | y=[goal_y] 355 | for i in path: 356 | x.append(i[0]) 357 | y.append(i[1]) 358 | # plt.plot(i[0],i[1],'o-') 359 | # plt.pause(0.5) 360 | x.append(current_x) 361 | y.append(current_y) 362 | # print x,y 363 | nx=x[::-1][1:] 364 | ny=y[::-1][1:] 365 | 366 | sx=0 367 | sy=0 368 | st=0 369 | for a,b in zip(nx,ny): 370 | ex,ey=a,b 371 | et=(np.arctan2(ey-goal_y, ex-goal_x)) 372 | 373 | # dubins 374 | paths=[LSL,RSR,LSR,RSL] 375 | minS=float("inf") 376 | ans=[] 377 | count=0 378 | curve="" 379 | ans_count=1 380 | for i in paths: 381 | path_ans=i(sx,sy,st,ex,ey,et) 382 | count+=1 383 | if path_ans: 384 | if minS>path_ans[3]: 385 | ans=path_ans 386 | minS=path_ans[3] 387 | ans_count=count 388 | curve=path_ans[12] 389 | 390 | plt.plot([ans[4],ans[6]],[ans[5],ans[7]]) 391 | 392 | start_angle1 = np.arctan2(sy - ans[9], sx - ans[8]) * 180 / np.pi 393 | end_angle1 = np.arctan2(ans[5] - ans[9], ans[4] - ans[8]) * 180 / np.pi 394 | 395 | if curve[0]=="L": 396 | arc1 = patches.Arc((ans[8], ans[9]), 2*r_min,2*r_min,theta1=start_angle1, theta2=end_angle1) 397 | else: 398 | arc1 = patches.Arc((ans[8], ans[9]), 2*r_min,2*r_min,theta1=end_angle1, theta2=start_angle1) 399 | plt.gca().add_patch(arc1) 400 | 401 | start_angle2 = np.arctan2(ans[7] - ans[11], ans[6] - ans[10]) * 180 / np.pi 402 | end_angle2 = np.arctan2(ey - ans[11], ex - ans[10]) * 180 / np.pi 403 | 404 | if curve[1]=="L": 405 | arc2 = patches.Arc((ans[10], ans[11]), 2*r_min,2*r_min,theta1=start_angle2, theta2=end_angle2) 406 | else: 407 | arc2 = patches.Arc((ans[10], ans[11]), 2*r_min,2*r_min,theta1=end_angle2, theta2=start_angle2) 408 | 409 | plt.gca().add_patch(arc2) 410 | 411 | plt.gca().set_aspect('equal') 412 | 413 | # 414 | sx,sy,st=ex,ey,et 415 | # plt.plot(x, y, 'o-') 416 | # plt.pause(0.05) 417 | plt.show() 418 | RRT() -------------------------------------------------------------------------------- /rrt_star_dubins/rrt_star_dubins_minimized_euclidean.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import matplotlib.patches as patches 4 | import math 5 | import random 6 | import sys 7 | import copy 8 | 9 | class Node(): 10 | def __init__(self,x,y): 11 | self.x=x 12 | self.y=y 13 | self.p=None 14 | self.cost=0 15 | 16 | plt.axis([-0.1, 1.1, -0.1, 1.1]) 17 | # rectangle = plt.Rectangle((0.2, 0.2), 0.2, 0.2) 18 | # plt.gca().add_patch(rectangle) 19 | 20 | obs = plt.Circle((0.3, 0.3), radius=0.1, fc='r') 21 | plt.gca().add_patch(obs) 22 | 23 | obs = plt.Circle((0.6, 0.5), radius=0.2, fc='r') 24 | plt.gca().add_patch(obs) 25 | 26 | obs = plt.Circle((0.3, 0.5), radius=0.1, fc='r') 27 | plt.gca().add_patch(obs) 28 | 29 | obstacles = [(0.3,0.3,0.1),(0.6,0.5,0.2),(0.3,0.5,0.1)] 30 | 31 | rectangle = plt.Rectangle((0, 0), 1, 1,fill=False) 32 | plt.gca().add_patch(rectangle) 33 | 34 | 35 | current_x=0 36 | current_y=0 37 | current_th=0 #current angle in Degrees 38 | 39 | PI=3.14 40 | r_min=0.02 41 | step=0.3 42 | 43 | goal_x=0.8 44 | goal_y=0.8 45 | goal_th=0 #goal angle in Degrees 46 | 47 | goal_circle = plt.Circle((0.8, 0.8), radius=0.1, fc='b') 48 | plt.gca().add_patch(goal_circle) 49 | 50 | plt.axis('scaled') 51 | # goal_circle = 52 | #plt.show() 53 | 54 | def LSL(current_x,current_y,current_th,goal_x,goal_y,goal_th): 55 | c1_x=current_x-r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 56 | c1_y=current_y-r_min*math.sin(math.radians(current_th)-(PI/2)) 57 | 58 | c2_x=goal_x-r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 59 | c2_y=goal_y-r_min*math.sin(math.radians(goal_th)-(PI/2)) 60 | #print c1_x,c1_y,c2_x,c2_y, c2_y-c1_y, c2_x-c1_x 61 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 62 | 63 | tan_pt1_x=c1_x+r_min*math.cos(theta) #tangent point in first circle of min_turning radius 64 | tan_pt1_y=c1_y-r_min*math.sin(theta) 65 | 66 | tan_pt2_x=c2_x+r_min*math.cos(theta) #tangent point in second circle of min_turning radius 67 | tan_pt2_y=c2_y-r_min*math.sin(theta) 68 | 69 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 70 | # print S,theta 71 | # print (tan_pt1_x,tan_pt1_y) 72 | # print (tan_pt2_x,tan_pt2_y) 73 | # print (c1_x,c1_y,c2_x,c2_y) 74 | 75 | Ti = np.arctan2(current_x-c1_x,current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 76 | 77 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(goal_x - c2_x,goal_y- c2_y) #Final Turning angle 78 | 79 | if Ti<0: 80 | Ti+=2*np.pi 81 | if Tf<0: 82 | Tf+=2*np.pi 83 | 84 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 85 | 86 | # Ti= Ti*180/np.pi 87 | 88 | # print Ti,Tf, total_dist,S 89 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"LL"] 90 | 91 | return ans 92 | 93 | def RSR(current_x,current_y,current_th,goal_x,goal_y,goal_th): 94 | c1_x=current_x+r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 95 | c1_y=current_y+r_min*math.sin(math.radians(current_th)-(PI/2)) 96 | 97 | c2_x=goal_x+r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 98 | c2_y=goal_y+r_min*math.sin(math.radians(goal_th)-(PI/2)) 99 | 100 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 101 | 102 | tan_pt1_x=c1_x-r_min*math.cos(theta) #tangent point in first circle of min_turning radius 103 | tan_pt1_y=c1_y+r_min*math.sin(theta) 104 | 105 | tan_pt2_x=c2_x-r_min*math.cos(theta) #tangent point in second circle of min_turning radius 106 | tan_pt2_y=c2_y+r_min*math.sin(theta) 107 | 108 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 109 | 110 | # print (tan_pt1_x,tan_pt1_y) 111 | # print (tan_pt2_x,tan_pt2_y) 112 | # print (c2_x,c2_y) 113 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(current_x-c1_x,current_y-c1_y) #Initial Turning angle 114 | 115 | Tf = np.arctan2(goal_x - c2_x,goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 116 | 117 | if Ti<0: 118 | Ti+=2*np.pi 119 | if Tf<0: 120 | Tf+=2*np.pi 121 | 122 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 123 | 124 | Ti= Ti*180/np.pi 125 | 126 | 127 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"RR"] 128 | return ans 129 | 130 | def LSR(current_x,current_y,current_th,goal_x,goal_y,goal_th): 131 | c1_x=current_x-r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 132 | c1_y=current_y-r_min*math.sin(math.radians(current_th)-(PI/2)) 133 | 134 | c2_x=goal_x+r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 135 | c2_y=goal_y+r_min*math.sin(math.radians(goal_th)-(PI/2)) 136 | 137 | # if c2_x-c1_x<0: 138 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*r_min)) 139 | # elif c2_x-c1_x>0: 140 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*r_min)) 141 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 142 | 143 | if(-r_min**2 + (S1**2)/4.0 )<0: 144 | # print"Holaaaaaa" 145 | return None 146 | 147 | S = 2*((-r_min**2 + (S1**2)/4.0 )**0.5) 148 | theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y)) - np.arctan2(r_min,S/2.0) 149 | 150 | 151 | tan_pt1_x=c1_x+r_min*math.cos(theta) #tangent point in first circle of min_turning radius 152 | tan_pt1_y=c1_y-r_min*math.sin(theta) 153 | 154 | tan_pt2_x=c2_x-r_min*math.cos(theta) #tangent point in second circle of min_turning radius 155 | tan_pt2_y=c2_y+r_min*math.sin(theta) 156 | 157 | 158 | Ti = np.arctan2(current_x-c1_x,current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 159 | 160 | Tf = np.arctan2(goal_x - c2_x,goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 161 | 162 | if Ti<0: 163 | Ti+=2*np.pi 164 | if Tf<0: 165 | Tf+=2*np.pi 166 | 167 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 168 | 169 | Ti= Ti*180/np.pi 170 | # print Ti, current_x,current_y,current_th,goal_x,goal_y,goal_th 171 | 172 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"LR"] 173 | return ans 174 | 175 | def RSL(current_x,current_y,current_th,goal_x,goal_y,goal_th): 176 | c1_x=current_x+r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 177 | c1_y=current_y+r_min*math.sin(math.radians(current_th)-(PI/2)) 178 | 179 | c2_x=goal_x-r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 180 | c2_y=goal_y-r_min*math.sin(math.radians(goal_th)-(PI/2)) 181 | 182 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 183 | # print (tan_pt1_x,tan_pt1_y) 184 | # print (tan_pt2_x,tan_pt2_y) 185 | if(-r_min**2 + (S1**2)/4.0 )<0: 186 | # print"Holaaaaaa" 187 | return None 188 | S = 2*((-r_min**2 + (S1**2)/4.0 )**0.5) 189 | 190 | # if c2_x-c1_x<0: 191 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*r_min)) 192 | # elif c2_x-c1_x>0: 193 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*r_min)) 194 | 195 | theta = + (np.arctan2((c2_x-c1_x), c2_y-c1_y)) + np.arctan2(r_min,S/2.0) 196 | 197 | # print "THETA ", theta, "c1_x =",c1_x, c1_y,c2_x,c2_y,"S=",S 198 | 199 | tan_pt1_x=c1_x-r_min*math.cos(theta) #tangent point in first circle of min_turning radius 200 | tan_pt1_y=c1_y+r_min*math.sin(theta) 201 | 202 | tan_pt2_x=c2_x+r_min*math.cos(theta) #tangent point in second circle of min_turning radius 203 | tan_pt2_y=c2_y-r_min*math.sin(theta) 204 | 205 | 206 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(current_x-c1_x,current_y-c1_y) #Initial Turning angle 207 | 208 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(goal_x - c2_x,goal_y- c2_y) #Final Turning angle 209 | 210 | if Ti<0: 211 | Ti+=2*np.pi 212 | if Tf<0: 213 | Tf+=2*np.pi 214 | 215 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 216 | 217 | Ti= Ti*180/np.pi 218 | # print Ti, current_x,current_y,current_th,goal_x,goal_y,goal_th 219 | 220 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"RL"] 221 | # 0 1. 2. 3. 4. 5. 6. 7. 8. 9. 10. 11 222 | return ans 223 | 224 | tree = [Node(current_x,current_y)] 225 | 226 | def check_for_obstacles(x,y): 227 | for ox,oy,orad in obstacles: 228 | if ((x-ox)**2 + (y-oy)**2)**0.5 < orad*2: 229 | return False 230 | return True 231 | 232 | def findNearest(sample): 233 | min_d = sys.maxint 234 | ind=0 235 | min_node=tree[0] 236 | for indd,node in enumerate(tree): 237 | dis = ((node.x-sample.x)**2 + (node.y-sample.y)**2)**0.5 238 | if min_d>dis: 239 | min_d=dis 240 | min_node=node 241 | ind=indd 242 | return min_node,ind 243 | 244 | def find_nearby_nodes(newNode): 245 | radius = step*2 246 | dist_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2 for node in tree] 247 | nearinds = [dist_list.index(i) for i in dist_list if i <= (radius ** 2)] 248 | return nearinds 249 | 250 | def check_collision_extend(node, theta, d): 251 | 252 | temp_node = copy.deepcopy(node) 253 | 254 | for i in range(int(d / step)): 255 | temp_node.x += step * math.cos(theta) 256 | temp_node.y += step * math.sin(theta) 257 | if not check_for_obstacles(temp_node.x,temp_node.y): 258 | return False 259 | if temp_node.x>1 or temp_node.y>1 or temp_node.x<0 or temp_node.y<0: 260 | return False 261 | return True 262 | 263 | def rewire(node, nearinds): 264 | for i in nearinds: 265 | nearNode = tree[i] 266 | dx = node.x - nearNode.x 267 | dy = node.y - nearNode.y 268 | d = math.sqrt(dx ** 2 + dy ** 2) 269 | 270 | if nearNode.cost>node.cost+d: 271 | theta = np.arctan2(dy, dx) 272 | if check_collision_extend(tree[i], theta, d): 273 | nearNode.p=tree.index(node) 274 | nearNode.cost=node.cost+d 275 | 276 | def choose_parent(newNode, nearinds): 277 | if not nearinds: 278 | return newNode 279 | dist_list = [] 280 | for i in nearinds: 281 | dx = newNode.x - tree[i].x 282 | dy = newNode.y - tree[i].y 283 | d = math.sqrt(dx ** 2 + dy ** 2) 284 | theta = np.arctan2(dy, dx) 285 | if check_collision_extend(tree[i], theta, d): 286 | dist_list.append(tree[i].cost + d) 287 | 288 | if not dist_list: 289 | return newNode 290 | 291 | mincost = min(dist_list) 292 | minind = nearinds[dist_list.index(mincost)] 293 | 294 | newNode.cost = mincost 295 | newNode.p = minind 296 | 297 | return newNode 298 | 299 | def RRT(): 300 | while True: 301 | sample = Node(random.uniform(0,1),random.uniform(0,1)) 302 | 303 | nearestNode,ind = findNearest(sample) 304 | newNode = copy.deepcopy(nearestNode) 305 | theta = (np.arctan2(sample.y-nearestNode.y, sample.x-nearestNode.x)) 306 | newNode.x+=step*math.cos(theta) 307 | newNode.y+=step*math.sin(theta) 308 | newNode.p=ind 309 | newNode.cost+=step 310 | # plt.plot(newNode.x,newNode.y,'r+') 311 | # plt.plot(nearestNode.x,nearestNode.y,'bo') 312 | # plt.pause(2) 313 | # plt.plot(nearestNode.x,nearestNode.y,'wo') 314 | # plt.pause(2) 315 | 316 | # if ((newNode.x-0.3)**2 + (newNode.y-0.3)**2)**0.5 < 0.2: 317 | # # plt.plot(newNode.x,newNode.y,'w+') 318 | # # plt.pause(2) 319 | # continue 320 | # if ((newNode.x-0.6)**2 + (newNode.y-0.5)**2)**0.5 < 0.2: 321 | # # plt.plot(newNode.x,newNode.y,'w+') 322 | # # plt.pause(2) 323 | # continue 324 | # if ((newNode.x-0.3)**2 + (newNode.y-0.5)**2)**0.5 < 0.2: 325 | # # plt.plot(newNode.x,newNode.y,'w+') 326 | # # plt.pause(2) 327 | # continue 328 | if not check_for_obstacles(newNode.x,newNode.y): 329 | continue 330 | if newNode.x>1 or newNode.y>1 or newNode.x<0 or newNode.y<0: 331 | # plt.plot(newNode.x,newNode.y,'w+') 332 | # plt.pause(2) 333 | continue 334 | nearbyNodes = find_nearby_nodes(newNode) 335 | newNode = choose_parent(newNode, nearbyNodes) 336 | # plt.plot(newNode.x,newNode.y,'bo') 337 | # plt.pause(2) 338 | # plt.plot(newNode.x,newNode.y,'wo') 339 | # plt.pause(2) 340 | tree.append(newNode) 341 | rewire(newNode,nearbyNodes) 342 | 343 | if ((newNode.x-goal_x)**2 + (newNode.y-goal_y)**2)**0.5 < 0.1: 344 | break 345 | # DrawGraph(sample) 346 | path=[[newNode.x,newNode.y]] 347 | while True: 348 | if not newNode.p: 349 | break 350 | #plt.plot([newNode.x,newNode.y],[tree[newNode.p].x,tree[newNode.p].y],'ro-') 351 | path.append([tree[newNode.p].x,tree[newNode.p].y]) 352 | newNode=tree[newNode.p] 353 | x=[goal_x] 354 | y=[goal_y] 355 | for i in path: 356 | x.append(i[0]) 357 | y.append(i[1]) 358 | # plt.plot(i[0],i[1],'o-') 359 | # plt.pause(0.5) 360 | x.append(current_x) 361 | y.append(current_y) 362 | # print x,y 363 | nx=x[::-1][1:] 364 | ny=y[::-1][1:] 365 | 366 | sx=0 367 | sy=0 368 | st=0 369 | for a,b in zip(nx,ny): 370 | ex,ey=a,b 371 | et=(np.arctan2(ey-goal_y, ex-goal_x)) 372 | 373 | # dubins 374 | paths=[LSL,RSR,LSR,RSL] 375 | minS=float("inf") 376 | ans=[] 377 | count=0 378 | curve="" 379 | ans_count=1 380 | for i in paths: 381 | path_ans=i(sx,sy,st,ex,ey,et) 382 | count+=1 383 | if path_ans: 384 | if minS>path_ans[3]: 385 | ans=path_ans 386 | minS=path_ans[3] 387 | ans_count=count 388 | curve=path_ans[12] 389 | 390 | plt.plot([ans[4],ans[6]],[ans[5],ans[7]]) 391 | 392 | start_angle1 = np.arctan2(sy - ans[9], sx - ans[8]) * 180 / np.pi 393 | end_angle1 = np.arctan2(ans[5] - ans[9], ans[4] - ans[8]) * 180 / np.pi 394 | 395 | if curve[0]=="L": 396 | arc1 = patches.Arc((ans[8], ans[9]), 2*r_min,2*r_min,theta1=start_angle1, theta2=end_angle1) 397 | else: 398 | arc1 = patches.Arc((ans[8], ans[9]), 2*r_min,2*r_min,theta1=end_angle1, theta2=start_angle1) 399 | plt.gca().add_patch(arc1) 400 | 401 | start_angle2 = np.arctan2(ans[7] - ans[11], ans[6] - ans[10]) * 180 / np.pi 402 | end_angle2 = np.arctan2(ey - ans[11], ex - ans[10]) * 180 / np.pi 403 | 404 | if curve[1]=="L": 405 | arc2 = patches.Arc((ans[10], ans[11]), 2*r_min,2*r_min,theta1=start_angle2, theta2=end_angle2) 406 | else: 407 | arc2 = patches.Arc((ans[10], ans[11]), 2*r_min,2*r_min,theta1=end_angle2, theta2=start_angle2) 408 | 409 | plt.gca().add_patch(arc2) 410 | 411 | plt.gca().set_aspect('equal') 412 | 413 | # 414 | sx,sy,st=ex,ey,et 415 | # plt.plot(x, y, 'o-') 416 | # plt.pause(0.05) 417 | plt.show() 418 | RRT() -------------------------------------------------------------------------------- /rrt_star_dubins/rrt_star_dubins_minimzed_dubins_paths.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import matplotlib.patches as patches 4 | import math 5 | import random 6 | import sys 7 | import copy 8 | 9 | class Node(): 10 | def __init__(self,x,y): 11 | self.x=x 12 | self.y=y 13 | self.p=None 14 | self.th=0 15 | self.cost=0 16 | 17 | plt.axis([-0.1, 1.1, -0.1, 1.1]) 18 | # rectangle = plt.Rectangle((0.2, 0.2), 0.2, 0.2) 19 | # plt.gca().add_patch(rectangle) 20 | 21 | obs = plt.Circle((0.3, 0.3), radius=0.1, fc='r') 22 | plt.gca().add_patch(obs) 23 | 24 | obs = plt.Circle((0.6, 0.5), radius=0.2, fc='r') 25 | plt.gca().add_patch(obs) 26 | 27 | obs = plt.Circle((0.3, 0.5), radius=0.1, fc='r') 28 | plt.gca().add_patch(obs) 29 | 30 | obstacles = [(0.3,0.3,0.1),(0.6,0.5,0.2),(0.3,0.5,0.1)] 31 | 32 | rectangle = plt.Rectangle((0, 0), 1, 1,fill=False) 33 | plt.gca().add_patch(rectangle) 34 | 35 | 36 | current_x=0 37 | current_y=0 38 | current_th=0 #current angle in Degrees 39 | 40 | PI=3.14 41 | r_min=0.02 42 | step=0.3 43 | 44 | goal_x=0.8 45 | goal_y=0.8 46 | goal_th=0 #goal angle in Degrees 47 | 48 | pathDic={0:"LSL",1:"RSR",2:"LSR",3:"RSL"} 49 | 50 | goal_circle = plt.Circle((0.8, 0.8), radius=0.1, fc='b') 51 | plt.gca().add_patch(goal_circle) 52 | 53 | plt.axis('scaled') 54 | # goal_circle = 55 | #plt.show() 56 | 57 | def LSL(current_x,current_y,current_th,goal_x,goal_y,goal_th): 58 | c1_x=current_x-r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 59 | c1_y=current_y-r_min*math.sin(math.radians(current_th)-(PI/2)) 60 | 61 | c2_x=goal_x-r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 62 | c2_y=goal_y-r_min*math.sin(math.radians(goal_th)-(PI/2)) 63 | #print c1_x,c1_y,c2_x,c2_y, c2_y-c1_y, c2_x-c1_x 64 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 65 | 66 | tan_pt1_x=c1_x+r_min*math.cos(theta) #tangent point in first circle of min_turning radius 67 | tan_pt1_y=c1_y-r_min*math.sin(theta) 68 | 69 | tan_pt2_x=c2_x+r_min*math.cos(theta) #tangent point in second circle of min_turning radius 70 | tan_pt2_y=c2_y-r_min*math.sin(theta) 71 | 72 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 73 | # print S,theta 74 | # print (tan_pt1_x,tan_pt1_y) 75 | # print (tan_pt2_x,tan_pt2_y) 76 | # print (c1_x,c1_y,c2_x,c2_y) 77 | 78 | Ti = np.arctan2(current_x-c1_x,current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 79 | 80 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(goal_x - c2_x,goal_y- c2_y) #Final Turning angle 81 | 82 | if Ti<0: 83 | Ti+=2*np.pi 84 | if Tf<0: 85 | Tf+=2*np.pi 86 | 87 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 88 | 89 | # Ti= Ti*180/np.pi 90 | 91 | # print Ti,Tf, total_dist,S 92 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"LL"] 93 | 94 | return ans 95 | 96 | def RSR(current_x,current_y,current_th,goal_x,goal_y,goal_th): 97 | c1_x=current_x+r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 98 | c1_y=current_y+r_min*math.sin(math.radians(current_th)-(PI/2)) 99 | 100 | c2_x=goal_x+r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 101 | c2_y=goal_y+r_min*math.sin(math.radians(goal_th)-(PI/2)) 102 | 103 | theta = (np.arctan2(c2_x-c1_x, c2_y-c1_y)) 104 | 105 | tan_pt1_x=c1_x-r_min*math.cos(theta) #tangent point in first circle of min_turning radius 106 | tan_pt1_y=c1_y+r_min*math.sin(theta) 107 | 108 | tan_pt2_x=c2_x-r_min*math.cos(theta) #tangent point in second circle of min_turning radius 109 | tan_pt2_y=c2_y+r_min*math.sin(theta) 110 | 111 | S = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 112 | 113 | # print (tan_pt1_x,tan_pt1_y) 114 | # print (tan_pt2_x,tan_pt2_y) 115 | # print (c2_x,c2_y) 116 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(current_x-c1_x,current_y-c1_y) #Initial Turning angle 117 | 118 | Tf = np.arctan2(goal_x - c2_x,goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 119 | 120 | if Ti<0: 121 | Ti+=2*np.pi 122 | if Tf<0: 123 | Tf+=2*np.pi 124 | 125 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 126 | 127 | Ti= Ti*180/np.pi 128 | 129 | 130 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"RR"] 131 | return ans 132 | 133 | def LSR(current_x,current_y,current_th,goal_x,goal_y,goal_th): 134 | c1_x=current_x-r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 135 | c1_y=current_y-r_min*math.sin(math.radians(current_th)-(PI/2)) 136 | 137 | c2_x=goal_x+r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 138 | c2_y=goal_y+r_min*math.sin(math.radians(goal_th)-(PI/2)) 139 | 140 | # if c2_x-c1_x<0: 141 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*r_min)) 142 | # elif c2_x-c1_x>0: 143 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*r_min)) 144 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 145 | 146 | if(-r_min**2 + (S1**2)/4.0 )<0: 147 | # print"Holaaaaaa" 148 | return None 149 | 150 | S = 2*((-r_min**2 + (S1**2)/4.0 )**0.5) 151 | theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y)) - np.arctan2(r_min,S/2.0) 152 | 153 | 154 | tan_pt1_x=c1_x+r_min*math.cos(theta) #tangent point in first circle of min_turning radius 155 | tan_pt1_y=c1_y-r_min*math.sin(theta) 156 | 157 | tan_pt2_x=c2_x-r_min*math.cos(theta) #tangent point in second circle of min_turning radius 158 | tan_pt2_y=c2_y+r_min*math.sin(theta) 159 | 160 | 161 | Ti = np.arctan2(current_x-c1_x,current_y-c1_y) - np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) #Initial Turning angle 162 | 163 | Tf = np.arctan2(goal_x - c2_x,goal_y- c2_y) - np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) #Final Turning angle 164 | 165 | if Ti<0: 166 | Ti+=2*np.pi 167 | if Tf<0: 168 | Tf+=2*np.pi 169 | 170 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 171 | 172 | Ti= Ti*180/np.pi 173 | # print Ti, current_x,current_y,current_th,goal_x,goal_y,goal_th 174 | 175 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"LR"] 176 | return ans 177 | 178 | def RSL(current_x,current_y,current_th,goal_x,goal_y,goal_th): 179 | c1_x=current_x+r_min*math.cos(math.radians(current_th)-(PI/2)) #center of first circle of min_turning radius 180 | c1_y=current_y+r_min*math.sin(math.radians(current_th)-(PI/2)) 181 | 182 | c2_x=goal_x-r_min*math.cos(math.radians(goal_th)-(PI/2)) #center of second circle of min_turning radius 183 | c2_y=goal_y-r_min*math.sin(math.radians(goal_th)-(PI/2)) 184 | 185 | S1 = ((c2_x-c1_x)**2 + (c2_y-c1_y)**2)**0.5 #Distance from (c1_x,c1_y) to (c2_x,c2_y) 186 | # print (tan_pt1_x,tan_pt1_y) 187 | # print (tan_pt2_x,tan_pt2_y) 188 | if(-r_min**2 + (S1**2)/4.0 )<0: 189 | # print"Holaaaaaa" 190 | return None 191 | S = 2*((-r_min**2 + (S1**2)/4.0 )**0.5) 192 | 193 | # if c2_x-c1_x<0: 194 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y+2*r_min)) 195 | # elif c2_x-c1_x>0: 196 | # theta = (np.arctan2((c2_x-c1_x), c2_y-c1_y-2*r_min)) 197 | 198 | theta = + (np.arctan2((c2_x-c1_x), c2_y-c1_y)) + np.arctan2(r_min,S/2.0) 199 | 200 | # print "THETA ", theta, "c1_x =",c1_x, c1_y,c2_x,c2_y,"S=",S 201 | 202 | tan_pt1_x=c1_x-r_min*math.cos(theta) #tangent point in first circle of min_turning radius 203 | tan_pt1_y=c1_y+r_min*math.sin(theta) 204 | 205 | tan_pt2_x=c2_x+r_min*math.cos(theta) #tangent point in second circle of min_turning radius 206 | tan_pt2_y=c2_y-r_min*math.sin(theta) 207 | 208 | 209 | Ti = np.arctan2(tan_pt1_x-c1_x,tan_pt1_y-c1_y) - np.arctan2(current_x-c1_x,current_y-c1_y) #Initial Turning angle 210 | 211 | Tf = np.arctan2(tan_pt2_x - c2_x,tan_pt2_y- c2_y) - np.arctan2(goal_x - c2_x,goal_y- c2_y) #Final Turning angle 212 | 213 | if Ti<0: 214 | Ti+=2*np.pi 215 | if Tf<0: 216 | Tf+=2*np.pi 217 | 218 | total_dist = Ti*r_min + Tf*r_min + S #Total Distance from current to Goal 219 | 220 | Ti= Ti*180/np.pi 221 | # print Ti, current_x,current_y,current_th,goal_x,goal_y,goal_th 222 | 223 | ans=[Ti,Tf, total_dist,S,tan_pt1_x,tan_pt1_y,tan_pt2_x,tan_pt2_y,c1_x,c1_y,c2_x,c2_y,"RL"] 224 | # 0 1. 2. 3. 4. 5. 6. 7. 8. 9. 10. 11 225 | return ans 226 | 227 | tree = [Node(current_x,current_y)] 228 | 229 | def check_for_obstacles(x,y): 230 | for ox,oy,orad in obstacles: 231 | if ((x-ox)**2 + (y-oy)**2)**0.5 < orad*2: 232 | return False 233 | return True 234 | 235 | def findNearest(sample): 236 | min_d = sys.maxint 237 | ind=0 238 | min_node=tree[0] 239 | for indd,node in enumerate(tree): 240 | dis = ((node.x-sample.x)**2 + (node.y-sample.y)**2)**0.5 241 | if min_d>dis: 242 | min_d=dis 243 | min_node=node 244 | ind=indd 245 | return min_node,ind 246 | 247 | def find_nearby_nodes(newNode): 248 | radius = step*2 249 | dist_list=[] 250 | nearinds=[] 251 | for node in tree: 252 | minCost = float("inf") 253 | minPath = 0 254 | for i,path in enumerate([LSL,RSR,LSR,RSL]): 255 | path_ans = path(node.x,node.y,node.th,newNode.x,newNode.y,newNode.th) 256 | if path_ans: 257 | if minCost > path_ans[2]: 258 | minPath = i 259 | minCost = path_ans[2] 260 | dist_list.append(minCost) 261 | # dist_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2 for node in tree] 262 | # for i,(c,p) in enumerate(dist_list): 263 | # if c<=(radius ** 2): 264 | # nearinds.append(i) 265 | nearinds = [dist_list.index(i) for i in dist_list if i <= (radius ** 2)] 266 | return nearinds 267 | 268 | def check_collision_extend(node, theta, d): 269 | 270 | temp_node = copy.deepcopy(node) 271 | 272 | for i in range(int(d / step)): 273 | temp_node.x += step * math.cos(theta) 274 | temp_node.y += step * math.sin(theta) 275 | if not check_for_obstacles(temp_node.x,temp_node.y): 276 | return False 277 | if temp_node.x>1 or temp_node.y>1 or temp_node.x<0 or temp_node.y<0: 278 | return False 279 | return True 280 | 281 | def rewire(node, nearinds): 282 | for i in nearinds: 283 | nearNode = tree[i] 284 | dx = node.x - nearNode.x 285 | dy = node.y - nearNode.y 286 | # d = math.sqrt(dx ** 2 + dy ** 2) 287 | # d = [LSL,RSR,LSR,RSL][p](node.x,node.y,node.th,nearNode.x,nearNode.y,nearNode.th) 288 | 289 | d = float("inf") 290 | for path in [LSL,RSR,LSR,RSL]: 291 | path_ans = path(node.x,node.y,node.th,nearNode.x,nearNode.y,nearNode.th) 292 | if path_ans: 293 | d = min(d,path_ans[2]) 294 | 295 | if nearNode.cost>node.cost+d: 296 | theta = np.arctan2(dy, dx) 297 | if check_collision_extend(tree[i], theta, d): 298 | nearNode.p=tree.index(node) 299 | nearNode.cost=node.cost+d 300 | 301 | def choose_parent(newNode, nearinds): 302 | if not nearinds: 303 | return newNode 304 | dist_list = [] 305 | for i in nearinds: 306 | dx = newNode.x - tree[i].x 307 | dy = newNode.y - tree[i].y 308 | # d = math.sqrt(dx ** 2 + dy ** 2) 309 | # d = [LSL,RSR,LSR,RSL](newNode.x,newNode.y,newNode.th,tree[i].x,tree[i].y,tree[i].th) 310 | 311 | d = float("inf") 312 | for path in [LSL,RSR,LSR,RSL]: 313 | path_ans = path(newNode.x,newNode.y,newNode.th,tree[i].x,tree[i].y,tree[i].th) 314 | if path_ans: 315 | d = min(d,path_ans[2]) 316 | 317 | 318 | 319 | theta = np.arctan2(dy, dx) 320 | if check_collision_extend(tree[i], theta, d): 321 | dist_list.append(tree[i].cost + d) 322 | 323 | if not dist_list: 324 | return newNode 325 | 326 | mincost = min(dist_list) 327 | minind = nearinds[dist_list.index(mincost)] 328 | 329 | newNode.cost = mincost 330 | newNode.p = minind 331 | 332 | return newNode 333 | 334 | def RRT(): 335 | while True: 336 | sample = Node(random.uniform(0,1),random.uniform(0,1)) 337 | 338 | nearestNode,ind = findNearest(sample) 339 | newNode = copy.deepcopy(nearestNode) 340 | theta = (np.arctan2(sample.y-nearestNode.y, sample.x-nearestNode.x)) 341 | newNode.x+=step*math.cos(theta) 342 | newNode.y+=step*math.sin(theta) 343 | newNode.p=ind 344 | newNode.cost+=step 345 | newNode.th = theta 346 | # plt.plot(newNode.x,newNode.y,'r+') 347 | # plt.plot(nearestNode.x,nearestNode.y,'bo') 348 | # plt.pause(2) 349 | # plt.plot(nearestNode.x,nearestNode.y,'wo') 350 | # plt.pause(2) 351 | 352 | # if ((newNode.x-0.3)**2 + (newNode.y-0.3)**2)**0.5 < 0.2: 353 | # # plt.plot(newNode.x,newNode.y,'w+') 354 | # # plt.pause(2) 355 | # continue 356 | # if ((newNode.x-0.6)**2 + (newNode.y-0.5)**2)**0.5 < 0.2: 357 | # # plt.plot(newNode.x,newNode.y,'w+') 358 | # # plt.pause(2) 359 | # continue 360 | # if ((newNode.x-0.3)**2 + (newNode.y-0.5)**2)**0.5 < 0.2: 361 | # # plt.plot(newNode.x,newNode.y,'w+') 362 | # # plt.pause(2) 363 | # continue 364 | if not check_for_obstacles(newNode.x,newNode.y): 365 | continue 366 | if newNode.x>1 or newNode.y>1 or newNode.x<0 or newNode.y<0: 367 | # plt.plot(newNode.x,newNode.y,'w+') 368 | # plt.pause(2) 369 | continue 370 | nearbyNodes = find_nearby_nodes(newNode) 371 | newNode = choose_parent(newNode, nearbyNodes) 372 | # plt.plot(newNode.x,newNode.y,'bo') 373 | # plt.pause(2) 374 | # plt.plot(newNode.x,newNode.y,'wo') 375 | # plt.pause(2) 376 | tree.append(newNode) 377 | rewire(newNode,nearbyNodes) 378 | 379 | if ((newNode.x-goal_x)**2 + (newNode.y-goal_y)**2)**0.5 < 0.1: 380 | break 381 | # DrawGraph(sample) 382 | path=[[newNode.x,newNode.y]] 383 | while True: 384 | if not newNode.p: 385 | break 386 | #plt.plot([newNode.x,newNode.y],[tree[newNode.p].x,tree[newNode.p].y],'ro-') 387 | path.append([tree[newNode.p].x,tree[newNode.p].y]) 388 | newNode=tree[newNode.p] 389 | x=[goal_x] 390 | y=[goal_y] 391 | for i in path: 392 | x.append(i[0]) 393 | y.append(i[1]) 394 | # plt.plot(i[0],i[1],'o-') 395 | # plt.pause(0.5) 396 | x.append(current_x) 397 | y.append(current_y) 398 | # print x,y 399 | nx=x[::-1][1:] 400 | ny=y[::-1][1:] 401 | 402 | sx=0 403 | sy=0 404 | st=0 405 | for a,b in zip(nx,ny): 406 | ex,ey=a,b 407 | et=(np.arctan2(ey-goal_y, ex-goal_x)) 408 | 409 | # dubins 410 | paths=[LSL,RSR,LSR,RSL] 411 | minS=float("inf") 412 | ans=[] 413 | count=0 414 | curve="" 415 | ans_count=1 416 | for i in paths: 417 | path_ans=i(sx,sy,st,ex,ey,et) 418 | count+=1 419 | if path_ans: 420 | if minS>path_ans[3]: 421 | ans=path_ans 422 | minS=path_ans[3] 423 | ans_count=count 424 | curve=path_ans[12] 425 | 426 | plt.plot([ans[4],ans[6]],[ans[5],ans[7]]) 427 | 428 | start_angle1 = np.arctan2(sy - ans[9], sx - ans[8]) * 180 / np.pi 429 | end_angle1 = np.arctan2(ans[5] - ans[9], ans[4] - ans[8]) * 180 / np.pi 430 | 431 | if curve[0]=="L": 432 | arc1 = patches.Arc((ans[8], ans[9]), 2*r_min,2*r_min,theta1=start_angle1, theta2=end_angle1) 433 | else: 434 | arc1 = patches.Arc((ans[8], ans[9]), 2*r_min,2*r_min,theta1=end_angle1, theta2=start_angle1) 435 | plt.gca().add_patch(arc1) 436 | 437 | start_angle2 = np.arctan2(ans[7] - ans[11], ans[6] - ans[10]) * 180 / np.pi 438 | end_angle2 = np.arctan2(ey - ans[11], ex - ans[10]) * 180 / np.pi 439 | 440 | if curve[1]=="L": 441 | arc2 = patches.Arc((ans[10], ans[11]), 2*r_min,2*r_min,theta1=start_angle2, theta2=end_angle2) 442 | else: 443 | arc2 = patches.Arc((ans[10], ans[11]), 2*r_min,2*r_min,theta1=end_angle2, theta2=start_angle2) 444 | 445 | plt.gca().add_patch(arc2) 446 | 447 | plt.gca().set_aspect('equal') 448 | 449 | # 450 | sx,sy,st=ex,ey,et 451 | # plt.plot(x, y, 'o-') 452 | # plt.pause(0.05) 453 | plt.show() 454 | RRT() -------------------------------------------------------------------------------- /rrt_star_dubins/rrt_star_with_random_samplings.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/rrt_star_dubins/rrt_star_with_random_samplings.png -------------------------------------------------------------------------------- /rrt_star_dubins/rrt_star_with_random_samplings2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/rrt_star_dubins/rrt_star_with_random_samplings2.png -------------------------------------------------------------------------------- /rrt_star_dubins/rrtstar_minimized_dubins_paths.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/rrt_star_dubins/rrtstar_minimized_dubins_paths.png -------------------------------------------------------------------------------- /rrt_star_dubins/rrtstar_minimized_dubins_paths2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srnand/Mobile-Robots-Autonomous-Navigation/988336e40d4259a817912025b95a35135367f7a1/rrt_star_dubins/rrtstar_minimized_dubins_paths2.png --------------------------------------------------------------------------------