From fa1585d880ea771abd4215077eb699d194486eb4 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Mon, 8 Jun 2020 21:43:37 +0900 Subject: [PATCH] fix scanning error (#339) --- .../random_forward_kinematics.py | 16 +- .../random_inverse_kinematics.py | 12 +- Bipedal/bipedal_planner/bipedal_planner.py | 3 +- .../inverted_pendulum_mpc_control.py | 28 ++- .../lidar_to_grid_map/lidar_to_grid_map.py | 3 +- .../batch_informed_rrtstar.py | 158 ++++++++------ .../bidirectional_a_star.py | 135 ++++++------ .../bidirectional_breadth_first_search.py | 142 ++++++------ .../VoronoiRoadMap/dijkstra_search.py | 1 + PathTracking/cgmres_nmpc/cgmres_nmpc.py | 206 ++++++++++-------- ..._grid_based_sweep_coverage_path_planner.py | 175 ++++++++------- 11 files changed, 487 insertions(+), 392 deletions(-) diff --git a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py index b3426e53e85..f9caace3006 100644 --- a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py +++ b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py @@ -11,10 +11,9 @@ def random_val(min_val, max_val): return min_val + random.random() * (max_val - min_val) -if __name__ == "__main__": +def main(): print("Start solving Forward Kinematics 10 times") - - # init NLinkArm with Denavit-Hartenberg parameters of PR2 + # init NLinkArm with Denavit-Hartenberg parameters of PR2 n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.], [math.pi / 2, math.pi / 2, 0., 0.], [0., -math.pi / 2, 0., .4], @@ -22,10 +21,13 @@ def random_val(min_val, max_val): [0., -math.pi / 2, 0., .321], [0., math.pi / 2, 0., 0.], [0., 0., 0., 0.]]) - # execute FK 10 times - for i in range(10): + for _ in range(10): n_link_arm.set_joint_angles( - [random_val(-1, 1) for j in range(len(n_link_arm.link_list))]) + [random_val(-1, 1) for _ in range(len(n_link_arm.link_list))]) - ee_pose = n_link_arm.forward_kinematics(plot=True) + n_link_arm.forward_kinematics(plot=True) + + +if __name__ == "__main__": + main() diff --git a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py index a3838a6a389..91f6f1bba0f 100644 --- a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py +++ b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py @@ -11,10 +11,9 @@ def random_val(min_val, max_val): return min_val + random.random() * (max_val - min_val) -if __name__ == "__main__": +def main(): print("Start solving Inverse Kinematics 10 times") - - # init NLinkArm with Denavit-Hartenberg parameters of PR2 + # init NLinkArm with Denavit-Hartenberg parameters of PR2 n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.], [math.pi / 2, math.pi / 2, 0., 0.], [0., -math.pi / 2, 0., .4], @@ -22,12 +21,15 @@ def random_val(min_val, max_val): [0., -math.pi / 2, 0., .321], [0., math.pi / 2, 0., 0.], [0., 0., 0., 0.]]) - # execute IK 10 times - for i in range(10): + for _ in range(10): n_link_arm.inverse_kinematics([random_val(-0.5, 0.5), random_val(-0.5, 0.5), random_val(-0.5, 0.5), random_val(-0.5, 0.5), random_val(-0.5, 0.5), random_val(-0.5, 0.5)], plot=True) + + +if __name__ == "__main__": + main() diff --git a/Bipedal/bipedal_planner/bipedal_planner.py b/Bipedal/bipedal_planner/bipedal_planner.py index 60c03024bf8..01df63034ff 100644 --- a/Bipedal/bipedal_planner/bipedal_planner.py +++ b/Bipedal/bipedal_planner/bipedal_planner.py @@ -46,9 +46,8 @@ def walk(self, t_sup=0.8, z_c=0.8, a=10, b=1, plot=False): print("No footsteps") return - com_trajectory_for_plot, ax = None, None - # set up plotter + com_trajectory_for_plot, ax = None, None if plot: fig = plt.figure() ax = Axes3D(fig) diff --git a/InvertedPendulumCart/inverted_pendulum_mpc_control.py b/InvertedPendulumCart/inverted_pendulum_mpc_control.py index caf0c23e77a..e7b3077740b 100644 --- a/InvertedPendulumCart/inverted_pendulum_mpc_control.py +++ b/InvertedPendulumCart/inverted_pendulum_mpc_control.py @@ -3,11 +3,12 @@ author: Atsushi Sakai """ -import matplotlib.pyplot as plt -import numpy as np import math import time + import cvxpy +import matplotlib.pyplot as plt +import numpy as np # Model parameters @@ -39,10 +40,11 @@ def main(): for i in range(50): # calc control input - optimized_x, optimized_delta_x, optimized_theta, optimized_delta_theta, optimized_input = mpc_control(x) + opt_x, opt_delta_x, opt_theta, opt_delta_theta, opt_input = \ + mpc_control(x) # get input - u = optimized_input[0] + u = opt_input[0] # simulate inverted pendulum cart x = simulation(x, u) @@ -86,17 +88,19 @@ def mpc_control(x0): print("calc time:{0} [sec]".format(elapsed_time)) if prob.status == cvxpy.OPTIMAL: - ox = get_nparray_from_matrix(x.value[0, :]) - dx = get_nparray_from_matrix(x.value[1, :]) - theta = get_nparray_from_matrix(x.value[2, :]) - dtheta = get_nparray_from_matrix(x.value[3, :]) + ox = get_numpy_array_from_matrix(x.value[0, :]) + dx = get_numpy_array_from_matrix(x.value[1, :]) + theta = get_numpy_array_from_matrix(x.value[2, :]) + d_theta = get_numpy_array_from_matrix(x.value[3, :]) - ou = get_nparray_from_matrix(u.value[0, :]) + ou = get_numpy_array_from_matrix(u.value[0, :]) + else: + ox, dx, theta, d_theta, ou = None, None, None, None, None - return ox, dx, theta, dtheta, ou + return ox, dx, theta, d_theta, ou -def get_nparray_from_matrix(x): +def get_numpy_array_from_matrix(x): """ get build-in list from matrix """ @@ -133,7 +137,7 @@ def plot_cart(xt, theta): radius = 0.1 cx = np.array([-cart_w / 2.0, cart_w / 2.0, cart_w / - 2.0, -cart_w / 2.0, -cart_w / 2.0]) + 2.0, -cart_w / 2.0, -cart_w / 2.0]) cy = np.array([0.0, 0.0, cart_h, cart_h, 0.0]) cy += radius * 2.0 diff --git a/Mapping/lidar_to_grid_map/lidar_to_grid_map.py b/Mapping/lidar_to_grid_map/lidar_to_grid_map.py index 911a0bf0eaf..57278d4a471 100644 --- a/Mapping/lidar_to_grid_map/lidar_to_grid_map.py +++ b/Mapping/lidar_to_grid_map/lidar_to_grid_map.py @@ -19,7 +19,8 @@ def file_read(f): """ Reading LIDAR laser beams (angles and corresponding distance data) """ - measures = [line.split(",") for line in open(f)] + with open(f) as data: + measures = [line.split(",") for line in data] angles = [] distances = [] for measure in measures: diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 227b3f9d25a..b5eabbb39bc 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -27,7 +27,8 @@ class RTree(object): # Class to represent the explicit tree created # while sampling through the state space - def __init__(self, start=None, lowerLimit=None, upperLimit=None, resolution=1.0): + def __init__(self, start=None, lowerLimit=None, upperLimit=None, + resolution=1.0): if upperLimit is None: upperLimit = [10, 10] @@ -96,10 +97,11 @@ def grid_coordinate_to_node_id(self, coord): def real_world_to_node_id(self, real_coord): # first convert the given coordinates to grid space and then # convert the grid space coordinates to a unique node id - return self.grid_coordinate_to_node_id(self.real_coords_to_grid_coord(real_coord)) + return self.grid_coordinate_to_node_id( + self.real_coords_to_grid_coord(real_coord)) def grid_coord_to_real_world_coord(self, coord): - # This function smaps a grid coordinate in discrete space + # This function maps a grid coordinate in discrete space # to a configuration in the full configuration space config = [0] * self.dimension for i in range(0, len(coord)): @@ -126,7 +128,8 @@ def node_id_to_grid_coord(self, node_id): def node_id_to_real_world_coord(self, nid): # This function maps a node in discrete space to a configuration # in the full configuration space - return self.grid_coord_to_real_world_coord(self.node_id_to_grid_coord(nid)) + return self.grid_coord_to_real_world_coord( + self.node_id_to_grid_coord(nid)) class BITStar(object): @@ -137,9 +140,9 @@ def __init__(self, start, goal, self.start = start self.goal = goal - self.minrand = randArea[0] - self.maxrand = randArea[1] - self.maxIter = maxIter + self.min_rand = randArea[0] + self.max_rand = randArea[1] + self.max_iIter = maxIter self.obstacleList = obstacleList self.startId = None self.goalId = None @@ -186,18 +189,19 @@ def setup_planning(self): [(self.start[1] + self.goal[1]) / 2.0], [0]]) a1 = np.array([[(self.goal[0] - self.start[0]) / cMin], [(self.goal[1] - self.start[1]) / cMin], [0]]) - etheta = math.atan2(a1[1], a1[0]) - # first column of idenity matrix transposed + eTheta = math.atan2(a1[1], a1[0]) + # first column of identity matrix transposed id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3) M = np.dot(a1, id1_t) U, S, Vh = np.linalg.svd(M, True, True) C = np.dot(np.dot(U, np.diag( - [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh) + [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), + Vh) self.samples.update(self.informed_sample( 200, cBest, cMin, xCenter, C)) - return etheta, cMin, xCenter, C, cBest + return eTheta, cMin, xCenter, C, cBest def setup_sample(self, iterations, foundGoal, cMin, xCenter, C, cBest): @@ -228,18 +232,19 @@ def setup_sample(self, iterations, foundGoal, cMin, xCenter, C, cBest): def plan(self, animation=True): - etheta, cMin, xCenter, C, cBest = self.setup_planning() + eTheta, cMin, xCenter, C, cBest = self.setup_planning() iterations = 0 foundGoal = False # run until done - while iterations < self.maxIter: + while iterations < self.max_iIter: cBest = self.setup_sample(iterations, foundGoal, cMin, xCenter, C, cBest) # expand the best vertices until an edge is better than the vertex # this is done because the vertex cost represents the lower bound # on the edge cost - while self.best_vertex_queue_value() <= self.best_edge_queue_value(): + while self.best_vertex_queue_value() <= \ + self.best_edge_queue_value(): self.expand_vertex(self.best_in_vertex_queue()) # add the best edge to the tree @@ -247,11 +252,17 @@ def plan(self, animation=True): self.edge_queue.remove(bestEdge) # Check if this can improve the current solution - estimatedCostOfVertex = self.g_scores[bestEdge[0]] + self.compute_distance_cost( - bestEdge[0], bestEdge[1]) + self.compute_heuristic_cost(bestEdge[1], self.goalId) - estimatedCostOfEdge = self.compute_distance_cost(self.startId, bestEdge[0]) + self.compute_heuristic_cost( - bestEdge[0], bestEdge[1]) + self.compute_heuristic_cost(bestEdge[1], self.goalId) - actualCostOfEdge = self.g_scores[bestEdge[0]] + self.compute_distance_cost(bestEdge[0], bestEdge[1]) + estimatedCostOfVertex = self.g_scores[bestEdge[ + 0]] + self.compute_distance_cost( + bestEdge[0], bestEdge[1]) + self.compute_heuristic_cost( + bestEdge[1], self.goalId) + estimatedCostOfEdge = self.compute_distance_cost( + self.startId, bestEdge[0]) + self.compute_heuristic_cost( + bestEdge[0], bestEdge[1]) + self.compute_heuristic_cost( + bestEdge[1], self.goalId) + actualCostOfEdge = self.g_scores[ + bestEdge[0]] + self.compute_distance_cost( + bestEdge[0], bestEdge[1]) f1 = estimatedCostOfVertex < self.g_scores[self.goalId] f2 = estimatedCostOfEdge < self.g_scores[self.goalId] @@ -277,10 +288,12 @@ def plan(self, animation=True): try: del self.samples[bestEdge[1]] except KeyError: + # invalid sample key pass eid = self.tree.add_vertex(nextCoord) self.vertex_queue.append(eid) - if eid == self.goalId or bestEdge[0] == self.goalId or bestEdge[1] == self.goalId: + if eid == self.goalId or bestEdge[0] == self.goalId or \ + bestEdge[1] == self.goalId: print("Goal found") foundGoal = True @@ -288,14 +301,18 @@ def plan(self, animation=True): g_score = self.compute_distance_cost( bestEdge[0], bestEdge[1]) - self.g_scores[bestEdge[1]] = g_score + self.g_scores[bestEdge[0]] - self.f_scores[bestEdge[1]] = g_score + self.compute_heuristic_cost(bestEdge[1], self.goalId) + self.g_scores[bestEdge[1]] = g_score + self.g_scores[ + bestEdge[0]] + self.f_scores[ + bestEdge[1]] = g_score + self.compute_heuristic_cost( + bestEdge[1], self.goalId) self.update_graph() # visualize new edge if animation: self.draw_graph(xCenter=xCenter, cBest=cBest, - cMin=cMin, etheta=etheta, samples=self.samples.values(), + cMin=cMin, eTheta=eTheta, + samples=self.samples.values(), start=firstCoord, end=secondCoord) self.remove_queue(lastEdge, bestEdge) @@ -330,7 +347,8 @@ def remove_queue(self, lastEdge, bestEdge): for edge in self.edge_queue: if edge[1] == bestEdge[1]: dist_cost = self.compute_distance_cost(edge[1], bestEdge[1]) - if self.g_scores[edge[1]] + dist_cost >= self.g_scores[self.goalId]: + if self.g_scores[edge[1]] + dist_cost >= \ + self.g_scores[self.goalId]: if (lastEdge, bestEdge[1]) in self.edge_queue: self.edge_queue.remove( (lastEdge, bestEdge[1])) @@ -338,8 +356,9 @@ def remove_queue(self, lastEdge, bestEdge): def connect(self, start, end): # A function which attempts to extend from a start coordinates # to goal coordinates - steps = int(self.compute_distance_cost(self.tree.real_world_to_node_id( - start), self.tree.real_world_to_node_id(end)) * 10) + steps = int(self.compute_distance_cost( + self.tree.real_world_to_node_id(start), + self.tree.real_world_to_node_id(end)) * 10) x = np.linspace(start[0], end[0], num=steps) y = np.linspace(start[1], end[1], num=steps) for i in range(len(x)): @@ -409,8 +428,8 @@ def sample_unit_ball(): return np.array([[sample[0]], [sample[1]], [0]]) def sample_free_space(self): - rnd = [random.uniform(self.minrand, self.maxrand), - random.uniform(self.minrand, self.maxrand)] + rnd = [random.uniform(self.min_rand, self.max_rand), + random.uniform(self.min_rand, self.max_rand)] return rnd @@ -418,7 +437,8 @@ def best_vertex_queue_value(self): if len(self.vertex_queue) == 0: return float('inf') values = [self.g_scores[v] - + self.compute_heuristic_cost(v, self.goalId) for v in self.vertex_queue] + + self.compute_heuristic_cost(v, self.goalId) for v in + self.vertex_queue] values.sort() return values[0] @@ -427,21 +447,25 @@ def best_edge_queue_value(self): return float('inf') # return the best value in the queue by score g_tau[v] + c(v,x) + h(x) values = [self.g_scores[e[0]] + self.compute_distance_cost(e[0], e[1]) - + self.compute_heuristic_cost(e[1], self.goalId) for e in self.edge_queue] + + self.compute_heuristic_cost(e[1], self.goalId) for e in + self.edge_queue] values.sort(reverse=True) return values[0] def best_in_vertex_queue(self): # return the best value in the vertex queue - v_plus_vals = [(v, self.g_scores[v] + self.compute_heuristic_cost(v, self.goalId)) - for v in self.vertex_queue] - v_plus_vals = sorted(v_plus_vals, key=lambda x: x[1]) - # print(v_plus_vals) - return v_plus_vals[0][0] + v_plus_values = [(v, self.g_scores[v] + + self.compute_heuristic_cost(v, self.goalId)) + for v in self.vertex_queue] + v_plus_values = sorted(v_plus_values, key=lambda x: x[1]) + # print(v_plus_values) + return v_plus_values[0][0] def best_in_edge_queue(self): - e_and_values = [(e[0], e[1], self.g_scores[e[0]] + self.compute_distance_cost( - e[0], e[1]) + self.compute_heuristic_cost(e[1], self.goalId)) for e in self.edge_queue] + e_and_values = [ + (e[0], e[1], self.g_scores[e[0]] + self.compute_distance_cost( + e[0], e[1]) + self.compute_heuristic_cost(e[1], self.goalId)) + for e in self.edge_queue] e_and_values = sorted(e_and_values, key=lambda x: x[2]) return e_and_values[0][0], e_and_values[0][1] @@ -454,18 +478,19 @@ def expand_vertex(self, vid): # get the nearest value in vertex for every one in samples where difference is # less than the radius - neigbors = [] - for sid, scoord in self.samples.items(): - scoord = np.array(scoord) - if np.linalg.norm(scoord - currCoord, 2) <= self.r and sid != vid: - neigbors.append((sid, scoord)) + neighbors = [] + for sid, s_coord in self.samples.items(): + s_coord = np.array(s_coord) + if np.linalg.norm(s_coord - currCoord, 2) <= self.r and sid != vid: + neighbors.append((sid, s_coord)) # add an edge to the edge queue is the path might improve the solution - for neighbor in neigbors: + for neighbor in neighbors: sid = neighbor[0] h_cost = self.compute_heuristic_cost(sid, self.goalId) estimated_f_score = self.compute_distance_cost( - self.startId, vid) + h_cost + self.compute_distance_cost(vid, sid) + self.startId, vid) + h_cost + self.compute_distance_cost(vid, + sid) if estimated_f_score < self.g_scores[self.goalId]: self.edge_queue.append((vid, sid)) @@ -476,17 +501,21 @@ def add_vertex_to_edge_queue(self, vid, currCoord): if vid not in self.old_vertices: neighbors = [] 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: v_coord = self.tree.node_id_to_real_world_coord(v) if np.linalg.norm(currCoord - v_coord, 2) <= self.r: neighbors.append((vid, v_coord)) for neighbor in neighbors: sid = neighbor[0] - estimated_f_score = self.compute_distance_cost(self.startId, vid) + self.compute_distance_cost( + estimated_f_score = self.compute_distance_cost( + self.startId, vid) + self.compute_distance_cost( vid, sid) + self.compute_heuristic_cost(sid, self.goalId) if estimated_f_score < self.g_scores[self.goalId] and ( - self.g_scores[vid] + self.compute_distance_cost(vid, sid)) < self.g_scores[sid]: + self.g_scores[vid] + + self.compute_distance_cost(vid, sid)) < \ + self.g_scores[sid]: self.edge_queue.append((vid, sid)) def update_graph(self): @@ -511,37 +540,40 @@ def update_graph(self): # find a non visited successor to the current node successors = self.tree.vertices[currId] - for succesor in successors: - if succesor in closedSet: + for successor in successors: + if successor in closedSet: continue else: - # claculate tentative g score + # calculate tentative g score g_score = self.g_scores[currId] + \ - self.compute_distance_cost(currId, succesor) - if succesor not in openSet: + self.compute_distance_cost(currId, successor) + if successor not in openSet: # add the successor to open set - openSet.append(succesor) - elif g_score >= self.g_scores[succesor]: + openSet.append(successor) + elif g_score >= self.g_scores[successor]: continue # update g and f scores - self.g_scores[succesor] = g_score - self.f_scores[succesor] = g_score + self.compute_heuristic_cost(succesor, self.goalId) + self.g_scores[successor] = g_score + self.f_scores[ + successor] = g_score + self.compute_heuristic_cost( + successor, self.goalId) # store the parent and child - self.nodes[succesor] = currId + self.nodes[successor] = currId - def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None, + 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]) + 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") if cBest != float('inf'): - self.plot_ellipse(xCenter, cBest, cMin, etheta) + self.plot_ellipse(xCenter, cBest, cMin, eTheta) if start is not None and end is not None: plt.plot([start[0], start[1]], [end[0], end[1]], "-g") @@ -556,11 +588,11 @@ def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None, plt.pause(0.01) @staticmethod - def plot_ellipse(xCenter, cBest, cMin, etheta): # pragma: no cover + def plot_ellipse(xCenter, cBest, cMin, eTheta): # pragma: no cover a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0 b = cBest / 2.0 - angle = math.pi / 2.0 - etheta + angle = math.pi / 2.0 - eTheta cx = xCenter[0] cy = xCenter[1] diff --git a/PathPlanning/BidirectionalAStar/bidirectional_a_star.py b/PathPlanning/BidirectionalAStar/bidirectional_a_star.py index cf76ab67cdf..ecb875f4104 100644 --- a/PathPlanning/BidirectionalAStar/bidirectional_a_star.py +++ b/PathPlanning/BidirectionalAStar/bidirectional_a_star.py @@ -17,7 +17,7 @@ class BidirectionalAStarPlanner: - def __init__(self, ox, oy, reso, rr): + def __init__(self, ox, oy, resolution, rr): """ Initialize grid map for a star planning @@ -27,21 +27,24 @@ def __init__(self, ox, oy, reso, rr): rr: robot radius[m] """ - self.reso = reso + self.min_x, self.min_y = None, None + self.max_x, self.max_y = None, None + self.x_width, self.y_width, self.obstacle_map = None, None, None + self.resolution = resolution self.rr = rr self.calc_obstacle_map(ox, oy) self.motion = self.get_motion_model() class Node: - def __init__(self, x, y, cost, pind): + def __init__(self, x, y, cost, parent_index): self.x = x # index of grid self.y = y # index of grid self.cost = cost - self.pind = pind + self.parent_index = parent_index def __str__(self): return str(self.x) + "," + str(self.y) + "," + str( - self.cost) + "," + str(self.pind) + self.cost) + "," + str(self.parent_index) def planning(self, sx, sy, gx, gy): """ @@ -58,18 +61,19 @@ def planning(self, sx, sy, gx, gy): ry: y position list of the final path """ - nstart = self.Node(self.calc_xyindex(sx, self.minx), - self.calc_xyindex(sy, self.miny), 0.0, -1) - ngoal = self.Node(self.calc_xyindex(gx, self.minx), - self.calc_xyindex(gy, self.miny), 0.0, -1) + start_node = self.Node(self.calc_xy_index(sx, self.min_x), + self.calc_xy_index(sy, self.min_y), 0.0, -1) + goal_node = self.Node(self.calc_xy_index(gx, self.min_x), + self.calc_xy_index(gy, self.min_y), 0.0, -1) open_set_A, closed_set_A = dict(), dict() open_set_B, closed_set_B = dict(), dict() - open_set_A[self.calc_grid_index(nstart)] = nstart - open_set_B[self.calc_grid_index(ngoal)] = ngoal + open_set_A[self.calc_grid_index(start_node)] = start_node + open_set_B[self.calc_grid_index(goal_node)] = goal_node - current_A = nstart - current_B = ngoal + current_A = start_node + current_B = goal_node + meet_point_A, meet_point_B = None, None while 1: if len(open_set_A) == 0: @@ -94,22 +98,23 @@ def planning(self, sx, sy, gx, gy): # show graph if show_animation: # pragma: no cover - plt.plot(self.calc_grid_position(current_A.x, self.minx), - self.calc_grid_position(current_A.y, self.miny), "xc") - plt.plot(self.calc_grid_position(current_B.x, self.minx), - self.calc_grid_position(current_B.y, self.miny), "xc") + plt.plot(self.calc_grid_position(current_A.x, self.min_x), + self.calc_grid_position(current_A.y, self.min_y), + "xc") + plt.plot(self.calc_grid_position(current_B.x, self.min_x), + self.calc_grid_position(current_B.y, self.min_y), + "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]) + plt.gcf().canvas.mpl_connect( + 'key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if len(closed_set_A.keys()) % 10 == 0: plt.pause(0.001) if current_A.x == current_B.x and current_A.y == current_B.y: print("Found goal") - meetpointA = current_A - meetpointB = current_B + meet_point_A = current_A + meet_point_B = current_B break # Remove the item from the open set @@ -158,7 +163,7 @@ def planning(self, sx, sy, gx, gy): open_set_B[n_ids[1]] = c_nodes[1] rx, ry = self.calc_final_bidirectional_path( - meetpointA, meetpointB, closed_set_A, closed_set_B) + meet_point_A, meet_point_B, closed_set_A, closed_set_B) return rx, ry @@ -175,16 +180,16 @@ def calc_final_bidirectional_path(self, n1, n2, setA, setB): return rx, ry - def calc_final_path(self, ngoal, closedset): + def calc_final_path(self, goal_node, closed_set): # generate final course - rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [ - self.calc_grid_position(ngoal.y, self.miny)] - pind = ngoal.parent_index - while pind != -1: - n = closedset[pind] - rx.append(self.calc_grid_position(n.x, self.minx)) - ry.append(self.calc_grid_position(n.y, self.miny)) - pind = n.parent_index + rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], \ + [self.calc_grid_position(goal_node.y, self.min_y)] + parent_index = goal_node.parent_index + while parent_index != -1: + n = closed_set[parent_index] + rx.append(self.calc_grid_position(n.x, self.min_x)) + ry.append(self.calc_grid_position(n.y, self.min_y)) + parent_index = n.parent_index return rx, ry @@ -210,69 +215,69 @@ def find_total_cost(self, open_set, lambda_, n1): f_cost = g_cost + h_cost return f_cost - def calc_grid_position(self, index, minp): + def calc_grid_position(self, index, min_position): """ calc grid position :param index: - :param minp: + :param min_position: :return: """ - pos = index * self.reso + minp + pos = index * self.resolution + min_position return pos - def calc_xyindex(self, position, min_pos): - return round((position - min_pos) / self.reso) + def calc_xy_index(self, position, min_pos): + return round((position - min_pos) / self.resolution) def calc_grid_index(self, node): - return (node.y - self.miny) * self.xwidth + (node.x - self.minx) + return (node.y - self.min_y) * self.x_width + (node.x - self.min_x) def verify_node(self, node): - px = self.calc_grid_position(node.x, self.minx) - py = self.calc_grid_position(node.y, self.miny) + px = self.calc_grid_position(node.x, self.min_x) + py = self.calc_grid_position(node.y, self.min_y) - if px < self.minx: + if px < self.min_x: return False - elif py < self.miny: + elif py < self.min_y: return False - elif px >= self.maxx: + elif px >= self.max_x: return False - elif py >= self.maxy: + elif py >= self.max_y: return False # collision check - if self.obmap[node.x][node.y]: + if self.obstacle_map[node.x][node.y]: return False return True def calc_obstacle_map(self, ox, oy): - self.minx = round(min(ox)) - self.miny = round(min(oy)) - self.maxx = round(max(ox)) - self.maxy = round(max(oy)) - print("min_x:", self.minx) - print("min_y:", self.miny) - print("max_x:", self.maxx) - print("max_y:", self.maxy) + self.min_x = round(min(ox)) + self.min_y = round(min(oy)) + self.max_x = round(max(ox)) + self.max_y = round(max(oy)) + print("min_x:", self.min_x) + print("min_y:", self.min_y) + print("max_x:", self.max_x) + print("max_y:", self.max_y) - self.xwidth = round((self.maxx - self.minx) / self.reso) - self.ywidth = round((self.maxy - self.miny) / self.reso) - print("x_width:", self.xwidth) - print("y_width:", self.ywidth) + self.x_width = round((self.max_x - self.min_x) / self.resolution) + self.y_width = round((self.max_y - self.min_y) / self.resolution) + print("x_width:", self.x_width) + print("y_width:", self.y_width) # obstacle map generation - self.obmap = [[False for _ in range(self.ywidth)] - for _ in range(self.xwidth)] - for ix in range(self.xwidth): - x = self.calc_grid_position(ix, self.minx) - for iy in range(self.ywidth): - y = self.calc_grid_position(iy, self.miny) + self.obstacle_map = [[False for _ in range(self.y_width)] + for _ in range(self.x_width)] + for ix in range(self.x_width): + x = self.calc_grid_position(ix, self.min_x) + for iy in range(self.y_width): + y = self.calc_grid_position(iy, self.min_y) for iox, ioy in zip(ox, oy): d = math.hypot(iox - x, ioy - y) if d <= self.rr: - self.obmap[ix][iy] = True + self.obstacle_map[ix][iy] = True break @staticmethod diff --git a/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py b/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py index 328898db855..dd4282573e4 100644 --- a/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py +++ b/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py @@ -17,7 +17,7 @@ class BidirectionalBreadthFirstSearchPlanner: - def __init__(self, ox, oy, reso, rr): + def __init__(self, ox, oy, resolution, rr): """ Initialize grid map for bfs planning @@ -27,22 +27,25 @@ def __init__(self, ox, oy, reso, rr): rr: robot radius[m] """ - self.reso = reso + self.min_x, self.min_y = None, None + self.max_x, self.max_y = None, None + self.x_width, self.y_width, self.obstacle_map = None, None, None + self.resolution = resolution self.rr = rr self.calc_obstacle_map(ox, oy) self.motion = self.get_motion_model() class Node: - def __init__(self, x, y, cost, pind, parent): + def __init__(self, x, y, cost, parent_index, parent): self.x = x # index of grid self.y = y # index of grid self.cost = cost - self.pind = pind + self.parent_index = parent_index self.parent = parent def __str__(self): return str(self.x) + "," + str(self.y) + "," + str( - self.cost) + "," + str(self.pind) + self.cost) + "," + str(self.parent_index) def planning(self, sx, sy, gx, gy): """ @@ -59,15 +62,19 @@ def planning(self, sx, sy, gx, gy): ry: y position list of the final path """ - nstart = self.Node(self.calc_xyindex(sx, self.minx), - self.calc_xyindex(sy, self.miny), 0.0, -1, None) - ngoal = self.Node(self.calc_xyindex(gx, self.minx), - self.calc_xyindex(gy, self.miny), 0.0, -1, None) + start_node = self.Node(self.calc_xy_index(sx, self.min_x), + self.calc_xy_index(sy, self.min_y), 0.0, -1, + None) + goal_node = self.Node(self.calc_xy_index(gx, self.min_x), + self.calc_xy_index(gy, self.min_y), 0.0, -1, + None) open_set_A, closed_set_A = dict(), dict() open_set_B, closed_set_B = dict(), dict() - open_set_B[self.calc_grid_index(ngoal)] = ngoal - open_set_A[self.calc_grid_index(nstart)] = nstart + open_set_B[self.calc_grid_index(goal_node)] = goal_node + open_set_A[self.calc_grid_index(start_node)] = start_node + + meet_point_A, meet_point_B = None, None while 1: if len(open_set_A) == 0: @@ -89,28 +96,29 @@ def planning(self, sx, sy, gx, gy): # show graph if show_animation: # pragma: no cover - plt.plot(self.calc_grid_position(current_A.x, self.minx), - self.calc_grid_position(current_A.y, self.miny), "xc") - plt.plot(self.calc_grid_position(current_B.x, self.minx), - self.calc_grid_position(current_B.y, self.miny), "xc") + plt.plot(self.calc_grid_position(current_A.x, self.min_x), + self.calc_grid_position(current_A.y, self.min_y), + "xc") + plt.plot(self.calc_grid_position(current_B.x, self.min_x), + self.calc_grid_position(current_B.y, self.min_y), + "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]) + plt.gcf().canvas.mpl_connect( + 'key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if len(closed_set_A.keys()) % 10 == 0: plt.pause(0.001) if c_id_A in closed_set_B: print("Find goal") - meetpointA = closed_set_A[c_id_A] - meetpointB = closed_set_B[c_id_A] + meet_point_A = closed_set_A[c_id_A] + meet_point_B = closed_set_B[c_id_A] break elif c_id_B in closed_set_A: print("Find goal") - meetpointA = closed_set_A[c_id_B] - meetpointB = closed_set_B[c_id_B] + meet_point_A = closed_set_A[c_id_B] + meet_point_B = closed_set_B[c_id_B] break # expand_grid search grid based on motion model @@ -137,20 +145,18 @@ def planning(self, sx, sy, gx, gy): if not self.verify_node(node_B): breakB = True - if (n_id_A not in closed_set_A) and (n_id_A not in - open_set_A) and (not - breakA): + if (n_id_A not in closed_set_A) and \ + (n_id_A not in open_set_A) and (not breakA): node_A.parent = current_A open_set_A[n_id_A] = node_A - if (n_id_B not in closed_set_B) and (n_id_B not in - open_set_B) and (not - breakB): + if (n_id_B not in closed_set_B) and \ + (n_id_B not in open_set_B) and (not breakB): node_B.parent = current_B open_set_B[n_id_B] = node_B rx, ry = self.calc_final_path_bidir( - meetpointA, meetpointB, closed_set_A, closed_set_B) + meet_point_A, meet_point_B, closed_set_A, closed_set_B) return rx, ry # takes both set and meeting nodes and calculate optimal path @@ -166,81 +172,81 @@ def calc_final_path_bidir(self, n1, n2, setA, setB): return rx, ry - def calc_final_path(self, ngoal, closedset): + def calc_final_path(self, goal_node, closed_set): # generate final course - rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [ - self.calc_grid_position(ngoal.y, self.miny)] - n = closedset[ngoal.parent_index] + rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [ + self.calc_grid_position(goal_node.y, self.min_y)] + n = closed_set[goal_node.parent_index] while n is not None: - rx.append(self.calc_grid_position(n.x, self.minx)) - ry.append(self.calc_grid_position(n.y, self.miny)) + rx.append(self.calc_grid_position(n.x, self.min_x)) + ry.append(self.calc_grid_position(n.y, self.min_y)) n = n.parent return rx, ry - def calc_grid_position(self, index, minp): + def calc_grid_position(self, index, min_position): """ calc grid position :param index: - :param minp: + :param min_position: :return: """ - pos = index * self.reso + minp + pos = index * self.resolution + min_position return pos - def calc_xyindex(self, position, min_pos): - return round((position - min_pos) / self.reso) + def calc_xy_index(self, position, min_pos): + return round((position - min_pos) / self.resolution) def calc_grid_index(self, node): - return (node.y - self.miny) * self.xwidth + (node.x - self.minx) + return (node.y - self.min_y) * self.x_width + (node.x - self.min_x) def verify_node(self, node): - px = self.calc_grid_position(node.x, self.minx) - py = self.calc_grid_position(node.y, self.miny) + px = self.calc_grid_position(node.x, self.min_x) + py = self.calc_grid_position(node.y, self.min_y) - if px < self.minx: + if px < self.min_x: return False - elif py < self.miny: + elif py < self.min_y: return False - elif px >= self.maxx: + elif px >= self.max_x: return False - elif py >= self.maxy: + elif py >= self.max_y: return False # collision check - if self.obmap[node.x][node.y]: + if self.obstacle_map[node.x][node.y]: return False return True def calc_obstacle_map(self, ox, oy): - self.minx = round(min(ox)) - self.miny = round(min(oy)) - self.maxx = round(max(ox)) - self.maxy = round(max(oy)) - print("min_x:", self.minx) - print("min_y:", self.miny) - print("max_x:", self.maxx) - print("max_y:", self.maxy) + self.min_x = round(min(ox)) + self.min_y = round(min(oy)) + self.max_x = round(max(ox)) + self.max_y = round(max(oy)) + print("min_x:", self.min_x) + print("min_y:", self.min_y) + print("max_x:", self.max_x) + print("max_y:", self.max_y) - self.xwidth = round((self.maxx - self.minx) / self.reso) - self.ywidth = round((self.maxy - self.miny) / self.reso) - print("x_width:", self.xwidth) - print("y_width:", self.ywidth) + self.x_width = round((self.max_x - self.min_x) / self.resolution) + self.y_width = round((self.max_y - self.min_y) / self.resolution) + print("x_width:", self.x_width) + print("y_width:", self.y_width) # obstacle map generation - self.obmap = [[False for _ in range(self.ywidth)] - for _ in range(self.xwidth)] - for ix in range(self.xwidth): - x = self.calc_grid_position(ix, self.minx) - for iy in range(self.ywidth): - y = self.calc_grid_position(iy, self.miny) + self.obstacle_map = [[False for _ in range(self.y_width)] + for _ in range(self.x_width)] + for ix in range(self.x_width): + x = self.calc_grid_position(ix, self.min_x) + for iy in range(self.y_width): + y = self.calc_grid_position(iy, self.min_y) for iox, ioy in zip(ox, oy): d = math.hypot(iox - x, ioy - y) if d <= self.rr: - self.obmap[ix][iy] = True + self.obstacle_map[ix][iy] = True break @staticmethod diff --git a/PathPlanning/VoronoiRoadMap/dijkstra_search.py b/PathPlanning/VoronoiRoadMap/dijkstra_search.py index 54d15ea9fb7..aef07ebbaf3 100644 --- a/PathPlanning/VoronoiRoadMap/dijkstra_search.py +++ b/PathPlanning/VoronoiRoadMap/dijkstra_search.py @@ -125,6 +125,7 @@ def find_id(self, node_x_list, node_y_list, target_node): if self.is_same_node_with_xy(node_x_list[i], node_y_list[i], target_node): return i + return None @staticmethod def is_same_node_with_xy(node_x, node_y, node_b): diff --git a/PathTracking/cgmres_nmpc/cgmres_nmpc.py b/PathTracking/cgmres_nmpc/cgmres_nmpc.py index 98a12ab10cb..bb5699b8888 100644 --- a/PathTracking/cgmres_nmpc/cgmres_nmpc.py +++ b/PathTracking/cgmres_nmpc/cgmres_nmpc.py @@ -5,17 +5,18 @@ author Atsushi Sakai (@Atsushi_twi) Ref: - -- Shunichi09/nonlinear_control: Implementing the nonlinear model predictive control, sliding mode control https://github.com/Shunichi09/nonlinear_control +Shunichi09/nonlinear_control: Implementing the nonlinear model predictive +control, sliding mode control https://github.com/Shunichi09/nonlinear_control """ -import numpy as np +from math import cos, sin, radians, atan2 + import matplotlib.pyplot as plt -import math +import numpy as np U_A_MAX = 1.0 -U_OMEGA_MAX = math.radians(45.0) +U_OMEGA_MAX = radians(45.0) PHI_V = 0.01 PHI_OMEGA = 0.01 WB = 0.25 # [m] wheel base @@ -24,19 +25,18 @@ def differential_model(v, yaw, u_1, u_2): - - dx = math.cos(yaw) * v - dy = math.sin(yaw) * v + dx = cos(yaw) * v + dy = sin(yaw) * v dv = u_1 - dyaw = v / WB * math.sin(u_2) # tan is not good for nonlinear optimization + # tangent is not good for nonlinear optimization + d_yaw = v / WB * sin(u_2) - return dx, dy, dyaw, dv + return dx, dy, d_yaw, dv -class TwoWheeledSystem(): +class TwoWheeledSystem: def __init__(self, init_x, init_y, init_yaw, init_v): - self.x = init_x self.y = init_y self.yaw = init_yaw @@ -47,12 +47,11 @@ def __init__(self, init_x, init_y, init_yaw, init_v): self.history_v = [init_v] def update_state(self, u_1, u_2, dt=0.01): - - dx, dy, dyaw, dv = differential_model(self.v, self.yaw, u_1, u_2) + dx, dy, d_yaw, dv = differential_model(self.v, self.yaw, u_1, u_2) self.x += dt * dx self.y += dt * dy - self.yaw += dt * dyaw + self.yaw += dt * d_yaw self.v += dt * dv # save @@ -62,13 +61,15 @@ def update_state(self, u_1, u_2, dt=0.01): self.history_v.append(self.v) -class NMPCSimulatorSystem(): +class NMPCSimulatorSystem: def calc_predict_and_adjoint_state(self, x, y, yaw, v, u_1s, u_2s, N, dt): + # by using state equation x_s, y_s, yaw_s, v_s = self._calc_predict_states( - x, y, yaw, v, u_1s, u_2s, N, dt) # by using state equation + x, y, yaw, v, u_1s, u_2s, N, dt) + # by using adjoint equation lam_1s, lam_2s, lam_3s, lam_4s = self._calc_adjoint_states( - x_s, y_s, yaw_s, v_s, u_1s, u_2s, N, dt) # by using adjoint equation + x_s, y_s, yaw_s, v_s, u_2s, N, dt) return x_s, y_s, yaw_s, v_s, lam_1s, lam_2s, lam_3s, lam_4s @@ -88,7 +89,7 @@ def _calc_predict_states(self, x, y, yaw, v, u_1s, u_2s, N, dt): return x_s, y_s, yaw_s, v_s - def _calc_adjoint_states(self, x_s, y_s, yaw_s, v_s, u_1s, u_2s, N, dt): + def _calc_adjoint_states(self, x_s, y_s, yaw_s, v_s, u_2s, N, dt): lam_1s = [x_s[-1]] lam_2s = [y_s[-1]] lam_3s = [yaw_s[-1]] @@ -97,8 +98,8 @@ def _calc_adjoint_states(self, x_s, y_s, yaw_s, v_s, u_1s, u_2s, N, dt): # backward adjoint state calc for i in range(N - 1, 0, -1): temp_lam_1, temp_lam_2, temp_lam_3, temp_lam_4 = self._adjoint_state_with_oylar( - x_s[i], y_s[i], yaw_s[i], v_s[i], lam_1s[0], lam_2s[0], lam_3s[0], lam_4s[0], - u_1s[i], u_2s[i], dt) + yaw_s[i], v_s[i], lam_1s[0], lam_2s[0], lam_3s[0], lam_4s[0], + u_2s[i], dt) lam_1s.insert(0, temp_lam_1) lam_2s.insert(0, temp_lam_2) lam_3s.insert(0, temp_lam_3) @@ -106,7 +107,8 @@ def _calc_adjoint_states(self, x_s, y_s, yaw_s, v_s, u_1s, u_2s, N, dt): return lam_1s, lam_2s, lam_3s, lam_4s - def _predict_state_with_oylar(self, x, y, yaw, v, u_1, u_2, dt): + @staticmethod + def _predict_state_with_oylar(x, y, yaw, v, u_1, u_2, dt): dx, dy, dyaw, dv = differential_model( v, yaw, u_1, u_2) @@ -118,21 +120,21 @@ def _predict_state_with_oylar(self, x, y, yaw, v, u_1, u_2, dt): return next_x_1, next_x_2, next_x_3, next_x_4 - def _adjoint_state_with_oylar(self, x, y, yaw, v, lam_1, lam_2, lam_3, lam_4, u_1, u_2, dt): + @staticmethod + def _adjoint_state_with_oylar(yaw, v, lam_1, lam_2, lam_3, lam_4, u_2, dt): # ∂H/∂x pre_lam_1 = lam_1 + dt * 0.0 pre_lam_2 = lam_2 + dt * 0.0 - pre_lam_3 = lam_3 + dt * \ - (- lam_1 * math.sin(yaw) * v + lam_2 * math.cos(yaw) * v) - pre_lam_4 = lam_4 + dt * \ - (lam_1 * math.cos(yaw) + lam_2 * - math.sin(yaw) + lam_3 * math.sin(u_2) / WB) + tmp1 = - lam_1 * sin(yaw) * v + lam_2 * cos(yaw) * v + pre_lam_3 = lam_3 + dt * tmp1 + tmp2 = lam_1 * cos(yaw) + lam_2 * sin(yaw) + lam_3 * sin(u_2) / WB + pre_lam_4 = lam_4 + dt * tmp2 return pre_lam_1, pre_lam_2, pre_lam_3, pre_lam_4 -class NMPCController_with_CGMRES(): +class NMPCControllerCGMRES: """ Attributes ------------ @@ -145,7 +147,7 @@ class NMPCController_with_CGMRES(): alpha : float gain of predict time N : int - predicte step, discritize value + predict step, discrete value threshold : float cgmres's threshold value input_num : int @@ -226,20 +228,22 @@ def calc_input(self, x, y, yaw, v, time): dx_4 = x_4_dot * self.ht x_s, y_s, yaw_s, v_s, lam_1s, lam_2s, lam_3s, lam_4s = self.simulator.calc_predict_and_adjoint_state( - x + dx_1, y + dx_2, yaw + dx_3, v + dx_4, self.u_1s, self.u_2s, self.N, dt) + x + dx_1, y + dx_2, yaw + dx_3, v + dx_4, self.u_1s, self.u_2s, + self.N, dt) # Fxt:F(U,x+hx˙,t+h) - Fxt = self._calc_f(x_s, y_s, yaw_s, v_s, lam_1s, lam_2s, lam_3s, lam_4s, - self.u_1s, self.u_2s, self.dummy_u_1s, self.dummy_u_2s, - self.raw_1s, self.raw_2s, self.N, dt) + Fxt = self._calc_f(v_s, lam_3s, lam_4s, + self.u_1s, self.u_2s, self.dummy_u_1s, + self.dummy_u_2s, + self.raw_1s, self.raw_2s, self.N) # F:F(U,x,t) x_s, y_s, yaw_s, v_s, lam_1s, lam_2s, lam_3s, lam_4s = self.simulator.calc_predict_and_adjoint_state( x, y, yaw, v, self.u_1s, self.u_2s, self.N, dt) - F = self._calc_f(x_s, y_s, yaw_s, v_s, lam_1s, lam_2s, lam_3s, lam_4s, + F = self._calc_f(v_s, lam_3s, lam_4s, self.u_1s, self.u_2s, self.dummy_u_1s, self.dummy_u_2s, - self.raw_1s, self.raw_2s, self.N, dt) + self.raw_1s, self.raw_2s, self.N) right = -self.zeta * F - ((Fxt - F) / self.ht) @@ -251,17 +255,19 @@ def calc_input(self, x, y, yaw, v, time): draw_2 = self.raw_2s * self.ht x_s, y_s, yaw_s, v_s, lam_1s, lam_2s, lam_3s, lam_4s = self.simulator.calc_predict_and_adjoint_state( - x + dx_1, y + dx_2, yaw + dx_3, v + dx_4, self.u_1s + du_1, self.u_2s + du_2, self.N, dt) + x + dx_1, y + dx_2, yaw + dx_3, v + dx_4, self.u_1s + du_1, + self.u_2s + du_2, self.N, dt) # Fuxt:F(U+hdU(0),x+hx˙,t+h) - Fuxt = self._calc_f(x_s, y_s, yaw_s, v_s, lam_1s, lam_2s, lam_3s, lam_4s, + Fuxt = self._calc_f(v_s, lam_3s, lam_4s, self.u_1s + du_1, self.u_2s + du_2, - self.dummy_u_1s + ddummy_u_1, self.dummy_u_2s + ddummy_u_2, - self.raw_1s + draw_1, self.raw_2s + draw_2, self.N, dt) + self.dummy_u_1s + ddummy_u_1, + self.dummy_u_2s + ddummy_u_2, + self.raw_1s + draw_1, self.raw_2s + draw_2, self.N) left = ((Fuxt - Fxt) / self.ht) - # calculationg cgmres + # calculating cgmres r0 = right - left r0_norm = np.linalg.norm(r0) @@ -276,6 +282,9 @@ def calc_input(self, x, y, yaw, v, time): ys_pre = None + du_1_new, du_2_new, draw_1_new, draw_2_new = None, None, None, None + ddummy_u_1_new, ddummy_u_2_new = None, None + for i in range(self.max_iteration): du_1 = vs[::self.input_num, i] * self.ht du_2 = vs[1::self.input_num, i] * self.ht @@ -285,12 +294,15 @@ def calc_input(self, x, y, yaw, v, time): draw_2 = vs[5::self.input_num, i] * self.ht x_s, y_s, yaw_s, v_s, lam_1s, lam_2s, lam_3s, lam_4s = self.simulator.calc_predict_and_adjoint_state( - x + dx_1, y + dx_2, yaw + dx_3, v + dx_4, self.u_1s + du_1, self.u_2s + du_2, self.N, dt) + x + dx_1, y + dx_2, yaw + dx_3, v + dx_4, self.u_1s + du_1, + self.u_2s + du_2, self.N, dt) - Fuxt = self._calc_f(x_s, y_s, yaw_s, v_s, lam_1s, lam_2s, lam_3s, lam_4s, + Fuxt = self._calc_f(v_s, lam_3s, lam_4s, self.u_1s + du_1, self.u_2s + du_2, - self.dummy_u_1s + ddummy_u_1, self.dummy_u_2s + ddummy_u_2, - self.raw_1s + draw_1, self.raw_2s + draw_2, self.N, dt) + self.dummy_u_1s + ddummy_u_1, + self.dummy_u_2s + ddummy_u_2, + self.raw_1s + draw_1, self.raw_2s + draw_2, + self.N) Av = ((Fuxt - Fxt) / self.ht) @@ -312,14 +324,17 @@ def calc_input(self, x, y, yaw, v, time): judge_value = r0_norm * e[:i + 1] - np.dot(hs[:i + 1, :i], ys[:i]) - if np.linalg.norm(judge_value) < self.threshold or i == self.max_iteration - 1: - update_value = np.dot(vs[:, :i - 1], ys_pre[:i - 1]).flatten() - du_1_new = du_1 + update_value[::self.input_num] - du_2_new = du_2 + update_value[1::self.input_num] - ddummy_u_1_new = ddummy_u_1 + update_value[2::self.input_num] - ddummy_u_2_new = ddummy_u_2 + update_value[3::self.input_num] - draw_1_new = draw_1 + update_value[4::self.input_num] - draw_2_new = draw_2 + update_value[5::self.input_num] + flag1 = np.linalg.norm(judge_value) < self.threshold + + flag2 = i == self.max_iteration - 1 + if flag1 or flag2: + update_val = np.dot(vs[:, :i - 1], ys_pre[:i - 1]).flatten() + du_1_new = du_1 + update_val[::self.input_num] + du_2_new = du_2 + update_val[1::self.input_num] + ddummy_u_1_new = ddummy_u_1 + update_val[2::self.input_num] + ddummy_u_2_new = ddummy_u_2 + update_val[3::self.input_num] + draw_1_new = draw_1 + update_val[4::self.input_num] + draw_2_new = draw_2 + update_val[5::self.input_num] break ys_pre = ys @@ -335,9 +350,9 @@ def calc_input(self, x, y, yaw, v, time): x_s, y_s, yaw_s, v_s, lam_1s, lam_2s, lam_3s, lam_4s = self.simulator.calc_predict_and_adjoint_state( x, y, yaw, v, self.u_1s, self.u_2s, self.N, dt) - F = self._calc_f(x_s, y_s, yaw_s, v_s, lam_1s, lam_2s, lam_3s, lam_4s, + F = self._calc_f(v_s, lam_3s, lam_4s, self.u_1s, self.u_2s, self.dummy_u_1s, self.dummy_u_2s, - self.raw_1s, self.raw_2s, self.N, dt) + self.raw_1s, self.raw_2s, self.N) print("norm(F) = {0}".format(np.linalg.norm(F))) @@ -352,36 +367,38 @@ def calc_input(self, x, y, yaw, v, time): return self.u_1s, self.u_2s - def _calc_f(self, x_s, y_s, yaw_s, v_s, lam_1s, lam_2s, lam_3s, lam_4s, - u_1s, u_2s, dummy_u_1s, dummy_u_2s, raw_1s, raw_2s, N, dt): + @staticmethod + def _calc_f(v_s, lam_3s, lam_4s, u_1s, u_2s, dummy_u_1s, dummy_u_2s, + raw_1s, raw_2s, N): F = [] for i in range(N): # ∂H/∂u(xi, ui, λi) F.append(u_1s[i] + lam_4s[i] + 2.0 * raw_1s[i] * u_1s[i]) F.append(u_2s[i] + lam_3s[i] * v_s[i] / - WB * math.cos(u_2s[i])**2 + 2.0 * raw_2s[i] * u_2s[i]) + WB * cos(u_2s[i]) ** 2 + 2.0 * raw_2s[i] * u_2s[i]) F.append(-PHI_V + 2.0 * raw_1s[i] * dummy_u_1s[i]) F.append(-PHI_OMEGA + 2.0 * raw_2s[i] * dummy_u_2s[i]) # C(xi, ui, λi) - F.append(u_1s[i]**2 + dummy_u_1s[i]**2 - U_A_MAX**2) - F.append(u_2s[i]**2 + dummy_u_2s[i]**2 - U_OMEGA_MAX**2) + F.append(u_1s[i] ** 2 + dummy_u_1s[i] ** 2 - U_A_MAX ** 2) + F.append(u_2s[i] ** 2 + dummy_u_2s[i] ** 2 - U_OMEGA_MAX ** 2) return np.array(F) -def plot_figures(plant_system, controller, iteration_num, dt): # pragma: no cover +def plot_figures(plant_system, controller, iteration_num, + dt): # pragma: no cover # figure # time history fig_p = plt.figure() fig_u = plt.figure() fig_f = plt.figure() - # traj + # trajectory fig_t = plt.figure() - fig_traj = fig_t.add_subplot(111) - fig_traj.set_aspect('equal') + fig_trajectory = fig_t.add_subplot(111) + fig_trajectory.set_aspect('equal') x_1_fig = fig_p.add_subplot(411) x_2_fig = fig_p.add_subplot(412) @@ -443,11 +460,11 @@ def plot_figures(plant_system, controller, iteration_num, dt): # pragma: no cov f_fig.set_xlabel("time [s]") f_fig.set_ylabel("optimal error") - fig_traj.plot(plant_system.history_x, - plant_system.history_y, "-r") - fig_traj.set_xlabel("x [m]") - fig_traj.set_ylabel("y [m]") - fig_traj.axis("equal") + fig_trajectory.plot(plant_system.history_x, + plant_system.history_y, "-r") + fig_trajectory.set_xlabel("x [m]") + fig_trajectory.set_ylabel("y [m]") + fig_trajectory.axis("equal") # start state plot_car(plant_system.history_x[0], @@ -462,23 +479,25 @@ def plot_figures(plant_system, controller, iteration_num, dt): # pragma: no cov plt.show() -def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): # pragma: no cover +def plot_car(x, y, yaw, steer=0.0, truck_color="-k"): # pragma: no cover # Vehicle parameters LENGTH = 0.4 # [m] WIDTH = 0.2 # [m] - BACKTOWHEEL = 0.1 # [m] + BACK_TO_WHEEL = 0.1 # [m] WHEEL_LEN = 0.03 # [m] WHEEL_WIDTH = 0.02 # [m] TREAD = 0.07 # [m] - outline = np.array([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), - -BACKTOWHEEL, -BACKTOWHEEL], - [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]]) + outline = np.array( + [[-BACK_TO_WHEEL, (LENGTH - BACK_TO_WHEEL), (LENGTH - BACK_TO_WHEEL), + -BACK_TO_WHEEL, -BACK_TO_WHEEL], + [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]]) - fr_wheel = np.array([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN], - [-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - - TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]]) + fr_wheel = np.array( + [[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN], + [-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - + TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]]) rr_wheel = np.copy(fr_wheel) @@ -487,10 +506,10 @@ def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): # pragma: n rl_wheel = np.copy(rr_wheel) rl_wheel[1, :] *= -1 - Rot1 = np.array([[math.cos(yaw), math.sin(yaw)], - [-math.sin(yaw), math.cos(yaw)]]) - Rot2 = np.array([[math.cos(steer), math.sin(steer)], - [-math.sin(steer), math.cos(steer)]]) + Rot1 = np.array([[cos(yaw), sin(yaw)], + [-sin(yaw), cos(yaw)]]) + Rot2 = np.array([[cos(steer), sin(steer)], + [-sin(steer), cos(steer)]]) fr_wheel = (fr_wheel.T.dot(Rot2)).T fl_wheel = (fl_wheel.T.dot(Rot2)).T @@ -516,20 +535,19 @@ def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): # pragma: n rl_wheel[1, :] += y plt.plot(np.array(outline[0, :]).flatten(), - np.array(outline[1, :]).flatten(), truckcolor) + np.array(outline[1, :]).flatten(), truck_color) plt.plot(np.array(fr_wheel[0, :]).flatten(), - np.array(fr_wheel[1, :]).flatten(), truckcolor) + np.array(fr_wheel[1, :]).flatten(), truck_color) plt.plot(np.array(rr_wheel[0, :]).flatten(), - np.array(rr_wheel[1, :]).flatten(), truckcolor) + np.array(rr_wheel[1, :]).flatten(), truck_color) plt.plot(np.array(fl_wheel[0, :]).flatten(), - np.array(fl_wheel[1, :]).flatten(), truckcolor) + np.array(fl_wheel[1, :]).flatten(), truck_color) plt.plot(np.array(rl_wheel[0, :]).flatten(), - np.array(rl_wheel[1, :]).flatten(), truckcolor) + np.array(rl_wheel[1, :]).flatten(), truck_color) plt.plot(x, y, "*") def animation(plant, controller, dt): - skip = 2 # skip index for animation for t in range(1, len(controller.history_u_1), skip): @@ -543,12 +561,13 @@ def animation(plant, controller, dt): if abs(v) <= 0.01: steer = 0.0 else: - steer = math.atan2(controller.history_u_2[t] * WB / v, 1.0) + steer = 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.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") @@ -568,7 +587,7 @@ def main(): init_x = -4.5 init_y = -2.5 - init_yaw = math.radians(45.0) + init_yaw = radians(45.0) init_v = -1.0 # plant @@ -576,14 +595,15 @@ def main(): init_x, init_y, init_yaw, init_v) # controller - controller = NMPCController_with_CGMRES() + controller = NMPCControllerCGMRES() iteration_num = int(iteration_time / dt) for i in range(1, iteration_num): time = float(i) * dt # make input u_1s, u_2s = controller.calc_input( - plant_system.x, plant_system.y, plant_system.yaw, plant_system.v, time) + plant_system.x, plant_system.y, plant_system.yaw, plant_system.v, + time) # update state plant_system.update_state(u_1s[0], u_2s[0]) diff --git a/tests/test_grid_based_sweep_coverage_path_planner.py b/tests/test_grid_based_sweep_coverage_path_planner.py index fa2d0a7a64e..12ef98df39b 100644 --- a/tests/test_grid_based_sweep_coverage_path_planner.py +++ b/tests/test_grid_based_sweep_coverage_path_planner.py @@ -2,8 +2,10 @@ import sys from unittest import TestCase -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../PathPlanning/GridBasedSweepCPP") -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../Mapping/grid_map_lib") +sys.path.append(os.path.dirname( + os.path.abspath(__file__)) + "/../PathPlanning/GridBasedSweepCPP") +sys.path.append( + os.path.dirname(os.path.abspath(__file__)) + "/../Mapping/grid_map_lib") try: import grid_based_sweep_coverage_path_planner except ImportError: @@ -14,88 +16,109 @@ class TestPlanning(TestCase): + RIGHT = grid_based_sweep_coverage_path_planner.\ + SweepSearcher.MovingDirection.RIGHT + LEFT = grid_based_sweep_coverage_path_planner. \ + SweepSearcher.MovingDirection.LEFT + UP = grid_based_sweep_coverage_path_planner. \ + SweepSearcher.SweepDirection.UP + DOWN = grid_based_sweep_coverage_path_planner. \ + SweepSearcher.SweepDirection.DOWN + def test_planning1(self): ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0, 0.0] oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0, 0.0] - reso = 5.0 - - px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso, - moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT, - sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN, - ) - self.assertTrue(len(px) >= 5) - - px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso, - moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.LEFT, - sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN, - ) - self.assertTrue(len(px) >= 5) - - px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso, - moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT, - sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.UP, - ) - self.assertTrue(len(px) >= 5) - - px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso, - moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT, - sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN, - ) - self.assertTrue(len(px) >= 5) + resolution = 5.0 + + px, py = grid_based_sweep_coverage_path_planner.planning( + ox, oy, resolution, + moving_direction=self.RIGHT, + sweeping_direction=self.DOWN, + ) + self.assertGreater(len(px), 5) + + px, py = grid_based_sweep_coverage_path_planner.planning( + ox, oy, resolution, + moving_direction=self.LEFT, + sweeping_direction=self.DOWN, + ) + self.assertGreater(len(px), 5) + + px, py = grid_based_sweep_coverage_path_planner.planning( + ox, oy, resolution, + moving_direction=self.RIGHT, + sweeping_direction=self.UP, + ) + self.assertGreater(len(px), 5) + + px, py = grid_based_sweep_coverage_path_planner.planning( + ox, oy, resolution, + moving_direction=self.RIGHT, + sweeping_direction=self.UP, + ) + self.assertGreater(len(px), 5) def test_planning2(self): ox = [0.0, 50.0, 50.0, 0.0, 0.0] oy = [0.0, 0.0, 30.0, 30.0, 0.0] - reso = 1.3 - - px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso, - moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT, - sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN, - ) - self.assertTrue(len(px) >= 5) - - px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso, - moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.LEFT, - sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN, - ) - self.assertTrue(len(px) >= 5) - - px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso, - moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT, - sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.UP, - ) - self.assertTrue(len(px) >= 5) - - px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso, - moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT, - sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN, - ) - self.assertTrue(len(px) >= 5) + resolution = 1.3 + + px, py = grid_based_sweep_coverage_path_planner.planning( + ox, oy, resolution, + moving_direction=self.RIGHT, + sweeping_direction=self.DOWN, + ) + self.assertGreater(len(px), 5) + + px, py = grid_based_sweep_coverage_path_planner.planning( + ox, oy, resolution, + moving_direction=self.LEFT, + sweeping_direction=self.DOWN, + ) + self.assertGreater(len(px), 5) + + px, py = grid_based_sweep_coverage_path_planner.planning( + ox, oy, resolution, + moving_direction=self.RIGHT, + sweeping_direction=self.UP, + ) + self.assertGreater(len(px), 5) + + px, py = grid_based_sweep_coverage_path_planner.planning( + ox, oy, resolution, + moving_direction=self.RIGHT, + sweeping_direction=self.DOWN, + ) + self.assertGreater(len(px), 5) def test_planning3(self): ox = [0.0, 20.0, 50.0, 200.0, 130.0, 40.0, 0.0] oy = [0.0, -80.0, 0.0, 30.0, 60.0, 80.0, 0.0] - reso = 5.1 - px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso, - moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT, - sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN, - ) - self.assertTrue(len(px) >= 5) - - px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso, - moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.LEFT, - sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN, - ) - self.assertTrue(len(px) >= 5) - - px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso, - moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT, - sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.UP, - ) - self.assertTrue(len(px) >= 5) - - px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso, - moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT, - sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN, - ) - self.assertTrue(len(px) >= 5) + resolution = 5.1 + px, py = grid_based_sweep_coverage_path_planner.planning( + ox, oy, resolution, + moving_direction=self.RIGHT, + sweeping_direction=self.DOWN, + ) + self.assertGreater(len(px), 5) + + px, py = grid_based_sweep_coverage_path_planner.planning( + ox, oy, resolution, + moving_direction=self.LEFT, + sweeping_direction=self.DOWN, + ) + self.assertGreater(len(px), 5) + + px, py = grid_based_sweep_coverage_path_planner.planning( + ox, oy, resolution, + moving_direction=self.RIGHT, + sweeping_direction=self.UP, + ) + self.assertGreater(len(px), 5) + + px, py = grid_based_sweep_coverage_path_planner.planning( + ox, oy, resolution, + moving_direction=self.RIGHT, + sweeping_direction=self.DOWN, + ) + self.assertGreater(len(px), 5)