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 delete(A, delete_index, new_time, order):
"""
Delete waypoints with specified indices and set duration of joining segment
Args:
A: previous block diagonal coo scipy sparse matrix of constraints
delete_index: indices of waypoints to delete
new_time: duration of segment that joins the waypoints on each side
of the deletion
Returns:
A: Block diagonal coo scipy sparse matrix for joint optimization
Raises:
"""
if A.format != 'lil':
A_calc = A.tolil(copy=True)
n_coeff = utils.n_coeffs_free_ders(order)[0]
n_seg = A.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
A_new = sp.sparse.lil_matrix(
(n_coeff * (n_seg - 1), n_coeff * (n_seg - 1)))
if index > 0:
A_new[:index * n_coeff, :index *
n_coeff] = A_calc[:index * n_coeff, :index * n_coeff]
if index < n_seg - 1:
A_new[index * n_coeff:(n_seg - 1) * n_coeff,
index * n_coeff:(n_seg - 1) *
n_coeff] = A_calc[(index + 1) * n_coeff:n_seg * n_coeff,
(index + 1) * n_coeff:n_seg * n_coeff]
A = A_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(A, [index - 1], new_time, order)
return A
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 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_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 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_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 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:
quest.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")
