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")