def invert(A, order): n_coeff, n_der = utils.n_coeffs_free_ders(order) n_seg = np.shape(A)[0] / 2 / n_der # not square or can't figure out the format if (np.shape(A)[0] != np.shape(A)[1] or n_seg != np.shape(A)[1] / n_coeff or round(n_seg) != n_seg): return np.asmatrix(sp.linalg.pinv(A.todense())) else: blocks = [] # use Schur complement as described in: # Real-Time Visual-Inertial Mapping, Re-localization and Planning # Onboard MAVs in Unknown Environments # TODO([email protected]) - don't loop here # TODO([email protected]) - is there a better way to slice A? for seg in range(n_seg): A_seg = A[seg * n_coeff:(seg + 1) * n_coeff, seg * n_coeff:(seg + 1) * n_coeff].todense() sigma = np.array(A_seg[:n_der, :n_der].diagonal()).squeeze() sigma_inv = np.matrix(np.diag(1 / sigma)) gamma = np.matrix(A_seg[n_der:, :n_der]) delta = A_seg[n_der:, n_der:] # TODO([email protected]) - is this always invertible? delta_inv = np.matrix(np.linalg.inv(delta)) A_seg_inv = np.r_[np.c_[sigma_inv, np.zeros((n_der, n_der))], np.c_[- delta_inv * gamma * sigma_inv, delta_inv]] blocks.append(A_seg_inv) return sp.sparse.block_diag(blocks, 'csr')
def __init__(self, waypoints, order, costs, der_fixed, times=None, der_ineq=None, delta=None, print_output=False, closed_loop=False): if np.shape(waypoints) != np.shape(der_fixed): raise exceptions.InputError( waypoints, "Mismatch between size of waypoints" + " array and size of derivative fixed" + "array") # if der_ineq is None: # no inequalities set der_ineq = np.array(der_fixed) der_ineq[:, :] = False # elif np.shape(der_ineq) != np.shape(der_fixed): # raise exceptions.InputError(der_ineq, # "Mismatch between size of der_ineq array and size of derivative fixed array") # elif (der_ineq[der_ineq]==der_fixed[der_ineq]).any(): # raise exceptions.InputError(der_ineq,"Invalid inequality and fixed input arrays;\n Have conflicting selections of constraints i.e. both are true for the same derivative") # constants self.waypoints = np.array(waypoints).copy() self.order = order self.n_der = utils.n_coeffs_free_ders(order)[1] self.costs = np.array(costs).copy() self.der_fixed = np.array(der_fixed).copy() self.der_ineq = np.array(der_ineq).copy() self.delta = delta self.n_seg = np.shape(waypoints)[1] - 1 self.print_output = print_output self.closed_loop = closed_loop self.opt_time = 0.0 self.Q = None self.M = None self.A = None self.R = None self.pinv_A = None self.free_ders = None self.fixed_ders = None self.piece_poly = None self.coeffs = None # don't need segment times to create the object # allow deferral of joint optimization if times is not None: self.times = np.array(times).copy() self.get_free_ders() self.get_poly_coeffs() self.get_piece_poly()
def sparse_indices(times, order): """Generate the sparse indices for the constraint matrix Richter, Bry, Roy; Polynomial Trajectory Planning for Quadrotor Flight Equations 18 & 20; using joint formulation Args: times: time in which to complete each segment order: the order of the polynomial segments Returns: A tuple with a_0_row, a_0_col, a_t_row, a_t_col, where: a_0_row: 0th derivative row indices a_0_col: 0th derivative column indices a_t_row: higher derivative row indices a_t_col: higher derivative column indices Raises: """ # TODO([email protected]) could reuse this by returning it n_coeff, n_der = utils.n_coeffs_free_ders(order) n_seg = np.size(times) i, j = np.nonzero(np.tri(n_coeff, n_der, dtype=np.int)) a_0_row = np.r_[:n_der * n_seg] + np.repeat(np.r_[:n_seg] * n_der, n_der) if order % 2 == 0: # for even polynomial order, each block is taller by 1 a_0_col = a_0_row + np.repeat(np.r_[:n_seg], n_der) # 2 * n_der is correct here; there are two submatrices of n_der rows each # per segment a_t_row = np.tile(j, (n_seg)) + \ np.repeat(np.r_[:n_seg] * 2 * n_der, i.size) + n_der # n_coeff is correct here; there is one matrix of n_coeff columns per # segment a_t_col = np.tile(i, (n_seg)) + \ np.repeat(np.r_[:n_seg] * n_coeff, i.size) else: a_0_col = a_0_row # no copy is ok because a_0_row is not modified later # n_coeff is correct here; there is one matrix of n_coeff columns per # segment a_t_row = np.tile(j, (n_seg)) + \ np.repeat(np.r_[:n_seg] * n_coeff, i.size) + n_der # 2 * n_der is correct here; there are two submatrices of n_der rows each # per segment a_t_col = np.tile(i, (n_seg)) + \ np.repeat(np.r_[:n_seg] * 2 * n_der, i.size) return (a_0_row, a_0_col, a_t_row, a_t_col)
def delete(Q, delete_index, new_time, costs, order): """ Delete one waypoint with specified index and set duration of joining segment Args: Q: previous block diagonal csc or coo scipy sparse matrix of costs delete_indices: indices of waypoints to delete new_time: duration of segment that joins the waypoints on each side of the deletion Returns: Q: Block diagonal coo scipy sparse matrix for joint optimization Raises: """ if Q.format != 'lil': Q_calc = Q.tolil(copy=True) n_coeff = utils.n_coeffs_free_ders(order)[0] n_seg = Q.shape[0]/n_coeff if delete_index == n_seg: # Change index if the last segment index = delete_index - 1 else: index = delete_index # Compute indices of parts of matrix to keep # start_ind = np.arange(0,index*n_coeff,1) # end_ind = np.arange((index+1)*n_coeff,n_seg*n_coeff,1) # keep_ind = np.concatenate((start_ind,end_ind)) # Remove from delete index Q_new = sp.sparse.lil_matrix((n_coeff*(n_seg-1),n_coeff*(n_seg-1))) if index > 0: Q_new[:index*n_coeff,:index*n_coeff] = Q_calc[:index*n_coeff,:index*n_coeff] if index < n_seg-1: Q_new[index*n_coeff:(n_seg-1)*n_coeff,index*n_coeff:(n_seg-1)*n_coeff] = Q_calc[(index+1)*n_coeff:n_seg*n_coeff,(index+1)*n_coeff:n_seg*n_coeff] Q = Q_new.tocsc().copy() if delete_index != 0 and delete_index != n_seg: # Nothing to update for the first segment or last segment # Update time for adjoining segment update(Q, [index-1], new_time, costs, order) return Q
def insert(A, new_index, new_times, order): """ Insert new waypoints to start at the selected index Args: A: previous block diagonal coo scipy sparse matrix of constraints new_index: index that new waypoints should start at after insertion new_waypoints: numpy array new_times: new_der_fixed: Returns: A: Block diagonal coo scipy sparse matrix for joint optimization Raises: """ if A.format != 'csc': A = A.tocsc(copy=True) # Compute new matrix part A_insert = block_constraint(new_times[1], order).tocsc(copy=True) if A_insert.format != 'csc' or A.format != 'csc': msg = 'Can only column-insert in place with both csc format, not {0} and {1}.' msg = msg.format(A_insert.format, A.format) raise ValueError(msg) n_coeff = utils.n_coeffs_free_ders(order)[0] n_new_seg = np.size(new_times)/2 n_seg = A.shape[0]/n_coeff if new_index > n_seg: index = new_index - 1 else: index = new_index # Construct new matrix A_new = sp.sparse.block_diag((A[:index*n_coeff,:index*n_coeff],A_insert,A[index*n_coeff:,index*n_coeff:])) A = A_new.tocsc(copy=True) # modify connecting segment if new_index != 0 and new_index != n_seg+1: # Nothing to update for the first segment or last segment # Update time for adjoining segment update(A, [new_index-1], new_times[0], order) return A
def insert(Q, new_index, new_times, costs, order): """ Insert one new waypoint to start at the selected index Args: Q: previous block diagonal csc or coo scipy sparse matrix of costs new_index: index that new waypoints should start at after insertion new_waypoints: numpy array new_times: array of size 2 with the times for the two segments on either side of the inserted point Returns: Q: Block diagonal coo scipy sparse matrix for joint optimization Raises: """ if Q.format != 'csc': Q = Q.tocsc(copy=True) # Compute new matrix part Q_insert = block_cost(new_times[1], costs, order).tocsc(copy=True) if Q_insert.format != 'csc' or Q.format != 'csc': msg = 'Can only column-insert in place with both csc format, not {0} and {1}.' msg = msg.format(Q_insert.format, Q.format) raise ValueError(msg) n_coeff = utils.n_coeffs_free_ders(order)[0] n_new_seg = np.size(new_times)/2 n_seg = Q.shape[0]/n_coeff if new_index > n_seg: index = new_index - 1 else: index = new_index # Construct new matrix Q_new = sp.sparse.block_diag((Q[:index*n_coeff,:index*n_coeff],Q_insert,Q[index*n_coeff:,index*n_coeff:])) Q = Q_new.tocsc(copy=True) # modify connecting segment if new_index != 0 and new_index != n_seg+1: # Nothing to update for the first segment or last segment # Update time for adjoining segment update(Q, [new_index-1], new_times[0], costs, order) return Q
def sparse_values(times, order): """Generate the sparse values for the constraint matrix Richter, Bry, Roy; Polynomial Trajectory Planning for Quadrotor Flight Equations 18 & 20; using joint formulation Args: times: time in which to complete each segment order: the order of the polynomial segments Returns: A tuple with a_0_val, a_t_val, where: a_0_val: 0th derivative values a_t_val: higher derivative values Raises: """ # TODO([email protected]) could reuse this by returning it n_coeff, n_der = utils.n_coeffs_free_ders(order) n_seg = np.size(times) n_minus_r, prod = utils.poly_der_coeffs(order) # raise segment times to n_minus_r power t_power = np.reshape(times, (n_seg, 1, 1)) ** n_minus_r a_0_val = np.tile(np.diag(prod), (n_seg)) # equation 18 a_t = np.tile(prod, (n_seg, 1, 1)) * t_power # equation 20 # unlike MATLAB, have to convert to linear sequence, index, then convert back # which values are below the diagonal? a_t_sel = np.squeeze(np.tile( [np.tri(n_coeff, n_der, dtype=np.int)], (n_seg, 1, 1)).reshape((1, 1, -1))) # transpose a_t and a_t_sel? # use triu_indices instead? # get the indices for all values a_t_idx = np.r_[:n_coeff * n_der * n_seg] a_t = np.squeeze(a_t.reshape((1, 1, -1))) # get only the values below the diagonal a_t_val = a_t[a_t_idx[a_t_sel > 0]] return (a_0_val, a_t_val)
def update(Q, new_indices, new_times, costs, order): #TODO([email protected]) - make this an Object to store costs & order? n_coeff = utils.n_coeffs_free_ders(order)[0] n_seg = np.size(new_times) Q_update = block_cost(new_times, costs, order).tocsc(copy=True) if Q_update.format != 'csc' or Q.format != 'csc': msg = 'Can only column-update in place with both csc format, not {0} and {1}.' msg = msg.format(Q_update.format, Q.format) raise ValueError(msg) # replace the appropriate sections of Q with sections of Q_update for i in range(0, len(new_indices)): # the slow way: Q[new_indices[i] * n_coeff:(new_indices[i] + 1) * n_coeff, new_indices[i] * n_coeff:(new_indices[i] + 1) * n_coeff] = Q_update[i * n_coeff:(i + 1) * n_coeff, i * n_coeff:(i + 1) * n_coeff]
def update(A, new_indices, new_times, order): #TODO([email protected]) - make this an Object to store order? n_coeff, n_der = utils.n_coeffs_free_ders(order) n_seg = np.size(new_times) A_update = block_constraint(new_times, order).tocsc(copy=True) if A_update.format != 'csc' or A.format != 'csc': msg = 'Can only column-update in place with both csc format, not {0} and {1}.' msg = msg.format(A_update.format, A.format) raise ValueError(msg) # replace the appropriate sections of A with sections of A_update for i in range(0, len(new_indices)): # the slow way: A[new_indices[i] * 2 * n_der:(new_indices[i] + 1) * 2 * n_der, new_indices[i] * 2 * n_der:(new_indices[i] + 1) * 2 * n_der] = A_update[i * 2 * n_der:(i + 1) * 2 * n_der, i * 2 * n_der:(i + 1) * 2 * n_der]
def block_cost(times, costs, order): """Generate the cost matrix for each segment. Richter, Bry, Roy; Polynomial Trajectory Planning for Quadrotor Flight Equations 10-15; using joint formulation Args: times: time in which to complete each segment cost: weight in the sum of Hessian matrices (Equation 15) order: the order of the polynomial segments Returns: Block diagonal coo scipy sparse matrix for joint optimization before summing across cost terms (sum happens upon matrix conversion to CSR or CSC format) Raises: """ n_coeff = utils.n_coeffs_free_ders(order)[0] n_seg = np.size(times) block_cost_sparse = block_cost_per_order(times, costs, order) # Weighted sum of Hessian cost for each order # derivatives from r = 0 to polyOrder where cost is not zero # numpy.nonzero returns a tuple; one element for each dimension of input # array der = np.nonzero(costs)[0] costs = np.array(costs)[der] # TODO([email protected]) duplicate entries are not summed until # converted to CSR/CSC - is this ok? return sp.sparse.coo_matrix((costs[block_cost_sparse[0]] * block_cost_sparse[3], (block_cost_sparse[1], block_cost_sparse[2])), shape=(n_coeff * n_seg, n_coeff * n_seg))
def block_constraint(times, order): """Generate the constraint matrix for beginning and end of each segment. Maps between polynomial coefficients and derivatives evaluated at beginning and end of segments. Bry, Richter, Bachrach and Roy; Agressive Flight of Fixed-Wing and Quadrotor Aircraft in Dense Indoor Environments Equations 116 - 122; using joint formulation Continuity A is from eqn 12 Boundary Conditions A is from eqn 13 Args: times: time in which to complete each segment order: the order of the polynomial segments Returns: A: Block diagonal coo scipy sparse matrix for joint optimization Raises: """ n_coeff, n_der = utils.n_coeffs_free_ders(order) n_seg = np.size(times) a_0_val, a_t_val = sparse_values(times, order) (a_0_row, a_0_col, a_t_row, a_t_col) = sparse_indices(times, order) A = sp.sparse.coo_matrix((np.concatenate([a_0_val, a_t_val]), (np.concatenate([a_0_row, a_t_row]), np.concatenate([a_0_col, a_t_col]))), shape=(2 * n_der * n_seg, n_coeff * n_seg)) # Make the matrix sparse in format A = sp.sparse.coo_matrix(A) return A
def get_free_ders(self, waypoints_changed=None, der_fixed_changed=None, times_changed=None): """Solve for free derivatives of polynomial segments Richter, Bry, Roy; Polynomial Trajectory Planning for Quadrotor Flight Equation 31 Uses: self. waypoints: Numpy array of the waypoints (including derivatives) times: time in which to complete each segment (not the transition times) cost: weight in the sum of Hessian matrices (Equation 15) order: the order of the polynomial segments der_fixed: Boolean array of n_der (max potential number of free derivatives per segment) x n_seg + 1 (one column per waypoint). Fixing a derivative at a waypoint is performed by setting the corresponding entry to True. # TODO([email protected]) - decide how many of these fields to store vs # recompute Modifies: self. free_ders: Numpy matrix (column vector) of the free derivatives fixed_ders: Numpy matrix (column vector) of the fixed derivatives Q: the cost matrix in terms of polynomial coefficients M: the selector matrix mapping the ordering of derivatives to/from the form where free and fixed derivatives are partitioned A: the constraint matrix for segment boundaries pinv_A: the (pseudo-)inverse of A R: the cost matrix in terms of the free derivatives Args: Returns: Raises: minsnap.exceptions.InputError if np.size(times) != np.shape(der_fixed)[1] - 1 """ # def get_free_ders_setup_matrices(self, waypoints_changed=None, # der_fixed_changed=None, # times_changed=None): start_timer = time.time() if times_changed is None: times_changed = np.r_[0:self.n_seg] if der_fixed_changed is None: der_fixed_changed = np.r_[0:self.n_seg + 1] if waypoints_changed is None: waypoints_changed = np.r_[0:self.n_seg + 1] waypoints = self.waypoints times = self.times costs = self.costs order = self.order der_fixed = self.der_fixed der_ineq = self.der_ineq delta = self.delta n_seg = self.n_seg n_der = utils.n_coeffs_free_ders(order)[1] n_ineq = sum(sum(der_ineq)) if np.size(times) != np.shape(der_fixed)[1] - 1: raise exceptions.InputError( times, "Mismatch between number of segment times" + " and number of segments") if np.shape(waypoints) != np.shape(der_fixed): raise exceptions.InputError( waypoints, "Mismatch between size of waypoints" + " array and size of derivative fixed" + "array") waypoints = np.matrix(waypoints) if n_ineq > 0: #TODO([email protected] & [email protected]) - investigate other ways to prevent singular matrices here limit = 0.03 if sum(np.array(times) < limit) > 0: print( "Warning: changing times because a lower limit has been reached" ) temp = np.array(times) temp[temp < limit] = limit times = np.array(temp) self.times = times # See README.md on MATLAB vs Octave vs Numpy linear equation system solving # Porting this code: R{i} = M{i} / A{i}' * Q{i} / A{i} * M{i}'; # With even polynomial order, the odd number of coefficients is not equal # to twice the number of free derivatives (floor of (odd # / 2)). # Therefore, A is not square. # Based on some quick checks, the inverse of A is still quite sparse, # especially when only the 0th derivative is fixed at internal # waypoints. # convert COO sparse output to CSC for fast matrix arithmetic if np.size(times_changed) or self.Q is None or self.A is None: if (self.Q is None or self.A is None): #or np.size(times_changed) > 1): Q = cost.block_cost(times, costs, order).tocsc(copy=True) A = constraint.block_constraint(times, order).tocsc(copy=True) else: # if we know which segment times have changed, only update # the corresponding blocks in the block matrix cost.update(self.Q, times_changed, times[times_changed], costs, order) constraint.update(self.A, times_changed, times[times_changed], order) Q = self.Q A = self.A if (A.shape[0] == A.shape[1]): pinv_A = sp.sparse.linalg.inv(A) # TODO([email protected]) - could this be made to outperform the line above? #pinv_A = constraint.invert(A, order) else: # is there some way to keep this sparse? sparse SVD? pinv_A = np.asmatrix(sp.linalg.pinv(A.todense())) else: # TODO([email protected]) - is this the cleanest way? Q = self.Q A = self.A pinv_A = self.pinv_A if np.size(der_fixed_changed) or self.M is None: M = selector.block_selector( der_fixed, closed_loop=self.closed_loop).tocsc(copy=True) else: # TODO([email protected]) - is this the cleanest way? M = self.M if np.size(times_changed) or np.size( der_fixed_changed) or self.R is None: # all are matrices; OK to use * for multiply # R{i} = M{i} * pinv(full(A{i}))' * Q{i} * pinv(full(A{i})) * M{i}'; R = M * pinv_A.T * Q * pinv_A * M.T # partition R by slicing along columns, converting to csr, then slicing # along rows if self.closed_loop: num_der_fixed = np.sum(der_fixed[:, :-1]) else: num_der_fixed = np.sum(der_fixed) # Equation 29 try: R_free = R[:, num_der_fixed:].tocsr() except AttributeError: # oops - we are a numpy matrix R_free = R[:, num_der_fixed:] R_fixed_free = R_free[:num_der_fixed, :] R_free_free = R_free[num_der_fixed:, :] else: R = self.R if hasattr(self, 'R_fixed_free'): R_fixed_free = self.R_fixed_free R_free_free = self.R_free_free else: # partition R by slicing along columns, converting to csr, then slicing # along rows if self.closed_loop: num_der_fixed = np.sum(der_fixed[:, :-1]) else: num_der_fixed = np.sum(der_fixed) # Equation 29 try: R_free = R[:, num_der_fixed:].tocsr() except AttributeError: # oops - we are a numpy matrix R_free = R[:, num_der_fixed:] R_fixed_free = R_free[:num_der_fixed, :] R_free_free = R_free[num_der_fixed:, :] # TODO([email protected]) - transpose waypoints and der_fixed at input if not self.closed_loop: fixed_ders = waypoints.T[np.nonzero(der_fixed.T)].T # Equation 31 else: fixed_ders = waypoints[:, :-1].T[np.nonzero( der_fixed[:, :-1].T)].T # Equation 31 """ Solve """ # the fixed derivatives, D_F in the paper, are just the waypoints for which # der_fixed is true # DP{i} = -RPP \ RFP' * DF{i}; if n_ineq == 0: # Run for unconstrained case: # Solve for the free derivatives # Solve the unconstrained system free_ders = np.asmatrix( sp.sparse.linalg.spsolve(R_free_free, -R_fixed_free.T * fixed_ders)).T # Run for Constrained cases elif n_ineq > 0: # If there are inequality constraints # Inequalities # Isolate the waypoints for which there is an inequality ineq_way_p = waypoints.T[np.nonzero(der_ineq.T)].T if type(delta) != float: # Take out the delta for the inequality constrained parts ineq_delta = delta.T[np.nonzero(der_ineq.T)].reshape( [ineq_way_p.shape[0], 1]) else: # Just a constant ineq_delta = delta W = np.concatenate(( (ineq_way_p - ineq_delta), (-ineq_way_p - ineq_delta), ), axis=0) # Selector matix if np.size(der_fixed_changed) or not hasattr(self, 'E'): E = constraint.block_ineq_constraint(der_fixed, der_ineq).tocsc(copy=True) elif self.E is None: E = constraint.block_ineq_constraint(der_fixed, der_ineq).tocsc(copy=True) else: E = self.E ### Solve with CVXOPT # Setup matrices P = matrix(2 * R_free_free.toarray(), tc='d') q_vec = matrix(np.array(2 * fixed_ders.T * R_fixed_free).T, tc='d') G = matrix(-E.toarray().astype(np.double), tc='d') h_vec = matrix(np.array(-W), tc='d') # Checks on the problem setup if np.linalg.matrix_rank(np.concatenate((P, G))) < P.size[0]: print('Warning: rank of [P;G] {} is less than size of P: {}'. format(np.linalg.matrix_rank(np.concatenate((P, G))), P.size[0])) else: if self.print_output: print( 'Rank of A is {} size is {}\nRank for Q is {}, size is {}' .format(np.linalg.matrix_rank(A.toarray()), A.shape, np.linalg.matrix_rank(Q.toarray()), Q.shape)) print('Rank of R is {}, size is {}'.format( np.linalg.matrix_rank(R.toarray()), R.shape)) print( 'Rank P is {}\nRank of G is: {}\nRank of [P;G] {} size of P: {}\n' .format(np.linalg.matrix_rank(P), np.linalg.matrix_rank(G), np.linalg.matrix_rank(np.concatenate((P, G))), P.size[0])) # To suppress output solvers.options['show_progress'] = False # Run cvxopt solver sol = solvers.qp(P, q_vec, G, h_vec) #,initvals=primalstart) # Solution free_ders = np.matrix(sol['x']) if self.print_output: print('cvx solution is:\n{}'.format(free_ders)) print('cvx cost is:\n{}'.format(sol['primal objective'])) print('Constraints are: \n{}'.format(E * free_ders - W)) # self.fixed_terms = fixed_terms self.opt_time = time.time() - start_timer self.free_ders = free_ders self.fixed_ders = fixed_ders self.Q = Q self.M = M self.A = A self.pinv_A = pinv_A self.R = R self.R_fixed_free = R_fixed_free self.R_free_free = R_free_free
def main(): costs = [0, 0, 0, 0, 1] order = max(np.nonzero(costs)[0]) * 2 - 1 n_der = utils.n_coeffs_free_ders(order)[1] num_internal = 1 #98 # float derivatives at internal waypoints and fix at beginning and end inner = [[True] + [False] * num_internal + [True]] * (n_der - 1) # fix 0th derivative at internal waypoints der_fixed = [[True] * (num_internal + 2)] der_fixed.extend(inner) # waypoints = [range(num_internal + 2)] waypoints = [[0, 1, 0]] waypoints.extend([[0] * (num_internal + 2)] * (n_der - 1)) times = [1] * (num_internal + 1) print("\nTrajectory for:\nWaypoints:\n") pp(np.array(waypoints)) print("\nSegment times:\n") pp(np.array(times)) print("\ncost {}; poly order {};\n".format(costs, order)) result = PolyTraj(waypoints, order, costs, der_fixed, times, closed_loop=True) print(result.free_ders) if not result.check_continuity(): print("\nFailed continuity check") costs = [0, 0, 0, 0, 1] num_internal = 0 # give the optimization enough free derivatives order = max(np.nonzero(costs)[0] + 1) * 2 - 1 n_der = utils.n_coeffs_free_ders(order)[1] print(n_der) num_internal = 4 # float derivatives at internal waypoints and fix at beginning and end inner = [[True] + [False] * num_internal + [True]] * (n_der - 1) # fix 0th derivative at internal waypoints der_fixed = [[True] * (num_internal + 2)] der_fixed.extend(inner) print(der_fixed) waypoints = [range(num_internal + 2)] waypoints.extend([[0] * (num_internal + 2)] * (n_der - 1)) times = [0.2] * (num_internal + 1) print("\nTrajectory for:\nWaypoints:\n") pp(np.array(waypoints)) print("\nSegment times:\n") pp(np.array(times)) print("\ncost {}; poly order {};\n".format(costs, order)) result = PolyTraj(waypoints, order, costs, der_fixed, times) print(result.free_ders) # CHANGE FOR INEQUALITY CONSTRAINTS delta = 0.2 der_ineq = np.array(der_fixed) der_ineq[:, [0, -1]] = False # der_ineq[:,:] = False der_fixed = np.array(der_fixed) der_fixed[0, 1:-1] = False print("fixed mat = \n{}\n".format(der_fixed)) print("ineq mat = \n{}\n".format(der_ineq)) result2 = PolyTraj(waypoints, order, costs, der_fixed, times, der_ineq, delta) result2.update_times(times) # print(result.free_ders) # costs = [0, 0, 0, 0, 1] # num_internal = 0 # # # give the optimization enough free derivatives # order = max(np.nonzero(costs)[0] + 1) * 2 - 1 # n_der = utils.n_coeffs_free_ders(order)[1] # # # float derivatives at internal waypoints and fix at beginning and end # inner = [[True] + [False] * num_internal + [True]] * (n_der - 1) # # fix 0th derivative at internal waypoints # der_fixed = [[True] * (num_internal + 2)] # der_fixed.extend(inner) # # waypoints = [range(num_internal + 2)] # waypoints.extend([[0] * (num_internal + 2)] * (n_der - 1)) # # times = [1] * (num_internal + 1) # print("\nTrajectory for:\nWaypoints:\n") # pp(np.array(waypoints)) # print("\nSegment times:\n") # pp(np.array(times)) # print("\ncost {}; poly order {};\n".format(costs, order)) # # result = PolyTraj(waypoints, order, costs, der_fixed) # result.update_times(times) # print(result.free_ders) # Test new functions costs = [0, 0, 0, 0, 1] order = max(np.nonzero(costs)[0]) * 2 - 1 n_der = utils.n_coeffs_free_ders(order)[1] num_internal = 3 # float derivatives at internal waypoints and fix at beginning and end inner = [[True] + [False] * num_internal + [True]] * (n_der - 1) # fix 0th derivative at internal waypoints der_fixed = [[True] * (num_internal + 2)] der_fixed.extend(inner) waypoints = np.zeros([n_der, num_internal + 2]) waypoints[0, :] = np.arange(0, num_internal + 2, 1.0) times = [1.0] * (num_internal + 1) pol = PolyTraj(waypoints, order, costs, der_fixed, times) new_index = waypoints.shape[1] - 1 new_waypoint = pol.waypoints[:, new_index].copy() new_waypoint[0] = 1.5 new_times = [result.times[new_index] / 2, result.times[new_index] / 2] new_der_fixed = pol.der_fixed[:, new_index].copy() a1 = pol.free_ders.copy() pol.insert(new_index + 1, new_waypoint, new_times, new_der_fixed, defer=False) a2 = pol.free_ders.copy() pol.delete(new_index + 1, 1.0, defer=False) a3 = pol.free_ders.copy() import pdb pdb.set_trace() costs = [0, 0, 0, 1] order = max(np.nonzero(costs)[0]) * 2 - 1 waypoints_n = np.zeros((3, 5)) waypoints_n[0, :] = [0, 1.0, 2.0, 3.0, 0] der_fixed_n = np.zeros(waypoints_n.shape, dtype=bool) # der_fixed_n[:,[0,2]] = True der_fixed_n[0, :] = True times = np.ones(waypoints_n.shape[1] - 1) pol = PolyTraj(waypoints_n, order, costs, der_fixed_n, times, closed_loop=True) n_laps = 2 entry_ID = 3 exit_ID = 1 import pdb pdb.set_trace() out_pol = pol.create_n_laps(n_laps, entry_ID, exit_ID) import pdb pdb.set_trace() if not result.check_continuity(): print("\nFailed continuity check")
def test_n_coeffs_free_ders_odd(self): """Test # coeffs and free ders with odd order""" np.testing.assert_equal(utils.n_coeffs_free_ders(9), (10, 5))
def test_n_coeffs_free_ders_even(self): """Test # coeffs and free ders with even order""" np.testing.assert_equal(utils.n_coeffs_free_ders(10), (11, 5))
def test_n_coeffs_free_ders_negative(self): np.testing.assert_equal(utils.n_coeffs_free_ders(self.negative_order), (0, 0))
def block_cost_per_order(times, costs, order, return_all=False): """Generate the sparse indices and values for all cost orders Richter, Bry, Roy; Polynomial Trajectory Planning for Quadrotor Flight Equations 10-15; using joint formulation This is broken out as a separate function for testing purposes. Args: times: time in which to complete each segment cost: weight in the sum of Hessian matrices (Equation 15) order: the order of the polynomial segments return_all: Whether to return all cost terms even if only some are nonzero Returns: A tuple, (i, j, k, val) where i is the index of the cost order, j is the row, k is the column, and val is the appropriate Hessian value Raises: """ n_coeff = utils.n_coeffs_free_ders(order)[0] n_seg = np.size(times) # debugging feature that returns the Hessian for each derivative order if return_all: der = np.ones(n_coeff) # derivatives from r = 0 to polyOrder where cost is not zero # numpy.nonzero returns a tuple; one element for each dimension of input # array der = np.nonzero(costs)[0] # changed dimension order here with respect to MATLAB scripts # due to numpy native print order der_outer_prods = np.zeros((np.size(der), n_coeff, n_coeff)) t_power = np.zeros(np.shape(der_outer_prods)) for i in range(np.size(der)): der_order = der[i] if der_order > 0: der_outer_prods[i, :, :] = utils.poly_hessian_coeffs(order, der_order)[1] else: # empty product because r=0-1 is less than m=0 der_outer_prods[i, :, :] = np.ones((n_coeff, n_coeff)) [row, col] = np.mgrid[:n_coeff, :n_coeff] # coefficients of terms according to their order; t is raised to this # power, and each term is divided by this value as well t_power[i, :, :] = row + col + np.ones(np.shape(row)) * \ (-2 * der_order + 1) # See Equation 14 - strictly greater than 0 gt_zero_idx = np.nonzero(t_power > 0) # this works for a single cost term and for multiple cost terms cost_per_order = 2 * np.expand_dims(der_outer_prods[gt_zero_idx], axis=1) \ * times ** np.expand_dims(t_power[gt_zero_idx], axis=1) \ / np.expand_dims(t_power[gt_zero_idx], axis=1) # first list in tuple determines index for 3rd dimension of sparse matrix, # which gives the derivative order that the cost was calculated for # index in cost_per_order corresponding to the length of the segment times # list tells which block in sparse matrix to insert into # need to duplicate gt_zero_idx and offset to the appropriate block tiled_idx = np.tile(gt_zero_idx, (1, np.size(times))) tiled_idx[1:, np.size(gt_zero_idx[0]):] \ += np.tile(n_coeff * np.r_[1:np.size(times)], (np.size(gt_zero_idx[0]), 1)).T.flatten() return (tiled_idx[0], tiled_idx[1], tiled_idx[2], cost_per_order.T.flatten())
def test_n_coeffs_free_ders_float(self): np.testing.assert_equal(utils.n_coeffs_free_ders(self.float_order), (10, 5))