def test_insert_then_delete(self):
costs = [0, 0, 0, 1, 0]
times = [2, 2, 2, 2, 2]
order = 9
expect = cost.block_cost(times, costs, order).tocsc(copy=True)
result1 = cost.insert(expect, 3, [2, 2], costs, order)
result = cost.delete(result1, 3, 2, costs, order)
np.testing.assert_allclose(result.todense(), expect.todense())
def test_delete_multi_seg(self):
costs = [1, 0, 0, 0, 0]
times = [2, 2, 2]
order = 3
E1 = np.array([[6.0, 9.0, 18.0, 40.5], [9., 18., 40.5, 97.2],
[18., 40.5, 97.2, 243.],
[40.5, 97.2, 243., 624.85714286]])
E2 = np.array([[4., 4., 5.33333333, 8.], [4., 5.33333333, 8., 12.8],
[5.33333333, 8., 12.8, 21.33333333],
[8., 12.8, 21.33333333, 36.57142857]])
expect1 = sp.linalg.block_diag(E1, E2)
expect2 = sp.linalg.block_diag(E2, E1)
result = cost.block_cost(times, costs, order).tocsc(copy=True)
result1 = cost.delete(result, 1, 3, costs, order).copy()
result2 = cost.delete(result, 2, 3, costs, order).copy()
np.testing.assert_allclose(result1.todense(), expect1)
np.testing.assert_allclose(result2.todense(), expect2)
def test_delete_2_seg(self):
costs = [1, 0, 0, 0, 0]
times = [3, 2]
order = 3
expect1 = [[6.0, 9.0, 18.0, 40.5], [9., 18., 40.5, 97.2],
[18., 40.5, 97.2, 243.], [40.5, 97.2, 243., 624.85714286]]
expect2 = [[4., 4., 5.33333333, 8.], [4., 5.33333333, 8., 12.8],
[5.33333333, 8., 12.8, 21.33333333],
[8., 12.8, 21.33333333, 36.57142857]]
result = cost.block_cost(times, costs, order).tocsc(copy=True)
result1 = cost.delete(result, 2, 3, costs, order).copy()
result2 = cost.delete(result, 1, 3, costs, order).copy()
result3 = cost.delete(result, 0, 2, costs, order).copy()
np.testing.assert_allclose(result1.todense(), expect1)
np.testing.assert_allclose(result2.todense(), expect1)
np.testing.assert_allclose(result3.todense(), expect2)
def test_block_cost_odd_order(self):
times = 3
order = 9
costs = [0, 0, 0, 1]
expect = [[0, 0, 0, 0, 0, 0, 0, 0, 0,
0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 216, 1296, 6480, 29160, 122472, 489888, 1889568],
[
0, 0, 0, 1296, 10368, 58320, 279936, 1224720, 5038848,
19840464
],
[
0, 0, 0, 6480, 58320, 349920, 1749600, 7873200, 33067440,
132269760
],
[
0, 0, 0, 29160, 279936, 1749600, 8997942.85714286,
41334300, 176359680, 714256704
],
[
0, 0, 0, 122472, 1224720, 7873200, 41334300, 192893400,
833299488, 3408952450.90909
],
[
0, 0, 0, 489888, 5038848, 33067440, 176359680, 833299488,
3636215947.63636, 14999390784
],
[
0, 0, 0, 1889568, 19840464, 132269760, 714256704,
3408952450.90909, 14999390784, 62305161718.1538
]]
# Compare result and expected value
result = cost.block_cost(times, costs, order)
np.testing.assert_allclose(result.todense(), expect)
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
