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:
quest.exceptions.InputError if
np.size(waypoint) != np.shape(self.waypoints)[1]
quest.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:
quest.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 accel_yaw_from_attitude(attitude,thr_mag):
"""
Takes in an attitude and accelerration magnitude and outputs the
corresponding acceleration vector and yaw
Method is with the differentially flat derivation from:
'Minimum Snap Trajectory Generation and Control for Quadrotors'
Mellinger and Kumar 2011, page 2521
Args:
attitude: representaion of the attitude (Euler, matrix of quaternions). numpy arrays
thr_mag: magnitude of thrust
Returns:
accel_vec: length 3 np array of the acceleration vector
yaw: a float of the yaw
Raises:
InputError: invalid attitude input
"""
#TODO([email protected]) test and check whether there are cases where this fails
# extract acceleration direction from attitude
if attitude.size == 3:
#E uler angles
# Convert to matrix
R = transforms3d.euler.euler2mat(attitude[2],attitude[1],attitude[0],'rzyx')
# Last column of rotation matrix is the thrust
thr_dir = R[:,2]
# Get yaw
yaw = attitude[2]
elif attitude.size == 4:
# quaternions
thr_dir = transforms3d.quaternions.rotate_vector([0.,0.,1.],attitude)
# Get yaw
yaw, pitch, roll = transforms3d.euler.quat2euler(attitude,'rzyx')
elif attitude.size == 9:
# Rotation matrix
thr_dir = attitude[:,2]
# Get yaw
yaw, pitch, roll = transforms3d.euler.mat2euler(attitude,'rzyx')
else:
raise exceptions.InputError(attitude,
"invalid attitude input. Needs to be dimenions 3, 4 or (3,3)")
# Compute acceleration
accel_vec = thr_mag*thr_dir - np.array([0,0,settings.GRAVITY])
return accel_vec, yaw
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:
quest.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_x_y_body(yaw, z_b, x_b_current=None, y_b_current=None, deriv_type='check_x_b_current'):
"""
Calculates the x and y body vectors from a given z body axis and yaw angle
The x_b_current is to correct the derivation to be nearest to the current oreintation
Method is with the differentially flat derivation from:
'Minimum Snap Trajectory Generation and Control for Quadrotors'
Mellinger and Kumar 2011, page 2521
Args:
yaw: yaw value (in radians)
z_b: the z body axis (length 3 np array: x, y, z)
Returns:
x_b: the x body axis (length 3 np array: x, y, z)
y_b: the y body axis (length 3 np array: x, y, z)
Raises
"""
data = dict()
# intermediate yaw frame x and ys
if (deriv_type == 'use_x_b_current' or deriv_type == 'combined_x_b_current') and x_b_current is not None:
# Using the current body axis for x_c
x_c = x_b_current
# x_b_current = np.array([np.cos(yaw),np.sin(yaw),0])
else:
x_c = np.array([np.cos(yaw),np.sin(yaw),0])
if x_b_current is None:
x_b_current = x_c.copy()
if deriv_type == 'combined_x_b_current' and y_b_current is not None:
# Using the current body axis for y_c
y_c = y_b_current
# y_b_current = np.array([-1*np.sin(yaw),np.cos(yaw),0])
else:
y_c = np.array([-1*np.sin(yaw),np.cos(yaw),0])
if y_b_current is None:
y_b_current = y_c.copy()
# Use old method if no x_b_current is set
if deriv_type == "yaw_only" or deriv_type == 'use_x_b_current':
# Simplest method, using only x_c
y_b = np.cross(z_b,x_c)
y_b /= np.linalg.norm(y_b)
x_b = np.cross(y_b,z_b)
elif deriv_type == 'x_or_y_furthest':
# use x_c or y_c - whichever is furthest (closest to 90 deg)
# Check which axis to uses
axis_flag = check_furthest_axis(x_c,y_c,z_b)
# Compute the body axes
if axis_flag == 'x':
y_b = np.cross(z_b,x_c)
y_b /= np.linalg.norm(y_b)
x_b = np.cross(y_b,z_b)
elif axis_flag == 'y':
x_b = np.cross(y_c,z_b)
x_b /= np.linalg.norm(x_b)
y_b = np.cross(z_b,x_b)
elif deriv_type == 'check_x_b_current' or deriv_type[:8] == "combined":
# Work with both and then select the closes to the current orientation
# if z_b[2]<0:
# x_c = -x_c
# y_c = -y_c
# initialise
x_b_1 = np.zeros(z_b.shape)
x_b_2 = np.zeros(z_b.shape)
y_b_1 = np.zeros(z_b.shape)
y_b_2 = np.zeros(z_b.shape)
# Compute the body axes using x_c
y_b_1 = np.cross(z_b,x_c)
y_b_1 /= np.linalg.norm(y_b_1)
x_b_1 = np.cross(y_b_1,z_b)
# Compute the body axes using y_c
x_b_2 = np.cross(y_c,z_b)
x_b_2 /= np.linalg.norm(x_b_2)
y_b_2 = np.cross(z_b,x_b_2)
if deriv_type == 'check_x_b_current':
# Check for the closest axis
axis = check_closest_axis(x_b_1,x_b_2,x_b_current)
if axis == 1:
x_b = x_b_1
y_b = y_b_1
elif axis == 2:
x_b = x_b_2
y_b = y_b_2
elif deriv_type[:8] == "combined":
axes_flag, measure = check_closest_axes_set(x_b_1,y_b_1,x_b_2,y_b_2,x_b_current,y_b_current)
data['xc_yc'] = measure
if deriv_type == "combined" :#and np.min(measure) > (80*np.pi/180):
# Pass if deriv_type = "combined_multi_ax"
print("using current body")
# If change more than 10 degrees
# Use x_b_current
# Compute the body axes using x_c
y_b_12 = np.cross(z_b,x_b_current)
y_b_12 /= np.linalg.norm(y_b_12)
x_b_12 = np.cross(y_b_12,z_b)
# Compute the body axes using y_c
x_b_22 = np.cross(y_b_current,z_b)
x_b_22 /= np.linalg.norm(x_b_22)
y_b_22 = np.cross(z_b,x_b_22)
# if np.linalg.norm(x_b_1-x_b_2) > 0.05 and np.linalg.norm(x_b_1+x_b_2) > 0.05:
# import pdb; pdb.set_trace()
axes_flag2, measure = check_closest_axes_set(x_b_12,y_b_12,x_b_22,y_b_22,x_b_current,y_b_current)#,use_body_set=True)
data['xb_yb'] = measure
# if not np.allclose(measure[0],measure[2],atol=1e-4):
# print("x_b and y_b result is different")
# import pdb; pdb.set_trace()
else:
print("using x_c, y_c")
data['xb_yb'] = np.zeros(4)
print("Combined axes selection is: "+axes_flag)
if axes_flag == "+x_c":
x_b = x_b_1
y_b = y_b_1
elif axes_flag == "-x_c":
x_b = -x_b_1
y_b = -y_b_1
elif axes_flag == "+y_c":
x_b = x_b_2
y_b = y_b_2
elif axes_flag == "-y_c":
x_b = -x_b_2
y_b = -y_b_2
else:
print("ERROR: check closest axes set failed")
elif deriv_type == 'check_negative_set':
# Compute body axes
y_b = np.cross(z_b,x_c)
y_b /= np.linalg.norm(y_b)
x_b = np.cross(y_b,z_b)
# Check for the closest axis
axis = check_closest_axis(x_b,-x_b,x_b_current)
if axis == 2:
x_b = -x_b
y_b = -y_b
elif deriv_type[:11] == 'neg_or_quat':
# Faessler's Method
# Compute the body axes using y_c
SIGMA = 0.000001
if np.fabs(z_b[2]) > SIGMA:
x_b = np.cross(y_c,z_b)
if z_b[2]<0:
x_b *= -1
else:
# Thrust in xy plan, point x down
x_b = np.array([0,0,-1.0])
eps = 1e-10
if np.linalg.norm(x_b) <= eps:
# singularity
x_b, y_b = get_quat_pitch_roll(z_b)
else:
x_b /= np.linalg.norm(x_b)
y_b = np.cross(z_b,x_b)
elif deriv_type == "quat_pq_only":
x_b, y_b = get_quat_pitch_roll(z_b)
else:
raise exceptions.InputError(deriv_type,"Invalid deriv type or missing information")
return x_b, y_b, data
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 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()