Exemple #1
0
 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())
Exemple #2
0
 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)
Exemple #3
0
    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)
Exemple #4
0
    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)
Exemple #5
0
    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