def compute_objective(self, X, U, goal):
"""Set the objective."""
obj = self.__params.objective
# final destination objective
cost = (
obj.w_final * (X[-1, 0] - goal[0]) ** 2
+ obj.w_final * (X[-1, 1] - goal[1]) ** 2
)
# change in acceleration objective
for u1, u2 in util.pairwise(U[:, 0]):
_u = u1 - u2
cost += obj.w_ch_accel * _u * _u
# change in turning objective
for u1, u2 in util.pairwise(U[:, 1]):
_u = u1 - u2
cost += obj.w_ch_turning * _u * _u
# change in joint objective
for u1, u2 in util.pairwise(U[:]):
_u = u1 - u2
cost += obj.w_ch_joint * _u[0] * _u[1]
# acceleration objective
cost += obj.w_accel * np.sum(U[:, 0] ** 2)
# turning objective
cost += obj.w_turning * np.sum(U[:, 1] ** 2)
# joint objective
cost += 2. * obj.w_joint * np.sum(U[:, 0] * U[:, 1])
return cost
def test_3(self):
from numpy import poly1d
for i1, i2 in pairwise(range(len(self.t))):
t1 = self.t[i1]
t2 = self.t[i2]
q1 = self.q[i1]
q2 = self.q[i2]
v1 = self.v[i1]
v2 = self.v[i2]
a1 = self.a[i1]
a2 = self.a[i2]
j = (self.a[i2] - self.a[i1]) / self.h[i1]
t1_2 = t1 * t1
t1_3 = t1_2 * t1
t2_2 = t2 * t2
t2_3 = t2_2 * t2
ca = (t2 * a1 - t1 * a2) / (t2 - t1)
cv = t1_2 / (t1_2 -
t2_2) * (v2 - ca * t2) - (t2_2 /
(t1_2 - t2_2)) * (v1 - ca * t1)
cq = t1_3 / (t1_3 - t2_3) * (q2 - ca * t2_2 / 2 - cv * t2) - t2_3 / (
t1_3 - t2_3) * (q1 - ca * t1_2 / 2 - cv * t1)
A = j / 6
B = ca / 2
C = cv
D = cq
p = poly1d([A, B, C, D])
print 'pos:', p(self.t[0]), p(self.t[1])
print 'vel:', p.deriv(1)(self.t[0]), p.deriv(1)(self.t[1])
print 'accel:', p.deriv(2)(self.t[0]), p.deriv(2)(self.t[1])
print 'jerk:', p.deriv(3)(self.t[0]), p.deriv(3)(self.t[1])
def match_basic_tokens(char_spans, tok_list):
tok_pointer = 0
match_dict = dict()
for start in sorted(char_spans.keys()):
end, untok_string = char_spans[start][0]
untok_string = untok_string.strip()
found = False
for tok_index, tok in enumerate(tok_list[tok_pointer:]):
if match_token(untok_string, tok):
match_dict[(start, end)] = [tok_pointer + tok_index]
tok_pointer += tok_index + 1
found = True
break
if found:
continue
tok_index = tok_pointer
for tok1, tok2 in pairwise(tok_list[tok_pointer:]):
if match_2token(untok_string, tok1, tok2):
match_dict[(start, end)] = [tok_index, tok_index + 1]
tok_pointer = tok_index + 2
break
tok_index += 1
return match_dict
def compute_objective(self, X, U, goal):
"""Set the objective."""
obj = self.__params.objective
# final destination objective
cost = (X[-1, 0] - goal[0])**2 + (X[-1, 1] - goal[1])**2
# change in acceleration objective
for u1, u2 in util.pairwise(U[:, 0]):
_u = (u1 - u2)
cost += obj.w_ch_accel * _u * _u
# change in turning objective
for u1, u2 in util.pairwise(U[:, 1]):
_u = (u1 - u2)
cost += obj.w_ch_turning * _u * _u
# acceleration objective
cost += obj.w_accel * np.sum(U[:, 0]**2)
# turning objective
cost += obj.w_turning * np.sum(U[:, 1]**2)
return cost
def fill_spline_msg(self, spline_msg):
"""Fills a api.Spline msg with data from a cubic spline.
spline_msg: An api.Spline message instance. Ommits extreamly short
duration coefficients, because they add no useful information and add
large amounts of error.
"""
spline_msg.times.append(self.t[0])
for c, (ti, tf) in zip(self.coeffs, pairwise(self.t)):
poly_msg = spline_msg.polys.add()
poly_msg.coeffs.extend(c)
spline_msg.times.append(tf)
def genenerate_datasets():
for i, j in utility.pairwise(
itertools.accumulate(
[0, train_size, validation_size, test_size, 1.0])):
indices = perm[i:j]
movie_ids = self.movie_ids_[indices]
user_ids = self.user_ids_[indices]
dates = self.dates_[indices]
ratings = self.ratings_[indices]
yield DataSet(movie_ids, user_ids, dates, ratings,
self.user_map)
def is_cyclic(nums):
"""
>>> is_cyclic([8128, 2882, 8281])
True
>>> is_cyclic([8228, 2882, 8281])
False
"""
for a, b in pairwise(nums):
if not is_connection(a, b):
return False
if not is_connection(nums[-1], nums[0]):
return False
return True
def do_highlevel_control(self, params):
"""Decide parameters.
TODO finish description
"""
segs_polytopes, goal, seg_psi_bounds = self.compute_segs_polytopes_and_goal(
params)
"""Apply motion planning problem"""
n_segs = len(segs_polytopes)
Gamma, nu, nx = params.Gamma, params.nu, params.nx
T = self.__control_horizon
max_a, max_delta = self.__params.max_a, self.__params.max_delta
model = docplex.mp.model.Model(name="proposed_problem")
min_u = np.vstack((np.zeros(T), np.full(T,
-0.5 * max_delta))).T.ravel()
max_u = np.vstack(
(np.full(T, max_a), np.full(T, 0.5 * max_delta))).T.ravel()
u = np.array(model.continuous_var_list(nu * T,
lb=min_u,
ub=max_u,
name='u'),
dtype=object)
# State variables
X = (params.initial_rollout.local + util.obj_matmul(Gamma, u)).reshape(
T + 1, nx)
X = params.transform(X)
X = X[1:]
# Control variables
U = u.reshape(T, nu)
"""Apply motion dynamics constraints"""
model.add_constraints(
self.compute_dyamics_constraints(params, X[:, 3], X[:, 4]))
"""Apply road boundary constraints"""
if self.road_boundary_constraints:
# Slack variables from road obstacles
Omicron = np.array(model.binary_var_list(n_segs * T,
name="omicron"),
dtype=object).reshape(n_segs, T)
M_big, diag = self.__params.M, self.__params.diag
psi_0 = params.initial_state.world[2]
for t in range(self.__control_horizon):
for seg_idx, (A, b) in enumerate(segs_polytopes):
lhs = util.obj_matmul(A, X[t, :2]) - np.array(
M_big * (1 - Omicron[seg_idx, t]))
rhs = b + diag
"""Constraints on road boundaries"""
model.add_constraints([l <= r for (l, r) in zip(lhs, rhs)])
"""Constraints on angle boundaries"""
# lhs = X[t, 2] - psi_0 - M_big*(1 - Omicron[seg_idx, t])
# model.add_constraint(lhs <= seg_psi_bounds[seg_idx][1])
# lhs = X[t, 2] - psi_0 + M_big*(1 - Omicron[seg_idx, t])
# model.add_constraint(lhs >= seg_psi_bounds[seg_idx][0])
model.add_constraint(np.sum(Omicron[:, t]) >= 1)
"""Start from current vehicle position and minimize the objective"""
w_ch_accel = 0.
w_ch_turning = 0.
w_accel = 0.
w_turning = 0.
# final destination objective
cost = (X[-1, 0] - goal[0])**2 + (X[-1, 1] - goal[1])**2
# change in acceleration objective
for u1, u2 in util.pairwise(U[:, 0]):
_u = (u1 - u2)
cost += w_ch_accel * _u * _u
# change in turning rate objective
for u1, u2 in util.pairwise(U[:, 1]):
_u = (u1 - u2)
cost += w_ch_turning * _u * _u
cost += w_accel * np.sum(U[:, 0]**2)
# turning rate objective
cost += w_turning * np.sum(U[:, 1]**2)
model.minimize(cost)
# TODO: warmstarting
# if self.U_warmstarting is not None:
# # Warm start inputs if past iteration was run.
# warm_start = model.new_solution()
# for i, u in enumerate(self.U_warmstarting[self.__control_horizon:]):
# warm_start.add_var_value(f"u_{2*i}", u[0])
# warm_start.add_var_value(f"u_{2*i + 1}", u[1])
# # add delta_0 as hotfix to MIP warmstart as it needs
# # at least 1 integer value set.
# warm_start.add_var_value('delta_0', 0)
# model.add_mip_start(warm_start)
# model.print_information()
# model.parameters.read.datacheck = 1
if self.log_cplex:
model.parameters.mip.display = 2
s = model.solve(log_output=True)
else:
model.solve()
# model.print_solution()
f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value
cost = cost.solution_value
U_star = util.obj_vectorize(f, U)
X_star = util.obj_vectorize(f, X)
return util.AttrDict(cost=cost,
U_star=U_star,
X_star=X_star,
goal=goal)
def cover_along_waypoints_fixedsize(start_wp,
choices,
max_distance,
lane_width,
flip_x=False,
flip_y=False):
"""Compute covering polytopes over a path on the road network.
Parameters
==========
start_wp : carla.Waypoint
Starting waypoint of the path on the road network.
choices : list of int
The indices of the turns to make when reaching a fork on the road network.
`choices[0]` is the index of the first turn, `choices[1]` is the index of the second turn, etc.
max_distance : float
The max distance of the path starting from `start_wp`. Use to specify length of path.
lane_width : float
The width of the road.
Returns
=======
util.AttrDict
The data of the covering poytopes.
- spline : the spline fitting the path of road network.
- max_k : approx. max curvature of the spline.
- segment_length : length of spline segment covered by polytope.
- polytopes : list of ndarray polytopes with H-representation (A, b)
where points Ax <= b iff x is in the polytope.
- distances : ndarray of distances along the spline to follow from nearest
endpoint before encountering corresponding covering polytope
in index.
- positions : ndarray of 2D positions of center of the covering polytope
in index.
- tangents : list of tuple of ndarray of ndarray
The tangent vector components of the tangents entering and
exiting the spline.
"""
waypoints, points, distances = carlautil.collect_points_along_path(
start_wp, choices, max_distance, flip_x=flip_x, flip_y=flip_y)
# fit a spline and get the 1st and 2nd spline derivatives
# distances = util.npu.cumulative_points_distances(points)
# distances = np.insert(distances, 0, 0)
L = distances[-1]
spline = scipy.interpolate.CubicSpline(distances, points, axis=0)
dspline = spline.derivative(1)
ddspline = spline.derivative(2)
# compute approx. max curvature
# distances = np.linspace(0, L, 10)
max_k = np.max(np.linalg.norm(ddspline(distances), axis=1))
# compute the vertices of road covers
half_lane_width = 0.55 * lane_width
segment_length = min(10, compute_segment_length(0.25, max_k))
n = int(np.round(L / segment_length))
distances = np.linspace(0, L, n)
l = util.pairwise(
zip(spline(distances), dspline(distances), ddspline(distances)))
polytopes = [] # polytope representation of rectangular cover
vertex_set = [] # vertex representation of rectangular cover
tangents = []
for (X1, dX1, ddX1), (X2, dX2, ddX2) in l:
sgn1 = np.sign(dX1[0] * ddX1[1] - dX1[1] * ddX1[0])
sgn2 = np.sign(dX2[0] * ddX2[1] - dX2[1] * ddX2[0])
tangent1 = ddX1 / np.linalg.norm(ddX1)
tangent2 = ddX2 / np.linalg.norm(ddX2)
p1 = X1 + half_lane_width * sgn1 * tangent1
p2 = X2 + half_lane_width * sgn2 * tangent2
p3 = X2 - half_lane_width * sgn2 * tangent2
p4 = X1 - half_lane_width * sgn1 * tangent1
vertices = np.stack((p1, p2, p3, p4))
A, b = util.npu.vertices_to_halfspace_representation(vertices)
polytopes.append((A, b))
vertex_set.append(vertices)
tangents.append((dX1, dX2))
return util.AttrDict(spline=spline,
max_k=max_k,
segment_length=segment_length,
polytopes=polytopes,
distances=distances,
positions=np.mean(np.stack(vertex_set), axis=1),
tangents=tangents)
def do_highlevel_control(self, params, ovehicles):
"""Decide parameters.
Since we're doing multiple coinciding, we want to compute...
TODO finish description
"""
vertices = self.__compute_vertices(params, ovehicles)
A_unions, b_unions = self.__compute_overapproximations(
vertices, params, ovehicles)
"""Apply motion planning problem"""
N_traj, L, T, K, O, Gamma, nu, nx = params.N_traj, self.__params.L, self.__params.T, params.K, \
params.O, self.__params.Gamma, self.__params.nu, self.__params.nx
model = docplex.mp.model.Model(name='obstacle_avoidance')
# set up controls variables for each trajectory
u = np.array(
model.continuous_var_list(N_traj * T * nu,
lb=-np.inf,
ub=np.inf,
name='control'))
Delta = np.array(
model.binary_var_list(O * L * N_traj * T,
name='delta')).reshape(N_traj, T, O, L)
# U has shape (N_traj, T*nu)
U = u.reshape(N_traj, -1)
# Gamma has shape (nx*(T + 1), nu*T) so X has shape (N_traj, nx*(T + 1))
X = util.obj_matmul(U, Gamma.T)
# X, U have shapes (N_traj, T, nx) and (N_traj, T, nu) resp.
X = (X + params.States_free_init).reshape(N_traj, -1, nx)[..., 1:, :]
U = U.reshape(N_traj, -1, nu)
for _U, _X in zip(U, X):
# _X, _U have shapes (T, nx) and (T, nu) resp.
p_x, p_y = _X[..., 0], _X[..., 1]
v_x, v_y = _X[..., 2], _X[..., 3]
u_x, u_y = _U[..., 0], _U[..., 1]
model.add_constraints(self.compute_boundary_constraints(p_x, p_y))
model.add_constraints(self.compute_velocity_constraints(v_x, v_y))
model.add_constraints(
self.compute_acceleration_constraints(u_x, u_y))
# set up obstacle constraints
M_big, N_traj, T, diag = self.__params.M_big, params.N_traj, self.__params.T, self.__params.diag
for n in range(N_traj):
# for each obstacle/trajectory
A_union, b_union = A_unions[n], b_unions[n]
for t in range(T):
# for each timestep
As, bs = A_union[t], b_union[t]
for o, (A, b) in enumerate(zip(As, bs)):
lhs = util.obj_matmul(
A, X[n, t, :2]) + M_big * (1 - Delta[n, t, o])
rhs = b + diag
model.add_constraints([l >= r for (l, r) in zip(lhs, rhs)])
model.add_constraint(np.sum(Delta[n, t, o]) >= 1)
# set up coinciding constraints
for t in range(0, self.__n_coincide):
for x1, x2 in util.pairwise(X[:, t]):
model.add_constraints([l == r for (l, r) in zip(x1, x2)])
# start from current vehicle position and minimize the objective
p_x, p_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
flip_y=True)
if self.__goal.is_relative:
goal_x, goal_y = p_x + self.__goal.x, p_y + self.__goal.y
else:
goal_x, goal_y = self.__goal.x, self.__goal.y
start = np.array([p_x, p_y])
goal = np.array([goal_x, goal_y])
cost = np.sum((X[:, -1, :2] - goal)**2)
model.minimize(cost)
# model.print_information()
if self.log_cplex:
model.parameters.mip.display = 5
s = model.solve(log_output=True)
else:
model.solve()
# model.print_solution()
f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value
U_star = util.obj_vectorize(f, U)
cost = cost.solution_value
X_star = util.obj_vectorize(f, X)
return util.AttrDict(cost=cost,
U_star=U_star,
X_star=X_star,
A_unions=A_unions,
b_unions=b_unions,
vertices=vertices,
start=start,
goal=goal)
def find_intersection(self, other, start_time, end_time, dist):
"""Finds position intersections that occur between self and other (spline), in
the time range from start_time to end_time. Dist is the relative distance
between between self and other at start_time. Returns the time of the
earliest intersection, or None if no intersections occur.
"""
# OPTIMIZATION NOTE:
# This question/answer: http://stackoverflow.com/questions/109364/bezier-clipping
# suggests that testing for an intersection could be done faster by
# using a sylvester matrix and checking whether its determinant is zero.
# Not a high priority to implement, as intersection checking does not
# appear to be a bottleneck at this time.
assert isinstance(other, CubicSpline)
try:
self_idx_start = self._get_index_from_time(start_time)
self_idx_end = self._get_index_from_time(
end_time) + 1 # returns left index, want right
other_idx_start = other._get_index_from_time(start_time)
other_idx_end = other._get_index_from_time(
end_time) + 1 # returns left index, want right
except OutOfBoundsError:
return None
# Create a pair of iterators for self and other. Iterate over the splines
# supplying (coeffs, (start_valid_time, end_valid_time)) tuples
s_iter = iter(
zip(self.coeffs[self_idx_start:self_idx_end],
pairwise(self.t[self_idx_start:self_idx_end + 1])))
o_iter = iter(
zip(other.coeffs[other_idx_start:other_idx_end],
pairwise(other.t[other_idx_start:other_idx_end + 1])))
try:
s_c, (s_ti, s_tf) = s_iter.next()
o_c, (o_ti, o_tf) = o_iter.next()
except StopIteration:
return None
offset = dist - other.evaluate(start_time).pos + self.evaluate(
start_time).pos
while True:
test_coeffs = polysub(o_c, s_c)
test_coeffs[-1] = test_coeffs[-1] + offset
test_ti = max(s_ti, o_ti)
test_tf = min(s_tf, o_tf)
r = utility.real_roots(test_coeffs[0],
test_coeffs[1],
test_coeffs[2],
test_coeffs[3],
threshold=1E-6)
# weed out roots outside of the poly's valid range
pts = [t for t in r if
(t >= test_ti or abs(t - test_ti) < self.TIME_EPSILON) and \
(t <= test_tf or abs(t - test_tf) < self.TIME_EPSILON) and \
t >= start_time and t <= end_time]
pts.sort() # want the earliest intersection with target_pos
if len(pts) > 0:
return pts[0] # <------ normal exit point
# Advance polys such that all overlapping ranges are checked.
try:
if s_tf < o_tf:
s_c, (s_ti, s_tf) = s_iter.next()
elif o_tf < s_tf:
o_c, (o_ti, o_tf) = o_iter.next()
else:
s_c, (s_ti, s_tf) = s_iter.next()
o_c, (o_ti, o_tf) = o_iter.next()
except StopIteration:
return None # <------- failure exit point
def make_intervals(self):
return [(tf - ti) for ti, tf in pairwise(self.t)]
def do_multiple_control(self, params, ovehicles):
"""Decide parameters.
Since we're doing multiple coinciding, we want to compute...
TODO finish description
"""
vertices = self.__compute_vertices(params, ovehicles)
A_unions, b_unions = self.__compute_overapproximations(
vertices, params, ovehicles)
segs_polytopes, goal = self.compute_segs_polytopes_and_goal(params)
"""Optimize all trajectories if MCC, else partial trajectories."""
# params.N_select params.N_traj params.subtraj_indices
N_select = params.N_select if self.__agent_type == "rmcc" else params.N_traj
"""Common to MCC+RMCC: setup CPLEX variables"""
L, T, K, O, Gamma, nu, nx = self.__params.L, self.__params.T, \
params.K, params.O, self.__params.Gamma, self.__params.nu, self.__params.nx
n_segs = len(segs_polytopes)
u_max = self.__params.u_max
model = docplex.mp.model.Model(name='obstacle_avoidance')
# set up controls variables for each trajectory
u = np.array(
model.continuous_var_list(N_select * T * nu,
lb=-u_max,
ub=u_max,
name='control'))
Delta = np.array(
model.binary_var_list(O * L * N_select * T,
name='delta')).reshape(N_select, T, O, L)
Omicron = np.array(model.binary_var_list(N_select * n_segs * T,
name="omicron"),
dtype=object).reshape(N_select, n_segs, T)
"""Common to MCC+RMCC: compute state from input variables"""
# U has shape (N_select, T*nu)
U = u.reshape(N_select, -1)
# Gamma has shape (nx*(T + 1), nu*T) so X has shape (N_select, nx*(T + 1))
X = util.obj_matmul(U, Gamma.T)
# X, U have shapes (N_select, T, nx) and (N_select, T, nu) resp.
X = (X + params.States_free_init).reshape(N_select, -1, nx)[..., 1:, :]
U = U.reshape(N_select, -1, nu)
"""Common to MCC+RMCC: apply dynamics constraints to trajectories"""
for _U, _X in zip(U, X):
# _X, _U have shapes (T, nx) and (T, nu) resp.
v_x, v_y = _X[..., 2], _X[..., 3]
u_x, u_y = _U[..., 0], _U[..., 1]
model.add_constraints(self.compute_velocity_constraints(v_x, v_y))
model.add_constraints(
self.compute_acceleration_constraints(u_x, u_y))
"""Common to MCC+RMCC: apply road boundaries constraints to trajectories"""
M_big, T, diag = self.__params.M_big, self.__params.T, self.__params.diag
for n in range(N_select):
# for each trajectory
for t in range(T):
for seg_idx, (A, b) in enumerate(segs_polytopes):
lhs = util.obj_matmul(A, X[n, t, :2]) - np.array(
M_big * (1 - Omicron[n, seg_idx, t]))
rhs = b + diag
model.add_constraints([l <= r for (l, r) in zip(lhs, rhs)])
model.add_constraint(np.sum(Omicron[n, :, t]) >= 1)
"""Apply collision constraints"""
traj_indices = params.subtraj_indices if self.__agent_type == "rmcc" else range(
N_select)
M_big, T, diag = self.__params.M_big, self.__params.T, self.__params.diag
for n, i in enumerate(traj_indices):
# select outerapprox. by index i
# if MCC then iterates through every combination of obstacles
A_union, b_union = A_unions[i], b_unions[i]
for t in range(T):
# for each timestep
As, bs = A_union[t], b_union[t]
for o, (A, b) in enumerate(zip(As, bs)):
lhs = util.obj_matmul(
A, X[n, t, :2]) + M_big * (1 - Delta[n, t, o])
rhs = b + diag
model.add_constraints([l >= r for (l, r) in zip(lhs, rhs)])
model.add_constraint(np.sum(Delta[n, t, o]) >= 1)
"""Common to MCC+RMCC: set up coinciding constraints"""
for t in range(0, self.__n_coincide):
for x1, x2 in util.pairwise(X[:, t]):
model.add_constraints([l == r for (l, r) in zip(x1, x2)])
"""Common to MCC+RMCC: set objective and run solver"""
start = params.x0[:2]
cost = np.sum((X[:, -1, :2] - goal)**2)
model.minimize(cost)
# model.print_information()
if self.log_cplex:
model.parameters.mip.display = 5
s = model.solve(log_output=True)
else:
model.solve()
# model.print_solution()
f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value
U_star = util.obj_vectorize(f, U)
cost = cost.solution_value
X_star = util.obj_vectorize(f, X)
return util.AttrDict(cost=cost,
U_star=U_star,
X_star=X_star,
A_unions=A_unions,
b_unions=b_unions,
vertices=vertices,
start=start,
goal=goal)
def align(dmrs_xml, tok_list, debug=False):
'''
Align currently unaligned tokens to DMRS nodes based on heuristic rules.
:param dmrs_xml: Input DMRS XML
:param tok_list: Input token list
:param debug: Print out intermediary information
:return: Modified DMRS XML with all tokens aligned (NOTE: if a heuristic can't align a token it will remain unaligned)
'''
if debug:
print tok_list
# Find unaligned tokens and current alignment information
unaligned_tokens, toks_to_nodes = get_unaligned_tokens(dmrs_xml, len(tok_list))
tok_to_node_alignment = dict()
node_to_tok_alignment = defaultdict(list)
# Attempt to align each triple of unaligned tokens
for untoken_index_1, untoken_index_2, untoken_index_3 in triple(unaligned_tokens):
if untoken_index_1 + 1 != untoken_index_2 or untoken_index_2 + 1 != untoken_index_3:
continue
untoken_index_range = (untoken_index_1, untoken_index_3)
node_index = align_unaligned_token(untoken_index_range, tok_list, toks_to_nodes)
if node_index is not None:
tok_to_node_alignment[untoken_index_1] = node_index
tok_to_node_alignment[untoken_index_2] = node_index
tok_to_node_alignment[untoken_index_3] = node_index
node_to_tok_alignment[node_index].extend([untoken_index_1, untoken_index_2, untoken_index_3])
# Attempt to align each pair of unaligned tokens
for untoken_index_1, untoken_index_2 in pairwise(unaligned_tokens):
if untoken_index_1 + 1 != untoken_index_2:
continue
untoken_index_range = (untoken_index_1, untoken_index_2)
node_index = align_unaligned_token(untoken_index_range, tok_list, toks_to_nodes)
if node_index is not None:
tok_to_node_alignment[untoken_index_1] = node_index
tok_to_node_alignment[untoken_index_2] = node_index
node_to_tok_alignment[node_index].extend([untoken_index_1, untoken_index_2])
# Attempt to align each remaining unaligned token
for untoken_index in unaligned_tokens:
if untoken_index in tok_to_node_alignment:
continue
untoken_index_range = (untoken_index, untoken_index)
node_index = align_unaligned_token(untoken_index_range, tok_list, toks_to_nodes)
if node_index is not None:
tok_to_node_alignment[untoken_index] = node_index
node_to_tok_alignment[node_index].append(untoken_index)
if debug:
tmp = [entity for entity in dmrs_xml if entity.tag == 'node']
for untoken_index in unaligned_tokens:
untoken_print_out(tok_list[untoken_index], untoken_index, tok_to_node_alignment, tmp)
print
# Modify the node alignments in XML with the newly aligned tokens
node_index = 0
for entity in dmrs_xml:
if entity.tag != 'node':
continue
if node_index in node_to_tok_alignment:
unaligned_toks = node_to_tok_alignment[node_index]
toks = [int(x) for x in entity.attrib['tokalign'].split(' ') if int(x) != -1]
new_toks = sorted(toks + unaligned_toks)
tok_string = ' '.join(str(index) for index in new_toks)
entity.attrib['tokalign'] = tok_string
node_index += 1
return dmrs_xml
def do_highlevel_control(self, params, ovehicles):
"""Decide parameters."""
"""Compute road segments and goal."""
(segs_polytopes, goal,
seg_psi_bounds) = self.compute_segs_polytopes_and_goal(params)
"""Apply motion planning problem"""
N_select = params.N_select
x_bar, u_bar, Gamma = params.x_bar, params.u_bar, params.Gamma
nx, nu = params.nx, params.nu
T = self.__control_horizon
max_a, min_a = self.__params.max_a, self.__params.min_a
max_delta = self.__params.max_delta
"""Model, control and state variables"""
model = docplex.mp.model.Model(name="proposed_problem")
min_u = np.vstack((np.full(T, min_a), np.full(T, -max_delta))).T
min_u = np.repeat(min_u[None], N_select, axis=0).ravel()
max_u = np.vstack((np.full(T, max_a), np.full(T, max_delta))).T
max_u = np.repeat(max_u[None], N_select, axis=0).ravel()
# Slack variables for control
u = model.continuous_var_list(N_select * nu * T,
lb=min_u,
ub=max_u,
name="u")
# U has shape (N_select, T*nu)
U = np.array(u, dtype=object).reshape(N_select, -1)
U_delta = U - u_bar
x = util.obj_matmul(U_delta, Gamma.T) + x_bar
# State variables x, y, psi, v
X = x.reshape(N_select, T, nx)
# Control variables a, delta
U = U.reshape(N_select, T, nu)
"""Apply state constraints"""
for _X in X:
model.add_constraints(self.compute_state_constraints(params, _X))
"""Apply road boundary constraints"""
if self.road_boundary_constraints:
n_segs = len(segs_polytopes)
# Slack variables from road obstacles
Omicron = model.binary_var_list(N_select * n_segs * T,
name="omicron")
Omicron = np.array(Omicron,
dtype=object).reshape(N_select, n_segs, T)
M_big = self.__params.M_big
# diag = self.__params.diag
psi_0 = params.initial_state.world[2]
T = self.__control_horizon
for n in range(N_select):
for t in range(T):
for seg_idx, (A, b) in enumerate(segs_polytopes):
lhs = util.obj_matmul(A, X[n, t, :2]) - np.array(
M_big * (1 - Omicron[n, seg_idx, t]))
rhs = b # - diag
"""Constraints on road boundaries"""
model.add_constraints(
[l <= r for (l, r) in zip(lhs, rhs)])
"""Constraints on angle boundaries"""
if self.angle_boundary_constraints:
lhs = X[n, t, 2] - psi_0 - M_big * (
1 - Omicron[seg_idx, t])
model.add_constraint(
lhs <= seg_psi_bounds[n, seg_idx][1])
lhs = X[n, t, 2] - psi_0 + M_big * (
1 - Omicron[n, seg_idx, t])
model.add_constraint(
lhs >= seg_psi_bounds[seg_idx][0])
model.add_constraint(np.sum(Omicron[n, :, t]) >= 1)
"""Apply vehicle collision constraints"""
if params.O > 0:
vertices = self.__compute_vertices(params, ovehicles)
A_unions, b_unions = self.__compute_overapproximations(
vertices, params, ovehicles)
O, L = params.O, self.__params.L
# Slack variables for vehicle obstacles
Delta = model.binary_var_list(O * L * N_select * T, name='delta')
Delta = np.array(Delta, dtype=object).reshape(N_select, T, O, L)
M_big = self.__params.M_big
diag = self.__params.diag
# for n, i in enumerate(params.subtraj_indices):
# # select outerapprox. by index i
# A_union, b_union = A_unions[i], b_unions[i]
# for t in range(T):
# As, bs = A_union[t], b_union[t]
# for o, (A, b) in enumerate(zip(As, bs)):
# lhs = util.obj_matmul(A, X[n,t,:2]) + M_big*(1 - Delta[n,t,o])
# rhs = b + diag
# model.add_constraints([l >= r for (l,r) in zip(lhs, rhs)])
# model.add_constraint(np.sum(Delta[n,t,o]) >= 1)
for n in range(N_select):
# select outerapprox. by index n
A_union, b_union = A_unions[n], b_unions[n]
for t in range(T):
As, bs = A_union[t], b_union[t]
for o, (A, b) in enumerate(zip(As, bs)):
lhs = util.obj_matmul(
A, X[n, t, :2]) + M_big * (1 - Delta[n, t, o])
rhs = b + diag
model.add_constraints(
[l >= r for (l, r) in zip(lhs, rhs)])
model.add_constraint(np.sum(Delta[n, t, o]) >= 1)
else:
vertices = A_unions = b_unions = None
"""Set up coinciding constraints"""
for t in range(0, self.__n_coincide):
for u1, u2 in util.pairwise(U[:, t]):
model.add_constraints([l == r for (l, r) in zip(u1, u2)])
"""Compute and minimize objective"""
cost = 0
for _U, _X in zip(U, X):
cost += self.compute_objective(_X, _U, goal)
model.minimize(cost)
# TODO: add warmstarting to MCC
# if self.road_boundary_constraints and self.__U_warmstarting is not None:
# # Warm start inputs if past iteration was run.
# warm_start = model.new_solution()
# for i, u in enumerate(self.__U_warmstarting[self.__step_horizon :]):
# warm_start.add_var_value(f"u_{2*i}", u[0])
# warm_start.add_var_value(f"u_{2*i + 1}", u[1])
# # add omicron_0 as hotfix to MIP warmstart as it needs
# # at least 1 integer value set.
# warm_start.add_var_value("omicron_0", 1)
# model.add_mip_start(
# warm_start, write_level=docplex.mp.constants.WriteLevel.AllVars
# )
# model.print_information()
# model.parameters.read.datacheck = 1
if self.log_cplex:
model.parameters.mip.display = 2
s = model.solve(log_output=True)
else:
model.parameters.mip.display = 0
model.solve()
# model.print_solution()
"""Extract solution"""
cost = cost.solution_value
f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value
U_star = util.obj_vectorize(f, U)
X_star = util.obj_vectorize(f, X)
return util.AttrDict(cost=cost,
U_star=U_star,
X_star=X_star,
goal=goal,
A_unions=A_unions,
b_unions=b_unions,
vertices=vertices)
def do_highlevel_control(self, params, ovehicles):
"""Decide parameters."""
"""Compute road segments and goal."""
segments, goal = self.compute_segs_polytopes_and_goal(params)
"""Apply motion planning problem"""
N_select = params.N_select
x_bar, u_bar, Gamma = params.x_bar, params.u_bar, params.Gamma
nx, nu = params.nx, params.nu
T = self.__control_horizon
max_a, min_a = self.__params.max_a, self.__params.min_a
max_delta = self.__params.max_delta
"""Model, control and state variables"""
model = docplex.mp.model.Model(name="proposed_problem")
min_u = np.vstack((np.full(T, min_a), np.full(T, -max_delta))).T
min_u = np.repeat(min_u[None], N_select, axis=0).ravel()
max_u = np.vstack((np.full(T, max_a), np.full(T, max_delta))).T
max_u = np.repeat(max_u[None], N_select, axis=0).ravel()
# Slack variables for control
u = model.continuous_var_list(N_select * nu * T,
lb=min_u,
ub=max_u,
name="u")
# U has shape (N_select, T*nu)
U = np.array(u, dtype=object).reshape(N_select, -1)
U_delta = U - u_bar
x = util.obj_matmul(U_delta, Gamma.T) + x_bar
# State variables x, y, psi, v
X = x.reshape(N_select, T, nx)
# Control variables a, delta
U = U.reshape(N_select, T, nu)
"""Apply state constraints"""
model.add_constraints(self.compute_state_constraints(params, X))
"""Apply road boundary constraints"""
if self.road_boundary_constraints:
N_select = params.N_select
T = self.__control_horizon
I = len(segments.polytopes)
# Slack variables from road obstacles
Omicron = model.binary_var_list(N_select * I * T, name="omicron")
Omicron = np.array(Omicron, dtype=object).reshape(N_select, I, T)
model.add_constraints(
self.compute_road_boundary_constraints(params, X, Omicron,
segments))
else:
Omicron = None
"""Apply vehicle collision constraints"""
if params.O > 0:
N_select = params.N_select
T = self.__control_horizon
O, L = params.O, self.__params.L
# Slack variables for vehicle obstacles
Delta = model.binary_var_list(O * L * N_select * T, name='delta')
Delta = np.array(Delta, dtype=object).reshape(N_select, T, O, L)
(constraints, vertices, A_unions,
b_unions) = self.compute_obstacle_constraints(
params, ovehicles, X, Delta, Omicron, segments)
model.add_constraints(constraints)
else:
Delta = None
vertices = A_unions = b_unions = None
"""Set up coinciding constraints"""
for t in range(0, self.__n_coincide):
for u1, u2 in util.pairwise(U[:, t]):
model.add_constraints([l == r for (l, r) in zip(u1, u2)])
# TODO: constraints on binary variables actually makes optimization slower. Why?
# if Omicron is not None:
# for o1, o2 in util.pairwise(Omicron[:,:,t]):
# model.add_constraints([l == r for (l, r) in zip(o1, o2)])
# if Delta is not None:
# for d1, d2 in util.pairwise(Delta[:,t]):
# d1 = d1.reshape(-1)
# d2 = d2.reshape(-1)
# model.add_constraints([l == r for (l, r) in zip(d1, d2)])
"""Compute and minimize objective"""
cost = self.compute_mean_objective(X, U, goal)
model.minimize(cost)
# TODO: add warmstarting to MCC
# if self.road_boundary_constraints and self.__U_warmstarting is not None:
# # Warm start inputs if past iteration was run.
# warm_start = model.new_solution()
# for i, u in enumerate(self.__U_warmstarting[self.__step_horizon :]):
# warm_start.add_var_value(f"u_{2*i}", u[0])
# warm_start.add_var_value(f"u_{2*i + 1}", u[1])
# # add omicron_0 as hotfix to MIP warmstart as it needs
# # at least 1 integer value set.
# warm_start.add_var_value("omicron_0", 1)
# model.add_mip_start(
# warm_start, write_level=docplex.mp.constants.WriteLevel.AllVars
# )
# model.print_information()
# model.parameters.read.datacheck = 1
if self.log_cplex:
model.parameters.mip.display = 2
s = model.solve(log_output=True)
else:
model.parameters.mip.display = 0
model.solve()
# model.print_solution()
"""Extract solution"""
try:
cost = cost.solution_value
f = lambda x: x if isinstance(x, numbers.Number
) else x.solution_value
U_star = util.obj_vectorize(f, U)
X_star = util.obj_vectorize(f, X)
return util.AttrDict(cost=cost,
U_star=U_star,
X_star=X_star,
goal=goal,
A_unions=A_unions,
b_unions=b_unions,
vertices=vertices,
segments=segments), None
except docplex.mp.utils.DOcplexException as e:
# TODO: check model for infeasible solution
# docplex.util.environment.logger:environment.py:627 Notify end solve,
# status=JobSolveStatus.INFEASIBLE_SOLUTION, solve_time=None
return util.AttrDict(cost=None,
U_star=None,
X_star=None,
goal=goal,
A_unions=A_unions,
b_unions=b_unions,
vertices=vertices,
segments=segments), InSimulationException(
"Optimizer failed to find a solution")
