def main():
x0 = np.array([[0.0], [0.0], [0.3], [0.0]])
x = np.copy(x0)
for i in range(50):
ox, dx, otheta, dtheta, ou = mpc_control(x)
u = ou[0]
x = simulation(x, u)
print(i)
print(x)
print(u)
plt.clf()
px = float(x[0])
theta = float(x[2])
show_cart(px, theta)
plt.xlim([-5.0, 2.0])
plt.pause(0.001)
# plt.show()
if animation:
matplotrecorder.save_frame()
if animation:
matplotrecorder.save_movie("animation.gif", 0.1)
def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
T = 500.0 # max simulation time
goal_dis = 0.3
stop_speed = 0.05
state = unicycle_model.State(x=-0.0, y=-0.0, yaw=0.0, v=0.0)
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = calc_nearest_index(state, cx, cy, cyaw)
e, e_th = 0.0, 0.0
while T >= time:
dl, target_ind, e, e_th = lqr_steering_control(state, cx, cy, cyaw, ck,
e, e_th)
ai = PIDControl(speed_profile[target_ind], state.v)
# state = unicycle_model.update(state, ai, di)
state = unicycle_model.update(state, ai, dl)
if abs(state.v) <= stop_speed:
target_ind += 1
time = time + unicycle_model.dt
# check goal
dx = state.x - goal[0]
dy = state.y - goal[1]
if math.sqrt(dx**2 + dy**2) <= goal_dis:
print("Goal")
break
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
if target_ind % 1 == 0:
plt.cla()
plt.plot(cx, cy, "-r", label="course")
plt.plot(x, y, "ob", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2)) +
",target index:" + str(target_ind))
plt.pause(0.0001)
matplotrecorder.save_frame() # save each frame
plt.close()
return t, x, y, yaw, v
def show_trajectory(target, xc, yc):
plt.clf()
plot_arrow(target.x, target.y, target.yaw)
plt.plot(xc, yc, "-r")
plt.axis("equal")
plt.grid(True)
plt.pause(0.1)
matplotrecorder.save_frame()
def main():
print(__file__ + " start!!")
# start and goal position
sx = 10.0 # [m]
sy = 10.0 # [m]
gx = 50.0 # [m]
gy = 50.0 # [m]
grid_size = 1.0 # [m]
robot_size = 1.0 # [m]
ox = []
oy = []
for i in range(60):
ox.append(i)
oy.append(0.0)
for i in range(60):
ox.append(60.0)
oy.append(i)
for i in range(61):
ox.append(i)
oy.append(60.0)
for i in range(61):
ox.append(0.0)
oy.append(i)
for i in range(40):
ox.append(20.0)
oy.append(i)
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "xr")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
rx, ry = dijkstra_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
plt.plot(rx, ry, "-r")
for i in range(10):
matplotrecorder.save_frame()
plt.show()
matplotrecorder.save_movie("animation.gif", 0.1)
def main():
print(__file__ + " start!!")
# initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
# goal position [x(m), y(m)]
goal = np.array([10, 10])
# obstacles [x(m) y(m), ....]
ob = np.matrix([[-1, -1], [0, 2], [4.0, 2.0], [5.0, 4.0], [5.0, 5.0],
[5.0, 6.0], [5.0, 9.0], [8.0, 9.0], [7.0, 9.0],
[12.0, 12.0]])
u = np.array([0.0, 0.0])
config = Config()
traj = np.array(x)
for i in range(1000):
plt.cla()
u, ltraj = dwa_control(x, u, config, goal, ob)
x = motion(x, u, config.dt)
traj = np.vstack((traj, x)) # store state history
plt.plot(ltraj[:, 0], ltraj[:, 1], "-g")
plt.plot(x[0], x[1], "xr")
plt.plot(goal[0], goal[1], "xb")
plt.plot(ob[:, 0], ob[:, 1], "ok")
plot_arrow(x[0], x[1], x[2])
plt.axis("equal")
plt.grid(True)
plt.pause(0.0001)
matplotrecorder.save_frame()
# check goal
if math.sqrt((x[0] - goal[0])**2 +
(x[1] - goal[1])**2) <= config.robot_radius:
print("Goal!!")
break
print("Done")
plt.plot(traj[:, 0], traj[:, 1], "-r")
matplotrecorder.save_frame()
matplotrecorder.save_movie("animation.gif", 0.1)
plt.show()
def main():
print(__file__ + " start!!")
s = np.matrix([10.0, 5.0]).T # init state
gs = np.matrix([5.0, 7.0]).T # goal state
ob = np.matrix([[7.0, 8.0, 3.0, 8.0], [5.5, 6.0, 6.0,
10.0]]) # [xmin xmax ymin ymax]
# ob = np.matrix([[7.0, 8.0, 3.0, 8.0]])
h_sx = []
h_sy = []
for i in range(10000):
print("time:", i)
s_p, u_p = control(s, gs, ob)
s = A * s + B * u_p[:, 0] # simulation
if (math.sqrt((gs[0] - s[0])**2 + (gs[1] - s[1])**2) <= 0.1):
print("Goal!!!")
break
h_sx.append(s[0, 0])
h_sy.append(s[1, 0])
plt.cla()
plt.plot(gs[0], gs[1], "*r")
plot_obstacle(ob)
plt.plot(s_p[0, :], s_p[1, :], "xb")
plt.plot(h_sx, h_sy, "-b")
plt.plot(s[0], s[1], "or")
plt.axis("equal")
plt.grid(True)
plt.pause(0.0001)
matplotrecorder.save_frame()
matplotrecorder.save_movie("animation.gif", 0.1)
def main():
print(__file__ + " start!!")
# initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
x = np.array([0.0, 0.0, math.pi / 2.0, 0.0, 0.0])
# goal position [x(m), y(m)]
goal = np.array([0, 20])
# obstacles [x(m) y(m), ....]
ob = np.matrix([[-4.0, 5.0], [-4.0, 6.0], [-4.0, 7.0], [-4.0, 8.0],
[-4.0, 9.0], [-4.0, 10.0], [-3.0, 10.0], [-2.0, 10.0],
[-1.0, 10.0], [0.0, 10.0], [1.0, 10.0], [2.0, 10.0],
[3.0, 10.0], [4.0, 10.0], [5.0, 10.0], [6.0, 10.0],
[7.0, 10.0], [8.0, 10.0], [9.0, 10.0]])
u = np.array([0.0, 0.0])
config = Config()
traj = np.array(x)
for i in range(1000):
print("################# STEP {} #################".format(i))
# if i == 330:
# config.to_goal_cost_gain = 0.0
# config.speed_cost_gain = 0.0
# config.obs_cost_gain = 999
# print("################# escape ################# ")
plt.cla()
u, ltraj = dwa_control(x, u, config, goal, ob)
x = motion(x, u, config.dt)
traj = np.vstack((traj, x)) # store state history
plt.plot(ltraj[:, 0], ltraj[:, 1], "-g")
plt.plot(x[0], x[1], "xr")
plt.plot(goal[0], goal[1], "xb")
plt.plot(ob[:, 0], ob[:, 1], "ok")
plot_arrow(x[0], x[1], x[2])
## print v and yawrate
print("v = {:.5f}m/s\tyawrate = {:.5f}rad/s".format(x[3], x[4]))
## darw circle ((x[0], x[1]), config.robot_radius)
angles_circle = [kk * math.pi / 180.0 for kk in range(0, 360)]
cir_x = config.robot_radius * np.cos(angles_circle) + x[0]
cir_y = config.robot_radius * np.sin(angles_circle) + x[1]
plt.plot(cir_x, cir_y, '-r')
plt.axis("equal")
plt.grid(True)
plt.pause(0.0001)
matplotrecorder.save_frame()
# check goal
if math.sqrt((x[0] - goal[0])**2 +
(x[1] - goal[1])**2) <= config.goal_area:
print("Goal!!")
break
print("Done")
plt.plot(traj[:, 0], traj[:, 1], "-r")
matplotrecorder.save_frame()
matplotrecorder.save_movie("animation.gif", config.dt)
plt.show()
ax.scatter(a[0][i],a[1][i],a[2][i],c='black')
ay.scatter(a[0][i],a[1][i],s = 10,c = 'black')
if i <= 60:
draw_circle_on_plane(10,i/2,radius,i/2,ax)
circle1 = plt.Circle((x_center, 0+i/2), radius, color='r')
ay.add_artist(circle1)
plt.pause(0.1)
matplotrecorder.save_frame()
if i <= 59:
circle1.remove()
"""
for angle in range(0, 360):
ax.view_init(330, angle)
plt.draw()
plt.pause(.001)
"""
#plt.figure(1)
#plt.subplot(211)
#plt.plot(a[0],a[1])
#----------------------------------------------------------------------------------------------------
def dijkstra_planning(sx, sy, gx, gy, ox, oy, reso, rr):
"""
gx: goal x position [m]
gx: goal x position [m]
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[m]
"""
nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1)
ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
ox = [iox / reso for iox in ox]
oy = [ioy / reso for ioy in oy]
obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr)
motion = get_motion_model()
openset, closedset = dict(), dict()
openset[calc_index(nstart, xw, minx, miny)] = nstart
while 1:
c_id = min(openset, key=lambda o: openset[o].cost)
current = openset[c_id]
# print("current", current)
# show graph
plt.plot(current.x * reso, current.y * reso, "xc")
if len(closedset.keys()) % 10 == 0:
plt.pause(0.001)
matplotrecorder.save_frame()
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal")
ngoal.pind = current.pind
ngoal.cost = current.cost
break
# Remove the item from the open set
del openset[c_id]
# Add it to the closed set
closedset[c_id] = current
# expand search grid based on motion model
for i in range(len(motion)):
node = Node(current.x + motion[i][0], current.y + motion[i][1],
current.cost + motion[i][2], c_id)
n_id = calc_index(node, xw, minx, miny)
if not verify_node(node, obmap, minx, miny, maxx, maxy):
continue
if n_id in closedset:
continue
# Otherwise if it is already in the open set
if n_id in openset:
if openset[n_id].cost > node.cost:
openset[n_id].cost = node.cost
openset[n_id].pind = c_id
else:
openset[n_id] = node
# generate final course
rx, ry = [ngoal.x * reso], [ngoal.y * reso]
pind = ngoal.pind
while pind != -1:
n = closedset[pind]
rx.append(n.x * reso)
ry.append(n.y * reso)
pind = n.pind
return rx, ry
