diff --git a/guided_mrmp/conflict_resolvers/traj_opt_resolver.py b/guided_mrmp/conflict_resolvers/traj_opt_resolver.py
index 92e3a725491b153f00e0965a148353a6a2d595ca..aab7dac795fa891bd9d1f32662bc2a301c4f4e8d 100644
--- a/guided_mrmp/conflict_resolvers/traj_opt_resolver.py
+++ b/guided_mrmp/conflict_resolvers/traj_opt_resolver.py
@@ -1,6 +1,7 @@
 import numpy as np
 import matplotlib.pyplot as plt
 from matplotlib.patches import Circle, Rectangle
+from guided_mrmp.optimizer import Optimizer
 from casadi import *
 
 class TrajOptResolver():
@@ -148,37 +149,8 @@ class TrajOptResolver():
         return {'opti':opti, 'X':X, 'T':T}
 
     def solve_optimization_problem(self, problem, initial_guesses=None, solver_options=None):
-        opti = problem['opti']
-        
-        if initial_guesses:
-            for param, value in initial_guesses.items():
-                print(f"param = {param}")
-                print(f"value = {value}")
-                opti.set_initial(problem[param], value)
-
-        # Set numerical backend, with options if provided
-        if solver_options:
-            opti.solver('ipopt', solver_options)
-        else:
-            opti.solver('ipopt')
-
-        try:
-            sol = opti.solve()   # actual solve
-            status = 'succeeded'
-        except:
-            sol = None
-            status = 'failed'
-
-        results = {
-            'status' : status,
-            'solution' : sol,
-        }
-
-        if sol:
-            for var_name, var in problem.items():
-                if var_name != 'opti':
-                    results[var_name] = sol.value(var)
-
+        opt = Optimizer(problem)
+        results = opt.solve_optimization_problem(initial_guesses, solver_options)
         return results
     
     def solve(self, N, x_range, y_range, initial_guesses):
diff --git a/guided_mrmp/controllers/mpc.py b/guided_mrmp/controllers/mpc.py
index f4c9c729ebd7c5a054f4e153a556d7c9091b6c3f..d87557c17358d5aed2faf283a4bd60ebacfdb46e 100644
--- a/guided_mrmp/controllers/mpc.py
+++ b/guided_mrmp/controllers/mpc.py
@@ -1,5 +1,6 @@
 import numpy as np
 import casadi as ca
+from guided_mrmp.optimizer import Optimizer
 
 np.seterr(divide="ignore", invalid="ignore")
 
@@ -95,37 +96,8 @@ class MPC:
         }
 
     def solve_optimization_problem(self, problem, initial_guesses=None, solver_options=None):
-        opti = problem['opti']
-        
-        if initial_guesses:
-            for param, value in initial_guesses.items():
-                # print(f"param = {param}")
-                # print(f"value = {value}")
-                opti.set_initial(problem[param], value)
-
-        # Set numerical backend, with options if provided
-        if solver_options:
-            opti.solver('ipopt', solver_options)
-        else:
-            opti.solver('ipopt')
-
-        try:
-            sol = opti.solve()   # actual solve
-            status = 'succeeded'
-        except:
-            sol = None
-            status = 'failed'
-
-        results = {
-            'status' : status,
-            'solution' : sol,
-        }
-
-        if sol:
-            for var_name, var in problem.items():
-                if var_name != 'opti':
-                    results[var_name] = sol.value(var)
-
+        opt = Optimizer(problem)
+        results = opt.solve_optimization_problem(initial_guesses, solver_options)
         return results
 
     def step(self, initial_state, target, prev_cmd, initial_guesses=None):
@@ -147,6 +119,7 @@ class MPC:
 
         solver_options = {'ipopt.print_level': self.print_level, 
                           'print_time': self.print_time, 
+                          'ipopt.tol': self.acceptable_tol,
                           'ipopt.acceptable_tol': self.acceptable_tol, 
                           'ipopt.acceptable_iter': self.acceptable_iter}
 
diff --git a/guided_mrmp/optimizer.py b/guided_mrmp/optimizer.py
new file mode 100644
index 0000000000000000000000000000000000000000..5afa7cf42ce9a8c10f0efc614edf83f2114bb0e6
--- /dev/null
+++ b/guided_mrmp/optimizer.py
@@ -0,0 +1,37 @@
+class Optimizer:
+    def __init__(self, problem):
+        self.problem = problem
+
+    def solve_optimization_problem(self, initial_guesses=None, solver_options=None):
+        opti = self.problem['opti']
+        
+        if initial_guesses:
+            for param, value in initial_guesses.items():
+                print(f"param = {param}")
+                print(f"value = {value}")
+                opti.set_initial(self.problem[param], value)
+
+        # Set numerical backend, with options if provided
+        if solver_options:
+            opti.solver('ipopt', solver_options)
+        else:
+            opti.solver('ipopt')
+
+        try:
+            sol = opti.solve()   # actual solve
+            status = 'succeeded'
+        except:
+            sol = None
+            status = 'failed'
+
+        results = {
+            'status' : status,
+            'solution' : sol,
+        }
+
+        if sol:
+            for var_name, var in self.problem.items():
+                if var_name != 'opti':
+                    results[var_name] = sol.value(var)
+
+        return results