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

fix test and code clean up for lgtm

Browse Source
This commit is contained in:
Atsushi Sakai
2018-11-23 14:46:16 +09:00
parent 404cc79e66
commit a4a8591018
11 changed files with 54 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
AerialNavigation/drone_3d_trajectory_following/Quadrotor.py
View File

@@ -7,7 +7,6 @@ Author: Daniel Ingram (daniel-s-ingram)
from math import cos, sin from math import cos, sin
import numpy as np import numpy as np
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
class Quadrotor(): class Quadrotor():

5
AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py
View File

@@ -8,6 +8,7 @@ from math import cos, sin
import numpy as np import numpy as np
from Quadrotor import Quadrotor from Quadrotor import Quadrotor
from TrajectoryGenerator import TrajectoryGenerator from TrajectoryGenerator import TrajectoryGenerator
from mpl_toolkits.mplot3d import Axes3D
show_animation = True show_animation = True
@@ -69,8 +70,8 @@ def quad_sim(x_c, y_c, z_c):
while True: while True:
while t <= T: while t <= T:
des_x_pos = calculate_position(x_c[i], t) # des_x_pos = calculate_position(x_c[i], t)
des_y_pos = calculate_position(y_c[i], t) # des_y_pos = calculate_position(y_c[i], t)
des_z_pos = calculate_position(z_c[i], t) des_z_pos = calculate_position(z_c[i], t)
des_x_vel = calculate_velocity(x_c[i], t) des_x_vel = calculate_velocity(x_c[i], t)
des_y_vel = calculate_velocity(y_c[i], t) des_y_vel = calculate_velocity(y_c[i], t)

9
ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py
View File

@@ -4,7 +4,6 @@ Inverse kinematics for an n-link arm using the Jacobian inverse method
Author: Daniel Ingram (daniel-s-ingram) Author: Daniel Ingram (daniel-s-ingram)
Atsushi Sakai (@Atsushi_twi) Atsushi Sakai (@Atsushi_twi)
""" """
import matplotlib.pyplot as plt
import numpy as np import numpy as np
from NLinkArm import NLinkArm from NLinkArm import NLinkArm
@@ -34,8 +33,8 @@ def main():
state = WAIT_FOR_NEW_GOAL state = WAIT_FOR_NEW_GOAL
solution_found = False solution_found = False
while True: while True:
old_goal = np.ndarray(goal_pos) old_goal = np.array(goal_pos)
goal_pos = arm.goal goal_pos = np.array(arm.goal)
end_effector = arm.end_effector end_effector = arm.end_effector
errors, distance = distance_to_goal(end_effector, goal_pos) errors, distance = distance_to_goal(end_effector, goal_pos)
@@ -160,5 +159,5 @@ def ang_diff(theta1, theta2):
if __name__ == '__main__': if __name__ == '__main__':
# main() main()
animation() # animation()

1
PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py
View File

@@ -459,7 +459,6 @@ class BITStar(object):
# add an edge to the edge queue is the path might improve the solution # add an edge to the edge queue is the path might improve the solution
for neighbor in neigbors: for neighbor in neigbors:
sid = neighbor[0] sid = neighbor[0]
scoord = neighbor[1]
estimated_f_score = self.computeDistanceCost( estimated_f_score = self.computeDistanceCost(
self.startId, vid) + self.computeHeuristicCost(sid, self.goalId) + self.computeDistanceCost(vid, sid) self.startId, vid) + self.computeHeuristicCost(sid, self.goalId) + self.computeDistanceCost(vid, sid)
if estimated_f_score < self.g_scores[self.goalId]: if estimated_f_score < self.g_scores[self.goalId]:

8
PathPlanning/ClosedLoopRRTStar/pure_pursuit.py
View File

@@ -269,7 +269,7 @@ def main():
# plt.pause(0.1) # plt.pause(0.1)
# input() # input()
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(cx, cy, ".r", label="course") plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory") plt.plot(x, y, "-b", label="trajectory")
plt.legend() plt.legend()
@@ -278,7 +278,7 @@ def main():
plt.axis("equal") plt.axis("equal")
plt.grid(True) plt.grid(True)
flg, ax = plt.subplots(1) 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("Speed[km/h]") plt.ylabel("Speed[km/h]")
@@ -304,7 +304,7 @@ def main2():
t, x, y, yaw, v, a, d, flag = closed_loop_prediction( t, x, y, yaw, v, a, d, flag = closed_loop_prediction(
cx, cy, cyaw, speed_profile, goal) cx, cy, cyaw, speed_profile, goal)
flg, ax = plt.subplots(1) plt.subplots(1)
plt.plot(cx, cy, ".r", label="course") plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory") plt.plot(x, y, "-b", label="trajectory")
plt.plot(goal[0], goal[1], "xg", label="goal") plt.plot(goal[0], goal[1], "xg", label="goal")
@@ -314,7 +314,7 @@ def main2():
plt.axis("equal") plt.axis("equal")
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("Speed[km/h]") plt.ylabel("Speed[km/h]")

