def random_state(self) -> DubinsConfig:
seed = np.random.rand(3, 1)
state = self.origin + DubinsConfig(
float(seed[0]) * self.space_width,
float(seed[1]) * self.space_height,
float(seed[2]) * self.phi_span)
return state
def __init__(self,
phi_max=math.pi / 2,
s=1,
rou_min=0.5,
N=1000,
K=15,
cspace=None,
ob_center: list = None,
ob_radio: list = None):
if cspace is None:
self.cspace = [(-3, 3), (-1, 1)]
else:
self.cspace = [(-3, 3), (-1, 1)]
self.origin = DubinsConfig(self.cspace[0][0], self.cspace[1][0],
-phi_max)
self.space_width = self.cspace[0][1] - self.cspace[0][0]
self.space_height = self.cspace[1][1] - self.cspace[1][0]
self.obs = dict()
self.N = N
self.K = K
for index, ct in enumerate(ob_center):
self.obs[ct] = ob_radio[index]
self.speed = s
self.turnning_radius = rou_min
self.phi_max = phi_max
self.phi_span = phi_max * 2
def check_collision_edge(self, edge, collision_margin=0):
configurations = DubinsConfig.from_list(edge.configurations[1:])
for config in configurations:
if self.is_out_boundary(config):
return True
for ct, r in self.obs.items():
if config.line_distance_point(ct) - r <= collision_margin:
return True
return False
def random_state(self, p_thres, qG):
p = np.random.random()
if p < p_thres:
return qG
else:
seed = np.random.rand(3, 1)
state = self.origin + DubinsConfig(
float(seed[0]) * self.space_width,
float(seed[1]) * self.space_height,
float(seed[2]) * self.phi_span)
return state
x1 = 10
y1 = 9.9
theta1 = 0
q0 = (-1.2199559242110303, -0.8651945309735829, 6.074952869120775)
q1 = (-0.7607923756239869, -0.7272899164210449, 0.7917675619411886)
q0 = (-2, -0.5, 0)
q1 = (1.195952357338463, 0.26702924459657296, 0.9756583154337937)
q2 = (-0.9676497498599093, -0.9790110699545767, 4.2723201582020955)
turning_radius = 0.5
step_size = 0.01
# path = dubins.shortest_path(q0, q1, turning_radius)
# e = path.path_length()
# configurations, d = path.sample_many(step_size)
conf0 = DubinsConfig.from_tuple(q0)
conf1 = DubinsConfig.from_tuple(q1)
conf, length = DubinsConfig.shortest_path(conf0, conf1, turning_radius,
step_size)
nd0 = Node(conf0)
nd1 = Node(conf1)
ed = Edge(nd0, nd1, turning_radius)
s = 1
phi_max = 1
rou_min = 0.5
ob_center = [(0, -1), (0, 1)]
dt = 0.2
ob_r = [1 - dt, 1 - dt]
obs.append(
generate_circle(ob_center[0], ob_r[0], th0=0, th1=np.pi, num=100))
obs.append(
generate_circle(ob_center[1], ob_r[1], th0=0, th1=-np.pi, num=100))
obs_ct = Point.point_from_list(ob_center)
path = []
rrt = RRt(phi_max=phi_max,
s=s,
rou_min=rou_min,
cspace=cspace,
ob_center=obs_ct,
ob_radio=ob_r)
qI = (-2, -0.5, 0)
qG = (2, -0.5, math.pi / 2)
qi = DubinsConfig(qI[0], qI[1], qI[2])
qg = DubinsConfig(qG[0], qG[1], qG[2])
G, path_nd = rrt.generate_graph(qi, qg, K=100, goal_margin=0, p_thres=0.1)
for indx in range(len(path_nd) - 1):
ed = G.edgedict[(path_nd[indx], path_nd[indx + 1])]
config = ed.configurations
for conf in config:
path.append(conf)
fig, ax = plt.subplots()
#G.draw(ax)
obstacles = []
dc.draw(ax, cspace, obs, qI, qG, G, path, title="RRT planning")
plt.show(ax)
def is_achieved(self, x: Node, xG: DubinsConfig, margin=0):
config = x.item
if xG.is_close(config):
return True
return False