def compute_heuristic(self, config, goal):
"""
Use the Dubins path length from config to goal as the heuristic distance.
"""
# Implement here
px, py, pyaw, heuristic = dubins_path_planning(config, goal,
self.curvature)
return heuristic
def compute_heuristic(self, config, goal):
"""
Use the Dubins path length from config to goal as the heuristic distance.
"""
# Implement here
curvature = 1 / (np.linalg.norm(np.array(goal) - np.array(config)) / 2) # Curvature = 1 / radius
_, heuristic = dubins_path_planning(config, goal, curvature)
return heuristic
def generate_path(self, config1, config2):
"""
Generate a dubins path from config1 to config2
The generated path is not guaranteed to be collision free
Use dubins_path_planning to get a path
return: (numpy array of [x, y, yaw], curve length)
"""
# TODO
x, y, yaw, path_length = dubins_path_planning(config1, config2,
self.curvature)
path = np.array([x, y, yaw]).transpose()
return path, path_length
def generate_path(self, config1, config2):
"""
Generate a dubins path from config1 to config2
The generated path is not guaranteed to be collision free
Use dubins_path_planning to get a path
return: (numpy array of [x, y, yaw], curve length)
"""
# Implement here
config1[2] = math.radians(config1[2])
config2[2] = math.radians(config2[2])
px, py, pyaw, clen = dubins_path_planning(config1, config2,
self.curvature)
path = np.array([px, py, pyaw])
return path, clen
def compute_distances(self, start_config, end_configs):
"""
Compute distance from start_config and end_configs using Dubins path
@param start_config: tuple of start config
@param end_configs: list of tuples of end configs
@return numpy array of distances
"""
# Implement here
start_config[2] = math.radians(start_config[2])
end_configs[:, 2] = math.radians(end_configs[:, 2])
px, py, pyaw, distances = dubins_path_planning(
np.tile(np.array(start_config), len(end_configs)),
np.array(end_configs))
return distances
def generate_path(self, config1, config2):
"""
Generate a dubins path from config1 to config2
The generated path is not guaranteed to be collision free
Use dubins_path_planning to get a path
return: (numpy array of [x, y, yaw], curve length)
"""
# Implement here
ppx, ppy, ppyaw, clen = dubins_path_planning(tuple(config1),
tuple(config2),
self.curvature)
path = np.zeros((len(ppx), 3))
path[:, 0] = ppx
path[:, 1] = ppy
path[:, 2] = ppyaw
return path, clen
def compute_distances(self, start_config, end_configs):
"""
Compute distance from start_config and end_configs using Dubins path
@param start_config: tuple of start config
@param end_configs: list of tuples of end confings
@return numpy array of distances
"""
distances = np.zeros(np.shape(end_configs)[0])
for i in range(len(end_configs)):
if (tuple(start_config) == tuple(end_configs[i])):
continue
_, _, _, distances[i] = dubins_path_planning(
tuple(start_config), tuple(end_configs[i]), self.curvature)
return distances
def generate_path(self, config1, config2):
"""
Generate a dubins path from config1 to config2
The generated path is not guaranteed to be collision free
Use dubins_path_planning to get a path
return: (numpy array of [x, y, yaw], curve length)
"""
# Implement here
curvature = 1 / (np.sqrt(np.sum(((np.subtract(config2, config1))**2)), axis=1) / 2) # Curvature = 1 / radius
x_path, y_path, theta_path, path_length = dubins_path_planning(config1, config2, curvature)
path = np.array([x_path, y_path, theta_path])
clen = path_length
return path, clen
def compute_distance(arg):
"""
Compute distance from start_config and end_configs using Dubins path
@param start_config: tuple of start config
@param end_configs: list of tuples of end confings
@return numpy array of distances
"""
start_config, end_configs = arg
start_config, end_configs = np.array(start_config), np.array(end_configs)
num = end_configs.shape[0]
distances = np.zeros(num)
for i in range(num):
_1, _2, _3, distance = dubins_path_planning(start_config,
end_configs[i],
curvature=0.018)
distances[i] = distance
return distances