github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-11 18:25:04 -05:00
Code Issues Packages Projects Releases Wiki Activity

code clean up for lgtm

Browse Source
This commit is contained in:
Atsushi Sakai
2018-11-23 11:03:50 +09:00
parent 8dcc6f0f22
commit 3a7f3c5587
18 changed files with 69 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py
View File

@@ -210,7 +210,7 @@ class NLinkArm(object):
def __init__(self, link_lengths, joint_angles): def __init__(self, link_lengths, joint_angles):
self.n_links = len(link_lengths) self.n_links = len(link_lengths)
if self.n_links is not len(joint_angles): if self.n_links != len(joint_angles):
raise ValueError() raise ValueError()
self.link_lengths = np.array(link_lengths) self.link_lengths = np.array(link_lengths)

2
ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py
View File

@@ -12,7 +12,7 @@ class NLinkArm(object):
def __init__(self, link_lengths, joint_angles, goal, show_animation): def __init__(self, link_lengths, joint_angles, goal, show_animation):
self.show_animation = show_animation self.show_animation = show_animation
self.n_links = len(link_lengths) self.n_links = len(link_lengths)
if self.n_links is not len(joint_angles): if self.n_links != len(joint_angles):
raise ValueError() raise ValueError()
self.link_lengths = np.array(link_lengths) self.link_lengths = np.array(link_lengths)

4
ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py
View File

@@ -51,7 +51,7 @@ def main():
elif solution_found: elif solution_found:
state = MOVING_TO_GOAL state = MOVING_TO_GOAL
elif state is MOVING_TO_GOAL: elif state is MOVING_TO_GOAL:
if distance > 0.1 and (old_goal is goal_pos): if distance > 0.1 and (old_goal == goal_pos).all():
joint_angles = joint_angles + Kp * \ joint_angles = joint_angles + Kp * \
ang_diff(joint_goal_angles, joint_angles) * dt ang_diff(joint_goal_angles, joint_angles) * dt
else: else:
@@ -111,7 +111,7 @@ def animation():
elif solution_found: elif solution_found:
state = MOVING_TO_GOAL state = MOVING_TO_GOAL
elif state is MOVING_TO_GOAL: elif state is MOVING_TO_GOAL:
if distance > 0.1 and (old_goal is goal_pos): if distance > 0.1 and (old_goal == goal_pos).all():
joint_angles = joint_angles + Kp * \ joint_angles = joint_angles + Kp * \
ang_diff(joint_goal_angles, joint_angles) * dt ang_diff(joint_goal_angles, joint_angles) * dt
else: else:

2
PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py
View File

@@ -474,7 +474,7 @@ class BITStar(object):
for v, edges in self.tree.vertices.items(): for v, edges in self.tree.vertices.items():
if v != vid and (v, vid) not in self.edge_queue and (vid, v) not in self.edge_queue: if v != vid and (v, vid) not in self.edge_queue and (vid, v) not in self.edge_queue:
vcoord = self.tree.nodeIdToRealWorldCoord(v) vcoord = self.tree.nodeIdToRealWorldCoord(v)
if(np.linalg.norm(vcoord - currCoord, 2) <= self.r and v != vid): if(np.linalg.norm(vcoord - currCoord, 2) <= self.r):
neigbors.append((vid, vcoord)) neigbors.append((vid, vcoord))
for neighbor in neigbors: for neighbor in neigbors:

9
PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
View File

@@ -427,7 +427,6 @@ def main():
if not flag: if not flag:
print("cannot find feasible path") print("cannot find feasible path")
# flg, ax = plt.subplots(1)
# Draw final path # Draw final path
if show_animation: if show_animation:
rrt.DrawGraph() rrt.DrawGraph()
@@ -435,26 +434,26 @@ def main():
plt.grid(True) plt.grid(True)
plt.pause(0.001) plt.pause(0.001)
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(t, [np.rad2deg(iyaw) for iyaw in yaw[:-1]], '-r') plt.plot(t, [np.rad2deg(iyaw) for iyaw in yaw[:-1]], '-r')
plt.xlabel("time[s]") plt.xlabel("time[s]")
plt.ylabel("Yaw[deg]") plt.ylabel("Yaw[deg]")
plt.grid(True) plt.grid(True)
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], '-r') plt.plot(t, [iv * 3.6 for iv in v], '-r')
plt.xlabel("time[s]") plt.xlabel("time[s]")
plt.ylabel("velocity[km/h]") plt.ylabel("velocity[km/h]")
plt.grid(True) plt.grid(True)
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(t, a, '-r') plt.plot(t, a, '-r')
plt.xlabel("time[s]") plt.xlabel("time[s]")
plt.ylabel("accel[m/ss]") plt.ylabel("accel[m/ss]")
plt.grid(True) plt.grid(True)
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(t, [np.rad2deg(td) for td in d], '-r') plt.plot(t, [np.rad2deg(td) for td in d], '-r')
plt.xlabel("time[s]") plt.xlabel("time[s]")
plt.ylabel("Steering angle[deg]") plt.ylabel("Steering angle[deg]")

