From 91cf551ec528b063f5422c3522b5be13b6bd7476 Mon Sep 17 00:00:00 2001
From: rachelmoan <moanrachel516@gmail.com>
Date: Mon, 19 Aug 2024 17:24:41 -0500
Subject: [PATCH] Remove old plotting code
---
guided_mrmp/utils/figures.py | 284 -----------------------------------
1 file changed, 284 deletions(-)
delete mode 100644 guided_mrmp/utils/figures.py
diff --git a/guided_mrmp/utils/figures.py b/guided_mrmp/utils/figures.py
deleted file mode 100644
index a9579c9..0000000
--- a/guided_mrmp/utils/figures.py
+++ /dev/null
@@ -1,284 +0,0 @@
-import numpy as np
-import matplotlib.pyplot as plt
-import matplotlib.patches as patches
-
-from guided_mrmp.utils import Node,Env
-
-class Plotting:
-
- def __init__(self, env):
- # self.start = Node(start, start, 0, 0)
- # self.goal = Node(goal, goal, 0, 0)
- self.env = env
- self.fig = plt.figure("planning")
- self.ax = plt.gca()
-
- def show(self):
- plt.show()
-
- def animation(self, paths, name, cost=None, expand=None, history_pose=None, predict_path=None,
- lookahead_pts=None, cost_curve=None):
- name = name + "\ncost: " + str(cost) if cost else name
- # self.plot_env(name)
- # if expand:
- # self.plot_expand(expand)
- # if history_pose:
- # self.plot_history_pose(history_pose, predict_path, lookahead_pts)
-
- if len(paths) > 20:
- colors = plt.cm.hsv(np.linspace(0.2, 1.0, len(paths)))
- elif len(paths) > 10:
- colors = plt.cm.tab20(np.linspace(0, 1, len(paths)))
- else:
- colors = plt.cm.tab10(np.linspace(0, 1, len(paths)))
- for path,color in zip(paths, colors):
- self.plot_path(path, path_color=color)
-
- if cost_curve:
- plt.figure("cost curve")
- self.plot_cost_curve(cost_curve, name)
-
- # self.ax.axes.get_xaxis().set_ticks([])
- # self.ax.axes.get_yaxis().set_ticks([])
- # self.ax.set_aspect('equal', adjustable='box')
- # plt.tick_params(
- # axis='both',
- # which='both',
- # bottom=False, # ticks along the bottom edge are off
- # top=False, # ticks along the top edge are off
- # left=False,
- # right=False,
- # labelbottom=False) # labels along the bottom edge are off
- # plt.show()
-
- def plot_env(self, name):
- '''
- Plot environment with static obstacles.
-
- Parameters
- ----------
- name: Algorithm name or some other information
- '''
- # plt.plot(self.start.x, self.start.y, marker="s", color="#ff0000")
- # plt.plot(self.goal.x, self.goal.y, marker="s", color="#1155cc")
-
-
-
- if isinstance(self.env, Env):
- ax = self.fig.add_subplot()
- # boundary
- # for (ox, oy, w, h) in self.env.obs_boundary:
- # ax.add_patch(patches.Rectangle(
- # (ox, oy), w, h,
- # edgecolor='black',
- # facecolor='black',
- # fill=False
- # )
- # )
- # rectangle obstacles
- for (ox, oy, w, h) in self.env.obs_rectangle:
- ax.add_patch(patches.Rectangle(
- (ox, oy), w, h,
- edgecolor='black',
- facecolor='gray',
- fill=True
- )
- )
- # circle obstacles
- for (ox, oy, r) in self.env.obs_circle:
- ax.add_patch(patches.Circle(
- (ox, oy), r,
- edgecolor='black',
- facecolor='gray',
- fill=True
- )
- )
-
- ax.axes.get_xaxis().set_ticks([])
- ax.axes.get_yaxis().set_ticks([])
-
- # plt.title(name)
- # plt.axis("equal")
-
- def plot_env_with_grid(self, top_left, grid_size, cell_size):
-
- # draw the grid
- for i in range(grid_size+1):
- x1, y1 = [top_left[0], top_left[1] - i*cell_size], [top_left[0] + grid_size*cell_size, top_left[1]- i*cell_size]
- x2, y2 = [top_left[0] + i*cell_size, top_left[1]], [top_left[0] + i*cell_size, top_left[1] - grid_size*cell_size]
- plt.plot(x1, y1, x2, y2)
-
-
- def plot_expand(self, expand):
- '''
- Plot expanded grids using in graph searching.
-
- Parameters
- ----------
- expand: Expanded grids during searching
- '''
- # if self.start in expand:
- # expand.remove(self.start)
- # if self.goal in expand:
- # expand.remove(self.goal)
-
- count = 0
-
-
- for x in expand:
-
- count += 1
- if x.parent:
- plt.plot([x.parent[0], x.x], [x.parent[1], x.y],
- color="#dddddd", linestyle="-")
- plt.gcf().canvas.mpl_connect('key_release_event',
- lambda event:
- [exit(0) if event.key == 'escape' else None])
- if count % 10 == 0:
- plt.pause(0.001)
-
- plt.pause(0.01)
-
- def plot_path(self,path, path_color="#13ae00", path_style="-"):
- '''
- Plot path in global planning.
-
- Parameters
- ----------
- path: Path found in global planning
- '''
- print("path = ",path)
- path_x = [path[i][0] for i in range(len(path))]
- path_y = [path[i][1] for i in range(len(path))]
- plt.plot(path_x, path_y, path_style, linewidth='2', color=path_color)
- # plt.plot(start.x, start.y, marker="s", color="#ff0000")
- # plt.plot(goal.x, goal.y, marker="s", color="#1155cc")
-
- def plotAgent(self, pose, radius):
- '''
- Plot agent with specifical pose.
-
- Parameters
- ----------
- pose: Pose of agent
- radius: Radius of agent
- '''
- x, y, theta = pose
- ref_vec = np.array([[radius / 2], [0]])
- rot_mat = np.array([[np.cos(theta), -np.sin(theta)],
- [np.sin(theta), np.cos(theta)]])
- end_pt = rot_mat @ ref_vec + np.array([[x], [y]])
-
- try:
- self.ax.artists.pop()
- for art in self.ax.get_children():
- if isinstance(art, plt.patches.FancyArrow):
- art.remove()
- except:
- pass
-
- self.ax.arrow(x, y, float(end_pt[0]) - x, float(end_pt[1]) - y,
- width=0.1, head_width=0.40, color="r")
- circle = plt.Circle((x, y), radius, color="r", fill=False)
- self.ax.add_artist(circle)
-
- def plot_history_pose(self, history_pose, predict_path=None, lookahead_pts=None):
- lookahead_handler = None
- for i, pose in enumerate(history_pose):
- if i < len(history_pose) - 1:
- plt.plot([history_pose[i][0], history_pose[i + 1][0]],
- [history_pose[i][1], history_pose[i + 1][1]], c="#13ae00")
- if predict_path is not None:
- plt.plot(predict_path[i][:, 0], predict_path[i][:, 1], c="#ddd")
- i += 1
-
- # agent
- self.plotAgent(pose)
-
- # lookahead
- if lookahead_handler is not None:
- lookahead_handler.remove()
- if lookahead_pts is not None:
- try:
- lookahead_handler = self.ax.scatter(lookahead_pts[i][0], lookahead_pts[i][1], c="b")
- except:
- lookahead_handler = self.ax.scatter(lookahead_pts[-1][0], lookahead_pts[-1][1], c="b")
-
- plt.gcf().canvas.mpl_connect('key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
- if i % 5 == 0: plt.pause(0.03)
-
- def plot_cost_curve(self, cost_list, name):
- '''
- Plot cost curve with epochs using in evolutionary searching.
-
- Parameters
- ----------
- cost_list: Cost with epochs
- name: Algorithm name or some other information
- '''
- plt.plot(cost_list, color="b")
- plt.xlabel("epochs")
- plt.ylabel("cost value")
- plt.title(name)
- plt.grid()
-
- def connect(self, name, func):
- self.fig.canvas.mpl_connect(name, func)
-
- def clean(self):
- plt.cla()
-
- def update(self):
- self.fig.canvas.draw_idle()
-
- @staticmethod
- def plotArrow(x, y, theta, length, color):
- angle = np.deg2rad(30)
- d = 0.5 * length
- w = 2
-
- x_start, y_start = x, y
- x_end = x + length * np.cos(theta)
- y_end = y + length * np.sin(theta)
-
- theta_hat_L = theta + np.pi - angle
- theta_hat_R = theta + np.pi + angle
-
- x_hat_start = x_end
- x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L)
- x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R)
-
- y_hat_start = y_end
- y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L)
- y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R)
-
- plt.plot([x_start, x_end], [y_start, y_end], color=color, linewidth=w)
- plt.plot([x_hat_start, x_hat_end_L], [y_hat_start, y_hat_end_L], color=color, linewidth=w)
- plt.plot([x_hat_start, x_hat_end_R], [y_hat_start, y_hat_end_R], color=color, linewidth=w)
-
- @staticmethod
- def plotCar(x, y, theta, width, length, color):
- theta_B = np.pi + theta
-
- xB = x + length / 4 * np.cos(theta_B)
- yB = y + length / 4 * np.sin(theta_B)
-
- theta_BL = theta_B + np.pi / 2
- theta_BR = theta_B - np.pi / 2
-
- x_BL = xB + width / 2 * np.cos(theta_BL) # Bottom-Left vertex
- y_BL = yB + width / 2 * np.sin(theta_BL)
- x_BR = xB + width / 2 * np.cos(theta_BR) # Bottom-Right vertex
- y_BR = yB + width / 2 * np.sin(theta_BR)
-
- x_FL = x_BL + length * np.cos(theta) # Front-Left vertex
- y_FL = y_BL + length * np.sin(theta)
- x_FR = x_BR + length * np.cos(theta) # Front-Right vertex
- y_FR = y_BR + length * np.sin(theta)
-
- plt.plot([x_BL, x_BR, x_FR, x_FL, x_BL],
- [y_BL, y_BR, y_FR, y_FL, y_BL],
- linewidth=1, color=color)
-
- Plotting.plotArrow(x, y, theta, length / 2, color)
--
GitLab