diff --git a/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py b/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py index 452c43db..fd27c2f9 100644 --- a/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py +++ b/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py @@ -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(): diff --git a/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py b/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py index a4cdcbf2..3c980531 100644 --- a/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py +++ b/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py @@ -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) diff --git a/ArmNavigation/__init__.py b/ArmNavigation/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py b/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py index 27e9f9e4..3491b977 100644 --- a/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py +++ b/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py @@ -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: diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 66bcffbf..5dd31f63 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -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]: diff --git a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py index abe73d58..099114b7 100644 --- a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py +++ b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py @@ -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]") diff --git a/PathPlanning/Eta3SplinePath/eta3_spline_path.py b/PathPlanning/Eta3SplinePath/eta3_spline_path.py index 64f64faa..efb90407 100644 --- a/PathPlanning/Eta3SplinePath/eta3_spline_path.py +++ b/PathPlanning/Eta3SplinePath/eta3_spline_path.py @@ -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 diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py index 8b7c1052..a7ac4784 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -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") diff --git a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py index 946b08d8..b31c7d70 100644 --- a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py +++ b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py @@ -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() diff --git a/PathPlanning/RRTStarDubins/dubins_path_planning.py b/PathPlanning/RRTStarDubins/dubins_path_planning.py index cb37b24e..83221c1e 100644 --- a/PathPlanning/RRTStarDubins/dubins_path_planning.py +++ b/PathPlanning/RRTStarDubins/dubins_path_planning.py @@ -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] diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 5b04ebb0..4d7e3d38 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -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))): diff --git a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py index 5aca719f..b7aa2d9a 100644 --- a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py +++ b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py @@ -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 diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_controller/stanley_controller.py index 1ff9192d..ea013353 100644 --- a/PathTracking/stanley_controller/stanley_controller.py +++ b/PathTracking/stanley_controller/stanley_controller.py @@ -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]")