def buildup(state):
child = self.CircleNode(state[0], state[1], state[2], state[3])
# build the child
child.set_parent(circle)
# child.h = self.distance(child, self.goal)
child.h = reeds_shepp.path_length(
(child.x, child.y, child.a), (self.goal.x, self.goal.y, self.goal.a), 1. / self.maximum_curvature)
child.g = circle.g + reeds_shepp.path_length(
(circle.x, circle.y, circle.a), (child.x, child.y, child.a), 1. / self.maximum_curvature)
child.f = child.g + child.h
# add the child to expansion set
return child
def reeds_shepp_path_planning(start_x, start_y, start_yaw,
end_x, end_y, end_yaw, curvature):
q0 = [start_x, start_y, start_yaw]
q1 = [end_x, end_y, end_yaw]
step_size = 0.1
qs = reeds_shepp.path_sample(q0, q1, curvature, step_size)
xs = [q[0] for q in qs]
ys = [q[1] for q in qs]
yaw = [q[2] for q in qs]
xs.append(end_x)
ys.append(end_y)
yaw.append(end_yaw)
clen = reeds_shepp.path_length(q0, q1, curvature)
pathtypeTuple = reeds_shepp.path_type(q0, q1, curvature)
ptype = ""
for t in pathtypeTuple:
if t == 1:
ptype += "L"
elif t == 2:
ptype += "S"
elif t == 3:
ptype += "R"
return xs, ys, yaw, ptype, clen
def step(self, action):
# Need to add path planner
truck_trgt, excvtr_trgt = action
truck_trgt = np.clip(truck_trgt, self.action_space[0].low, self.action_space[0].high)
excvtr_trgt = np.clip(excvtr_trgt, self.action_space[1].low, self.action_space[1].high)
print('truck_trgt is {}, excvtr_trgt is {}'.format(truck_trgt, excvtr_trgt))
truck_current = [self.state[0], self.state[1], self.state[2]]
excvtr_current = [self.state[3], self.state[4], self.state[5]]
self.truck_path = self.truck_planner.get_path(truck_current,
truck_trgt)
self.excvtr_path = self.excvtr_planner.get_path(excvtr_current,
excvtr_trgt)
self.step_counter = 0
self.cntr += 1
observation = [self.state, self.state]
reward = -reeds_shepp.path_length(tuple(truck_current), tuple(
excvtr_current), self.truck_planner.rho)
done = False
if self.cntr >= 20:
done = True # reaches time limit
elif self._can_load_on_truck():
done = True
reward = 8000
info = None
return observation, [reward, reward], [done, done], info
def plan_motions(index_sec):
motion = []
v0, v1 = 0, 0
seg = discontinuities2[index_sec[0]:index_sec[1] + 1]
seg = list(zip(seg[:-1], seg[1:]))
extent = 0
for sec in seg:
s0, s1 = sec[0].state, sec[1].state
extent += np.abs(
reeds_shepp.path_length(s0, s1,
1. / self.maximum_curvature))
samples = []
for sec in seg:
s0, s1 = sec[0].state, sec[1].state
samples.extend(
reeds_shepp.path_sample(s0, s1,
1. / self.maximum_curvature, res))
acc = min([(v_max**2 - v1**2) / extent, a_cc])
vcc = np.sqrt(v1**2 + acc * extent)
for i, sample in enumerate(samples):
if i * res < extent / 2.:
vt = min([np.sqrt(v0**2 + 2 * acc * (i * res)), vcc])
else:
vt = min([
np.sqrt(v1**2 + 2 * acc * np.abs(extent - i * res)),
vcc
])
motion.append(
self.Configuration(sample[:3],
k=sample[3],
v=np.sign(sample[4]) * vt))
return motion
def main():
# points to be followed
pts = [(-6, -7), (-6, 0), (-4, 6), (0, 5), (0, -2), (-2, -6), (3, -5),
(3, 6), (6, 4)]
# generate PATH so the vectors are pointing at each other
PATH = []
for i in range(len(pts) - 1):
dx = pts[i + 1][0] - pts[i][0]
dy = pts[i + 1][1] - pts[i][1]
theta = math.atan2(dy, dx)
PATH.append((pts[i][0], pts[i][1], utils.rad2deg(theta)))
PATH.append((pts[-1][0], pts[-1][1], 0))
# or you can also manually set the angles:
# PATH = [(-5,5,90),(-5,5,-90),(1,4,180), (5,4,0), (6,-3,90), (4,-4,-40),(-2,0,240), \
# (-6, -7, 160), (-7,-1,80)]
# or just generate a random route:
# PATH = []
# for _ in range(10):
# PATH.append((rd.randint(-7,7), rd.randint(-7,7), rd.randint(0,359)))
# init turtle
tesla = turtle.Turtle()
tesla.speed(0) # 0: fast; 1: slow, 8.4: cool
tesla.shape('arrow')
tesla.resizemode('user')
tesla.shapesize(1, 1)
# draw vectors representing points in PATH
for pt in PATH:
draw.goto(tesla, pt)
draw.vec(tesla)
# draw all routes found
tesla.speed(0)
for i in range(len(PATH) - 1):
paths = rs.get_all_paths(PATH[i], PATH[i + 1])
for path in paths:
draw.set_random_pencolor(tesla)
draw.goto(tesla, PATH[i])
draw.draw_path(tesla, path)
# draw shortest route
tesla.pencolor(1, 0, 0)
tesla.pensize(3)
tesla.speed(10)
draw.goto(tesla, PATH[0])
path_length = 0
for i in range(len(PATH) - 1):
path = rs.get_optimal_path(PATH[i], PATH[i + 1])
path_length += rs.path_length(path)
draw.draw_path(tesla, path)
print("Shortest path length: {} px.".format(int(draw.scale(path_length))))
turtle.done()
def calculate_path_length(path, rho):
sequence = map(deepcopy, path)
sequence = map(list, sequence)
sequence = zip(sequence[:-1], sequence[1:])
length = 0
for x_from, x_to in sequence:
length += reeds_shepp.path_length(x_from, x_to, rho)
return length
def get_heuristic(self, grid, goal_grid):
#dx = goal_grid[0] - grid[0]
#dy = goal_grid[1] - grid[1]
#h_state = math.sqrt(dx ** 2 + dy ** 2)
#dist = 0.0
#if self.iterations % 1000 == 0:
# dist = reeds_shepp.path_length(grid, goal_grid, self.rho)
dist = reeds_shepp.path_length(grid, goal_grid, self.rho)
return dist
def plot_table(cols):
rows = ((len(qs)) / cols)
for i, (q0, q1) in enumerate(qs):
plt.subplot(rows, cols, i + 1)
plot_path(q0, q1)
dist = reeds_shepp.path_length(q0, q1, rho)
plt.title('length: {:.2f}'.format(dist))
plt.savefig('fig/demo.png')
plt.show()
def plot_table(cols):
rows = ((len(qs)) / cols)
for i,(q0, q1) in enumerate(qs):
plt.subplot(rows, cols, i+1)
plot_path(q0, q1)
dist = reeds_shepp.path_length(q0, q1, rho)
plt.title('length: {:.2f}'.format(dist))
plt.savefig('fig/demo.png')
plt.show()
def steer(self, p, q, eps):
delta = 0.2
length = rp.path_length(p, q, self.turn_radius)
points = rp.path_sample(p, q, self.turn_radius, delta)
points.append(q)
path = np.array(points)
if length > eps:
num_points = math.floor(eps / delta)
path = path[0:num_points + 1]
return Curve(path)
def plan_motions(sector):
q0, v0, q1, v1 = sector[0].state, sector[0].v, sector[
1].state, sector[1].v
extent = reeds_shepp.path_length(q0, q1,
1. / self.maximum_curvature)
acc = min([(v_max**2 - v1**2) / extent, a_cc])
vcc = np.sqrt(v1**2 + acc * extent)
samples = reeds_shepp.path_sample(q0, q1,
1. / self.maximum_curvature, res)
for i, sample in enumerate(samples):
if i * res < extent / 2.:
vt = min([np.sqrt(v0**2 + 2 * acc * (i * res)), vcc])
else:
vt = min(
[np.sqrt(v1**2 + 2 * acc * (extent - i * res)), vcc])
motions.append(
self.Configuration(sample[:3],
k=sample[3],
v=np.sign(sample[4]) * vt))
def trajectory_generation(self):
# DEFINIZIONE DELLA COORDINATA INIZIALE E DI QUELLA FINALE
coordinate = [(0.0,0.0,0.0),(3.0,-4.0,np.pi)]
# PARAMETRI UTILI ALLA GENERAZIONE DELLE CURVE DI REEDSSHEPP
step_size = 0.01823
rho = 5.8
# GENERAZIONE DELLE CURVE DI REEDS SHEPP
qs = reeds_shepp.path_sample(coordinate[0], coordinate[1], rho, step_size)
tmax = np.round(reeds_shepp.path_length(coordinate[0], coordinate[1], rho),2)
# ESTRAZIONE DEI PARAMETRI LUNGO I DUE ASSI E ARROTONDAMENTO ALLA TERZA DECIMALE
xs = np.round([coordinate[0] for coordinate in qs],3)
ys = np.round([coordinate[1] for coordinate in qs],3)
theta = np.round([coordinate[2] for coordinate in qs],3)
self.t = np.linspace(0,tmax,len(xs))
# Richiamo il metodo "trajectory" per l'estrazione dei segmenti rappresentanti le curve di Reeds Shepp a partire dalle circonferenze calcolate
v_d_val = 0.5 # m/s
element = 0
u1 = np.zeros(len(xs))
while element < len(theta)-1:
if theta[element] < np.pi/2 and theta[element] > -np.pi/2:
if xs[element] < xs[element+1]:
u1[element] = 1
else:
u1[element] = -1
else:
if xs[element] > xs[element+1]:
u1[element] = 1
else:
u1[element] = -1
element = element+1
self.dotx_d = u1*np.cos(theta)
self.doty_d = u1*np.sin(theta)
self.v_d = np.sqrt(self.dotx_d**2 + self.doty_d**2)
self.theta_d = np.arctan2(self.doty_d, self.dotx_d)
self.x_d = xs
self.y_d = ys
def initialize(self, start, goal, grid_map, grid_res, grid_ori, obstacle=255):
# type: (CircleNode, CircleNode, np.ndarray, float, CircleNode, int) -> OrientationSpaceExplorer
"""
:param start: start circle-node
:param goal: goal circle-node
:param grid_map: occupancy map(0-1), 2d-square, with a certain resolution: gird_res
:param grid_res: resolution of occupancy may (m/pixel)
:param obstacle: value of the pixels of obstacles region on occupancy map.
"""
self.start, self.goal = start, goal
self.grid_map, self.grid_res, self.grid_ori, self.obstacle = grid_map, grid_res, grid_ori, obstacle
# padding grid map for clearance calculation
s = int(np.ceil((self.maximum_radius + self.minimum_clearance) / self.grid_res))
self.grid_pad = np.pad(self.grid_map, ((s, s), (s, s)), 'constant',
constant_values=((self.obstacle, self.obstacle), (self.obstacle, self.obstacle)))
# complete the start and goal
self.start.r, self.start.g = self.clearance(self.start) - self.minimum_clearance, 0
self.start.h = reeds_shepp.path_length(
(start.x, start.y, start.a), (self.goal.x, self.goal.y, self.goal.a), 1. / self.maximum_curvature)
self.goal.r, self.goal.h, self.goal.g = self.clearance(self.goal) - self.minimum_clearance, 0, np.inf
self.start.f, self.goal.f = self.start.g + self.start.h, self.goal.g + self.goal.h
return self
import reeds_shepp as rs
import utils
import math
# a list of "vectors" (x, y, angle in degrees)
ROUTE = [(-2, 4, 180), (2, 4, 0), (2, -3, 90), (-5, -6, 240), (-6, -7, 160),
(-7, -1, 80)]
full_path = []
total_length = 0
for i in range(len(ROUTE) - 1):
path = rs.get_optimal_path(ROUTE[i], ROUTE[i + 1])
full_path += path
total_length += rs.path_length(path)
print("Shortest path length: {}".format(round(total_length, 2)))
for e in full_path:
print(e)
qd = [xs, ys, thetas]
return qd
def plot_table(cols):
rows = ((len(qs)) / cols)
for i, (q0, q1) in enumerate(qs):
plt.subplot(rows, cols, i + 1)
plot_path(q0, q1)
dist = reeds_shepp.path_length(q0, q1, rho)
plt.title('length: {:.2f}'.format(dist))
plt.savefig('fig/demo.png')
plt.show()
if __name__ == "__main__":
q = [(0.0, 0.0, 0.0), (3.0, -4.0, np.pi)]
q0 = q[0]
q1 = q[1]
qd = plot_path(q0, q1)
print("asse X:")
x = np.round(qd[0], 3)
print(x)
# PLOT
tmax = reeds_shepp.path_length(q[0], q[1], rho)
print(tmax)
dist = reeds_shepp.path_length(q0, q1, rho)
plt.title('length: {:.2f}'.format(dist))
plt.savefig('fig/demo.png')
plt.show()
qs = reeds_shepp.path_sample(q0, q1, rho, step_size)
xs = [q[0] for q in qs]
ys = [q[1] for q in qs]
plt.plot(xs, ys, 'b-')
plt.plot(xs, ys, 'r.')
plot_car(q0)
plot_car(q1)
plt.axis('equal')
def plot_table(cols):
rows = ((len(qs)) / cols)
for i, (q0, q1) in enumerate(qs):
plt.subplot(rows, cols, i + 1)
plot_path(q0, q1)
dist = reeds_shepp.path_length(q0, q1, rho)
plt.title('length: {:.2f}'.format(dist))
plt.savefig('fig/demo.png')
plt.show()
if __name__ == "__main__":
q = [(4.0, 4.0, 0.0), (3.0, 4.0, np.pi / 2)]
q0 = q[0]
q1 = q[1]
plot_path(q0, q1)
dist = reeds_shepp.path_length(q0, q1, rho)
plt.title('length: {:.2f}'.format(dist))
plt.savefig('fig/demo.png')
plt.show()
#plot_table(3)
def cost(self, x_from, x_to): # type: (StateNode, StateNode) -> float
"""calculate the cost from one state to another state"""
return reeds_shepp.path_length(x_from.state, x_to.state, 1. / self.maximum_curvature)
def step(self, action):
"""
Input: list(np.array([1x2]), np.array([1x2]))
Output: None
Use: Perform a step in the simulation environment.
"""
# scaled target state
scl_truck_trgt, scl_excvtr_trgt = action
# unscaled target state
truck_trgt = self.truck_low + np.multiply(scl_truck_trgt,
self.truck_range)
excvtr_trgt = self.excvtr_low + np.multiply(scl_excvtr_trgt,
self.excvtr_range)
# radian state
rd_state = self._get_radian_state()
truck_state = rd_state[:3].tolist()
truck_trgt = truck_trgt.tolist()
excvtr_state = rd_state[3:-1].tolist()
excvtr_trgt = excvtr_trgt.tolist()
# get reeds-shepp path
self.truck_path = self.truck_planner.get_path(truck_state, truck_trgt)
self.excvtr_path = self.excvtr_planner.get_path(
excvtr_state, excvtr_trgt)
# set_trace()
self.step_counter = 0
# self.counter += 1
info = [None, None]
# execute first 60 points on the path if collides terminate the episode
for _ in range(60):
if self.if_render:
self.render()
obsv, flag = self._update_state()
if flag:
# set_trace()
# print('collision occurred')
return obsv, [-1000, -1000], [True, True], info
# get current scaled state
scaled_state = self._get_scaled_state()
observation = [scaled_state, scaled_state]
# get current radian state
rd_state = self._get_radian_state()
truck_state = rd_state[:3].tolist()
excvtr_pos = rd_state[3:5]
arm_ori = rd_state[-1]
arm_pos = excvtr_pos + np.array(
[130 * math.cos(arm_ori), 130 * math.sin(arm_ori)])
arm_state = np.concatenate((arm_pos, np.pi / 2), axis=None)
# calculate reward
reward = -reeds_shepp.path_length(tuple(truck_state), tuple(arm_state),
self.truck_planner.rho)
# check for termination
done = False
if self._can_load_on_truck():
done = True
reward = 8000
# self.success_counter += 1
# print('loads on truck, count: {}'.format(self.success_counter))
return observation, [reward, reward], [done, done], info
def getDistance(self, p1, p2):
length = rp.path_length(p1, p2, self.turn_radius)
return length