12
PathPlanning/Eta3SplinePath/eta3_spline_path.py
View File

@@ -153,7 +153,8 @@ class eta3_path_segment(object):
- (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 * - (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
s_dot = lambda u : np.linalg.norm(self.coeffs[:, 1:].dot(np.array([1, 2.*u, 3.*u**2, 4.*u**3, 5.*u**4, 6.*u**5, 7.*u**6]))) def s_dot(u): return np.linalg.norm(self.coeffs[:, 1:].dot(
np.array([1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6])))
self.segment_length = quad(lambda u: s_dot(u), 0, 1)[0] self.segment_length = quad(lambda u: s_dot(u), 0, 1)[0]
""" """
@@ -164,6 +165,7 @@ class eta3_path_segment(object):
returns returns
(x,y) of point along the segment (x,y) of point along the segment
""" """
def calc_point(self, u): def calc_point(self, u):
assert(u >= 0 and u <= 1) assert(u >= 0 and u <= 1)
return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7])) return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7]))
@@ -187,8 +189,8 @@ def test1():
# interpolate at several points along the path # interpolate at several points along the path
ui = np.linspace(0, len(path_segments), 1001) ui = np.linspace(0, len(path_segments), 1001)
pos = np.empty((2, ui.size)) pos = np.empty((2, ui.size))
for i, u in enumerate(ui): for j, u in enumerate(ui):
pos[:, i] = path.calc_path_point(u) pos[:, j] = path.calc_path_point(u)
if show_animation: if show_animation:
# plot the path # plot the path
@@ -217,8 +219,8 @@ def test2():
# interpolate at several points along the path # interpolate at several points along the path
ui = np.linspace(0, len(path_segments), 1001) ui = np.linspace(0, len(path_segments), 1001)
pos = np.empty((2, ui.size)) pos = np.empty((2, ui.size))
for i, u in enumerate(ui): for j, u in enumerate(ui):
pos[:, i] = path.calc_path_point(u) pos[:, j] = path.calc_path_point(u)
if show_animation: if show_animation:
# plot the path # plot the path

56
PathPlanning/HybridAStar/hybrid_a_star.py
View File

