diff --git a/guided_mrmp/controllers/mpc.py b/guided_mrmp/controllers/mpc.py
index 30bb054dd56e689f42e7aa56740b0af7520fd24e..ba80d4d611330d865d4ea53863bfea37236d105a 100644
--- a/guided_mrmp/controllers/mpc.py
+++ b/guided_mrmp/controllers/mpc.py
@@ -49,8 +49,7 @@ class MPC:
         # weight for error in control
         self.R = np.diag(input_cost)
         self.P = np.diag(input_rate_cost)
-
-    
+ 
     def get_linear_model_matrices_roomba(self,x_bar,u_bar):
         """
         Computes the approximated LTI state space model x' = Ax + Bu + C
@@ -98,7 +97,6 @@ class MPC:
 
         return A_lin, B_lin, C_lin
     
-
     def get_linear_model_matrices(self, x_bar, u_bar):
         """
         Computes the approximated LTI state space model x' = Ax + Bu + C
@@ -221,7 +219,6 @@ class MPC:
         solution = prob.solve(solver=opt.OSQP, warm_start=True, verbose=False)
         return x, u
     
-
 if __name__ == "__main__":
     # Example usage:
     dt = 0.1
diff --git a/guided_mrmp/planners/singlerobot/RRT.py b/guided_mrmp/planners/singlerobot/RRT.py
index 69d5244f50c62d90a3f73dfb75ecc61ea5f7e519..507aeb9e838471baef6364a88994f062f4b0273f 100644
--- a/guided_mrmp/planners/singlerobot/RRT.py
+++ b/guided_mrmp/planners/singlerobot/RRT.py
@@ -4,7 +4,7 @@ RRT implementation
 import math
 import numpy as np
 
-from guided_mrmp.utils import Node, Env, Plotting
+from guided_mrmp.utils import Node, Env
 
 class RRT:
     def __init__(self, env, s_start, s_goal, step_len, goal_sample_rate, iter_max):
@@ -16,7 +16,7 @@ class RRT:
         self.sampled_vertices = [self.s_start]
 
         self.env = env
-        self.plotting = Plotting(self.env)
+        # self.plotting = Plotting(self.env)
         # self.utils = utils.Utils()
 
         self.x_range = self.env.boundary[0]
diff --git a/guided_mrmp/planners/singlerobot/RRTStar.py b/guided_mrmp/planners/singlerobot/RRTStar.py
index e56b2339c0b0f52aedf8277b7a65b079ad193ac0..8f1a0da5250c88a452fb6d7d85e910582a82c0d3 100644
--- a/guided_mrmp/planners/singlerobot/RRTStar.py
+++ b/guided_mrmp/planners/singlerobot/RRTStar.py
@@ -1,8 +1,6 @@
 """
 RRT*
 """
-from guided_mrmp.utils import Node, Env, Plotting
-
 from guided_mrmp.planners.singlerobot.RRT import RRT
 
 class RRTStar(RRT):
diff --git a/guided_mrmp/utils/__init__.py b/guided_mrmp/utils/__init__.py
index 3bf91119df5483393e0f930659f19cb2f201c010..d14f7f371678e18155e057e7ec655f0b9a275ca9 100644
--- a/guided_mrmp/utils/__init__.py
+++ b/guided_mrmp/utils/__init__.py
@@ -1,8 +1,6 @@
 from .conflict import Conflict
 from .control import Roomba
 from .environment import Node, Env
-from .figures import Plotting
 from .library import Library
 from .robot import Robot
-from .template_matcher import TemplateMatcher
 from .helpers import *
\ No newline at end of file
diff --git a/guided_mrmp/utils/environment.py b/guided_mrmp/utils/environment.py
index 7efec0bfc3e3b4f88af73a9a15f22bc7248b5329..4b850d544ec730d6f1d9263d36aa025a2ade061b 100644
--- a/guided_mrmp/utils/environment.py
+++ b/guided_mrmp/utils/environment.py
@@ -61,10 +61,6 @@ class Env:
         self.circle_obs = circle_obs
         self.rect_obs = rectangle_obs
 
-    def update_obs(self, obs_cir, obs_bound, obs_rec):
-        self.circle_obs = obs_cir
-        self.obs_boundary = obs_bound
-        self.rect_obs = obs_rec
 
     def get_obs_vertex(self):
         delta = self.delta
diff --git a/guided_mrmp/utils/robot.py b/guided_mrmp/utils/robot.py
index f5fd0f3e2a8198c975e9dae72842b2c8ad1154c9..2f73fe3f20c915a36ed083abffc1631b9e337beb 100644
--- a/guided_mrmp/utils/robot.py
+++ b/guided_mrmp/utils/robot.py
@@ -1,7 +1,3 @@
-import numpy as np
-
-
-
 class Robot:
     def __init__(self, label, color, radius, start, goal, target_v, T, DT, rrtpath,waypoints):
         self.label = label
diff --git a/guided_mrmp/utils/template_matcher.py b/guided_mrmp/utils/template_matcher.py
deleted file mode 100644
index 3feae0200c0842092b02e3bbaf43921437a76a05..0000000000000000000000000000000000000000
--- a/guided_mrmp/utils/template_matcher.py
+++ /dev/null
@@ -1,15 +0,0 @@
-"""
-Match a local nxm regiaon to its corresponding database tmeplate
-"""
-
-class TemplateMatcher:
-    def __init__(self,type, starts, goals):
-        """
-        inputs:
-            - type (string): the type of subproblem
-        """
-        self.type = type
-        self.starts = starts
-        self.goals = goals 
-
-