6
PathPlanning/ClosedLoopRRTStar/pure_pursuit.py
View File

@@ -1,6 +1,4 @@
#! /usr/bin/python """
# -*- coding: utf-8 -*-
u"""
Path tracking simulation with pure pursuit steering control and PID speed control. Path tracking simulation with pure pursuit steering control and PID speed control.
@@ -251,7 +249,7 @@ def main():
while T >= time and lastIndex > target_ind: while T >= time and lastIndex > target_ind:
ai = PIDControl(target_speed, state.v) ai = PIDControl(target_speed, state.v)
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind) di, target_ind, _ = pure_pursuit_control(state, cx, cy, target_ind)
state = unicycle_model.update(state, ai, di) state = unicycle_model.update(state, ai, di)
time = time + unicycle_model.dt time = time + unicycle_model.dt

5
PathPlanning/ClosedLoopRRTStar/unicycle_model.py
View File

@@ -7,6 +7,7 @@ author Atsushi Sakai
""" """
import math import math
import numpy as np
dt = 0.05 # [s] dt = 0.05 # [s]
L = 0.9 # [m] L = 0.9 # [m]
@@ -66,12 +67,12 @@ if __name__ == '__main__':
yaw.append(state.yaw) yaw.append(state.yaw)
v.append(state.v) v.append(state.v)
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(x, y) plt.plot(x, y)
plt.axis("equal") plt.axis("equal")
plt.grid(True) plt.grid(True)
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(v) plt.plot(v)
plt.grid(True) plt.grid(True)

6
PathPlanning/CubicSpline/cubic_spline_planner.py
View File

@@ -209,7 +209,7 @@ def main():
ryaw.append(sp.calc_yaw(i_s)) ryaw.append(sp.calc_yaw(i_s))
rk.append(sp.calc_curvature(i_s)) rk.append(sp.calc_curvature(i_s))
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(x, y, "xb", label="input") plt.plot(x, y, "xb", label="input")
plt.plot(rx, ry, "-r", label="spline") plt.plot(rx, ry, "-r", label="spline")
plt.grid(True) plt.grid(True)
@@ -218,14 +218,14 @@ def main():
plt.ylabel("y[m]") plt.ylabel("y[m]")
plt.legend() plt.legend()
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw")
plt.grid(True) plt.grid(True)
plt.legend() plt.legend()
plt.xlabel("line length[m]") plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]") plt.ylabel("yaw angle[deg]")
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(s, rk, "-r", label="curvature") plt.plot(s, rk, "-r", label="curvature")
plt.grid(True) plt.grid(True)
plt.legend() plt.legend()

22
PathPlanning/DubinsPath/dubins_path_planning.py
View File

@@ -235,18 +235,18 @@ def generate_course(length, mode, c):
elif m is "R": # right turn elif m is "R": # right turn
pyaw.append(pyaw[-1] - d) pyaw.append(pyaw[-1] - d)
pd += d pd += d
else:
d = l - pd
px.append(px[-1] + d / c * math.cos(pyaw[-1]))
py.append(py[-1] + d / c * math.sin(pyaw[-1]))
if m is "L": # left turn d = l - pd
pyaw.append(pyaw[-1] + d) px.append(px[-1] + d / c * math.cos(pyaw[-1]))
elif m is "S": # Straight py.append(py[-1] + d / c * math.sin(pyaw[-1]))
pyaw.append(pyaw[-1])
elif m is "R": # right turn if m is "L": # left turn
pyaw.append(pyaw[-1] - d) pyaw.append(pyaw[-1] + d)
pd += d elif m is "S": # Straight
pyaw.append(pyaw[-1])
elif m is "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
return px, py, pyaw return px, py, pyaw

8
PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py
View File

@@ -10,7 +10,7 @@ import bisect
class Spline: class Spline:
u""" """
Cubic Spline class Cubic Spline class
""" """
@@ -209,7 +209,7 @@ def main():
ryaw.append(sp.calc_yaw(i_s)) ryaw.append(sp.calc_yaw(i_s))
rk.append(sp.calc_curvature(i_s)) rk.append(sp.calc_curvature(i_s))
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(x, y, "xb", label="input") plt.plot(x, y, "xb", label="input")
plt.plot(rx, ry, "-r", label="spline") plt.plot(rx, ry, "-r", label="spline")
plt.grid(True) plt.grid(True)
@@ -218,14 +218,14 @@ def main():
plt.ylabel("y[m]") plt.ylabel("y[m]")
plt.legend() plt.legend()
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw")
plt.grid(True) plt.grid(True)
plt.legend() plt.legend()
plt.xlabel("line length[m]") plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]") plt.ylabel("yaw angle[deg]")
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(s, rk, "-r", label="curvature") plt.plot(s, rk, "-r", label="curvature")
plt.grid(True) plt.grid(True)
plt.legend() plt.legend()

2
PathPlanning/HybridAStar/hybrid_a_star.py
View File

@@ -151,7 +151,7 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
# print(current) # print(current)
rx, ry, ryaw = [], [], [] # rx, ry, ryaw = [], [], []
return rx, ry, ryaw return rx, ry, ryaw

22
PathPlanning/RRTDubins/dubins_path_planning.py
View File

@@ -245,18 +245,18 @@ def generate_course(length, mode, c):
elif m is "R": # right turn elif m is "R": # right turn
pyaw.append(pyaw[-1] - d) pyaw.append(pyaw[-1] - d)
pd += d pd += d
else:
d = l - pd
px.append(px[-1] + d * c * math.cos(pyaw[-1]))
py.append(py[-1] + d * c * math.sin(pyaw[-1]))
if m is "L": # left turn d = l - pd
pyaw.append(pyaw[-1] + d) px.append(px[-1] + d * c * math.cos(pyaw[-1]))
elif m is "S": # Straight py.append(py[-1] + d * c * math.sin(pyaw[-1]))
pyaw.append(pyaw[-1])
elif m is "R": # right turn if m is "L": # left turn
pyaw.append(pyaw[-1] - d) pyaw.append(pyaw[-1] + d)
pd += d elif m is "S": # Straight
pyaw.append(pyaw[-1])
elif m is "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
return px, py, pyaw return px, py, pyaw

22
PathPlanning/RRTStarDubins/dubins_path_planning.py
View File

@@ -240,18 +240,18 @@ def generate_course(length, mode, c):
elif m is "R": # right turn elif m is "R": # right turn
pyaw.append(pyaw[-1] - d) pyaw.append(pyaw[-1] - d)
pd += d pd += d
else:
d = l - pd
px.append(px[-1] + d * c * math.cos(pyaw[-1]))
py.append(py[-1] + d * c * math.sin(pyaw[-1]))
if m is "L": # left turn d = l - pd
pyaw.append(pyaw[-1] + d) px.append(px[-1] + d * c * math.cos(pyaw[-1]))
elif m is "S": # Straight py.append(py[-1] + d * c * math.sin(pyaw[-1]))
pyaw.append(pyaw[-1])
elif m is "R": # right turn if m is "L": # left turn
pyaw.append(pyaw[-1] - d) pyaw.append(pyaw[-1] + d)
pd += d elif m is "S": # Straight
pyaw.append(pyaw[-1])
elif m is "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
return px, py, pyaw return px, py, pyaw

1
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
View File

@@ -290,7 +290,6 @@ def generate_local_course(L, lengths, mode, maxc, step_size):
else: else:
d = -step_size d = -step_size
pd = d
ll = 0.0 ll = 0.0
for (m, l, i) in zip(mode, lengths, range(len(mode))): for (m, l, i) in zip(mode, lengths, range(len(mode))):

7
PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
View File

@@ -173,7 +173,6 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
yaw = [state.yaw] yaw = [state.yaw]
v = [state.v] v = [state.v]
t = [0.0] t = [0.0]
target_ind = calc_nearest_index(state, cx, cy, cyaw)
e, e_th = 0.0, 0.0 e, e_th = 0.0, 0.0
@@ -261,7 +260,7 @@ def main():
if show_animation: if show_animation:
plt.close() plt.close()
flg, _ = plt.subplots(1) plt.subplots(1)
plt.plot(ax, ay, "xb", label="waypoints") plt.plot(ax, ay, "xb", label="waypoints")
plt.plot(cx, cy, "-r", label="target course") plt.plot(cx, cy, "-r", label="target course")
plt.plot(x, y, "-g", label="tracking") plt.plot(x, y, "-g", label="tracking")
@@ -271,14 +270,14 @@ def main():
plt.ylabel("y[m]") plt.ylabel("y[m]")
plt.legend() plt.legend()
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw") plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True) plt.grid(True)
plt.legend() plt.legend()
plt.xlabel("line length[m]") plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]") plt.ylabel("yaw angle[deg]")
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature") plt.plot(s, ck, "-r", label="curvature")
plt.grid(True) plt.grid(True)
plt.legend() plt.legend()

11
PathTracking/lqr_steer_control/lqr_steer_control.py
View File

@@ -171,7 +171,6 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
yaw = [state.yaw] yaw = [state.yaw]
v = [state.v] v = [state.v]
t = [0.0] t = [0.0]
target_ind = calc_nearest_index(state, cx, cy, cyaw)
e, e_th = 0.0, 0.0 e, e_th = 0.0, 0.0
@@ -237,10 +236,6 @@ def calc_speed_profile(cx, cy, cyaw, target_speed):
speed_profile[-1] = 0.0 speed_profile[-1] = 0.0
# flg, ax = plt.subplots(1)
# plt.plot(speed_profile, "-r")
# plt.show()
return speed_profile return speed_profile
@@ -260,7 +255,7 @@ def main():
if show_animation: if show_animation:
plt.close() plt.close()
flg, _ = plt.subplots(1) plt.subplots(1)
plt.plot(ax, ay, "xb", label="input") plt.plot(ax, ay, "xb", label="input")
plt.plot(cx, cy, "-r", label="spline") plt.plot(cx, cy, "-r", label="spline")
plt.plot(x, y, "-g", label="tracking") plt.plot(x, y, "-g", label="tracking")
@@ -270,14 +265,14 @@ def main():
plt.ylabel("y[m]") plt.ylabel("y[m]")
plt.legend() plt.legend()
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw") plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True) plt.grid(True)
plt.legend() plt.legend()
plt.xlabel("line length[m]") plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]") plt.ylabel("yaw angle[deg]")
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature") plt.plot(s, ck, "-r", label="curvature")
plt.grid(True) plt.grid(True)
plt.legend() plt.legend()

10
PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
View File

@@ -181,10 +181,6 @@ def calc_speed_profile(cx, cy, cyaw, target_speed):
speed_profile[-1] = 0.0 speed_profile[-1] = 0.0
# flg, ax = plt.subplots(1)
# plt.plot(speed_profile, "-r")
# plt.show()
return speed_profile return speed_profile
@@ -208,7 +204,7 @@ def main():
if show_animation: if show_animation:
plt.close() plt.close()
flg, _ = plt.subplots(1) plt.subplots(1)
plt.plot(ax, ay, "xb", label="input") plt.plot(ax, ay, "xb", label="input")
plt.plot(cx, cy, "-r", label="spline") plt.plot(cx, cy, "-r", label="spline")
plt.plot(x, y, "-g", label="tracking") plt.plot(x, y, "-g", label="tracking")
@@ -218,14 +214,14 @@ def main():
plt.ylabel("y[m]") plt.ylabel("y[m]")
plt.legend() plt.legend()
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw") plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True) plt.grid(True)
plt.legend() plt.legend()
plt.xlabel("line length[m]") plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]") plt.ylabel("yaw angle[deg]")
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature") plt.plot(s, ck, "-r", label="curvature")
plt.grid(True) plt.grid(True)
plt.legend() plt.legend()

8
SLAM/iterative_closest_point/iterative_closest_point.py
View File

@@ -45,7 +45,7 @@ def ICP_matching(ppoints, cpoints):
Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints) Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints)
# update current points # update current points
cpoints = (Rt @ cpoints) + Tt[:,np.newaxis] cpoints = (Rt @ cpoints) + Tt[:, np.newaxis]
H = update_homogenerous_matrix(H, Rt, Tt) H = update_homogenerous_matrix(H, Rt, Tt)
@@ -114,8 +114,8 @@ def SVD_motion_estimation(ppoints, cpoints):
pm = np.mean(ppoints, axis=1) pm = np.mean(ppoints, axis=1)
cm = np.mean(cpoints, axis=1) cm = np.mean(cpoints, axis=1)
pshift = ppoints - pm[:,np.newaxis] pshift = ppoints - pm[:, np.newaxis]
cshift = cpoints - cm[:,np.newaxis] cshift = cpoints - cm[:, np.newaxis]
W = cshift @ pshift.T W = cshift @ pshift.T
u, s, vh = np.linalg.svd(W) u, s, vh = np.linalg.svd(W)
@@ -151,6 +151,8 @@ def main():
cpoints = np.vstack((cx, cy)) cpoints = np.vstack((cx, cy))
R, T = ICP_matching(ppoints, cpoints) R, T = ICP_matching(ppoints, cpoints)
print("R:", R)
print("T:", T)
if __name__ == '__main__': if __name__ == '__main__':