def update_waypoint(self,
index,
new_waypoint,
new_der_fixed=None,
defer=False):
"""Set new waypoint value and whether derivatives are fixed
Uses:
Modifies:
self.
waypoints:
der_fixed:
Args:
new_waypoint: array of waypoint derivatives with length equal to
n_der, the number of free derivatives given the polynomial order
new_der_fixed: Boolean array of n_der (max potential number of free
derivatives per segment) x 1 (one column per waypoint).
Fixing a derivative at a waypoint is performed by setting the
corresponding entry to True.
Returns:
Raises:
minsnap.exceptions.InputError if
np.size(waypoint) != np.shape(self.waypoints)[1]
minsnap.exceptions.InputError if
np.size(der_fixed) != np.shape(self.der_fixed)[1]
"""
if new_der_fixed is not None:
if np.size(new_der_fixed) != np.shape(self.der_fixed)[0]:
msg = "Mismatch between length of new derivative fixed array "
msg = msg + "and size of 2nd dimension of stored derivative "
msg = msg + "fixed array"
raise exceptions.InputError(new_der_fixed, msg)
der_fixed_changed = np.array(index).copy()
else:
der_fixed_changed = np.array([])
if np.size(new_waypoint) != np.shape(self.der_fixed)[0]:
msg = "Mismatch between length of new waypoint array and size of"
msg = msg + "2nd dimension of stored derivative fixed array"
raise exceptions.InputError(new_waypoint, msg)
if np.size(der_fixed_changed):
self.der_fixed[:, index] = np.array(new_der_fixed).copy()
self.waypoints[:, index] = np.array(new_waypoint).copy()
if not defer:
self.get_free_ders(waypoints_changed=np.array(index),
der_fixed_changed=der_fixed_changed,
times_changed=np.array([]))
self.get_poly_coeffs()
self.get_piece_poly()
def update_waypoints(self, new_waypoints, new_der_fixed=None, defer=False):
"""Set new waypoint values and whether derivatives are fixed
Uses:
Modifies:
self.
waypoints:
der_fixed:
Args:
new_waypoints: full array of waypoints and derivatives with the same
size as the internal der_fixed array
new_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.
Returns:
Raises:
minsnap.exceptions.InputError if
np.shape(waypoints) != np.shape(self.der_fixed)
"""
if new_der_fixed is not None:
if np.shape(new_der_fixed) != np.shape(self.der_fixed):
raise exceptions.InputError(
new_der_fixed,
"Mismatch between shape of new derivative fixed" +
" array and shape of stored derivative fixed" + "array")
else:
# this is for convenience only (we are not updating der_fixed)
new_der_fixed = self.der_fixed
if np.shape(new_waypoints) != np.shape(new_der_fixed):
raise exceptions.InputError(
new_waypoints, "Mismatch between shape of new waypoints" +
" array and shape of new derivative fixed" + "array")
self.der_fixed = np.array(new_der_fixed).copy()
self.waypoints = np.array(new_waypoints).copy()
if not defer:
# update all data structures
self.get_free_ders()
self.get_poly_coeffs()
self.get_piece_poly()
def get_seed_times(poses, avg_vel_des):
"""
Get the seed times for a given set of distances between points
Args:
poses: a dictionary with keys x, y, z and values that are lists
for each coordinate
avg_vel_des: the desired average velocity
Returns:
seed_times: the seed time for each segment before relative optimization
Raises:
minsnap.exceptions.InputError if the input values are of incorrect
sizes or shapes
"""
for key in poses.keys():
if len(np.shape(poses[key])) != 1:
raise exceptions.InputError(
poses[key], "Only pass the 0th derivatives to" +
" minsnap.utils.get_seed_times" +
" as an array with 1 dimension")
diff_norms = np.sqrt(
np.diff(poses['x'])**2.0 + np.diff(poses['y'])**2.0 +
np.diff(poses['z'])**2.0)
return diff_norms / avg_vel_des
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 dup_internal(to_dup, axis=0):
"""
Duplicate all internal elements of the desired axis
If to_dup is one dimensional, add a dimension so that the desired axis
can be duplicated along.
Args:
to_dup: numpy array to duplicate
Eg: [[0, 1, 2, 3],
[4, 5, 6, 7]]
axis: axis to duplicate internal elements along
Returns:
A numpy array with every internal column duplicated.
Eg: [[0, 1, 1, 2, 2, 3],
[4, 5, 5, 6, 6, 7]]
Raises:
minsnap.exceptions.InputError if to_dup has more than two dimensions
"""
# to_dup should already be a numpy array, but make it one anyway
to_dup = np.array(to_dup)
# TODO([email protected]) - is this the desired way to handle these two
# cases?
# 1d array can be saved by adding dimension
if len(np.shape(to_dup)) == 1:
if axis == 0:
to_dup = np.expand_dims(to_dup, 1)
if axis == 1:
to_dup = np.expand_dims(to_dup, 0)
# we choose not to handle more than 2 dimensions
if len(np.shape(to_dup)) > 2:
raise exceptions.InputError(to_dup, "Input has more than 2 dimensions")
# if there are no internal elements along the desired axis:
if np.shape(to_dup)[axis] <= 2:
return to_dup
dupped = np.repeat(
to_dup,
np.r_[1, 2 * np.ones(np.shape(to_dup)[axis] - 2, dtype=int), 1],
axis=axis)
# dupped = np.c_[to_dup[:, [0]],
# np.repeat(to_dup[:, 1:-1], 2, axis=1),
# to_dup[:, [-1]]]
return dupped
def get_cost(self, times, times_changed=None, defer=False):
"""Utility function to calculate trajectory cost
Richter, Bry, Roy; Polynomial Trajectory Planning for Quadrotor Flight
Equation 31
Uses:
Everything used by the following functions:
self.get_free_ders()
self.get_poly_coeffs()
Modifies:
self.
times: time in which to complete each segment (not the transition
times)
Everything modified by the following functions:
self.get_free_ders()
self.get_poly_coeffs()
Args:
Returns:
self.
piece_poly_cost: cost for integral of squared norm
of specified derivatives
Raises:
minsnap.exceptions.InputError if
np.size(times) != np.shape(der_fixed)[1] - 1
"""
if np.size(times) != np.shape(self.der_fixed)[1] - 1:
raise exceptions.InputError(
times, "Mismatch between number of segment times" +
" and number of segments")
self.times = np.array(times).copy()
# for reuse by update_times()
if not defer:
self.get_free_ders(waypoints_changed=np.array([]),
der_fixed_changed=np.array([]),
times_changed=times_changed)
self.get_poly_coeffs()
return self.piece_poly_cost
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 insert(self,
new_index,
new_waypoint,
new_times,
new_der_fixed,
defer=False):
"""
Insert new waypoints to start at the selected index
Uses:
Everything used by the following functions:
self.get_cost()
self.get_piece_poly()
Modifies:
self.
waypoints:
der_fixed:
n_seg:
times: time in which to complete each segment (not the transition
times)
Everything modified by the following functions:
self.get_cost()
self.get_piece_poly()
Args:
new_index: index that new waypoints should start at after insertion
new_waypoint: numpy array
new_times:
new_der_fixed:
Returns:
Raises:
"""
# Check input
if new_der_fixed is not None:
if np.shape(new_der_fixed)[0] != np.shape(self.der_fixed)[0]:
msg = "Mismatch between length of new derivative fixed array "
msg = msg + "and size of 2nd dimension of stored derivative "
msg = msg + "fixed array"
raise exceptions.InputError(new_der_fixed, msg)
der_fixed_changed = np.array(new_index).copy()
else:
der_fixed_changed = np.array([])
if np.shape(new_waypoint)[0] != np.shape(self.der_fixed)[0]:
msg = "Mismatch between length of new waypoint array and size of"
msg = msg + "2nd dimension of stored derivative fixed array"
raise exceptions.InputError(new_waypoint, msg)
# modify waypoints
self.waypoints = np.insert(self.waypoints,
new_index,
np.array(new_waypoint),
axis=1)
# modify der_fixed
self.der_fixed = np.insert(self.der_fixed,
new_index,
np.array(new_der_fixed),
axis=1)
# Correct der fixed around it
if new_index == 0:
self.der_fixed[1:, 1] = False
elif new_index == (self.n_seg + 1):
self.der_fixed[1:, -2] = False
# modify n_seg - increase by 1
self.n_seg += 1
# modify times - add a time and change time for segments on either side of new point
if new_index > self.times.size:
self.times = np.append(self.times, new_times[0])
else:
self.times = np.insert(self.times, new_index, new_times[1])
if new_index != 0 and new_index <= self.times.size - 1:
self.times[new_index - 1] = new_times[0]
# modify Q and A matrices
self.Q = cost.insert(self.Q,
new_index=new_index,
new_times=new_times,
costs=self.costs,
order=self.order)
self.A = constraint.insert(self.A,
new_index=new_index,
new_times=new_times,
order=self.order)
# Get inverse A
if (self.A.shape[0] == self.A.shape[1]):
pinv_A = sp.sparse.linalg.inv(self.A)
else:
# is there some way to keep this sparse? sparse SVD?
pinv_A = np.asmatrix(sp.linalg.pinv(self.A.todense()))
self.pinv_A = pinv_A
if not defer:
self.get_free_ders(waypoints_changed=None,
der_fixed_changed=None,
times_changed=np.array([]))
self.get_poly_coeffs()
self.get_piece_poly()
