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