-
Notifications
You must be signed in to change notification settings - Fork 11
/
Copy pathutils.py
155 lines (120 loc) · 4.82 KB
/
utils.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
import math
import numpy as np
import os
import sys
import env
from rrt import Node
class Utils:
def __init__(self):
self.env = env.Env()
self.delta = 0.5
self.obs_circle = self.env.obs_circle
self.obs_rectangle = self.env.obs_rectangle
self.obs_boundary = self.env.obs_boundary
def update_obs(self, obs_cir, obs_bound, obs_rec):
self.obs_circle = obs_cir
self.obs_boundary = obs_bound
self.obs_rectangle = obs_rec
def get_obs_vertex(self):
delta = self.delta
obs_list = []
for (ox, oy, w, h) in self.obs_rectangle:
vertex_list = [[ox - delta, oy - delta],
[ox + w + delta, oy - delta],
[ox + w + delta, oy + h + delta],
[ox - delta, oy + h + delta]]
obs_list.append(vertex_list)
return obs_list
def is_intersect_rec(self, start, end, o, d, a, b):
v1 = [o[0] - a[0], o[1] - a[1]]
v2 = [b[0] - a[0], b[1] - a[1]]
v3 = [-d[1], d[0]]
div = np.dot(v2, v3)
if div == 0:
return False
t1 = np.linalg.norm(np.cross(v2, v1)) / div
t2 = np.dot(v1, v3) / div
if t1 >= 0 and 0 <= t2 <= 1:
shot = Node((o[0] + t1 * d[0], o[1] + t1 * d[1]))
dist_obs = self.get_dist(start, shot)
dist_seg = self.get_dist(start, end)
if dist_obs <= dist_seg:
return True
return False
def is_intersect_circle(self, o, d, a, r):
d2 = np.dot(d, d)
delta = self.delta
if d2 == 0:
return False
t = np.dot([a[0] - o[0], a[1] - o[1]], d) / d2
if 0 <= t <= 1:
shot = Node((o[0] + t * d[0], o[1] + t * d[1]))
if self.get_dist(shot, Node(a)) <= r + delta:
return True
return False
def is_collision(self, start, end):
if self.is_inside_obs(start) or self.is_inside_obs(end):
return True
o, d = self.get_ray(start, end)
obs_vertex = self.get_obs_vertex()
for (v1, v2, v3, v4) in obs_vertex:
if self.is_intersect_rec(start, end, o, d, v1, v2):
return True
if self.is_intersect_rec(start, end, o, d, v2, v3):
return True
if self.is_intersect_rec(start, end, o, d, v3, v4):
return True
if self.is_intersect_rec(start, end, o, d, v4, v1):
return True
for (x, y, r) in self.obs_circle:
if self.is_intersect_circle(o, d, [x, y], r):
return True
return False
def is_inside_obs(self, node):
delta = self.delta
for (x, y, r) in self.obs_circle:
if math.hypot(node.x - x, node.y - y) <= r + delta:
return True
for (x, y, w, h) in self.obs_rectangle:
if 0 <= node.x - (x - delta) <= w + 2 * delta \
and 0 <= node.y - (y - delta) <= h + 2 * delta:
return True
for (x, y, w, h) in self.obs_boundary:
if 0 <= node.x - (x - delta) <= w + 2 * delta \
and 0 <= node.y - (y - delta) <= h + 2 * delta:
return True
return False
@staticmethod
def get_ray(start, end):
orig = [start.x, start.y]
direc = [end.x - start.x, end.y - start.y]
return orig, direc
@staticmethod
def get_dist(start, end):
return math.hypot(end.x - start.x, end.y - start.y)
@staticmethod
def integrate_single_integrator(self, x_init, u, dt):
# dimension 2, position [x1,x2]^T, control u = [u1,u2]^T, dt comes from control updates
x_traj = np.array([x_init])
x_current = x_traj.copy()
num_steps = len(u)
for i in range(num_steps):
u_current = u[i]
x_current[0, 0] = x_current[0, 1] + dt * u_current[0]
x_current[0, 1] = x_current[0, 1] + dt * u_current[1]
x_traj = np.concatenate((x_traj, x_current), axis=0)
return x_traj
@staticmethod
def integrate_double_integrator(x_init, u, dt):
# dimension 4, [x1,x2,x3,x4]^T [pos_1, vel_1, pos_2, vel_2]^T, control u = [u1,u2]^T, dt comes from control updates
x_traj = np.array([x_init],dtype=np.float32)
x_current = x_traj.copy()
num_steps = len(u)
for i in range(num_steps):
u_current = u[i]
x_current[0,0] = x_current[0,0] + x_current[0,1] * dt + 0.5 * u_current[0] * dt ** 2
x_current[0,1] = x_current[0,1] + dt * u_current[0]
x_current[0,2] = x_current[0,2] + x_current[0,3] * dt + 0.5 * u_current[1] * dt ** 2
x_current[0,3] = x_current[0,3] + dt * u_current[1]
x_traj = np.concatenate((x_traj, x_current), axis=0)
return x_traj