mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-14 03:58:35 -05:00
Merge branch 'master' of github.com:AtsushiSakai/PythonRobotics
This commit is contained in:
Atsushi Sakai
13
.github/workflows/greetings.yml
vendored
13
.github/workflows/greetings.yml
vendored
@@ -1,13 +0,0 @@
|
||||
name: Greetings
|
||||
|
||||
on: [pull_request, issues]
|
||||
|
||||
jobs:
|
||||
greeting:
|
||||
runs-on: ubuntu-latest
|
||||
steps:
|
||||
- uses: actions/first-interaction@v1
|
||||
with:
|
||||
repo-token: ${{ secrets.GITHUB_TOKEN }}
|
||||
issue-message: 'Message that will be displayed on users'' This is first issue for you on this project'
|
||||
pr-message: 'Message that will be displayed on users'' This is first pr for you on this project'
|
||||
@@ -8,7 +8,6 @@ from math import cos, sin
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
|
||||
class Quadrotor():
|
||||
def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25, show_animation=True):
|
||||
self.p1 = np.array([size / 2, 0, 0, 1]).T
|
||||
@@ -24,6 +23,10 @@ class Quadrotor():
|
||||
if self.show_animation:
|
||||
plt.ion()
|
||||
fig = plt.figure()
|
||||
# for stopping simulation with the esc key.
|
||||
fig.canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
|
||||
self.ax = fig.add_subplot(111, projection='3d')
|
||||
|
||||
self.update_pose(x, y, z, roll, pitch, yaw)
|
||||
@@ -81,4 +84,4 @@ class Quadrotor():
|
||||
plt.ylim(-5, 5)
|
||||
self.ax.set_zlim(0, 10)
|
||||
|
||||
plt.pause(0.001)
|
||||
plt.pause(0.001)
|
||||
|
||||
@@ -567,6 +567,9 @@ def plot_animation(X, U): # pragma: no cover
|
||||
|
||||
fig = plt.figure()
|
||||
ax = fig.gca(projection='3d')
|
||||
# for stopping simulation with the esc key.
|
||||
fig.canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
|
||||
for k in range(K):
|
||||
plt.cla()
|
||||
|
||||
@@ -162,6 +162,9 @@ def astar_torus(grid, start_node, goal_node):
|
||||
for i in range(1, len(route)):
|
||||
grid[route[i]] = 6
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None)
|
||||
plt.show()
|
||||
plt.pause(1e-2)
|
||||
@@ -262,4 +265,4 @@ class NLinkArm(object):
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
main()
|
||||
|
||||
@@ -193,6 +193,9 @@ def astar_torus(grid, start_node, goal_node):
|
||||
for i in range(1, len(route)):
|
||||
grid[route[i]] = 6
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None)
|
||||
plt.show()
|
||||
plt.pause(1e-2)
|
||||
|
||||
@@ -51,6 +51,9 @@ class NLinkArm(object):
|
||||
|
||||
def plot(self): # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
|
||||
for i in range(self.n_links + 1):
|
||||
if i is not self.n_links:
|
||||
|
||||
@@ -114,6 +114,9 @@ def animation():
|
||||
def main(): # pragma: no cover
|
||||
fig = plt.figure()
|
||||
fig.canvas.mpl_connect("button_press_event", click)
|
||||
# for stopping simulation with the esc key.
|
||||
fig.canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
two_joint_arm()
|
||||
|
||||
|
||||
|
||||
@@ -111,6 +111,9 @@ class BipedalPlanner(object):
|
||||
if c > len(com_trajectory_for_plot):
|
||||
# set up plotter
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
ax.set_zlim(0, z_c * 2)
|
||||
ax.set_aspect('equal', 'datalim')
|
||||
|
||||
|
||||
@@ -213,6 +213,9 @@ def main():
|
||||
|
||||
if show_animation:
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
|
||||
for i in range(len(z[:, 0])):
|
||||
plt.plot([xTrue[0, 0], z[i, 2]], [xTrue[1, 0], z[i, 3]], "-k")
|
||||
|
||||
@@ -191,6 +191,9 @@ def main():
|
||||
|
||||
if show_animation:
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(hz[0, :], hz[1, :], ".g")
|
||||
plt.plot(hxTrue[0, :].flatten(),
|
||||
hxTrue[1, :].flatten(), "-b")
|
||||
|
||||
@@ -233,6 +233,9 @@ def main():
|
||||
|
||||
if show_animation:
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
draw_heat_map(grid_map.data, mx, my)
|
||||
plt.plot(xTrue[0, :], xTrue[1, :], "xr")
|
||||
plt.plot(RF_ID[:, 0], RF_ID[:, 1], ".k")
|
||||
|
||||
@@ -230,6 +230,9 @@ def main():
|
||||
|
||||
if show_animation:
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
|
||||
for i in range(len(z[:, 0])):
|
||||
plt.plot([xTrue[0, 0], z[i, 1]], [xTrue[1, 0], z[i, 2]], "-k")
|
||||
|
||||
@@ -240,6 +240,9 @@ def main():
|
||||
|
||||
if show_animation:
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(hz[0, :], hz[1, :], ".g")
|
||||
plt.plot(np.array(hxTrue[0, :]).flatten(),
|
||||
np.array(hxTrue[1, :]).flatten(), "-b")
|
||||
|
||||
@@ -124,6 +124,9 @@ def main():
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.axis("equal")
|
||||
plt.plot(0.0, 0.0, "*r")
|
||||
plot_circle(cx, cy, cr)
|
||||
|
||||
@@ -73,6 +73,9 @@ def main():
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso)
|
||||
plt.plot(ox, oy, "xr")
|
||||
plt.plot(0.0, 0.0, "ob")
|
||||
|
||||
@@ -133,8 +133,10 @@ def main():
|
||||
# for animation
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
clusters.plot_cluster()
|
||||
|
||||
plt.plot(cx, cy, "or")
|
||||
plt.xlim(-2.0, 10.0)
|
||||
plt.ylim(-2.0, 10.0)
|
||||
|
||||
@@ -124,6 +124,9 @@ def main():
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
|
||||
plt.plot(ox, oy, "xr")
|
||||
plt.plot(0.0, 0.0, "ob")
|
||||
|
||||
@@ -242,6 +242,9 @@ def main():
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.axis("equal")
|
||||
plt.plot(0.0, 0.0, "*r")
|
||||
v1.plot()
|
||||
|
||||
@@ -79,6 +79,9 @@ class AStarPlanner:
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(self.calc_grid_position(current.x, self.minx),
|
||||
self.calc_grid_position(current.y, self.miny), "xc")
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if len(closed_set.keys()) % 10 == 0:
|
||||
plt.pause(0.001)
|
||||
|
||||
|
||||
@@ -534,6 +534,9 @@ class BITStar(object):
|
||||
def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None,
|
||||
samples=None, start=None, end=None):
|
||||
plt.clf()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
for rnd in samples:
|
||||
if rnd is not None:
|
||||
plt.plot(rnd[0], rnd[1], "^k")
|
||||
|
||||
@@ -135,6 +135,9 @@ def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal):
|
||||
|
||||
if target_ind % 1 == 0 and animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
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")
|
||||
|
||||
@@ -69,6 +69,9 @@ class Dijkstra:
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(self.calc_position(current.x, self.minx),
|
||||
self.calc_position(current.y, self.miny), "xc")
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if len(closedset.keys()) % 10 == 0:
|
||||
plt.pause(0.001)
|
||||
|
||||
|
||||
@@ -318,6 +318,9 @@ def test():
|
||||
|
||||
if show_animation:
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(px, py, label="final course " + str(mode))
|
||||
|
||||
# plotting
|
||||
|
||||
@@ -268,6 +268,9 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle):
|
||||
|
||||
if show_animation:
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(predicted_trajectory[:, 0], predicted_trajectory[:, 1], "-g")
|
||||
plt.plot(x[0], x[1], "xr")
|
||||
plt.plot(goal[0], goal[1], "xb")
|
||||
|
||||
@@ -213,6 +213,9 @@ def test1():
|
||||
if show_animation:
|
||||
# plot the path
|
||||
plt.plot(pos[0, :], pos[1, :])
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.pause(1.0)
|
||||
|
||||
if show_animation:
|
||||
|
||||
@@ -49,13 +49,6 @@ class quintic_polynomial:
|
||||
def __init__(self, xs, vxs, axs, xe, vxe, axe, T):
|
||||
|
||||
# calc coefficient of quintic polynomial
|
||||
self.xs = xs
|
||||
self.vxs = vxs
|
||||
self.axs = axs
|
||||
self.xe = xe
|
||||
self.vxe = vxe
|
||||
self.axe = axe
|
||||
|
||||
self.a0 = xs
|
||||
self.a1 = vxs
|
||||
self.a2 = axs / 2.0
|
||||
@@ -99,12 +92,7 @@ class quartic_polynomial:
|
||||
|
||||
def __init__(self, xs, vxs, axs, vxe, axe, T):
|
||||
|
||||
# calc coefficient of quintic polynomial
|
||||
self.xs = xs
|
||||
self.vxs = vxs
|
||||
self.axs = axs
|
||||
self.vxe = vxe
|
||||
self.axe = axe
|
||||
# calc coefficient of quartic polynomial
|
||||
|
||||
self.a0 = xs
|
||||
self.a1 = vxs
|
||||
@@ -184,7 +172,7 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
|
||||
fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
|
||||
fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
|
||||
|
||||
# Loongitudinal motion planning (Velocity keeping)
|
||||
# Longitudinal motion planning (Velocity keeping)
|
||||
for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
|
||||
tfp = copy.deepcopy(fp)
|
||||
lon_qp = quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
|
||||
@@ -347,6 +335,9 @@ def main():
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(tx, ty)
|
||||
plt.plot(ob[:, 0], ob[:, 1], "xk")
|
||||
plt.plot(path.x[1:], path.y[1:], "-or")
|
||||
|
||||
@@ -213,6 +213,9 @@ def sweep_path_search(sweep_searcher, gmap, grid_search_animation=False):
|
||||
|
||||
if grid_search_animation:
|
||||
fig, ax = plt.subplots()
|
||||
# for stopping simulation with the esc key.
|
||||
fig.canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
|
||||
while True:
|
||||
cxind, cyind = sweep_searcher.move_target_grid(cxind, cyind, gmap)
|
||||
@@ -266,6 +269,9 @@ def planning_animation(ox, oy, reso): # pragma: no cover
|
||||
if do_animation:
|
||||
for ipx, ipy in zip(px, py):
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(ox, oy, "-xb")
|
||||
plt.plot(px, py, "-r")
|
||||
plt.plot(ipx, ipy, "or")
|
||||
|
||||
@@ -78,6 +78,9 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr):
|
||||
# show graph
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(current.x * reso, current.y * reso, "xc")
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if len(closedset.keys()) % 10 == 0:
|
||||
plt.pause(0.001)
|
||||
|
||||
@@ -228,4 +231,4 @@ def main():
|
||||
|
||||
if __name__ == '__main__':
|
||||
show_animation = True
|
||||
main()
|
||||
main()
|
||||
|
||||
@@ -327,6 +327,9 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(current.xlist[-1], current.ylist[-1], "xc")
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if len(closedList.keys()) % 10 == 0:
|
||||
plt.pause(0.001)
|
||||
|
||||
|
||||
@@ -266,8 +266,10 @@ class InformedRRTStar:
|
||||
return path
|
||||
|
||||
def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None, rnd=None):
|
||||
|
||||
plt.clf()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if rnd is not None:
|
||||
plt.plot(rnd[0], rnd[1], "^k")
|
||||
if cBest != float('inf'):
|
||||
|
||||
@@ -54,6 +54,9 @@ class LQRPlanner:
|
||||
|
||||
# animation
|
||||
if show_animation: # pragma: no cover
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(sx, sy, "or")
|
||||
plt.plot(gx, gy, "ob")
|
||||
plt.plot(rx, ry, "-r")
|
||||
|
||||
@@ -102,6 +102,9 @@ class LQRRRTStar(RRTStar):
|
||||
|
||||
def draw_graph(self, rnd=None):
|
||||
plt.clf()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if rnd is not None:
|
||||
plt.plot(rnd.x, rnd.y, "^k")
|
||||
for node in self.node_list:
|
||||
|
||||
@@ -98,6 +98,9 @@ def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
|
||||
|
||||
if show_animation:
|
||||
draw_heatmap(pmap)
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(ix, iy, "*k")
|
||||
plt.plot(gix, giy, "*m")
|
||||
|
||||
|
||||
@@ -194,6 +194,9 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
|
||||
|
||||
# show graph
|
||||
if show_animation and len(closedset.keys()) % 2 == 0:
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(current.x, current.y, "xg")
|
||||
plt.pause(0.001)
|
||||
|
||||
|
||||
@@ -26,14 +26,7 @@ class QuinticPolynomial:
|
||||
|
||||
def __init__(self, xs, vxs, axs, xe, vxe, axe, T):
|
||||
|
||||
# calc coefficient of quinic polynomial
|
||||
self.xs = xs
|
||||
self.vxs = vxs
|
||||
self.axs = axs
|
||||
self.xe = xe
|
||||
self.vxe = vxe
|
||||
self.axe = axe
|
||||
|
||||
# calc coefficient of quintic polynomial
|
||||
self.a0 = xs
|
||||
self.a1 = vxs
|
||||
self.a2 = axs / 2.0
|
||||
@@ -151,6 +144,9 @@ def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_
|
||||
if show_animation: # pragma: no cover
|
||||
for i, _ in enumerate(time):
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.grid(True)
|
||||
plt.axis("equal")
|
||||
plot_arrow(sx, sy, syaw)
|
||||
|
||||
@@ -138,6 +138,9 @@ class RRT:
|
||||
|
||||
def draw_graph(self, rnd=None):
|
||||
plt.clf()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if rnd is not None:
|
||||
plt.plot(rnd.x, rnd.y, "^k")
|
||||
for node in self.node_list:
|
||||
|
||||
@@ -106,6 +106,9 @@ class RRTDubins(RRT):
|
||||
|
||||
def draw_graph(self, rnd=None): # pragma: no cover
|
||||
plt.clf()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if rnd is not None:
|
||||
plt.plot(rnd.x, rnd.y, "^k")
|
||||
for node in self.node_list:
|
||||
|
||||
@@ -112,6 +112,9 @@ class RRTStarDubins(RRTStar):
|
||||
|
||||
def draw_graph(self, rnd=None):
|
||||
plt.clf()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if rnd is not None:
|
||||
plt.plot(rnd.x, rnd.y, "^k")
|
||||
for node in self.node_list:
|
||||
|
||||
@@ -122,6 +122,9 @@ class RRTStarReedsShepp(RRTStar):
|
||||
|
||||
def draw_graph(self, rnd=None):
|
||||
plt.clf()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if rnd is not None:
|
||||
plt.plot(rnd.x, rnd.y, "^k")
|
||||
for node in self.node_list:
|
||||
|
||||
@@ -395,6 +395,9 @@ def test():
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(px, py, label="final course " + str(mode))
|
||||
|
||||
# plotting
|
||||
|
||||
@@ -185,6 +185,9 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
|
||||
# show graph
|
||||
if show_animation and len(closedset.keys()) % 2 == 0: # pragma: no cover
|
||||
plt.plot(current.x, current.y, "xg")
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.pause(0.001)
|
||||
|
||||
if c_id == (len(road_map) - 1):
|
||||
|
||||
@@ -546,6 +546,9 @@ def animation(plant, controller, dt):
|
||||
steer = math.atan2(controller.history_u_2[t] * WB / v, 1.0)
|
||||
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(plant.history_x, plant.history_y, "-r", label="trajectory")
|
||||
plot_car(x, y, yaw, steer=steer)
|
||||
plt.axis("equal")
|
||||
|
||||
@@ -224,6 +224,9 @@ def do_simulation(cx, cy, cyaw, ck, speed_profile, goal):
|
||||
|
||||
if target_ind % 1 == 0 and show_animation:
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
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")
|
||||
|
||||
@@ -203,6 +203,9 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
|
||||
|
||||
if target_ind % 1 == 0 and show_animation:
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
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")
|
||||
|
||||
@@ -429,6 +429,9 @@ def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state):
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if ox is not None:
|
||||
plt.plot(ox, oy, "xr", label="MPC")
|
||||
plt.plot(cx, cy, "-r", label="course")
|
||||
|
||||
@@ -93,6 +93,10 @@ def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover
|
||||
|
||||
plt.plot(x_traj, y_traj, 'b--')
|
||||
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
|
||||
plt.xlim(0, 20)
|
||||
plt.ylim(0, 20)
|
||||
|
||||
|
||||
@@ -3,6 +3,7 @@
|
||||
Path tracking simulation with pure pursuit steering control and PID speed control.
|
||||
|
||||
author: Atsushi Sakai (@Atsushi_twi)
|
||||
Guillaume Jacquenot (@Gjacquenot)
|
||||
|
||||
"""
|
||||
import numpy as np
|
||||
@@ -17,7 +18,6 @@ dt = 0.1 # [s]
|
||||
L = 2.9 # [m] wheel base of vehicle
|
||||
|
||||
|
||||
old_nearest_point_index = None
|
||||
show_animation = True
|
||||
|
||||
|
||||
@@ -31,17 +31,37 @@ class State:
|
||||
self.rear_x = self.x - ((L / 2) * math.cos(self.yaw))
|
||||
self.rear_y = self.y - ((L / 2) * math.sin(self.yaw))
|
||||
|
||||
def update(self, a, delta):
|
||||
|
||||
def update(state, a, delta):
|
||||
self.x += self.v * math.cos(self.yaw) * dt
|
||||
self.y += self.v * math.sin(self.yaw) * dt
|
||||
self.yaw += self.v / L * math.tan(delta) * dt
|
||||
self.v += a * dt
|
||||
self.rear_x = self.x - ((L / 2) * math.cos(self.yaw))
|
||||
self.rear_y = self.y - ((L / 2) * math.sin(self.yaw))
|
||||
|
||||
state.x = state.x + state.v * math.cos(state.yaw) * dt
|
||||
state.y = state.y + state.v * math.sin(state.yaw) * dt
|
||||
state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
|
||||
state.v = state.v + a * dt
|
||||
state.rear_x = state.x - ((L / 2) * math.cos(state.yaw))
|
||||
state.rear_y = state.y - ((L / 2) * math.sin(state.yaw))
|
||||
def calc_distance(self, point_x, point_y):
|
||||
|
||||
return state
|
||||
dx = self.rear_x - point_x
|
||||
dy = self.rear_y - point_y
|
||||
return math.hypot(dx, dy)
|
||||
|
||||
|
||||
class States:
|
||||
|
||||
def __init__(self):
|
||||
self.x = []
|
||||
self.y = []
|
||||
self.yaw = []
|
||||
self.v = []
|
||||
self.t = []
|
||||
|
||||
def append(self, t , state):
|
||||
self.x.append(state.x)
|
||||
self.y.append(state.y)
|
||||
self.yaw.append(state.yaw)
|
||||
self.v.append(state.v)
|
||||
self.t.append(t)
|
||||
|
||||
|
||||
def PIDControl(target, current):
|
||||
@@ -50,20 +70,57 @@ def PIDControl(target, current):
|
||||
return a
|
||||
|
||||
|
||||
def pure_pursuit_control(state, cx, cy, pind):
|
||||
class Trajectory:
|
||||
def __init__(self, cx, cy):
|
||||
self.cx = cx
|
||||
self.cy = cy
|
||||
self.old_nearest_point_index = None
|
||||
|
||||
ind = calc_target_index(state, cx, cy)
|
||||
def search_target_index(self, state):
|
||||
if self.old_nearest_point_index is None:
|
||||
# search nearest point index
|
||||
dx = [state.rear_x - icx for icx in self.cx]
|
||||
dy = [state.rear_y - icy for icy in self.cy]
|
||||
d = np.hypot(dx, dy)
|
||||
ind = np.argmin(d)
|
||||
self.old_nearest_point_index = ind
|
||||
else:
|
||||
ind = self.old_nearest_point_index
|
||||
distance_this_index = state.calc_distance(self.cx[ind], self.cy[ind])
|
||||
while True:
|
||||
ind = ind + 1 if (ind + 1) < len(self.cx) else ind
|
||||
distance_next_index = state.calc_distance(self.cx[ind], self.cy[ind])
|
||||
if distance_this_index < distance_next_index:
|
||||
break
|
||||
distance_this_index = distance_next_index
|
||||
self.old_nearest_point_index = ind
|
||||
|
||||
L = 0.0
|
||||
|
||||
Lf = k * state.v + Lfc
|
||||
|
||||
# search look ahead target point index
|
||||
while Lf > L and (ind + 1) < len(self.cx):
|
||||
L = state.calc_distance(self.cx[ind], self.cy[ind])
|
||||
ind += 1
|
||||
|
||||
return ind
|
||||
|
||||
|
||||
def pure_pursuit_control(state, trajectory, pind):
|
||||
|
||||
ind = trajectory.search_target_index(state)
|
||||
|
||||
if pind >= ind:
|
||||
ind = pind
|
||||
|
||||
if ind < len(cx):
|
||||
tx = cx[ind]
|
||||
ty = cy[ind]
|
||||
if ind < len(trajectory.cx):
|
||||
tx = trajectory.cx[ind]
|
||||
ty = trajectory.cy[ind]
|
||||
else:
|
||||
tx = cx[-1]
|
||||
ty = cy[-1]
|
||||
ind = len(cx) - 1
|
||||
tx = trajectory.cx[-1]
|
||||
ty = trajectory.cy[-1]
|
||||
ind = len(trajectory.cx) - 1
|
||||
|
||||
alpha = math.atan2(ty - state.rear_y, tx - state.rear_x) - state.yaw
|
||||
|
||||
@@ -73,48 +130,6 @@ def pure_pursuit_control(state, cx, cy, pind):
|
||||
|
||||
return delta, ind
|
||||
|
||||
def calc_distance(state, point_x, point_y):
|
||||
|
||||
dx = state.rear_x - point_x
|
||||
dy = state.rear_y - point_y
|
||||
return math.hypot(dx, dy)
|
||||
|
||||
|
||||
def calc_target_index(state, cx, cy):
|
||||
|
||||
global old_nearest_point_index
|
||||
|
||||
if old_nearest_point_index is None:
|
||||
# search nearest point index
|
||||
dx = [state.rear_x - icx for icx in cx]
|
||||
dy = [state.rear_y - icy for icy in cy]
|
||||
d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
|
||||
ind = d.index(min(d))
|
||||
old_nearest_point_index = ind
|
||||
else:
|
||||
ind = old_nearest_point_index
|
||||
distance_this_index = calc_distance(state, cx[ind], cy[ind])
|
||||
while True:
|
||||
ind = ind + 1 if (ind + 1) < len(cx) else ind
|
||||
distance_next_index = calc_distance(state, cx[ind], cy[ind])
|
||||
if distance_this_index < distance_next_index:
|
||||
break
|
||||
distance_this_index = distance_next_index
|
||||
old_nearest_point_index = ind
|
||||
|
||||
L = 0.0
|
||||
|
||||
Lf = k * state.v + Lfc
|
||||
|
||||
# search look ahead target point index
|
||||
while Lf > L and (ind + 1) < len(cx):
|
||||
dx = cx[ind] - state.rear_x
|
||||
dy = cy[ind] - state.rear_y
|
||||
L = math.hypot(dx, dy)
|
||||
ind += 1
|
||||
|
||||
return ind
|
||||
|
||||
|
||||
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
|
||||
"""
|
||||
@@ -122,7 +137,7 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
|
||||
"""
|
||||
|
||||
if not isinstance(x, float):
|
||||
for (ix, iy, iyaw) in zip(x, y, yaw):
|
||||
for ix, iy, iyaw in zip(x, y, yaw):
|
||||
plot_arrow(ix, iy, iyaw)
|
||||
else:
|
||||
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
|
||||
@@ -144,31 +159,27 @@ def main():
|
||||
|
||||
lastIndex = len(cx) - 1
|
||||
time = 0.0
|
||||
x = [state.x]
|
||||
y = [state.y]
|
||||
yaw = [state.yaw]
|
||||
v = [state.v]
|
||||
t = [0.0]
|
||||
target_ind = calc_target_index(state, cx, cy)
|
||||
states = States()
|
||||
states.append(time, state)
|
||||
trajectory = Trajectory(cx, cy)
|
||||
target_ind = trajectory.search_target_index(state)
|
||||
|
||||
while T >= time and lastIndex > target_ind:
|
||||
ai = PIDControl(target_speed, state.v)
|
||||
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
|
||||
state = update(state, ai, di)
|
||||
di, target_ind = pure_pursuit_control(state, trajectory, target_ind)
|
||||
state.update(ai, di)
|
||||
|
||||
time = time + dt
|
||||
|
||||
x.append(state.x)
|
||||
y.append(state.y)
|
||||
yaw.append(state.yaw)
|
||||
v.append(state.v)
|
||||
t.append(time)
|
||||
time += dt
|
||||
states.append(time, state)
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plot_arrow(state.x, state.y, state.yaw)
|
||||
plt.plot(cx, cy, "-r", label="course")
|
||||
plt.plot(x, y, "-b", label="trajectory")
|
||||
plt.plot(states.x, states.y, "-b", label="trajectory")
|
||||
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
|
||||
plt.axis("equal")
|
||||
plt.grid(True)
|
||||
@@ -181,7 +192,7 @@ def main():
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
plt.plot(cx, cy, ".r", label="course")
|
||||
plt.plot(x, y, "-b", label="trajectory")
|
||||
plt.plot(states.x, states.y, "-b", label="trajectory")
|
||||
plt.legend()
|
||||
plt.xlabel("x[m]")
|
||||
plt.ylabel("y[m]")
|
||||
@@ -189,7 +200,7 @@ def main():
|
||||
plt.grid(True)
|
||||
|
||||
plt.subplots(1)
|
||||
plt.plot(t, [iv * 3.6 for iv in v], "-r")
|
||||
plt.plot(states.t, [iv * 3.6 for iv in states.v], "-r")
|
||||
plt.xlabel("Time[s]")
|
||||
plt.ylabel("Speed[km/h]")
|
||||
plt.grid(True)
|
||||
|
||||
@@ -149,6 +149,9 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
|
||||
|
||||
if target_ind % 1 == 0 and show_animation:
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
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")
|
||||
|
||||
@@ -185,6 +185,9 @@ def main():
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(cx, cy, ".r", label="course")
|
||||
plt.plot(x, y, "-b", label="trajectory")
|
||||
plt.plot(cx[target_idx], cy[target_idx], "xg", label="target")
|
||||
|
||||
@@ -605,5 +605,8 @@ This is a list: [Users comments](https://github.com/AtsushiSakai/PythonRobotics/
|
||||
|
||||
- [Ryohei Sasaki](https://github.com/rsasaki0109)
|
||||
|
||||
- [Göktuğ Karakaşlı](https://github.com/goktug97)
|
||||
|
||||
- [Guillaume Jacquenot](https://github.com/Gjacquenot)
|
||||
|
||||
|
||||
|
||||
@@ -235,6 +235,9 @@ def main():
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
|
||||
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
|
||||
plt.plot(xEst[0], xEst[1], ".r")
|
||||
|
||||
@@ -365,6 +365,9 @@ def main():
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
|
||||
|
||||
for i in range(N_PARTICLE):
|
||||
|
||||
@@ -390,6 +390,9 @@ def main():
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
|
||||
|
||||
for iz in range(len(z[:, 0])):
|
||||
|
||||
@@ -303,7 +303,9 @@ def main():
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
|
||||
|
||||
plt.plot(hxTrue[0, :].flatten(),
|
||||
@@ -319,4 +321,4 @@ def main():
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
main()
|
||||
|
||||
@@ -36,6 +36,9 @@ def icp_matching(previous_points, current_points):
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(previous_points[0, :], previous_points[1, :], ".r")
|
||||
plt.plot(current_points[0, :], current_points[1, :], ".b")
|
||||
plt.plot(0.0, 0.0, "xr")
|
||||
|
||||
@@ -1,9 +1,11 @@
|
||||
name: python_robotics
|
||||
dependencies:
|
||||
- python
|
||||
- pip
|
||||
- matplotlib
|
||||
- scipy
|
||||
- numpy==1.15
|
||||
- pandas
|
||||
- coverage
|
||||
- pip:
|
||||
- cvxpy
|
||||
|
||||
@@ -404,6 +404,9 @@ URL: https://www.semanticscholar.org/paper/A-Review-of-Robot-Rescue-Simulation-P
|
||||
7. Ghosh, Ritwika, et al. "CyPhyHouse: A Programming, Simulation, and Deployment Toolchain for Heterogeneous Distributed Coordination." arXiv preprint arXiv:1910.01557 (2019).
|
||||
URL: https://arxiv.org/abs/1910.01557
|
||||
|
||||
8. Hahn, Carsten, et al. "Dynamic Path Planning with Stable Growing Neural Gas." (2019).
|
||||
URL: https://pdfs.semanticscholar.org/5c06/f3cb9542a51e1bf1a32523c1bc7fea6cecc5.pdf
|
||||
|
||||
# Others
|
||||
|
||||
- Autonomous Vehicle Readings https://richardkelley.io/readings
|
||||
|
||||
Reference in New Issue
Block a user