def test_dup_internal_multi_segment_columns_more_than_two(self):
"""Test duplication function with multiple internal columns"""
to_dup = np.array([[0, 1, 2, 3, 4, 5, 6], [7, 8, 9, 10, 11, 12, 13],
[14, 15, 16, 17, 18, 19, 20],
[21, 22, 23, 24, 25, 26, 27],
[28, 29, 30, 31, 32, 33, 34]])
expect = np.array([[0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6],
[7, 8, 8, 9, 9, 10, 10, 11, 11, 12, 12, 13],
[14, 15, 15, 16, 16, 17, 17, 18, 18, 19, 19, 20],
[21, 22, 22, 23, 23, 24, 24, 25, 25, 26, 26, 27],
[28, 29, 29, 30, 30, 31, 31, 32, 32, 33, 33, 34]])
result = utils.dup_internal(to_dup, 1)
np.testing.assert_equal(result, expect)
def test_dup_internal_multi_segment_columns(self):
"""Test duplication function with multiple internal columns"""
to_dup = np.array([[0, 1, 2, 3], [4, 5, 6, 7]])
expect = np.array([[0, 1, 1, 2, 2, 3], [4, 5, 5, 6, 6, 7]])
result = utils.dup_internal(to_dup, 1)
np.testing.assert_equal(result, expect)
def test_dup_internal_multi_segment_rows(self):
"""Test duplication function with multiple internal rows"""
to_dup = np.array([[0, 4], [1, 5], [2, 6], [3, 7]])
expect = np.array([[0, 4], [1, 5], [1, 5], [2, 6], [2, 6], [3, 7]])
result = utils.dup_internal(to_dup)
np.testing.assert_equal(result, expect)
def test_dup_internal_1d_list_rows(self):
"""Test duplication function with 1d plain list as input (not numpy)"""
to_dup = [0, 1, 2]
expect = np.array([[0], [1], [1], [2]])
np.testing.assert_equal(utils.dup_internal(to_dup), expect)
def block_selector(der_fixed, closed_loop=False):
"""Generate sparse selector matrix
Richter, Bry, Roy; Polynomial Trajectory Planning for Quadrotor Flight
Equation 31
This matrix maps between the partitioned arrangement of free and fixed
derivatives, Eqn. 26, and the ordering in the constraint matrix, A.
Args:
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.
order: the highest order of the polynomials used in the optimisation
Returns:
M: sparse selector matrix (nonsquare)
Raises:
"""
# TODO([email protected]) - the operations utils.dup_columns(der_fixed)
# and np.logical_not(der_fixed) are carried out repeatedly to avoid having
# extra named variables. If you are trying to improve performance, storing
# the result the first time these functions are evaluated may make sense.
# der_fixed should already be a numpy array, but make it one anyway
der_fixed = np.array(der_fixed)
num_der_fixed = np.sum(der_fixed)
if closed_loop:
num_der_fixed_end = np.sum(der_fixed[:, 0])
# examples from MATLAB use column major indexing; we want to use row-major
# indexing internally in this function for Numpy ease
der_fixed = der_fixed.T
# import pdb; pdb.set_trace()
# fixed derivatives mapping to [start end] list of segment derivatives
row = np.flatnonzero(utils.dup_internal(der_fixed))
# necessary because numpy is 0-based
tmp_col = -1 * np.ones(np.shape(der_fixed), dtype=int)
tmp_col[der_fixed] = np.arange(num_der_fixed, dtype=int)
# tmp_col[der_fixed] = np.r_[:num_der_fixed, dtype=int]
tmp_col = utils.dup_internal(tmp_col)
if closed_loop:
# Copy first column to the last to force continuity
tmp_col[-1, :] = tmp_col[0, :]
col = tmp_col[tmp_col >= 0]
# free derivatives mapping to [start end] list of segment derivatives
# these are the derivatives that are optimized
row = np.append(
row, np.flatnonzero(utils.dup_internal(np.logical_not(der_fixed))))
# necessary because numpy is 0-based
tmp_col = -1 * np.ones(np.shape(der_fixed), dtype=int)
tmp_col[np.logical_not(der_fixed)] = np.arange(np.size(der_fixed) -
num_der_fixed,
dtype=int)
# tmp_col[np.logical_not(der_fixed)] = np.r_[:(np.size(der_fixed)
# - num_der_fixed)]
tmp_col = utils.dup_internal(tmp_col)
if closed_loop:
# Copy first column to the last to force continuity
tmp_col[-1, :] = tmp_col[0, :]
col = np.append(col, (num_der_fixed - num_der_fixed_end) +
tmp_col[tmp_col >= 0])
else:
col = np.append(col, num_der_fixed + tmp_col[tmp_col >= 0])
val = np.ones(np.size(row), dtype=int)
# the row & col indices we generated are for M_trans, so just flip them
if closed_loop:
M = sp.sparse.coo_matrix(
(val, (col, row)),
shape=(np.size(der_fixed) - der_fixed.shape[1],
np.size(utils.dup_internal(der_fixed))),
dtype=int)
else:
M = sp.sparse.coo_matrix(
(val, (col, row)),
shape=(np.size(der_fixed), np.size(utils.dup_internal(der_fixed))),
dtype=int)
return M