def pose_control(self, robot):
robot.goal = self.look_ahead(robot)
# distance to goal
distance = utils.euclidean_distance(robot.states, robot.goal)
# PID controller for velocity
velocity_control = self.gains[0] * distance
# angle control
angle_to_point = (
utils.angle_between_points(robot.states, robot.goal) -
robot.states[2][0] + math.pi) % (2 * math.pi) - math.pi
beta = (robot.goal[2] - robot.states[2][0] - angle_to_point +
math.pi) % (2 * math.pi) - math.pi
# derivative term for angle control
error_diff = angle_to_point - self.error_old
self.error_old = angle_to_point
# maneuver
if angle_to_point > math.pi / 2 or angle_to_point < -math.pi / 2:
velocity_control = -velocity_control
return np.array([
velocity_control, self.gains[2] * angle_to_point +
self.gains[3] * beta + self.gains[1] * error_diff
])
def compute_solution(self):
self.solution = []
for index, node in self.data.items():
if index == 1:
continue
best_neuron = None
best_distance = None
for count, neuron in enumerate(self.neurons):
if not neuron[2]:
continue
distance = euclidean_distance(node, neuron)
if not best_neuron or distance < best_distance:
best_distance = distance
best_neuron = count
self.solution.append((index, best_neuron))
# add depots between nodes
for count, depot in enumerate(self.neurons):
if depot[2]:
continue
self.solution.append((1, count))
# sort solution according to neuron index
self.solution.sort(key=lambda x: x[1])
# split solution in salesmen
temp_solution = []
last_index = 0
for index, node in enumerate(self.solution):
if node[0] == 1:
temp_solution.append(
[item[0] for item in self.solution[last_index:index]])
last_index = index + 1
self.solution = temp_solution
def look_ahead(self, robot):
"""
search for the closest point on the reference path to the robot and return
a look ahed point (a point a little further than the closest one)
"""
min_dist = float('inf')
dist = 0.0
idx = 0
goal = [0.0, 0.0, 0.0]
for i in range(len(robot.path[0]) -
robot.lookahead_idx): # find closest path point
dist = utils.euclidean_distance(
(robot.path[0][i], robot.path[1][i]), robot.states)
if dist < min_dist:
min_dist = dist
idx = i
goal[0] = robot.path[0][i + robot.lookahead_idx]
goal[1] = robot.path[1][i + robot.lookahead_idx]
goal[2] = math.atan2(robot.path[1][i + 1] - robot.path[1][i],
robot.path[0][i + 1] - robot.path[0][i])
if len(
robot.path[0]
) < robot.lookahead_idx + 1: # if close to final goal, returns final goal
goal[0] = robot.path[0][-1]
goal[1] = robot.path[1][-1]
goal[2] = math.atan2(
robot.path[1][i + 1] - robot.path[1][i],
robot.path[0][i + 1] - robot.path[0][i])
for i in range(idx): # delete passed path points
robot.path = np.delete(robot.path, [i], 1)
return goal
def get_distance(self, x, y):
if x > y:
x, y = y, x
if (x, y) in self.distance_cache:
return self.distance_cache[(x, y)]
distance = euclidean_distance(self.data[x], self.data[y])
self.distance_cache[(x, y)] = distance
return distance
def merge_duplicate_nodes(nodes, edges, distance_threshold=DISTANCE_THRESHOLD):
r"""
Merge duplicate nodes and modify edges accordingly. Nodes are considered coincident if closer than a threshold
:param nodes: list of nodes (x, y)
:param edges: list of edges (node_a_id, node_b_id, road_id)
:param distance_threshold: maximum distance for points to be considered coincident.
:return nodes: list of nodes (x, y) after cleaning
:return edges: list of edges (node_a_id, node_b_id, road_id) after cleaning
:return len(to_del): how many nodes have been deleted in the merging procedure
"""
to_del = set() # nodes to be deleted (duplicates)
to_del_edges = [] # edges to be deleted (duplicates)
replace_destination = dict() # to which node a duplicate will be substituted. used to handle recurrent dependencies
for i, (ax, ay) in enumerate(nodes):
for j in range(i + 1, len(nodes)):
bx, by = nodes[j]
if euclidean_distance(ax, ay, bx, by) < distance_threshold:
# check if a coincides with b
replace_destination[j] = i
for k, edge in enumerate(edges):
# update all edges that have the removed duplicate node to point to the substitutive one
a, b, id_road = edge
a = a if a != j else i
b = b if b != j else i
edges[k] = [a, b, id_road]
to_del.add(j)
to_del = list(to_del)
old_nodes = nodes[:] # make a copy of nodes, for the following self-edge removal
# simplify recursive dependencies in the destinations for the replacemente
for k in replace_destination.keys():
while replace_destination[k] in replace_destination.keys():
replace_destination[k] = replace_destination[replace_destination[k]]
# delete duplicate nodes starting from the end of the list to not break indexing in the iteration
for i in reversed(sorted(to_del)):
del nodes[i]
# delete self-edges (we found some self-edges from point to itself. This removes this self-edges)
for i in range(len(edges)):
a, b, id_road = edges[i]
a = nodes.index(old_nodes[a]) if a not in replace_destination.keys() else nodes.index(old_nodes[replace_destination[a]])
b = nodes.index(old_nodes[b]) if b not in replace_destination.keys() else nodes.index(old_nodes[replace_destination[b]])
if a == b:
to_del_edges.append(i)
edges[i] = [a, b, id_road]
# delete duplicate edges starting from the end of the list to not break indexing in the iteration
if len(edges) > 1:
for i in reversed(to_del_edges):
del edges[i]
return nodes, edges, len(to_del)
def steer(self, xrand, closest_node, closest_idx):
orientation = math.atan2(xrand[1] - closest_node[1],
xrand[0] - closest_node[0])
distance = utils.euclidean_distance(xrand, closest_node)
new_node = [xrand[0], xrand[1], infinity, None]
if distance > self.expand_distance:
new_node[0] = closest_node[0] + self.expand_distance * math.cos(
orientation)
new_node[1] = closest_node[1] + self.expand_distance * math.sin(
orientation)
return new_node
def get_costs(self, new_node, near_nodes_idx):
cost_list = []
if near_nodes_idx:
for idx in near_nodes_idx:
dx = new_node[0] - self.nodes[idx][0]
dy = new_node[1] - self.nodes[idx][1]
d = math.sqrt(dx**2 + dy**2)
theta = math.atan2(dy, dx)
distance = utils.euclidean_distance(new_node, self.nodes[idx])
orientation = math.atan2(new_node[1] - self.nodes[idx][1],
new_node[0] - self.nodes[idx][0])
if distance != d or orientation != theta:
pass
if self.collision_free(new_node, self.nodes[idx]):
cost_list.append(
self.nodes[idx][2] +
utils.euclidean_distance(new_node, self.nodes[idx]))
else:
cost_list.append(infinity)
return cost_list
def collision_free(self, node, parent):
# node_ = [node[0], node[1]]
parent_ = copy.deepcopy(parent)
distance = utils.euclidean_distance(parent_, node)
orientation = math.atan2(node[1] - parent_[1], node[0] - parent_[0])
line = np.arange(0, distance, 0.1)
for i in line:
parent_[0] += self.expand_distance * math.cos(orientation)
parent_[1] += self.expand_distance * math.sin(orientation)
if not self.obstacle_free(parent_):
return False
return True
def rewire_tree(self, near_nodes_idx, new_node):
cardV = len(self.nodes)
for idx in near_nodes_idx:
node = self.nodes[idx]
distance = utils.euclidean_distance(new_node, node)
cost = new_node[2] + distance
if node[2] > cost:
if self.collision_free(new_node, node):
node[2] = cost
node[3] = cardV - 1
def join_intersection(line, point, threshold=STEP/50):
r"""
Check if the two newly generated lines from intersection are long enough. Return the one that are long enough.
We threshold the minimum length of the segment to prevent noise or irrepresentability in the low-resolution
semantic segmentation.
:param line: input line
:param point: intersection point
:param threshold: minimum length for a new line to be accepted
:return: new accepted lines after intersection
"""
new_lines = []
for p in [line.a, line.b]:
dist = euclidean_distance(p, point)
if dist > threshold:
new_lines.append(Line(p, point))
return new_lines
def get_index(self):
# find closest node to the goal with best cost
distance2goal = [
utils.euclidean_distance(node, self.goal) for node in self.nodes
]
indexes = []
for distance in distance2goal:
if distance <= 2 * self.expand_distance:
indexes.append(distance2goal.index(distance))
if indexes:
mincost = min([self.nodes[i][2] for i in indexes])
for idx in indexes:
if self.nodes[idx][2] == mincost:
return idx
return None
def lqr_steer_control(self, robot):
self.Q = np.eye(robot.states.shape[0])
self.R = np.eye(2)
robot.goal = self.look_ahead(robot)
######## Linearization ########
vel = utils.euclidean_distance(robot.states, robot.goal)
A, B = robot.jacobi()
######## DARE ########
X = self.solve_DARE(A, B)
######## LQR solution ########
lqr_k = self.solve_lqr(X, A, B)
error = robot.error()
ustar = -lqr_k @ error
# calc steering input
delta = utils.truncate_angle(ustar[1])
return np.array([vel, delta])
def sorted_cand_ind(eval_df, cand_df, n_eval, n_cand):
"""
Sort indices of candidate data according to
their Euclidean distance to each evaluation data in deep feature space.
Args:
eval_df (tensor): deep features of evaluation data.
cand_df (tensor): deep features of evaluation data.
n_eval (int): number of evaluation data.
n_cand (int): number of candidate data.
Returns
sorted_cand_ind (tensor): sorted indices of candidate set w.r.t. each evaluation data.
"""
# Sort indices of candidate set according to distance w.r.t. evaluation set in deep feature space
# Preprocess feature vectors to facilitate vector-wise distance computation
eval_df_repeat = eval_df.repeat([1, n_cand]).reshape(
[n_eval * n_cand, eval_df.shape[1]])
cand_df_tile = cand_df.repeat([n_eval, 1])
# Compute distance between evaluation and candidate feature vectors
distance_vector = euclidean_distance(eval_df_repeat, cand_df_tile)
# Turn distance vector into distance matrix
distance_matrix = distance_vector.reshape((n_eval, n_cand))
# Sort candidate set indices based on distance
sorted_cand_ind_ = distance_matrix.argsort(1)
return sorted_cand_ind_
def get_best_matching_unit(self, node):
return min(enumerate(filter(lambda k: k[2], self.neurons)),
key=lambda x: euclidean_distance(x[1], node))
path[1],
ds=0.25)
path = [rx, ry]
################ Initialize model and control #################
robot_ground_truth = extended_bicycle("RK2", states, controls, path, dT)
robot = extended_bicycle("RK3", states, controls, path, dT)
control = controllers(robot.gains)
ekf = Kalman_filter(states.shape[0])
mc = ManualControl()
mpc = NonlinearMPC(2.5, dT)
print("Start Simulation")
while utils.euclidean_distance(goal, robot.states) > 0.3:
# robot.controls = mc.get_controls(anime.fig, robot.states, dT)
control.lqr_vel_steer_control(robot)
robot.controls = mpc.MPC(robot.states, robot.path)
robot_ground_truth.controls = copy.deepcopy(robot.controls)
#
add_noise = False
robot_ground_truth.run(add_noise) # ground truth
# add_noise = True
robot.run(add_noise)
# robot = ekf.run_filter(robot, robot_ground_truth.states)
# utils.collision_square_obstacle(robot.states, )
states_history = np.hstack((states_history, robot.states))
states_history_ground_truth = np.hstack(