def test_insert_2_seg(self):
costs = [1, 0, 0, 0, 0]
times = [2,2]
order = 3
E1 = np.array([[ 1., 0., 0., 0.],
[ 0., 1., 0., 0.],
[ 1., 3., 9., 27.],
[ 0., 1., 6., 27.]])
E2 = np.array([[ 1, 0, 0, 0],
[ 0, 1, 0, 0],
[ 1, 2, 4, 8],
[ 0, 1, 4, 12]])
expect0 = sp.linalg.block_diag(E1,E2,E2)
expect1 = sp.linalg.block_diag(E1,E1,E2)
expect2 = sp.linalg.block_diag(E2,E1,E1)
expect3 = sp.linalg.block_diag(E2,E2,E1)
result = constraint.block_constraint(times, order).tocsc(copy=True)
result0 = constraint.insert(result,0,[3,3], order).copy()
result1 = constraint.insert(result,1,[3,3], order).copy()
result2 = constraint.insert(result,2,[3,3], order).copy()
result3 = constraint.insert(result,3,[3,3], order).copy()
np.testing.assert_allclose(result0.todense(), expect0)
np.testing.assert_allclose(result1.todense(), expect1)
np.testing.assert_allclose(result2.todense(), expect2)
np.testing.assert_allclose(result3.todense(), expect3)
def test_insert_then_delete(self):
costs = [0, 0, 0, 1, 0]
times = [2,2,2,2,2]
order = 9
expect = constraint.block_constraint(times, order).tocsc(copy=True)
result1 = constraint.insert(expect,3,[2,2], order)
result = constraint.delete(result1,3,2, order)
np.testing.assert_allclose(result.todense(), expect.todense())
def test_block_constraint_even_order(self):
times = 3.5
order = 4
expect = [[1.0, 0.0, 0.00, 0.000, 0.0000],
[0.0, 1.0, 0.00, 0.000, 0.0000],
[1.0, 3.5, 12.25, 42.875, 150.0625],
[0.0, 1.0, 7.00, 36.750, 171.5000]]
# Compare result and expected value
result = constraint.block_constraint(times, order)
np.testing.assert_allclose(result.todense(), expect)
def test_block_constraint_even_order(self):
times = [2.9, 1.2]
order = 4
expect = [[1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[1, 2.9, 8.41, 24.389, 70.7281, 0, 0, 0, 0, 0],
[0, 1, 5.8, 25.23, 97.556, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 1.2, 1.44, 1.728, 2.0736],
[0, 0, 0, 0, 0, 0, 1, 2.4, 4.32, 6.912]]
# Compare result and expected value
result = constraint.block_constraint(times, order)
np.testing.assert_allclose(result.todense(), expect)
def test_block_constraint_even_order(self):
times = 2
order = 8
expect = [[1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 2, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 6, 0, 0, 0, 0, 0],
[1, 2, 4, 8, 16, 32, 64, 128, 256],
[0, 1, 4, 12, 32, 80, 192, 448, 1024],
[0, 0, 2, 12, 48, 160, 480, 1344, 3584],
[0, 0, 0, 6, 48, 240, 960, 3360, 10752]]
# Compare result and expected value
result = constraint.block_constraint(times, order)
np.testing.assert_array_equal(result.todense(), expect)
def test_block_constraint_odd_order(self):
times = 3
order = 9
expect = [[1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 2, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 6, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 24, 0, 0, 0, 0, 0],
[1, 3, 9, 27, 81, 243, 729, 2187, 6561, 19683],
[0, 1, 6, 27, 108, 405, 1458, 5103, 17496, 59049],
[0, 0, 2, 18, 108, 540, 2430, 10206, 40824, 157464],
[0, 0, 0, 6, 72, 540, 3240, 17010, 81648, 367416],
[0, 0, 0, 0, 24, 360, 3240, 22680, 136080, 734832]]
# Compare result and expected value
result = constraint.block_constraint(times, order)
np.testing.assert_array_equal(result.todense(), expect)
def test_delete_2_seg(self):
costs = [1, 0, 0, 0, 0]
times = [3,2]
order = 3
E1 = np.array([[ 1., 0., 0., 0.],
[ 0., 1., 0., 0.],
[ 1., 3., 9., 27.],
[ 0., 1., 6., 27.]])
E2 = np.array([[ 1, 0, 0, 0],
[ 0, 1, 0, 0],
[ 1, 2, 4, 8],
[ 0, 1, 4, 12]])
result = constraint.block_constraint(times, order).tocsc(copy=True)
result1 = constraint.delete(result,2,3, order).copy()
result2 = constraint.delete(result,1,3, order).copy()
result3 = constraint.delete(result,0,2, order).copy()
np.testing.assert_allclose(result1.todense(), E1)
np.testing.assert_allclose(result2.todense(), E1)
np.testing.assert_allclose(result3.todense(), E2)
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