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 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 test_optimize_trajectory():
# target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(00.0))
target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(90.0))
k0 = 0.0
init_p = np.matrix([6.0, 0.0, 0.0]).T
x, y, yaw, p = optimize_trajectory(target, k0, init_p)
show_trajectory(target, x, y)
matplotrecorder.save_movie("animation.gif", 0.1)
# plt.plot(x, y, "-r")
plot_arrow(target.x, target.y, target.yaw)
plt.axis("equal")
plt.grid(True)
plt.show()
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("rear wheel feedback tracking start!!")
ax = [0.0, 6.0, 12.5, 5.0, 7.5, 3.0, -1.0]
ay = [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = pycubicspline.calc_spline_course(ax, ay, ds=0.1)
target_speed = 10.0 / 3.6
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)
if animation:
matplotrecorder.save_movie("animation.gif", 0.1) # gif is ok.
flg, _ = plt.subplots(1)
plt.plot(ax, ay, "xb", label="input")
plt.plot(cx, cy, "-r", label="spline")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
flg, ax = plt.subplots(1)
plt.plot(s, [math.degrees(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
flg, ax = plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")
plt.show()
def test_optimize_trajectory():
mcfg = motion_model.ModelConfig()
# target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(00.0))
target = motion_model.State(x=1.0, y=0.0, yaw=math.radians(30.0))
k0 = 0.0
init_p_len = math.sqrt(target.x**2 + target.y**2)
init_p = np.matrix([init_p_len, 0.0, 0.0]).T
init_p = limitP(init_p, mcfg)
x, y, yaw, p = optimize_trajectory(target, k0, init_p)
p = limitP(p, mcfg)
show_trajectory(target, x, y)
matplotrecorder.save_movie("animation.mp4", 0.1)
# plt.plot(x, y, "-r")
plot_arrow(target.x, target.y, target.yaw)
plt.axis("equal")
plt.grid(True)
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()