github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 20:28:07 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #135 from AtsushiSakai/dev

Browse Source 
code clean up for lgtm
This commit is contained in:
Atsushi Sakai
2018-11-23 16:39:40 +09:00
committed by GitHub
parent a26e4ad3fb 5f6cc19b15
commit b56f0f7a5a
13 changed files with 57 additions and 60 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
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
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
from Quadrotor import Quadrotor
from TrajectoryGenerator import TrajectoryGenerator
from mpl_toolkits.mplot3d import Axes3D
show_animation = True
@@ -69,8 +70,8 @@ def quad_sim(x_c, y_c, z_c):
while True:
while t <= T:
des_x_pos = calculate_position(x_c[i], t)
des_y_pos = calculate_position(y_c[i], t)
# des_x_pos = calculate_position(x_c[i], t)
# des_y_pos = calculate_position(y_c[i], t)
des_z_pos = calculate_position(z_c[i], t)
des_x_vel = calculate_velocity(x_c[i], t)
des_y_vel = calculate_velocity(y_c[i], t)

0
ArmNavigation/__init__.py Normal file
View File

13
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)
Atsushi Sakai (@Atsushi_twi)
"""
import matplotlib.pyplot as plt
import numpy as np
from NLinkArm import NLinkArm
@@ -34,8 +33,8 @@ def main():
state = WAIT_FOR_NEW_GOAL
solution_found = False
while True:
old_goal = goal_pos
goal_pos = arm.goal
old_goal = np.array(goal_pos)
goal_pos = np.array(arm.goal)
end_effector = arm.end_effector
errors, distance = distance_to_goal(end_effector, goal_pos)
@@ -51,7 +50,7 @@ def main():
elif solution_found:
state = MOVING_TO_GOAL
elif state is MOVING_TO_GOAL:
if distance > 0.1 and (old_goal == goal_pos).all():
if distance > 0.1 and all(old_goal == goal_pos):
joint_angles = joint_angles + Kp * \
ang_diff(joint_goal_angles, joint_angles) * dt
else:
@@ -93,8 +92,8 @@ def animation():
i_goal = 0
while True:
old_goal = goal_pos
goal_pos = arm.goal
old_goal = np.array(goal_pos)
goal_pos = np.array(arm.goal)
end_effector = arm.end_effector
errors, distance = distance_to_goal(end_effector, goal_pos)
@@ -111,7 +110,7 @@ def animation():
elif solution_found:
state = MOVING_TO_GOAL
elif state is MOVING_TO_GOAL:
if distance > 0.1 and (old_goal == goal_pos).all():
if distance > 0.1 and all(old_goal == goal_pos):
joint_angles = joint_angles + Kp * \
ang_diff(joint_goal_angles, joint_angles) * dt
else:

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
for neighbor in neigbors:
sid = neighbor[0]
scoord = neighbor[1]
estimated_f_score = self.computeDistanceCost(
self.startId, vid) + self.computeHeuristicCost(sid, self.goalId) + self.computeDistanceCost(vid, sid)
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)
# input()
flg, ax = plt.subplots(1)
plt.subplots(1)
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.legend()
@@ -278,7 +278,7 @@ def main():
plt.axis("equal")
plt.grid(True)
flg, ax = plt.subplots(1)
subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
@@ -304,7 +304,7 @@ def main2():
t, x, y, yaw, v, a, d, flag = closed_loop_prediction(
cx, cy, cyaw, speed_profile, goal)
flg, ax = plt.subplots(1)
plt.subplots(1)
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.plot(goal[0], goal[1], "xg", label="goal")
@@ -314,7 +314,7 @@ def main2():
plt.axis("equal")
plt.grid(True)
flg, ax = plt.subplots(1)
plt.subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")

14
PathPlanning/Eta3SplinePath/eta3_spline_path.py
View File

@@ -152,8 +152,9 @@ class eta3_path_segment(object):
+ (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * sb \
- (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
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]
"""
@@ -164,6 +165,7 @@ class eta3_path_segment(object):
returns
(x,y) of point along the segment
"""
def calc_point(self, u):
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]))
@@ -187,8 +189,8 @@ def test1():
# interpolate at several points along the path
ui = np.linspace(0, len(path_segments), 1001)
pos = np.empty((2, ui.size))
for i, u in enumerate(ui):
pos[:, i] = path.calc_path_point(u)
for j, u in enumerate(ui):
pos[:, j] = path.calc_path_point(u)
if show_animation:
# plot the path
@@ -217,8 +219,8 @@ def test2():
# interpolate at several points along the path
ui = np.linspace(0, len(path_segments), 1001)
pos = np.empty((2, ui.size))
for i, u in enumerate(ui):
pos[:, i] = path.calc_path_point(u)
for j, u in enumerate(ui):
pos[:, j] = path.calc_path_point(u)
if show_animation:
# plot the path

58
PathPlanning/HybridAStar/hybrid_a_star.py
View File

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

3
PathPlanning/PotentialFieldPlanning/potential_field_planning.py
View File

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

3
PathPlanning/RRTStarDubins/dubins_path_planning.py
View File

@@ -6,6 +6,7 @@ author Atsushi Sakai (@Atsushi_twi)
"""
import math
import matplotlib.pyplot as plt
def mod2pi(theta):
@@ -260,7 +261,6 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
u"""
Plot arrow
"""
import matplotlib.pyplot as plt
if not isinstance(x, float):
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__':
print("Dubins path planner sample start!!")
import matplotlib.pyplot as plt
start_x = 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:
directions[0] = -1
if lengths[0] > 0.0:
d = step_size
else:
d = -step_size
ll = 0.0
for (m, l, i) in zip(mode, lengths, range(len(mode))):

1
PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
View File

@@ -66,7 +66,6 @@ def solve_DARE(A, B, Q, R):
Xn = A.T * X * A - A.T * X * B * \
la.inv(R + B.T * X * B) * B.T * X * A + Q
if (abs(Xn - X)).max() < eps:
X = Xn
break
X = Xn

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)
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:
error_front_axle = - error_front_axle
@@ -201,7 +202,7 @@ def main():
plt.axis("equal")
plt.grid(True)
flg, ax = plt.subplots(1)
plt.subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")