@@ -13,7 +13,7 @@ import math
import numpy as np import numpy as np
import scipy.spatial import scipy.spatial
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import reeds_shepp_path_planning as rs # import reeds_shepp_path_planning as rs
import heapq import heapq
EXTEND_AREA = 5.0 # [m] EXTEND_AREA = 5.0 # [m]
@@ -118,40 +118,40 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
yawreso: yaw angle resolution [rad] yawreso: yaw angle resolution [rad]
""" """
start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2]) # start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2])
tox, toy = ox[:], oy[:] # tox, toy = ox[:], oy[:]
obkdtree = KDTree(np.vstack((tox, toy)).T) # obkdtree = KDTree(np.vstack((tox, toy)).T)
c = Config(tox, toy, xyreso, yawreso) # c = Config(tox, toy, xyreso, yawreso)
nstart = Node(int(start[0] / xyreso), int(start[1] / xyreso), int(start[2] / yawreso), # nstart = Node(int(start[0] / xyreso), int(start[1] / xyreso), int(start[2] / yawreso),
True, [start[0]], [start[1]], [start[2]], [True], 0.0, 0.0, -1) # True, [start[0]], [start[1]], [start[2]], [True], 0.0, 0.0, -1)
ngoal = Node(int(goal[0] / xyreso), int(goal[1] / xyreso), int(goal[2] / yawreso), # ngoal = Node(int(goal[0] / xyreso), int(goal[1] / xyreso), int(goal[2] / yawreso),
True, [goal[0]], [goal[1]], [goal[2]], [True], 0.0, 0.0, -1) # True, [goal[0]], [goal[1]], [goal[2]], [True], 0.0, 0.0, -1)
openList, closedList = {}, {} # openList, closedList = {}, {}
h = [] # h = []
# goalqueue = queue.PriorityQueue() # # goalqueue = queue.PriorityQueue()
pq = [] # pq = []
openList[calc_index(nstart, c)] = nstart # openList[calc_index(nstart, c)] = nstart
heapq.heappush(pq, (calc_index(nstart, c), calc_cost(nstart, h, ngoal, c))) # heapq.heappush(pq, (calc_index(nstart, c), calc_cost(nstart, h, ngoal, c)))
while True: # while True:
if not openList: # if not openList:
print("Error: Cannot find path, No open set") # print("Error: Cannot find path, No open set")
return [], [], [] # return [], [], []
c_id, cost = heapq.heappop(pq) # c_id, cost = heapq.heappop(pq)
current = openList.pop(c_id) # current = openList.pop(c_id)
closedList[c_id] = current # closedList[c_id] = current
isupdated, fpath = analytic_expantion( # isupdated, fpath = analytic_expantion(
current, ngoal, c, ox, oy, obkdtree) # current, ngoal, c, ox, oy, obkdtree)
# print(current) # # print(current)
# rx, ry, ryaw = [], [], [] rx, ry, ryaw = [], [], []
return rx, ry, ryaw return rx, ry, ryaw
@@ -208,8 +208,8 @@ def main():
start, goal, ox, oy, xyreso, yawreso) start, goal, ox, oy, xyreso, yawreso)
plt.plot(ox, oy, ".k") plt.plot(ox, oy, ".k")
rs.plot_arrow(start[0], start[1], start[2]) # rs.plot_arrow(start[0], start[1], start[2])
rs.plot_arrow(goal[0], goal[1], goal[2]) # rs.plot_arrow(goal[0], goal[1], goal[2])
plt.grid(True) plt.grid(True)
plt.axis("equal") plt.axis("equal")

3
PathPlanning/PotentialFieldPlanning/potential_field_planning.py
View File

@@ -160,6 +160,9 @@ def main():
rx, ry = potential_field_planning( rx, ry = potential_field_planning(
sx, sy, gx, gy, ox, oy, grid_size, robot_radius) sx, sy, gx, gy, ox, oy, grid_size, robot_radius)
print(rx)
print(ry)
if show_animation: if show_animation:
plt.show() plt.show()

3
PathPlanning/RRTStarDubins/dubins_path_planning.py
View File

@@ -6,6 +6,7 @@ author Atsushi Sakai (@Atsushi_twi)
""" """
import math import math
import matplotlib.pyplot as plt
def mod2pi(theta): def mod2pi(theta):
@@ -260,7 +261,6 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
u""" u"""
Plot arrow Plot arrow
""" """
import matplotlib.pyplot as plt
if not isinstance(x, float): if not isinstance(x, float):
for (ix, iy, iyaw) in zip(x, y, yaw): for (ix, iy, iyaw) in zip(x, y, yaw):
@@ -273,7 +273,6 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
if __name__ == '__main__': if __name__ == '__main__':
print("Dubins path planner sample start!!") print("Dubins path planner sample start!!")
import matplotlib.pyplot as plt
start_x = 1.0 # [m] start_x = 1.0 # [m]
start_y = 1.0 # [m] start_y = 1.0 # [m]

5
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
View File

@@ -285,11 +285,6 @@ def generate_local_course(L, lengths, mode, maxc, step_size):
else: else:
directions[0] = -1 directions[0] = -1
if lengths[0] > 0.0:
d = step_size
else:
d = -step_size
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))):

5
PathTracking/stanley_controller/stanley_controller.py
View File

@@ -134,7 +134,8 @@ def calc_target_index(state, cx, cy):
error_front_axle = min(d) error_front_axle = min(d)
target_idx = d.index(error_front_axle) target_idx = d.index(error_front_axle)
target_yaw = normalize_angle(np.arctan2(fy - cy[target_idx], fx - cx[target_idx]) - state.yaw) target_yaw = normalize_angle(np.arctan2(
fy - cy[target_idx], fx - cx[target_idx]) - state.yaw)
if target_yaw > 0.0: if target_yaw > 0.0:
error_front_axle = - error_front_axle error_front_axle = - error_front_axle
@@ -201,7 +202,7 @@ def main():
plt.axis("equal") plt.axis("equal")
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("Speed[km/h]") plt.ylabel("Speed[km/h]")