diff --git a/src/robot_behavior/src/robot_behavior/utils.py b/src/robot_behavior/src/robot_behavior/utils.py
index 4c8e5e2fdd2bd848345dfd4b075efff43d434619..866bff4297995f618b9db74f949b278f75121d1f 100755
--- a/src/robot_behavior/src/robot_behavior/utils.py
+++ b/src/robot_behavior/src/robot_behavior/utils.py
@@ -1,6 +1,8 @@
 #!/usr/bin/env python3
 import numpy as np
 import tf
+from scipy.interpolate import RectBivariateSpline
+
 
 def constraint_angle(angle, min_value=-np.pi, max_value=np.pi):
 
@@ -49,3 +51,78 @@ def meter_to_px(point, map_size, resolution):
     point_px[1] = abs(map_size[1][1] - point[1])/resolution
 
     return np.rint(point_px)
+
+def optimal_path_2d(travel_time, starting_point, dx, coords, goal=None, max_d_orientation=None, N=100):
+    """
+    Find the optimal path from starting_point to the zero contour
+    of travel_time. dx is the grid spacing
+    Solve the equation x_t = - grad t / | grad t |
+    """
+    
+    grad_t_y, grad_t_x = np.gradient(travel_time, dx)
+    if isinstance(travel_time, np.ma.MaskedArray):
+        grad_t_y[grad_t_y.mask] = 0.0
+        grad_t_y = grad_t_y.data
+        grad_t_x[grad_t_x.mask] = 0.0
+        grad_t_x = grad_t_x.data
+
+    d_interp = RectBivariateSpline(coords[1], coords[0],
+                                   travel_time)
+    gradx_interp = RectBivariateSpline(coords[1], coords[0],
+                                       grad_t_x)
+    grady_interp = RectBivariateSpline(coords[1], coords[0],
+                                       grad_t_y)
+
+    def get_velocity(position):
+        """ return normalized velocity at pos """
+        x, y = position
+        vel = np.array([gradx_interp(y, x)[0][0],
+                        grady_interp(y, x)[0][0]])
+        return vel / np.linalg.norm(vel)
+
+    def get_distance(position):
+        x, y = position
+        return d_interp(y, x)
+
+    def euler_point_update(pos, ds):
+        return pos - get_velocity(pos) * ds
+
+    def runge_kutta(pos, ds):
+        """ Fourth order Runge Kutta point update """
+        k1 = ds * get_velocity(pos)
+        k2 = ds * get_velocity(pos - k1/2.0)
+        k3 = ds * get_velocity(pos - k2/2.0)
+        k4 = ds * get_velocity(pos - k3)
+        v = (k1 + 2*k2 + 2*k3 + k4)/6.0
+        return pos - v, v
+
+    # def sym(x, v):
+    #     x = x[0], 0.5 * (coords[1][-1] + coords[1][0]) - x[1]
+    #     v = v[0], -v[1]
+    #     return x, v
+    #
+    # starting_point, _ = sym(starting_point, [0, 0])
+
+    x, v = runge_kutta(starting_point, dx)
+    xl, yl, vxl, vyl, dl = [], [], [], [], []
+    starting_distance = get_distance(starting_point)
+
+    for i in range(N):
+        # xp, vp = sym(x, v)
+        d = get_distance(x)[0][0]
+        # if negative value d < 0, waypoint in an obstacle
+        if d < 0:
+            return False
+        # goal is implicit defined in the travel_time map but we need to test it explicitly because we can not really trust d
+        if ((goal is not None and max_d_orientation is not None) and
+                (np.linalg.norm(goal - np.asarray(x)) < max_d_orientation)):
+            if dl:
+                break
+        xl.append(x[0])
+        yl.append(x[1])
+        vxl.append(v[0])
+        vyl.append(v[1])
+        dl.append(d)
+        x, v = runge_kutta(x, dx)
+
+    return xl, yl, vxl, vyl, dl