mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-30 00:50:46 +08:00
391311qy
40 lines
1.4 KiB
Python
40 lines
1.4 KiB
Python
# this is the three dimensional configuration space for rrt
|
|
#!/usr/bin/env python3
|
|
# -*- coding: utf-8 -*-
|
|
"""
|
|
@author: yue qi
|
|
"""
|
|
import numpy as np
|
|
|
|
def getblocks(resolution):
|
|
# AABBs
|
|
block = [[3.10e+00,0.00e+00,2.10e+00,3.90e+00,5.00e+00,6.00e+00],
|
|
[9.10e+00,0.00e+00,2.10e+00,9.90e+00,5.00e+00,6.00e+00],
|
|
[1.51e+01,0.00e+00,2.10e+00,1.59e+01,5.00e+00,6.00e+00],
|
|
[1.00e-01,0.00e+00,0.00e+00,9.00e-01,5.00e+00,3.90e+00],
|
|
[6.10e+00,0.00e+00,0.00e+00,6.90e+00,5.00e+00,3.90e+00],
|
|
[1.21e+01,0.00e+00,0.00e+00,1.29e+01,5.00e+00,3.90e+00],
|
|
[1.81e+01,0.00e+00,0.00e+00,1.89e+01,5.00e+00,3.90e+00]]
|
|
Obstacles = []
|
|
for i in block:
|
|
i = np.array(i)
|
|
Obstacles.append((i[0]/resolution,i[1]/resolution,i[2]/resolution,i[3]/resolution,i[4]/resolution,i[5]/resolution))
|
|
return np.array(Obstacles)
|
|
|
|
class env():
|
|
def __init__(self,xmin=0,ymin=0,zmin=0,xmax=20,ymax=5,zmax=6,resolution=1):
|
|
self.resolution = resolution
|
|
self.boundary = np.array([xmin,ymin,zmin,xmax,ymax,zmax])/resolution
|
|
self.blocks = getblocks(resolution)
|
|
self.start = np.array([0.5, 2.5, 5.5])
|
|
self.goal = np.array([19.0, 2.5, 5.5])
|
|
|
|
def visualize(self):
|
|
# fig = plt.figure()
|
|
# TODO: do visualizations
|
|
return
|
|
|
|
if __name__ == '__main__':
|
|
newenv = env()
|
|
X = StateSpace(newenv.boundary,newenv.resolution)
|
|
print(X) |