Beispiel #1
0
    def __cost_saturation_l(self, x, x_ref, covar_x, u, covar_u, delta_u, Q, R,
                            S):
        """ Stage Cost function: Expected Value of Saturating Cost
        """
        Nx = ca.MX.size1(Q)
        Nu = ca.MX.size1(R)

        # Create symbols
        Q_s = ca.SX.sym('Q', Nx, Nx)
        R_s = ca.SX.sym('Q', Nu, Nu)
        x_s = ca.SX.sym('x', Nx)
        u_s = ca.SX.sym('x', Nu)
        covar_x_s = ca.SX.sym('covar_z', Nx, Nx)
        covar_u_s = ca.SX.sym('covar_u', ca.MX.size(R))

        Z_x = ca.SX.eye(Nx) + 2 * covar_x_s @ Q_s
        Z_u = ca.SX.eye(Nu) + 2 * covar_u_s @ R_s

        cost_x = ca.Function('cost_x', [x_s, Q_s, covar_x_s], [
            1 - ca.exp(-(x_s.T @ ca.solve(Z_x.T, Q_s.T).T @ x_s)) /
            ca.sqrt(ca.det(Z_x))
        ])
        cost_u = ca.Function('cost_u', [u_s, R_s, covar_u_s], [
            1 - ca.exp(-(u_s.T @ ca.solve(Z_u.T, R_s.T).T @ u_s)) /
            ca.sqrt(ca.det(Z_u))
        ])

        return cost_x(x - x_ref, Q, covar_x) + cost_u(u, R, covar_u)
Beispiel #2
0
 def pinv(self, J):
     if self.options["pinv_method"] == "standard":
         pJ = cs.pinv(J)
     elif self.options["pinv_method"] == "damped":
         dmpng_fctr = self.options["damping_factor"]
         if J.size2() >= J.size1():
             inner = cs.mtimes(J, J.T)
             inner += dmpng_fctr * cs.DM.eye(J.size1())
             pJ = cs.solve(inner, J).T
         else:
             inner = cs.mtimes(J.T, J)
             inner += dmpng_fctr * cs.DM.eye(J.size2())
             pJ = cs.solve(inner, J.T)
     return pJ
Beispiel #3
0
    def get_forward_dynamics_crba(self, root, tip, gravity=None, f_ext=None):
        """Returns the forward dynamics as a casadi function by
        solving the Lagrangian eq. of motion.  OBS! Not appropriate
        for robots with a high number of dof -> use
        get_forward_dynamics_aba().
        """
        if self.robot_desc is None:
            raise ValueError('Robot description not loaded from urdf')
        n_joints = self.get_n_joints(root, tip)
        q = cs.SX.sym("q", n_joints)
        q_dot = cs.SX.sym("q_dot", n_joints)
        tau = cs.SX.sym("tau", n_joints)
        q_ddot = cs.SX.zeros(n_joints)
        i_X_p, Si, Ic = self._model_calculation(root, tip, q)

        M = self._get_M(Ic, i_X_p, Si, n_joints, q)
        M_inv = cs.solve(M, cs.SX.eye(M.size1()))

        C = self._get_C(i_X_p, Si, Ic, q, q_dot, n_joints, gravity, f_ext)

        q_ddot = cs.mtimes(M_inv, (tau - C))
        q_ddot = cs.Function("q_ddot", [q, q_dot, tau], [q_ddot],
                             self.func_opts)

        return q_ddot
Beispiel #4
0
def create_LSpoly(d, x, y):
    "creates polynomial by least squares fitting of order d. Builds Vandermonde matrix manually"
    # x = np.append(x,0)              # add (0,0) as data point
    # y = np.append(y,0)

    a = d + 1  # number of parameters including a0
    M = ca.SX.sym('M', 2 * d + 1)  # all the exponents from 1 to 2k
    sizex = x.shape[0]  # number of data points x
    M[0] = sizex  # first entry in matrix

    for k in range(1, M.shape[0]):  # collect all matrix entries
        M[k] = sum([x[o]**k for o in range(0, sizex)])

    sumM = ca.SX(a, a)  # create actual matrix
    for j in range(0, a):
        for k in range(0, a):
            sumM[j, k] = M[k + j]

    B = ca.SX(a, 1)  # create B vector (sum of y)
    for k in range(0, a):
        B[k] = sum([y[o] * x[o]**k for o in range(0, sizex)])

    X = ca.solve(sumM, B)  # parameters order: low to high power
    xvar = ca.SX.sym('xvar')
    poly = X[0]
    for k in range(1, X.shape[0]):
        poly += X[k] * xvar**(k)  # create polynomial
    pfun = ca.Function('poly', [xvar], [poly])
    return pfun, X, poly
    def buildDynamicalSystem(self):
        # q1 is first link q2 is second link
        q1 = ca.MX.sym('q1')
        dq1 = ca.MX.sym('dq1')
        q2 = ca.MX.sym('q2')
        dq2 = ca.MX.sym('dq2')
        u = ca.MX.sym('u')

        theta = ca.MX.sym("theta", 7, 1)

        if self.trainMotor:
            Rm = ca.MX.sym("Rm")
            km = ca.MX.sym("km")
            params = ca.vertcat(ca.MX.sym("params", 7, 1), Rm, km)
        else:
            Rm = self.Rm
            km = self.km
            params = theta

        states = ca.vertcat(q1, q2, dq1, dq2)
        controls = u  # km * (u - km * dq1) / Rm;

        if self.hackOn:
            # convert actions which are given as torques to voltage to be consistent with the controller inputs of Lukas
            u = u * Rm / km

        controls_torque = km * (u - km * dq1) / Rm

        # build matrix for mass matrix
        phi_1_1 = ca.MX(2, 7)
        phi_1_1[0, 0] = ca.MX.ones(1)
        phi_1_1[0, 1] = ca.sin(q2) * ca.sin(q2)
        phi_1_1[1, 2] = ca.cos(q2)
        phi_1_2 = ca.MX(2, 7)
        phi_1_2[0, 2] = -ca.cos(q2)
        phi_1_2[1, 3] = ca.MX.ones(1)
        mass_matrix = ca.horzcat(ca.mtimes(phi_1_1, params),
                                 ca.mtimes(phi_1_2, params))

        phi_2 = ca.MX(2, 7)
        phi_2[0, 1] = ca.sin(2 * q2) * dq1 * dq2
        phi_2[0, 2] = ca.sin(q2) * dq2 * dq2
        phi_2[0, 5] = dq1
        phi_2[1, 1] = -1 / 2 * ca.sin(2 * q2) * dq1 * dq1
        phi_2[1, 4] = self.g * ca.sin(q2)
        phi_2[1, 6] = dq2

        forces = ca.mtimes(phi_2, params)

        actions = ca.vertcat(controls_torque, ca.MX.zeros(1))

        b = actions - forces
        states_dd = ca.solve(mass_matrix, b)

        states_d = ca.vertcat(dq1, dq2, states_dd[0], states_dd[1])

        return states, states_d, controls, params
Beispiel #6
0
    def setup_oed(self, outputs, parameters, sigma, time_points, design="A"):
        """
        Transforms an Optimization Problem into an Optimal Experimental Design problem.

        Parameters::

            outputs --
                List of names for outputs.

                Type: [string]

            parameters --
                List of names for parameters to estimate.

                Type: [string]

            sigma --
                Experiment variance matrix.

                Type: [[float]]

            time_points --
                List of measurement time points.

                Type: [float]

            design --
                Design criterion.

                Possible values: "A", "T"
        """
        # Augment sensitivities and add timed variables
        self.augment_sensitivities(parameters)
        timed_sens = self.create_timed_sensitivities(outputs, parameters, time_points)
        
        # Create sensitivity and Fisher matrices
        Q = []
        for j in xrange(len(outputs)):
            Q.append(casadi.vertcat([casadi.horzcat([s.getVar() for s in timed_sens[i][j]])
                                     for i in xrange(len(time_points))]))
        Fisher = sum([sigma[i, j] * casadi.mul(Q[i].T, Q[j]) for i in xrange(len(outputs))
                      for j in xrange(len(outputs))])

        # Define the objective
        if design == "A":
            b = casadi.MX.sym("b", Fisher.shape[1], 1)
            Fisher_inv = casadi.jacobian(casadi.solve(Fisher, b), b)
            obj = casadi.trace(Fisher_inv)
        elif design == "T":
            obj = -casadi.trace(Fisher)
        elif design == "D":
            obj = -casadi.det(Fisher)
            raise NotImplementedError("D-optimal design is not supported.")
        else:
            raise ValueError("Invalid design %s." % design)
        old_obj = self.getObjective()
        self.setObjective(old_obj + obj)
Beispiel #7
0
 def construct_upd_z(self, problem=None, lineq_updz=True):
     if problem is not None:
         self.problem_upd_z = problem
         self._lineq_updz = lineq_updz
         return 0.
     # check if we have linear equality constraints
     self._lineq_updz, A, b = self._check_for_lineq()
     if not self._lineq_updz:
         raise ValueError('For now, only equality constrained QP ' +
                          'z-updates are allowed!')
     x_i = struct_symMX(self.q_i_struct)
     x_j = struct_symMX(self.q_ij_struct)
     l_i = struct_symMX(self.q_i_struct)
     l_ij = struct_symMX(self.q_ij_struct)
     t = MX.sym('t')
     T = MX.sym('T')
     rho = MX.sym('rho')
     par = struct_symMX(self.par_global_struct)
     inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
     t0 = t / T
     # put symbols in MX structs (necessary for transformation)
     x_i = self.q_i_struct(x_i)
     x_j = self.q_ij_struct(x_j)
     l_i = self.q_i_struct(l_i)
     l_ij = self.q_ij_struct(l_ij)
     # transform spline variables: only consider future piece of spline
     tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
     self._transform_spline([x_i, l_i], tf, self.q_i)
     self._transform_spline([x_j, l_ij], tf, self.q_ij)
     # fill in parameters
     A = A(par.cat)
     b = b(par.cat)
     # build KKT system and solve it via schur complement method
     l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat)
     f = -(l + rho * x)
     G = -(1 / rho) * mtimes(A, A.T)
     h = b + (1 / rho) * mtimes(A, f)
     mu = solve(G, h)
     z = -(1 / rho) * (mtimes(A.T, mu) + f)
     l_qi = self.q_i_struct.shape[0]
     l_qij = self.q_ij_struct.shape[0]
     z_i_new = self.q_i_struct(z[:l_qi])
     z_ij_new = self.q_ij_struct(z[l_qi:l_qi + l_qij])
     # transform back
     tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
     self._transform_spline(z_i_new, tf, self.q_i)
     self._transform_spline(z_ij_new, tf, self.q_ij)
     out = [z_i_new.cat, z_ij_new.cat]
     # create problem
     prob, buildtime = create_function('upd_z_' + str(self._index), inp,
                                       out, self.options)
     self.problem_upd_z = prob
     return buildtime
Beispiel #8
0
    def nllh_casf(self, grad=True, hess=False):
        """
        Creates a casadi function computing the negative log likelihood (without const terms) of the data
        dependent on hyperparameters v.
        :param grad: Whether the function should compute the gradient, too
        :param hess: Whether the function should compute the hessian, too
        :return: A casadi function taking a value of v as input and returning the neg log likelihood, gradient,
                 and hessian is desired
        """
        vshape = np.atleast_2d(self.v).shape
        v = cas.MX.sym("v", vshape[0], vshape[1])
        phi_x = self.phi_cas(self.x, v)

        sinv = self.sinv0 + self.beta * cas.mtimes(phi_x.T, phi_x)
        mean = cas.solve(
            sinv,
            cas.mtimes(self.sinv0, self.mean0) +
            self.beta * cas.mtimes(phi_x.T, self.y))

        y_pred = cas.mtimes(mean.T, phi_x.T).T
        sigma = 1 / self.beta + cas.sum2(phi_x * cas.solve(sinv, phi_x.T).T)

        y_true = self.y
        y_diff = y_true - y_pred
        llht = cas.sum2(y_diff * y_diff) / sigma
        llh = cas.sum1(llht)

        if hess:
            H, llh_grad = cas.hessian(llh, v)
        elif grad:
            llh_grad = cas.gradient(llh, v)

        res = [llh]
        if grad:
            res += [llh_grad]
        if hess:
            res += [H]

        f = cas.Function("f_mu", [v], res)
        return f
Beispiel #9
0
 def construct_upd_z(self, problem=None, lineq_updz=True):
     if problem is not None:
         self.problem_upd_z = problem
         self._lineq_updz = lineq_updz
         return 0.
     # check if we have linear equality constraints
     self._lineq_updz, A, b = self._check_for_lineq()
     if not self._lineq_updz:
         raise ValueError('For now, only equality constrained QP ' +
                          'z-updates are allowed!')
     x_i = struct_symMX(self.q_i_struct)
     x_j = struct_symMX(self.q_ij_struct)
     l_i = struct_symMX(self.q_i_struct)
     l_ij = struct_symMX(self.q_ij_struct)
     t = MX.sym('t')
     T = MX.sym('T')
     rho = MX.sym('rho')
     par = struct_symMX(self.par_global_struct)
     inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
     t0 = t/T
     # put symbols in MX structs (necessary for transformation)
     x_i = self.q_i_struct(x_i)
     x_j = self.q_ij_struct(x_j)
     l_i = self.q_i_struct(l_i)
     l_ij = self.q_ij_struct(l_ij)
     # transform spline variables: only consider future piece of spline
     tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
     self._transform_spline([x_i, l_i], tf, self.q_i)
     self._transform_spline([x_j, l_ij], tf, self.q_ij)
     # fill in parameters
     A = A(par.cat)
     b = b(par.cat)
     # build KKT system and solve it via schur complement method
     l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat)
     f = -(l + rho*x)
     G = -(1/rho)*mtimes(A, A.T)
     h = b + (1/rho)*mtimes(A, f)
     mu = solve(G, h)
     z = -(1/rho)*(mtimes(A.T, mu)+f)
     l_qi = self.q_i_struct.shape[0]
     l_qij = self.q_ij_struct.shape[0]
     z_i_new = self.q_i_struct(z[:l_qi])
     z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij])
     # transform back
     tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
     self._transform_spline(z_i_new, tf, self.q_i)
     self._transform_spline(z_ij_new, tf, self.q_ij)
     out = [z_i_new.cat, z_ij_new.cat]
     # create problem
     prob, buildtime = create_function('upd_z_'+str(self._index), inp, out, self.options)
     self.problem_upd_z = prob
     return buildtime
Beispiel #10
0
def solve(A, b):  # TODO get this working
    """
    Solve the linear system Ax=b for x.
    Args:
        A: A square matrix.
        b: A vector representing the RHS of the linear system.

    Returns: The solution vector x.

    """
    try:
        return onp.linalg.solve(A, b)
    except Exception:
        return cas.solve(A, b)
Beispiel #11
0
def solve(A, b):  # TODO get this working
    """
    Solve the linear system Ax=b for x.
    Args:
        A: A square matrix.
        b: A vector representing the RHS of the linear system.

    Returns: The solution vector x.

    """
    if not is_casadi_type([A, b]):
        return _onp.linalg.solve(A, b)

    else:
        return _cas.solve(A, b)
Beispiel #12
0
 def construct_upd_z(self):
     # check if we have linear equality constraints
     self._lineq_updz, A, b = self._check_for_lineq()
     if not self._lineq_updz:
         self._construct_upd_z_nlp()
     x_i = struct_symSX(self.q_i_struct)
     x_j = struct_symSX(self.q_ij_struct)
     l_i = struct_symSX(self.q_i_struct)
     l_ij = struct_symSX(self.q_ij_struct)
     t = SX.sym('t')
     T = SX.sym('T')
     rho = SX.sym('rho')
     par = struct_symSX(self.par_struct)
     inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
     t0 = t/T
     # put symbols in SX structs (necessary for transformation)
     x_i = self.q_i_struct(x_i)
     x_j = self.q_ij_struct(x_j)
     l_i = self.q_i_struct(l_i)
     l_ij = self.q_ij_struct(l_ij)
     # transform spline variables: only consider future piece of spline
     tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
     self._transform_spline([x_i, l_i], tf, self.q_i)
     self._transform_spline([x_j, l_ij], tf, self.q_ij)
     # fill in parameters
     A = A([par])[0]
     b = b([par])[0]
     # build KKT system
     E = rho*SX.eye(A.shape[1])
     l, x = vertcat([l_i.cat, l_ij.cat]), vertcat([x_i.cat, x_j.cat])
     f = -(l + rho*x)
     G = vertcat([horzcat([E, A.T]),
                  horzcat([A, SX.zeros(A.shape[0], A.shape[0])])])
     h = vertcat([-f, b])
     z = solve(G, h)
     l_qi = self.q_i_struct.shape[0]
     l_qij = self.q_ij_struct.shape[0]
     z_i_new = self.q_i_struct(z[:l_qi])
     z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij])
     # transform back
     tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
     self._transform_spline(z_i_new, tf, self.q_i)
     self._transform_spline(z_ij_new, tf, self.q_ij)
     out = [z_i_new.cat, z_ij_new.cat]
     # create problem
     prob, compile_time = self.father.create_function(
         'upd_z', inp, out, self.options)
     self.problem_upd_z = prob
Beispiel #13
0
    def __cost_saturation_lf(self, x, x_ref, covar_x, P):
        """ Terminal Cost function: Expected Value of Saturating Cost
        """
        Nx = ca.MX.size1(P)

        # Create symbols
        P_s = ca.SX.sym('P', Nx, Nx)
        x_s = ca.SX.sym('x', Nx)
        covar_x_s = ca.SX.sym('covar_z', Nx, Nx)

        Z_x = ca.SX.eye(Nx) + 2 * covar_x_s @ P_s
        cost_x = ca.Function('cost_x', [x_s, P_s, covar_x_s], [
            1 - ca.exp(-(x_s.T @ ca.solve(Z_x.T, P_s.T).T @ x_s)) /
            ca.sqrt(ca.det(Z_x))
        ])
        return cost_x(x - x_ref, P, covar_x)
    def buildDynamicalSystem(self):
        g = 9.8

        x = ca.MX.sym('x')
        dx = ca.MX.sym('dx')
        theta = ca.MX.sym('theta')
        dtheta = ca.MX.sym('dtheta')
        u = ca.MX.sym('u')

        states = ca.vertcat(x, dx, theta, dtheta);
        controls = u;

        param1 = ca.MX.sym('param_1')
        param2 = ca.MX.sym('param_2')
        param3 = ca.MX.sym('param_3')

        params = ca.vertcat(param1, param2, param3)

        print("params_shape", params.shape)

        # build matrix for mass matrix
        phi_1_1 = ca.MX(2, 3)
        phi_1_1[0, 0] = ca.MX.ones(1)
        phi_1_1[1, 1] = ca.cos(theta)
        phi_1_2 = ca.MX(2, 3)
        phi_1_2[0, 1] = ca.cos(theta)
        phi_1_2[1, 2] = 4/3*ca.MX.ones(1)

        mass_matrix = ca.horzcat(ca.mtimes(phi_1_1, params), ca.mtimes(phi_1_2, params))

        phi_2 = ca.MX(2, 3)
        phi_2[0, 1] = -1 * ca.sin(theta) * dtheta * dtheta
        phi_2[1, 1] = -g * ca.sin(theta)

        forces = ca.mtimes(phi_2, params)

        actions = ca.vertcat(controls, ca.MX.zeros(1))

        b = actions - forces
        states_dd = ca.solve(mass_matrix, b)

        states_d = ca.vertcat(dx, states_dd[0], dtheta, states_dd[1])

        return states, states_d, controls, params
Beispiel #15
0
    def integral(self, dy):
        """Calculate integral from derivative values
        """

        y0 = cs.MX.zeros(dy.shape[0])
        y = []
        k = 0
        for b in self._basis:
            N = b.numPoints - 1

            # Calculate y the equation 
            # dy = cs.mtimes(y[:, 1 :], D[: -1, 1 :].T)
            #invD = cs.inv(D[: -1, 1 :])
            #y.append(cs.transpose(cs.mtimes(invD, cs.transpose(dy[:, k : k + N]))))
            y.append(cs.transpose(cs.solve(b.D[: -1, 1 :], cs.transpose(dy[:, k : k + N]))) + cs.repmat(y0, 1, N))
            k += N
            y0 = y[-1][:, -1]

        return cs.horzcat(*y)
Beispiel #16
0
def tangent_approx(f: SYM, x: SYM, a: SYM = None, assert_linear: bool = False) -> Dict[str, SYM]:
    """
    Create a tangent approximation of a non-linear function f(x) about point a
    using a block lower triangular solver

    0 = f(x) = f(a) + J*x   # taylor series about a (if f(x) linear in x, then globally valid)
    J*x = -f(a)             # solve for x
    x = -J^{-1}f(a)         # but inverse is slow, so we use solve
    where J = df/dx
    """
    # find f(a)
    if a is None:
        a = ca.DM.zeros(x.numel(), 1)
    f_a = ca.substitute(f, x, a)  # f(a)
    J = ca.jacobian(f, x)
    if assert_linear and ca.depends_on(J, x):
        raise AssertionError('not linear')
    # solve is smart enough to to convert to blt if necessary
    return ca.solve(J, -f_a)
Beispiel #17
0
def sqrt_covariance_predict(W, F, Q):
    """
    Finds a sqrt factorization of the continuous time covariance
    propagation equations. Requires solving a linear system of equations
    to keep the sqrt lower triangular.

    'A Square Root Formulation of the Kalman Covariance Equations', Andrews 68

    W: sqrt P, symbolic, with sparsity lower triangulr
    F: dynamics matrix
    Q: process noise matrix

    returns:
    W_dot_sol: sqrt of P deriative, lower triangular
    """
    n_x = F.shape[0]
    XL = ca.SX.sym('X', ca.Sparsity_lower(n_x))
    X = (XL - XL.T)
    for i in range(n_x):
        X[i, i] = 0
    W_dot = ca.mtimes(F, W) + ca.mtimes(Q / 2 + X, ca.inv(W).T)

    # solve for XI that keeps W dot lower triangular
    y = ca.vertcat(*ca.triu(W_dot, False).nonzeros())
    x_dep = []
    for i, xi in enumerate(XL.nonzeros()):
        if ca.depends_on(y, xi):
            x_dep += [xi]
    x_dep = ca.vertcat(*x_dep)
    A = ca.jacobian(y, x_dep)
    for i, xi in enumerate(XL.nonzeros()):
        assert not ca.depends_on(A, xi)
    b = -ca.substitute(y, x_dep, 0)
    x_sol = ca.solve(A, b)

    X_sol = ca.SX(X)
    for i in range(x_dep.shape[0]):
        X_sol = ca.substitute(X_sol, x_dep[i], x_sol[i])
    X_sol = ca.sparsify(X_sol)
    W_dot_sol = ca.mtimes(F, W) + ca.mtimes(Q / 2 + X_sol, ca.inv(W).T)

    return W_dot_sol
Beispiel #18
0
 def getRobustSteadyStateNlpFunctions(self, dae, ref_dict = {}):
     xDotSol, zSol = dae.solveForXDotAndZ()
 
     ginv = Constraints()
     def constrainInvariantErrs():
         R_c2b = dae['R_c2b']
         self.makeOrthonormal(ginv, R_c2b)
         ginv.add(dae['c'], '==', 0, tag = ('c(0) == 0', None))
         ginv.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None))
         di = dae['cos_delta'] ** 2 + dae['sin_delta'] ** 2 - 1
         ginv.add(di, '==', 0, tag = ('delta invariant', None))
         
         ginv.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag =
                        ("Rotational velocities", None))
         
     constrainInvariantErrs()
     invariants = ginv.getG()
     
     J = C.jacobian(invariants,dae.xVec())
 
     # make steady state model
     g = Constraints()
     
     xds = C.vertcat([xDotSol[ name ] for name in dae.xNames()])
     jInv = C.mul(J.T,C.solve(C.mul(J,J.T),invariants))
     
     g.add(xds - jInv - dae.xDotVec(), '==', 0, tag = ('dae residual', None))            
 
     for name in ['alpha_deg', 'beta_deg', 'cL']:
         if name in ref_dict:
             g.addBnds(dae[name], ref_dict[name], tag = (name, None))
 
     dvs = C.veccat([dae.xVec(), dae.uVec(), dae.pVec(), dae.xDotVec()])
     
     obj = 0
     for name in dae.uNames() + ['aileron', 'elevator']:
         if name in dae:
             obj += dae[ name ] ** 2
     
     return dvs, obj, g.getG(), g.getLb(), g.getUb(), zSol
Beispiel #19
0
def tangent_approx(f: SYM,
                   x: SYM,
                   a: SYM = None,
                   assert_linear: bool = False) -> Dict[str, SYM]:
    """
    Create a tangent approximation of a non-linear function f(x) about point a
    using a block lower triangular solver

    0 = f(x) = f(a) + J*x   # taylor series about a (if f(x) linear in x, then globally valid)
    J*x = -f(a)             # solve for x
    x = -J^{-1}f(a)         # but inverse is slow, so we use solve
    where J = df/dx
    """
    # find f(a)
    if a is None:
        a = ca.DM.zeros(x.numel(), 1)
    f_a = ca.substitute(f, x, a)  # f(a)
    J = ca.jacobian(f, x)
    if assert_linear and ca.depends_on(J, x):
        raise AssertionError('not linear')
    # solve is smart enough to to convert to blt if necessary
    return ca.solve(J, -f_a)
Beispiel #20
0
    def solveForXDotAndZ(self):
        '''
        returns (xDotDict,zDict) where these dictionaries contain symbolic
        xdot and z which are only a function of x,u,p
        '''
        # get the residual fg(xdot,x,z)
        fg = self.getResidual()

        # take the jacobian w.r.t. xdot,z
        jac = C.jacobian(fg,C.veccat([self.xDotVec(), self.zVec()]))

        # make sure that it was linear in {xdot,z}, i.e. the jacobian is not a function of {xdot,z}
        testJac = C.SXFunction([self.xVec(),self.uVec(),self.pVec()], [jac])
        testJac.init()
        assert len(testJac.getFree()) == 0, \
            "can't convert dae to ode, residual jacobian is a function of {xdot,z}"

        # get the constant term
        fg_fun = C.SXFunction([self.xVec(),self.zVec(),self.uVec(),self.pVec(),self.xDotVec()], [fg])
        fg_fun.init()
        [fg_zero] = fg_fun([self.xVec(),0*self.zVec(),self.uVec(),self.pVec(),0*self.xDotVec()])
        testFun = C.SXFunction([self.xVec(),self.uVec(),self.pVec()], [fg_zero])
        testFun.init()
        assert len(testFun.getFree()) == 0, \
            "the \"impossible\" happened in solveForXDotAndZ"

        xDotAndZ = C.solve(jac, -fg_zero)
        xDot = xDotAndZ[0:len(self.xNames())]
        z = xDotAndZ[len(self.xNames()):]

        xDotDict = {}
        for k,name in enumerate(self.xNames()):
            xDotDict[name] = xDot[k]
        zDict = {}
        for k,name in enumerate(self.zNames()):
            zDict[name] = z[k]
        return (xDotDict, zDict)
    def __init__(self, inertial_frame_id='world'):
        Vehicle.__init__(self, inertial_frame_id)
    
        # Declaring state variables
        ## Generalized position vector
        self.eta = casadi.SX.sym('eta', 6)
        ## Generalized velocity vector
        self.nu = casadi.SX.sym('nu', 6)

        # Build the Coriolis matrix
        self.CMatrix = casadi.SX.zeros(6, 6)

        S_12 = - cross_product_operator(
            casadi.mtimes(self._Mtotal[0:3, 0:3], self.nu[0:3]) +
            casadi.mtimes(self._Mtotal[0:3, 3:6], self.nu[3:6]))
        S_22 = - cross_product_operator(
            casadi.mtimes(self._Mtotal[3:6, 0:3], self.nu[0:3]) +
            casadi.mtimes(self._Mtotal[3:6, 3:6], self.nu[3:6]))

        self.CMatrix[0:3, 3:6] = S_12
        self.CMatrix[3:6, 0:3] = S_12
        self.CMatrix[3:6, 3:6] = S_22

        # Build the damping matrix (linear and nonlinear elements)
        self.DMatrix = - casadi.diag(self._linear_damping)        
        self.DMatrix -= casadi.diag(self._linear_damping_forward_speed)
        self.DMatrix -= casadi.diag(self._quad_damping * self.nu)      

        # Build the restoring forces vectors wrt the BODY frame
        Rx = np.array([[1, 0, 0],
                       [0, casadi.cos(self.eta[3]), -1 * casadi.sin(self.eta[3])],
                       [0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]])
        Ry = np.array([[casadi.cos(self.eta[4]), 0, casadi.sin(self.eta[4])],
                       [0, 1, 0],
                       [-1 * casadi.sin(self.eta[4]), 0, casadi.cos(self.eta[4])]])
        Rz = np.array([[casadi.cos(self.eta[5]), -1 * casadi.sin(self.eta[5]), 0],
                       [casadi.sin(self.eta[5]), casadi.cos(self.eta[5]), 0],
                       [0, 0, 1]])

        R_n_to_b = casadi.transpose(casadi.mtimes(Rz, casadi.mtimes(Ry, Rx)))

        if inertial_frame_id == 'world_ned':
            Fg = casadi.SX([0, 0, -self.mass * self.gravity])
            Fb = casadi.SX([0, 0, self.volume * self.gravity * self.density])
        else:
            Fg = casadi.SX([0, 0, self.mass * self.gravity])
            Fb = casadi.SX([0, 0, -self.volume * self.gravity * self.density])

        self.gVec = casadi.SX.zeros(6)

        self.gVec[0:3] = -1 * casadi.mtimes(R_n_to_b, Fg + Fb)  
        self.gVec[3:6] = -1 * casadi.mtimes(
            R_n_to_b, casadi.cross(self._cog, Fg) + casadi.cross(self._cob, Fb))
        
        # Build Jacobian
        T = 1 / casadi.cos(self.eta[4]) * np.array(
            [[0, casadi.sin(self.eta[3]) * casadi.sin(self.eta[4]), casadi.cos(self.eta[3]) * casadi.sin(self.eta[4])],
             [0, casadi.cos(self.eta[3]) * casadi.cos(self.eta[4]), -casadi.cos(self.eta[4]) * casadi.sin(self.eta[3])],
             [0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]])

        self.eta_dot = casadi.vertcat(
            casadi.mtimes(casadi.transpose(R_n_to_b), self.nu[0:3]),
            casadi.mtimes(T, self.nu[3::]))

        self.u = casadi.SX.sym('u', 6)
        
        self.nu_dot = casadi.solve(
            self._Mtotal, 
            self.u - casadi.mtimes(self.CMatrix, self.nu) - casadi.mtimes(self.DMatrix, self.nu) - self.gVec)

        
Beispiel #22
0
def freeModel(endTimeSteps=None):
    stateNames = [
        "x"  # state 0
        ,
        "y"  # state 1
        ,
        "z"  # state 2
        ,
        "e11"  # state 3
        ,
        "e12"  # state 4
        ,
        "e13"  # state 5
        ,
        "e21"  # state 6
        ,
        "e22"  # state 7
        ,
        "e23"  # state 8
        ,
        "e31"  # state 9
        ,
        "e32"  # state 10
        ,
        "e33"  # state 11
        ,
        "dx"  # state 12
        ,
        "dy"  # state 13
        ,
        "dz"  # state 14
        ,
        "w1"  # state 15
        ,
        "w2"  # state 16
        ,
        "w3"  # state 17
    ]

    uNames = [
        "tc", "aileron", "elevator", "rudder"
        #             , 'ddr'
    ]

    pNames = ["wind_x"]

    uSyms = [C.ssym(name) for name in uNames]
    uDict = dict(zip(uNames, uSyms))
    uVec = C.veccat(uSyms)

    pSyms = [C.ssym(name) for name in pNames]
    pDict = dict(zip(pNames, pSyms))
    pVec = C.veccat(pSyms)

    stateSyms = [C.ssym(name) for name in stateNames]
    stateDict = dict(zip(stateNames, stateSyms))
    stateVec = C.veccat(stateSyms)

    dx = stateDict['dx']
    dy = stateDict['dy']
    dz = stateDict['dz']

    outputs = {}
    outputs['aileron(deg)'] = uDict['aileron'] * 180 / C.pi
    outputs['elevator(deg)'] = uDict['elevator'] * 180 / C.pi
    outputs['rudder(deg)'] = uDict['rudder'] * 180 / C.pi
    (massMatrix, rhs, dRexp) = modelInteg(stateDict, uDict, pDict, outputs)

    zVec = C.solve(massMatrix, rhs)

    ddX = zVec[0:3]
    dw = zVec[3:6]

    ode = C.veccat(
        [C.veccat([dx, dy, dz]),
         dRexp.trans().reshape([9, 1]), ddX, dw])

    if endTimeSteps is not None:
        endTime, nSteps = endTimeSteps
        pNames.append("endTime")
        pDict["endTime"] = endTime
        pVec = C.veccat([pVec, endTime])
        ode = ode  #/(endTime/(nSteps-1))

    dae = C.SXFunction(C.daeIn(x=stateVec, p=C.veccat([uVec, pVec])),
                       C.daeOut(ode=ode))
    return (dae, {
        'xVec': stateVec,
        'xDict': stateDict,
        'xNames': stateNames,
        'uVec': uVec,
        'uNames': uNames,
        'pVec': pVec,
        'pNames': pNames
    }, outputs)
Beispiel #23
0
def calc_NLL(hyper, X, Y, squaredist, meanFunc='zero', prior=None):
    """ Objective function

    Calculate the negative log likelihood function using Casadi SX symbols.

    # Arguments:
        hyper: Array with hyperparameters [ell_1 .. ell_Nx sf sn], where Nx is the
            number of inputs to the GP.
        X: Training data matrix with inputs of size (N x Nx).
        Y: Training data matrix with outpyts of size (N x Ny),
            with Ny number of outputs.

    # Returns:
        NLL: The negative log likelihood function (scalar)
    """

    N, Nx = ca.MX.size(X)
    ell = hyper[:Nx]
    sf2 = hyper[Nx]**2
    sn2 = hyper[Nx + 1]**2

    m = get_mean_function(hyper, X.T, func=meanFunc)

    # Calculate covariance matrix
    K_s = ca.SX.sym('K_s', N, N)
    sqdist = ca.SX.sym('sqd', N, N)
    elli = ca.SX.sym('elli')
    ki = ca.Function('ki', [sqdist, elli, K_s], [sqdist / elli**2 + K_s])
    K1 = ca.MX(N, N)
    for i in range(Nx):
        K1 = ki(squaredist[:, (i * N):(i + 1) * N], ell[i], K1)

    sf2_s = ca.SX.sym('sf2')
    exponent = ca.SX.sym('exp', N, N)
    K_exp = ca.Function('K', [exponent, sf2_s],
                        [sf2_s * ca.SX.exp(-.5 * exponent)])
    K2 = K_exp(K1, sf2)

    K = K2 + sn2 * ca.MX.eye(N)
    K = (K + K.T) * 0.5  # Make sure matrix is symmentric

    A = ca.SX.sym('A', ca.MX.size(K))
    cholesky = ca.Function('cholesky', [A], [ca.chol(A).T])
    L = cholesky(K)

    B = 2 * ca.sum1(ca.SX.log(ca.diag(A)))
    log_determinant = ca.Function('log_det', [A], [B])
    log_detK = log_determinant(L)

    Y_s = ca.SX.sym('Y', ca.MX.size(Y))
    L_s = ca.SX.sym('L', ca.Sparsity.lower(N))
    sol = ca.Function('sol', [L_s, Y_s], [ca.solve(L_s, Y_s)])
    invLy = sol(L, Y - m(X.T))

    invLy_s = ca.SX.sym('invLy', ca.MX.size(invLy))
    sol2 = ca.Function('sol2', [L_s, invLy_s], [ca.solve(L_s.T, invLy_s)])
    alpha = sol2(L, invLy)

    alph = ca.SX.sym('alph', ca.MX.size(alpha))
    detK = ca.SX.sym('det')

    # Calculate hyperpriors
    theta = ca.SX.sym('theta')
    mu = ca.SX.sym('mu')
    s2 = ca.SX.sym('s2')
    prior_gauss = ca.Function(
        'hyp_prior', [theta, mu, s2],
        [-(theta - mu)**2 / (2 * s2) - 0.5 * ca.log(2 * ca.pi * s2)])
    log_prior = 0
    if prior is not None:
        for i in range(Nx):
            log_prior += prior_gauss(ell[i], prior['ell_mean'],
                                     prior['ell_std']**2)
        log_prior += prior_gauss(sf2, prior['sf_mean'], prior['sf_std']**2)
        log_prior += prior_gauss(sn2, prior['sn_mean'], prior['sn_std']**2)

    NLL = ca.Function('NLL', [Y_s, alph, detK],
                      [0.5 * ca.mtimes(Y_s.T, alph) + 0.5 * detK])
    return NLL(Y - m(X.T), alpha, log_detK) + log_prior
Beispiel #24
0
def gp_exact_moment(invK, X, Y, hyper, inputmean, inputcov):
    """ Gaussian Process with Exact Moment Matching
    Copyright (c) 2018, Eric Bradford, Helge-André Langåker

    The first and second moments are used to compute the mean and covariance of the
    posterior distribution with a stochastic input distribution. This assumes a
    zero prior mean function and the squared exponential kernel.

    # Arguments
        invK: Array with the inverse covariance matrices of size (Ny x N x N),
            with Ny number of outputs from the GP and N number of training points.
        X: Training data matrix with inputs of size NxNx, with Nx number of
            inputs to the GP.
        Y: Training data matrix with outpyts of size (N x Ny).
        hyper: Array with hyperparameters [ell_1 .. ell_Nx sf sn].
        inputmean: Mean from the last GP iteration of size (1 x Nx)
        inputcov: Covariance matrix from the last GP iteration of size (Nx x Nx)

    # Returns
        mean: Array of the output mean of size (Ny x 1).
        covariance: Covariance matrix of size (Ny x Ny).
    """

    hyper = ca.log(hyper)
    Ny     = len(invK)
    N, Nx     = ca.MX.size(X)
    mean  = ca.MX.zeros(Ny, 1)
    beta  = ca.MX.zeros(N, Ny)
    log_k = ca.MX.zeros(N, Ny)
    v     = X - ca.repmat(inputmean, N, 1)

    covariance = ca.MX.zeros(Ny, Ny)

    #TODO: Fix that LinsolQr don't work with the extended graph?
    A = ca.SX.sym('A', inputcov.shape)
    [Q, R2] = ca.qr(A)
    determinant = ca.Function('determinant', [A], [ca.exp(ca.trace(ca.log(R2)))])

    for a in range(Ny):
        beta[:, a] = ca.mtimes(invK[a], Y[:, a])
        iLambda   = ca.diag(ca.exp(-2 * hyper[a, :Nx]))
        R  = inputcov + ca.diag(ca.exp(2 * hyper[a, :Nx]))
        iR = ca.mtimes(iLambda, (ca.MX.eye(Nx) - ca.solve((ca.MX.eye(Nx)
                + ca.mtimes(inputcov, iLambda)), (ca.mtimes(inputcov, iLambda)))))
        T  = ca.mtimes(v, iR)
        c  = ca.exp(2 * hyper[a, Nx]) / ca.sqrt(determinant(R)) \
                * ca.exp(ca.sum2(hyper[a, :Nx]))
        q2 = c * ca.exp(-ca.sum2(T * v) * 0.5)
        qb = q2 * beta[:, a]
        mean[a] = ca.sum1(qb)
        t  = ca.repmat(ca.exp(hyper[a, :Nx]), N, 1)
        v1 = v / t
        log_k[:, a] = 2 * hyper[a, Nx] - ca.sum2(v1 * v1) * 0.5

    # covariance with noisy input
    for a in range(Ny):
        ii = v / ca.repmat(ca.exp(2 * hyper[a, :Nx]), N, 1)
        for b in range(a + 1):
            R = ca.mtimes(inputcov, ca.diag(ca.exp(-2 * hyper[a, :Nx])
                + ca.exp(-2 * hyper[b, :Nx]))) + ca.MX.eye(Nx)
            t = 1.0 / ca.sqrt(determinant(R))
            ij = v / ca.repmat(ca.exp(2 * hyper[b, :Nx]), N, 1)
            Q = ca.exp(ca.repmat(log_k[:, a], 1, N)
                + ca.repmat(ca.transpose(log_k[:, b]), N, 1)
                + maha(ii, -ij, ca.solve(R, inputcov * 0.5), N))
            A = ca.mtimes(beta[:, a], ca.transpose(beta[:, b]))
            if b == a:
                A = A - invK[a]
            A = A * Q
            covariance[a, b] = t * ca.sum2(ca.sum1(A))
            covariance[b, a] = covariance[a, b]
        covariance[a, a] = covariance[a, a] + ca.exp(2 * hyper[a, Nx])
    covariance = covariance - ca.mtimes(mean, ca.transpose(mean))

    return [mean, covariance]
Beispiel #25
0
    def compute_covariance_matrix(self):
        r'''
        This function computes the covariance matrix of the estimated
        parameters from the inverse of the KKT matrix for the
        parameter estimation problem. This allows then for statements on the
        quality of the values of the estimated parameters.

        For efficiency, only the inverse of the relevant part of the matrix
        is computed using the Schur complement.

        A more detailed description of this function will follow in future
        versions.

        '''

        intro.pecas_intro()

        print('\n' + 20 * '-' + \
            ' PECas covariance matrix computation ' + 21 * '-')

        print('''
Computing the covariance matrix for the estimated parameters, 
this might take some time ...
''')

        self.tstart_cov_computation = time.time()

        try:

            N1 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \
                self.w.shape[0])

            N2 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \
                self.Vars.shape[0] - self.w.shape[0])

            hess = ca.blockcat([
                [N2, N1],
                [N1.T, ca.diag(self.w)],
            ])

            # hess = hess + 1e-10 * ca.diag(self.Vars)

            # J2 can be re-used from parameter estimation, right?

            J2 = ca.jacobian(self.g, self.Vars)

            kkt = ca.blockcat( \

                [[hess, \
                    J2.T], \

                [J2, \
                    ca.MX(self.g.size1(), self.g.size1())]] \

                    )

            B1 = kkt[:self.pesetup.np, :self.pesetup.np]
            E = kkt[self.pesetup.np:, :self.pesetup.np]
            D = kkt[self.pesetup.np:, self.pesetup.np:]

            Dinv = ca.solve(D, E, "csparse")

            F11 = B1 - ca.mul([E.T, Dinv])

            self.fbeta = ca.MXFunction("fbeta", [self.Vars],
                [ca.mul([self.R.T, self.R]) / \
                (self.yN.size + self.g.size1() - self.Vars.size())])

            [self.beta] = self.fbeta([self.Varshat])

            self.fcovp = ca.MXFunction("fcovp", [self.Vars], \
                [self.beta * ca.solve(F11, ca.MX.eye(F11.size1()))])

            [self.Covp] = self.fcovp([self.Varshat])

            print( \
'''Covariance matrix computation finished, run show_results() to visualize.''')

        except AttributeError as err:

            errmsg = '''
You must execute run_parameter_estimation() first before the covariance
matrix for the estimated parameters can be computed.
'''

            raise AttributeError(errmsg)

        finally:

            self.tend_cov_computation = time.time()
            self.duration_cov_computation = self.tend_cov_computation - \
                self.tstart_cov_computation
Beispiel #26
0
def solve_chain_mass_nmpc_qp(num_masses, num_intervals):
    time_step = 0.1

    u, y, dot_y = get_chain_mass_ode(num_masses)

    init_state = get_equilibrium_states(num_masses, y, dot_y, [0, 1, 0])
    ref_state = get_equilibrium_states(num_masses, y, dot_y, [1, 0, 0])

    # NMPC formulation:

    num_states = y.shape[0]
    num_controls = 3

    weights_states = numpy.ones(y.shape)
    weights_states[0:3 * num_masses] = 1e2
    weights_states[3 * num_masses:6 * num_masses] = 1e0
    weights_states[-3:] = 1e2

    weights_controls = numpy.ones((num_controls, 1)) * 1e-2

    # states 1..N
    states = casadi.SX.sym('states', num_states, num_intervals)
    # controls 0..N-1
    controls = casadi.SX.sym('controls', num_controls, num_intervals)

    weights_states_sqrt = numpy.sqrt(weights_states)
    weights_controls_sqrt = numpy.sqrt(weights_controls)

    objective_residuals = []
    ref_control = numpy.zeros(u.shape)
    for node in range(num_intervals):
        objective_residuals.append(
            (states[:, node] - ref_state) * weights_states_sqrt)
        objective_residuals.append(
            (controls[:, node] - ref_control) * weights_controls_sqrt)

    objective_residuals = casadi.veccat(*objective_residuals)

    ode = casadi.Function('ode', [u, y], [dot_y])

    rk4_k1 = ode(u, y)
    rk4_k2 = ode(u, y + time_step / 2.0 * rk4_k1)
    rk4_k3 = ode(u, y + time_step / 2.0 * rk4_k2)
    rk4_k4 = ode(u, y + time_step * rk4_k3)
    final_y = y + time_step / 6.0 * (rk4_k1 + 2 * rk4_k2 + 2 * rk4_k3 + rk4_k4)
    integrate = casadi.Function('integrate', [u, y], [final_y])

    states_for_integration = casadi.horzcat(init_state, states[:, :-1])
    integrated_states = integrate.map(num_intervals)(controls,
                                                     states_for_integration)
    equality_constraints = states - integrated_states
    equality_constraints = casadi.veccat(equality_constraints)

    # Prepare and condense the underlying QP.
    states_vec = casadi.veccat(states)
    controls_vec = casadi.veccat(controls)

    jac_obj_residuals_wrt_states = casadi.jacobian(objective_residuals,
                                                   states_vec)
    jac_obj_residuals_wrt_controls = casadi.jacobian(objective_residuals,
                                                     controls_vec)

    jac_eq_constraints_wrt_states = casadi.jacobian(equality_constraints,
                                                    states_vec)
    jac_eq_constraints_wrt_controls = casadi.jacobian(equality_constraints,
                                                      controls_vec)

    qp_h = jac_obj_residuals_wrt_controls.T @ jac_obj_residuals_wrt_controls

    delta_x_contrib = casadi.solve(jac_eq_constraints_wrt_states,
                                   jac_eq_constraints_wrt_controls)
    delta_x_qp_contrib = -jac_obj_residuals_wrt_states @ delta_x_contrib

    qp_h += delta_x_qp_contrib.T @ delta_x_qp_contrib

    qp_g = (jac_obj_residuals_wrt_controls +
            delta_x_qp_contrib).T @ objective_residuals

    states_lbx = numpy.zeros(y.shape)
    states_ubx = numpy.zeros(y.shape)

    states_lbx.fill(-numpy.inf)
    for m in range(num_masses):
        states_lbx[3 * m + 1] = -0.01
    states_ubx.fill(numpy.inf)

    qp_lb_a = []
    qp_ub_a = []

    for _ in range(num_intervals):
        qp_lb_a.append(states_lbx)
        qp_ub_a.append(states_ubx)
    qp_lb_a = numpy.concatenate(qp_lb_a, axis=0)
    qp_ub_a = numpy.concatenate(qp_ub_a, axis=0)

    init_states = numpy.concatenate([init_state for _ in range(num_intervals)],
                                    axis=0)
    delta_x_bound_contrib = casadi.solve(jac_eq_constraints_wrt_states,
                                         equality_constraints) + init_states

    qp_lb_a -= delta_x_bound_contrib
    qp_ub_a -= delta_x_bound_contrib

    qp_a = -delta_x_contrib

    qp_fcn = casadi.Function('qp_h_fcn', [controls_vec, states_vec],
                             [qp_h, qp_g, qp_a, qp_lb_a, qp_ub_a])

    init_controls = numpy.zeros(controls_vec.shape)
    qp_h_eval, qp_g_eval, qp_a_eval, qp_lb_a_eval, qp_ub_a_eval = qp_fcn(
        init_controls, init_states)

    qp_lbx = -numpy.ones(qp_g.shape)
    qp_ubx = numpy.ones(qp_g.shape)

    # Reduce the number of rows of the A-matrix to the minimum.
    qp_a_indices = []
    for el in range(qp_lb_a.shape[0]):
        if qp_lb_a_eval[el] <= -numpy.inf and qp_ub_a_eval[el] >= numpy.inf:
            continue
        qp_a_indices.append(el)

    qp_lb_a_eval = qp_lb_a_eval[qp_a_indices]
    qp_ub_a_eval = qp_ub_a_eval[qp_a_indices]
    qp_a_eval = qp_a_eval[qp_a_indices, :]

    qp_x = casadi.SX.sym('qp_x', *qp_g.shape)
    qp = {
        'x': qp_x,
        'f': 0.5 * qp_x.T @ qp_h_eval @ qp_x + qp_x.T @ qp_g_eval,
        'g': casadi.densify(qp_a_eval @ qp_x)
    }

    qp_solver = casadi.qpsol('qp_solver', 'qpoases', qp)

    sol = qp_solver(lbx=qp_lbx, ubx=qp_ubx, lbg=qp_lb_a_eval, ubg=qp_ub_a_eval)

    x_opt = numpy.asarray(sol['x']).flatten()
    y_opt = numpy.asarray(-casadi.vertcat(sol['lam_x'], sol['lam_g']))
    f_opt = sol['f']

    num_variables = qp_x.shape[0]
    num_constraints = qp_a_eval.shape[0]

    qp_h_flat = numpy.asarray(qp_h_eval).flatten()
    qp_g_flat = numpy.asarray(qp_g_eval).flatten()
    qp_a_flat = numpy.asarray(qp_a_eval).flatten()
    qp_lb_a_flat = numpy.asarray(qp_lb_a_eval).flatten()
    qp_ub_a_flat = numpy.asarray(qp_ub_a_eval).flatten()

    return (num_variables, num_constraints, qp_h_flat, qp_g_flat, qp_a_flat,
            qp_lbx, qp_ubx, qp_lb_a_flat, qp_ub_a_flat, x_opt, y_opt, f_opt)
Beispiel #27
0
    def compute_covariance_matrix(self):

        r'''
        This function computes the covariance matrix of the estimated
        parameters from the inverse of the KKT matrix for the
        parameter estimation problem. This allows then for statements on the
        quality of the values of the estimated parameters.

        For efficiency, only the inverse of the relevant part of the matrix
        is computed using the Schur complement.

        A more detailed description of this function will follow in future
        versions.

        '''

        intro.pecas_intro()
        
        print('\n' + 20 * '-' + \
            ' PECas covariance matrix computation ' + 21 * '-')

        print('''
Computing the covariance matrix for the estimated parameters, 
this might take some time ...
''')

        self.tstart_cov_computation = time.time()

        try:

            N1 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \
                self.w.shape[0])

            N2 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \
                self.Vars.shape[0] - self.w.shape[0])

            hess = ca.blockcat([[N2, N1], [N1.T, ca.diag(self.w)],])

            # hess = hess + 1e-10 * ca.diag(self.Vars)
            
            # J2 can be re-used from parameter estimation, right?

            J2 = ca.jacobian(self.g, self.Vars)

            kkt = ca.blockcat( \

                [[hess, \
                    J2.T], \

                [J2, \
                    ca.MX(self.g.size1(), self.g.size1())]] \

                    )

            B1 = kkt[:self.pesetup.np, :self.pesetup.np]
            E = kkt[self.pesetup.np:, :self.pesetup.np]
            D = kkt[self.pesetup.np:, self.pesetup.np:]

            Dinv = ca.solve(D, E, "csparse")

            F11 = B1 - ca.mul([E.T, Dinv])

            self.fbeta = ca.MXFunction("fbeta", [self.Vars], 
                [ca.mul([self.R.T, self.R]) / \
                (self.yN.size + self.g.size1() - self.Vars.size())])

            [self.beta] = self.fbeta([self.Varshat])

            self.fcovp = ca.MXFunction("fcovp", [self.Vars], \
                [self.beta * ca.solve(F11, ca.MX.eye(F11.size1()))])

            [self.Covp] = self.fcovp([self.Varshat])

            print( \
'''Covariance matrix computation finished, run show_results() to visualize.''')


        except AttributeError as err:

            errmsg = '''
You must execute run_parameter_estimation() first before the covariance
matrix for the estimated parameters can be computed.
'''

            raise AttributeError(errmsg)


        finally:

            self.tend_cov_computation = time.time()
            self.duration_cov_computation = self.tend_cov_computation - \
                self.tstart_cov_computation
Beispiel #28
0
def freeModel(endTimeSteps=None):
    stateNames = [ "x"   # state 0
                 , "y"   # state 1
                 , "z"   # state 2
                 , "e11" # state 3
                 , "e12" # state 4
                 , "e13" # state 5
                 , "e21" # state 6
                 , "e22" # state 7
                 , "e23" # state 8
                 , "e31" # state 9
                 , "e32" # state 10
                 , "e33" # state 11
                 , "dx"  # state 12
                 , "dy"  # state 13
                 , "dz"  # state 14
                 , "w1"  # state 15
                 , "w2"  # state 16
                 , "w3"  # state 17
                 ]
    
    uNames = [ "tc"
             , "aileron"
             , "elevator"
             , "rudder"
#             , 'ddr'
             ]

    pNames = [ "wind_x"
             ]
    
    uSyms = [C.ssym(name) for name in uNames]
    uDict = dict(zip(uNames,uSyms))
    uVec = C.veccat( uSyms )

    pSyms = [C.ssym(name) for name in pNames]
    pDict = dict(zip(pNames,pSyms))
    pVec = C.veccat( pSyms )

    stateSyms = [C.ssym(name) for name in stateNames]
    stateDict = dict(zip(stateNames,stateSyms))
    stateVec = C.veccat( stateSyms )

    dx = stateDict['dx']
    dy = stateDict['dy']
    dz = stateDict['dz']

    outputs = {}
    outputs['aileron(deg)']=uDict['aileron']*180/C.pi
    outputs['elevator(deg)']=uDict['elevator']*180/C.pi
    outputs['rudder(deg)']=uDict['rudder']*180/C.pi
    (massMatrix, rhs, dRexp) = modelInteg(stateDict, uDict, pDict, outputs)
    
    zVec = C.solve(massMatrix, rhs)
    
    ddX = zVec[0:3]
    dw = zVec[3:6]

    ode = C.veccat( [ C.veccat([dx,dy,dz])
                    , dRexp.trans().reshape([9,1])
                    , ddX
                    , dw
                    ] )

    
    if endTimeSteps is not None:
        endTime,nSteps = endTimeSteps
        pNames.append("endTime")
        pDict["endTime"] = endTime
        pVec = C.veccat([pVec,endTime])
        ode = ode#/(endTime/(nSteps-1))

    dae = C.SXFunction( C.daeIn( x=stateVec, p=C.veccat([uVec,pVec])),
                        C.daeOut( ode=ode))
    return (dae, {'xVec':stateVec,'xDict':stateDict,'xNames':stateNames,
                  'uVec':uVec,'uNames':uNames,
                  'pVec':pVec,'pNames':pNames}, outputs)
# Dynamics are satisfied when this vector expression is zero.
DAE_RHS = LHS-RHS

####################
# Derive the ODE form of the equations of motion
####################

# c + M * ddq - dLddq = generalized_forces
c_ = mul(J_temp[:,:nq],dq).reshape(dLddq.shape)
c = jacobianTimesVector(dLddq.T, q, dq).reshape(dLddq.shape)
M = J_temp[:,nq:] # Mass matrix, i.e. the term that got multiplied by ddq

# M*ddq = RHS
RHS = f_gen + dLdq.T - c.T

RHS2 = solve(M,RHS) # ddq = RHS2 Symbolic system solve.

ODE_RHS = vertcat([dq,RHS2]) # d[q;dq]/dt = [dq; RHS2] 

####################
# Function for setpoint computation.
####################


####################
# Linearize the system about a setpoint and determine stability
####################

####################
# Debugging and testing stuff...
####################
            pl.ones(3), \
            pl.zeros(N), \
            ydata_noise[0,:].T

        ])

    sol = nlpsolver(x0 = V0)

    p_est_single_shooting = sol["x"][:3]

    tstart_Sigma_p = time()

    J_s = ca.jacobian(r, V)

    F_s = ca.mul(J_s.T, J_s)

    beta = (ca.mul(r.T, r) / (r.size() - V.size())) 
    Sigma_p_s = beta * ca.solve(F_s, ca.MX.eye(F_s.shape[0]), "csparse")

    beta_fcn = ca.MXFunction("beta_fcn", [V], [beta])
    print beta_fcn([sol["x"]])[0]

    Sigma_p_s_fcn = ca.MXFunction("Sigma_p_s_fcn", \
        [V] , [Sigma_p_s])

    Cov_p = Sigma_p_s_fcn([sol["x"]])[0][:3, :3]

    tend_Sigma_p = time()

    time_covariance_matrix_evlaution.append(tend_Sigma_p - tstart_Sigma_p)
Beispiel #31
0
    def __init__(self,
                 m1=1.0,
                 m2=0.1,
                 l=0.5,
                 g=9.81,
                 u_max=10.0,
                 d_max=2.0,
                 tf=2.0,
                 N=5,
                 Q=np.array([100, 150, 5, 5]),
                 Qf=np.array([10, 50, 5, 5]),
                 R=5,
                 verbose=0):
        """
        Initializes a casadi optimization model that solves a trajectory optimization
        problem using direct hermite-simpson collocation in separated form, where the
        objective is finite horizon target state tracking using LQR costs.
        The mathematical expressions for collocation are based on Matthew Kelly 2017, 
        "An Introduction to Trajectory Optimization: How to Do Your Own Direct Collocation"

        Note: In hermite simpson collocation in separated form, a finite element consists 
        of three collocation points: left endpoint, midpoint, and right endpoint.
        Each finite element is indexed using the odd indices of the collocation points,
        as these correspond to the midpoints of the finite elements
        """

        self.nx = 4  #dimension of state vector
        self.nu = 1  #dimension of input vector
        self.m1 = m1  #cart mass
        self.m2 = m2  #pendulum mass
        self.l = l  #pendulum length
        self.g = g  #gravity
        self.u_max = u_max  #maximum actuator force
        self.d_max = d_max  #extent of rail that cart travels on

        self.tf = tf  #optimization horizon
        self.N = N  #number of finite elements (number of collocation points = 2*N+1)
        self.Q = Q  #state error cost weight
        self.Qf = Qf  #final state error cost weight
        self.R = R  #input regulation cost weight

        # integer from 0 to 12 indicating how much IPOPT output to print
        self.verbose = verbose

        # initialize optimization solution memory
        self.reset_sol_prev()

        # casadi optimization model
        self.opti = ca.Opti()

        # parameters
        self.x_init = self.opti.parameter(self.nx)  # initial state
        self.x_des = self.opti.parameter(self.nx,
                                         2 * self.N + 1)  # desired states

        # decision variables
        self.X = self.opti.variable(self.nx, 2 * self.N +
                                    1)  # state at each collocation point
        self.U = self.opti.variable(self.nu, 2 * self.N +
                                    1)  # input at each collocation point

        # objective
        # simpson quadrature coefficients, to be used to compute integrals
        simp = np.empty((1, 2 * N + 1))
        simp[0, ::2] = 2
        simp[0, 1::2] = 4
        simp[0, 0], simp[0, -1] = 1, 1

        # cost = finite horizon LQR cost computed with simpson quadrature
        J = 0.0
        J += ca.dot(simp, self.U[:, :] * self.U[:, :])
        for j in range(self.nx):
            if j == 1:
                # angle error must be computed using trig functions to account for
                # periodicity
                J += self.Q[j] * ca.dot(simp,(ca.cos(self.X[j,:])-ca.cos(self.x_des[j,:])) \
                    *(ca.cos(self.X[j,:])-ca.cos(self.x_des[j,:])))
                J += self.Q[j] * ca.dot(simp,(ca.sin(self.X[j,:])-ca.sin(self.x_des[j,:])) \
                    *(ca.sin(self.X[j,:])-ca.sin(self.x_des[j,:])))
                J += self.Qf[j] * (ca.cos(self.X[j,-1])-ca.cos(self.x_des[j,-1])) \
                    *(ca.cos(self.X[j,-1])-ca.cos(self.x_des[j,-1]))
                J += self.Qf[j] * (ca.sin(self.X[j,-1])-ca.sin(self.x_des[j,-1])) \
                    *(ca.sin(self.X[j,-1])-ca.sin(self.x_des[j,-1]))
            else:
                J += self.Q[j] * ca.dot(simp, (self.X[j,:]-self.x_des[j,:]) \
                    *(self.X[j,:]-self.x_des[j,:]))
                J += self.Qf[j] * (self.X[j,-1]-self.x_des[j,-1]) \
                    *(self.X[j,-1]-self.x_des[j,-1])

        self.opti.minimize(J)

        # position bound constraint
        self.opti.subject_to(self.opti.bounded( \
            np.full(self.X[0,:].shape, -self.d_max),
            self.X[0,:],
            np.full(self.X[0,:].shape, self.d_max)
            ))

        # control bound constraint
        self.opti.subject_to(self.opti.bounded( \
            np.full(self.U[:,:].shape, -self.u_max),
            self.U[:,:],
            np.full(self.U[:,:].shape, self.u_max)
            ))

        # initial condition constraint
        self.opti.subject_to(self.X[:, 0] == self.x_init)

        # dynamics
        # symbolic variables used to derive equations of motion
        x = ca.SX.sym('x', self.nx)  #state
        u = ca.SX.sym('u', self.nu)  #control
        m1, m2, l, g = self.m1, self.m2, self.l, self.g
        # equations of motion taken from Russ Tedrake underactuated robotics ch.3
        M = ca.SX(np.array([ \
            [m1 + m2, m2*l*ca.cos(x[1])],
            [m2*l*ca.cos(x[1]), m2*l**2]
            ]))
        C = ca.SX(np.array([ \
            [0.0, -m2*l*x[3]*ca.sin(x[1])],
            [0.0, 0.0]
            ]))
        tau_g = ca.SX(np.array([ \
            [0.0],
            [-m2*g*l*ca.sin(x[1])]
            ]))
        B = ca.SX(np.array([ \
            [1.0],
            [0.0]
            ]))
        xdot = ca.vertcat( \
            x[2:], # d/dt(q) = qdot
            ca.solve(M, -C@x[2:]+tau_g+B@u) # M@qddot = (-C@qdot+tau_g+B@u)
            )
        f = ca.Function('f', [x, u], [xdot])  # xdot = f(x,u)

        for i in range(2 * self.N + 1):
            if i % 2 != 0:
                # for each finite element:
                x_left, x_mid, x_right = self.X[:, i -
                                                1], self.X[:, i], self.X[:,
                                                                         i + 1]
                u_left, u_mid, u_right = self.U[:, i -
                                                1], self.U[:, i], self.U[:,
                                                                         i + 1]
                f_left, f_mid, f_right = f(x_left, u_left), f(x_mid, u_mid), f(
                    x_right, u_right)

                # interpolation constraints
                self.opti.subject_to( \
                    # equation (6.11) in Kelly 2017

                    x_mid == (x_left+x_right)/2.0 + self.tf/self.N*(f_left-f_right)/8.0)

                # collocation constraints
                self.opti.subject_to( \
                    # equation (6.12) in Kelly 2017

                    self.tf/self.N*(f_left+4*f_mid+f_right)/6.0 == x_right-x_left)
Beispiel #32
0
  def __init__(self,NR=4,debug=False,quatnorm=False):
    """
    Keyword arguments:
      NR    -- the number of rotors
      debug -- wether to print out debug info
      quatnorm -- add the quaternion norm to the DAE rhs
    """

    # ----------- system states and their derivatives ----
    pos = struct_symSX(["x","y","z"])     # rigid body centre of mass position [m]   {0}   
    v   = struct_symSX(["vx","vy","vz"])  # rigid body centre of mass position velocity [m/s] {0}

    NR = 4                               # Number of rotors
    
    states = struct_symSX([
      entry("p",struct=pos),
      entry("v",struct=v),
      entry("q",shape=4),                # quaternions  {0} -> {1}
      entry("w",shape=3),                # rigid body angular velocity w_101 [rad/s] {1}
      entry("r",shape=NR)                # spin speed of rotor, wrt to platform. [rad/s] Should be positive!
                                         # The signs are such that positive means lift generating, regardless of spin direction.
      
    ])
    
    pos, v, q, w, r = states[...]

    # ------------------------------------------------

    dist = struct_symSX([
      entry("Faer",shape=NR),             # Disturbance on aerodynamic forcing [N]
      entry("Caer",shape=NR)             # Disturbance on aerodynamic torques [Nm]
    ])


    # ----------------- Controls ---------------------
    controls = struct_symSX([
      entry("CR",shape=NR)              # [Nm]
          # Torques of the motors that drive the rotors, acting from platform on propeller
          # The torque signs are always positive when putting energy in the propellor,
          # regardless of spin direction.
          # 
    ])
    
    CR = controls["CR"]
    
    # ------------------------------------------------


    # ----------------  Temporary symbols --------------
    F = ssym("F",3)          # Forces acting on the platform in {1} [N]
    C = ssym("C",3)          # Torques acting on the platform in {1} [Nm]

    rotors_Faer = [ssym("Faer_%d" %i,3,1) for i in range(NR)] # Placeholder for aerodynamic force acting on propeller {1} [N]
    rotors_Caer = [ssym("Caer_%d" %i,3,1) for i in range(NR)] # Placeholder for aerodynamic torques acting on propeller {1} [Nm]

    # ---------------------------------------------------


    # ----------------- Parameters ---------------------
    
    rotor_model = struct_symSX([
         "c",        # c          Cord length [m]
         "R",        # R          Radius of propeller [m]
         "CL_alpha", # CL_alpha   Lift coefficient [-]
         "alpha_0",  # alpha_0
         "CD_alpha", # CD_alpha   Drag coefficient [-]
         "CD_i",     # CD_i       Induced drag coefficient [-]  
    ])
    
    p = struct_symSX([
      entry("rotors_model",repeat=NR,struct=rotor_model),    # Parameters that describe the rotor model
      entry("rotors_I",repeat=NR,shape=sp_diag(3)),  # Inertias of rotors [kg.m^2]
      entry("rotors_spin",repeat=NR),    # Direction of spin from each rotor. 1 means rotation around positive z.
      entry("rotors_p",repeat=NR,shape=3),  # position of rotors in {1} [m],
      entry("I",sym=casadi.diag(ssym("[Ix,Iy,Iz]"))), # Inertia of rigid body [kg.m^2]
      "m",       # Mass of the whole system [kg]
      "g",       # gravity [m/s^2]
      "rho",     # Air density [kg/m^3]
    ])
    
    I,m,g,rho = p[["I","m","g","rho"]]
 
    # --------------------------------------------------

   
    # ----------------- Parameters fillin's ---------------------

    p_ = p()
    p_["rotors_spin"] = [1,-1,1,-1]

    p_["rotors_model",:,{}] =  { "c": 0.01, "R" : 0.127, "CL_alpha": 6.0, "alpha_0": 0.15, "CD_alpha": 0.02, "CD_i": 0.05} # c          Cord length [m]

    p_["m"] = 0.5      # [kg]
    p_["g"] = 9.81     # [N/kg]
    p_["rho"] = 1.225  # [kg/m^3]

    L = 0.25
    
    I_max = p_["m"] * L**2 # Inertia of a point mass at a distance L
    I_ref = I_max/5   
    
    p_["I"] = casadi.diag([I_ref/2,I_ref/2,I_ref]) # [N.m^2]
    

    p_["rotors_p",0] = DM([L,0,0])
    p_["rotors_p",1] = DM([0,L,0])
    p_["rotors_p",2] = DM([-L,0,0])
    p_["rotors_p",3] = DM([0,-L,0])

    for i in range(NR):
        R_ = p_["rotors_model",i,"R"] #  Radius of propeller [m]
        m_ = 0.01 # Mass of a propeller [kg]
        I_max = m_ * R_**2 # Inertia of a point mass
        I_ref = I_max/5 
        p_["rotors_I",i] = casadi.diag([I_ref/2,I_ref/2,I_ref])

    if debug:
        print p.vecNZcat()
        
    dist_ = dist(0)
        
    # ----------------- Scaling ---------------------
    
    scaling_states   = states(1)
    scaling_controls = controls(1)
    
    scaling_states["r"] = 500
    scaling_controls["CR"] = 0.005
    
    scaling_dist = dist()
    
    scaling_dist["Faer"] = float(p_["m"]*p_["g"]/NR)
    scaling_dist["Caer"] = 0.0026

    # ----------- Frames ------------------
    T_10 = mul(tr(*pos),Tquat(*q))
    T_01 = kin_inv(T_10)
    R_10 = T2R(T_10)
    R_01 = R_10.T
    # -------------------------------------

    dstates = struct_symSX(states)
    
    dp,dv,dq,dw,dr = dstates[...]
    
    res = struct_SX(states) # DAE residual hand side
    # ----------- Dynamics of the body ----
    res["p"] = v - dp
    # Newton, but transform the force F from {1} to {0}
    res["v"] = mul(R_10,F) - m*dv
    # Kinematics of the quaterion.
    res["q"] = mul(quatDynamics(*q),w)-dq
    # This is a trick by Sebastien Gros to stabilize the quaternion evolution equation
    res["q"] += -q*(sumAll(q**2)-1)
    # Agular impulse H_1011
    H = mul(p["I"],w)    # Due to platform
    for i in range(NR):
      H+= mul(p["rotors_I",i], w + vertcat([0,0,p["rotors_spin",i]*r[i]])) # Due to rotor i

    dH = mul(jacobian(H,w),dw) + mul(jacobian(H,q),dq) + mul(jacobian(H,r),dr) + casadi.cross(w,H)

    res["w"] = C - dH

    for i in range(NR):
      res["r",i] = CR[i] + p["rotors_spin",i]*rotors_Caer[i][2] - p["rotors_I",i][2]*(dr[i]+dw[2]) # Dynamics of rotor i
    
    # ---------------------------------

    # Make a vector of f ?
    #if quatnorm:
    #    f = vertcat(f+[sumAll(q**2)-1])
    #else:
    #    f = vertcat(f)  

    # ------------ Force model ------------

    Fg = mul(R_01,vertcat([0,0,-g*m]))

    F_total = Fg + sum(rotors_Faer)    # Total force acting on the platform
    C_total = SX([0,0,0])                    # Total torque acting on the platform

    for i in range(NR):
       C_total[:2] += rotors_Caer[i][:2] # The x and y components propagate
       C_total[2] -= p["rotors_spin",i]*CR[i]         # the z compent moves through a serparate system
       C_total += casadi.cross(p["rotors_p",i],rotors_Faer[i]) # Torques due to thrust

    
    res = substitute(res,F,F_total)
    res = substitute(res,C,C_total)
    
    subs_before = []
    subs_after  = []
    
    v_global = mul(R_01,v)
    u_z = SX([0,0,1])
    
    # Now fill in the aerodynamic forces
    for i in range(NR):
        c,R,CL_alpha,alpha_0, CD_alpha, CD_i = p["rotors_model",i,...]
        #Bristeau P-jean, Martin P, Salaun E, Petit N. The role of propeller aerodynamics in the model of a quadrotor UAV. In: Proceedings of the European Control Conference 2009.; 2009:683-688.
        v_local = v_global + (casadi.cross(w,p["rotors_p",i])) # Velocity at rotor i
        rotors_Faer_physics =  (rho*c*R**3*r[i]**2*CL_alpha*(alpha_0/3.0-v_local[2]/(2.0*R*r[i]))) * u_z
        subs_before.append(rotors_Faer[i])
        subs_after.append(rotors_Faer_physics  + dist["Faer",i])
        rotors_Caer_physics = -p["rotors_spin",i]*rho*c*R**4*r[i]**2*(CD_alpha/4.0+CD_i*alpha_0**2*(alpha_0/4.0-2.0*v_local[2]/(3.0*r[i]*R))-CL_alpha*v_local[2]/(r[i]*R)*(alpha_0/3.0-v_local[2]/(2.0*r[i]*R))) * u_z
        subs_before.append(rotors_Caer[i])
        subs_after.append(rotors_Caer_physics  + dist["Caer",i])
    

    
    res = substitute(res,veccat(subs_before),veccat(subs_after))
    
    # Make an explicit ode
    rhs = - casadi.solve(jacobian(res,dstates),substitute(res,dstates,0))
    
    # --------------------------------------

    self.res_w = res
    self.res = substitute(res,dist,dist_)
    self.res_ = substitute(self.res,p,p_)
    
    resf = SXFunction([dstates, states, controls ],[self.res_])
    resf.init()
    self.resf = resf
    
    self.rhs_w = rhs
    
    self.rhs = substitute(rhs,dist,dist_)

    self.rhs_ = substitute(self.rhs,p,p_)

    t = SX("t")
    # We end up with a DAE that captures the system dynamics
    dae = SXFunction(daeIn(t=t,x=states,p=controls),daeOut(ode=self.rhs_))
    dae.init()
    
    self.dae = dae
    
    cdae = SXFunction(controldaeIn(t=t, x=states, u= controls,p=p),daeOut(ode=self.rhs))
    cdae.init()
    self.cdae = cdae

    self.states  = states
    self.dstates = dstates
    self.p = p
    self.p_ = p_
    self.controls = controls
    self.NR = NR
    self.w = dist
    self.w_ = dist_
    self.t = t
    
    self.states_  = states()
    self.dstates_ = states()
    self.controls_ = controls()
    
    self.scaling_states = scaling_states
    self.scaling_controls = scaling_controls
    self.scaling_dist = scaling_dist
Beispiel #33
0
    def __create_reference(self, wref, tuning, lam_g_ref):
        """ Create periodic reference and tuning data.
        """

        # period of reference
        self.__Nref = len(wref['u'])

        # create reference and tuning sequence
        # for each starting point in period
        ref_pr = []
        ref_du = []
        ref_du_struct = []
        H = []
        q = []

        for k in range(self.__Nref):

            # reference primal solution
            refk = []
            for j in range(self.__N):

                refk += [
                    wref['x', (k + j) % self.__Nref],
                    wref['u', (k + j) % self.__Nref]
                ]

                if 'us' in self.__vars:
                    refk += [wref['us', (k + j) % self.__Nref]]

            refk.append(wref['x', (k + self.__N) % self.__Nref])

            # reference dual solution
            lamgk = self.__g(0.0)
            lamgk['init'] = -lam_g_ref['dyn', (k - 1) % self.__Nref]
            for j in range(self.__N):
                lamgk['dyn', j] = lam_g_ref['dyn', (k + j) % self.__Nref]
                if 'g' in list(lamgk.keys()):
                    lamgk['g', j] = lam_g_ref['g', (k + j) % self.__Nref]
                if 'h' in list(lamgk.keys()):
                    lam_h = [lam_g_ref['h', (k + j) % self.__Nref]]
                    if 'usc' in self.__vars:
                        lam_h += [-self.__scost]  # TODO not entirely correct

                    lamgk['h', j] = ct.vertcat(*lam_h)
            lamgk['term'] = self.__p_operator(
                lam_g_ref['dyn', (k + self.__N - 1) % self.__Nref])

            # adjust dual solution of terminal constraint is projected
            if self.__nx_term != self.__nx:

                # find new terminal multiplier
                A_m = []
                b_m = []
                A_factor = ca.DM.eye(self.__nx)
                for j in range(self.__N):
                    A_m.append(
                        ct.mtimes(
                            ct.mtimes(
                                self.__S['B'][(self.__N - j - 1) %
                                              self.__Nref].T, A_factor),
                            self.__jac_p_operator(ca.DM.ones(self.__nx, 1)).T))
                    b_m.append(
                        ct.mtimes(
                            ct.mtimes(
                                self.__S['B'][(self.__N - j - 1) %
                                              self.__Nref].T, A_factor),
                            lam_g_ref['dyn',
                                      (k + self.__N - 1) % self.__Nref]))
                    A_factor = ct.mtimes(
                        self.__S['A'][(self.__N - j - 1) % self.__Nref].T,
                        A_factor)
                A_m = ct.vertcat(*A_m)
                b_m = ct.vertcat(*b_m)
                LI_indeces = [
                ]  # indeces of first full rank number linearly independent rows
                R0 = 0
                for i in range(A_m.shape[0]):
                    R = np.linalg.matrix_rank(A_m[LI_indeces + [i], :])
                    if R > R0:
                        LI_indeces.append(i)
                        R0 = R
                lamgk['term'] = ca.solve(A_m[LI_indeces, :],
                                         b_m[LI_indeces, :])

                # recursively update dynamics multipliers
                delta_lam = -lam_g_ref['dyn', (k + self.__N - 1) %
                                       self.__Nref] + ct.mtimes(
                                           self.__jac_p_operator(
                                               ca.DM.ones(self.__nx, 1)).T,
                                           lamgk['term'])
                lamgk['dyn', self.__N - 1] += delta_lam
                for j in range(1, self.__N + 1):
                    delta_lam = ct.mtimes(
                        self.__S['A'][(self.__N - j) % self.__Nref].T,
                        delta_lam)
                    if j < self.__N:
                        lamgk['dyn', self.__N - 1 - j] += delta_lam
                    else:
                        lamgk['init'] += -delta_lam

            ref_pr.append(ct.vertcat(*refk))
            ref_du.append(lamgk.cat)
            ref_du_struct.append(lamgk)

            if tuning is not None:
                H.append([
                    tuning['H'][(k + j) % self.__Nref] for j in range(self.__N)
                ])
                q.append([
                    tuning['q'][(k + j) % self.__Nref] for j in range(self.__N)
                ])

        self.__ref = ref_pr
        self.__ref_du = ref_du
        self.__ref_du_struct = ref_du_struct
        self.__Href = H
        self.__qref = q

        return None
Beispiel #34
0
def solve(a, b, solver):

    return ca.solve(a, b, solver)
def inverse_symbolic(x):
	assert type(x)==casadi.SXMatrix
	xinv = casadi.solve(x, casadi.SXMatrix.eye(x.size1()))
	return xinv
Beispiel #36
0
    def _priori_update(self, x_mean, x_cov, u, p, theta):
        x_samples = self._get_sampled_states(x_mean, x_cov)

        # Perform predictions via simulation
        simulation_results = []
        x_cal_x_k_at_k_minus_1 = []
        y_alg_cal_x_k_at_k_minus_1 = []
        for s in range(self.n_samples):
            x_0_i = DM(x_samples[s])
            simulation_results_i = self.model.simulate(x_0=x_0_i,
                                                       t_0=self.t,
                                                       t_f=self.t + self.t_s,
                                                       u=u,
                                                       p=p,
                                                       theta=theta,
                                                       y_0=self.y_guess)
            simulation_results.append(simulation_results_i)
            x_cal_x_k_at_k_minus_1.append(
                simulation_results_i.final_condition()[0])
            y_alg_cal_x_k_at_k_minus_1.append(
                simulation_results[s].final_condition()[1])

        # fit the polynomial for x

        a_x = []
        x_hat_k_minus = []
        for i in range(self.model.n_x):
            x_i_vector = vertcat(
                *[x_cal_x_k_at_k_minus_1[s][i] for s in range(self.n_samples)])
            a_x.append(mtimes(self._ls_factor, x_i_vector))

        # get the mean for x
        for i in range(self.model.n_x):
            x_hat_k_minus.append(a_x[i][0])
        x_hat_k_minus = vertcat(*x_hat_k_minus)

        # get the covariance for x
        p_k_minus = DM.zeros(self.model.n_x, self.model.n_x)
        for i in range(self.model.n_x):
            for j in range(self.model.n_x):
                p_k_minus[i, j] = sum(
                    [a_x[i][k] * a_x[j][k]
                     for k in range(1, self.n_samples)]) + self.r_v[i, j]

        # calculate the measurement for each sample
        y_cal_k_at_k_minus_1 = []
        for s in range(self.n_samples):
            y_cal_k_at_k_minus_1.append(
                self._get_measurement_from_prediction(
                    x_cal_x_k_at_k_minus_1[s], y_alg_cal_x_k_at_k_minus_1[s],
                    u))
        n_meas = y_cal_k_at_k_minus_1[0].numel()

        # find the measurements estimate
        a_meas = []
        y_meas_hat_k_minus = []
        for i in range(n_meas):
            y_meas_i_vector = vertcat(
                *[y_cal_k_at_k_minus_1[s][i] for s in range(self.n_samples)])
            a_meas.append(mtimes(self._ls_factor, y_meas_i_vector))

        # get the mean for the measurement
        for i in range(n_meas):
            y_meas_hat_k_minus.append(a_meas[i][0])
        y_meas_hat_k_minus = vertcat(*y_meas_hat_k_minus)

        # get the covariance for the meas
        p_yk_yk = DM.zeros(n_meas, n_meas)
        for i in range(n_meas):
            for j in range(n_meas):
                p_yk_yk[i, j] = sum([
                    a_meas[i][k] * a_meas[j][k]
                    for k in range(1, self.n_samples)
                ])
        p_yk_yk = p_yk_yk + self.r_n

        # get cross-covariance
        p_xk_yk = DM.zeros(self.model.n_x, n_meas)
        for i in range(self.model.n_x):
            for j in range(n_meas):
                p_xk_yk[i, j] = sum([
                    a_x[i][k] * a_meas[j][k] for k in range(1, self.n_samples)
                ])

        # k_gain = mtimes(p_xk_yk, inv(p_yk_yk))
        k_gain = solve(p_yk_yk.T, p_xk_yk.T).T

        return x_hat_k_minus, p_k_minus, y_meas_hat_k_minus, p_yk_yk, k_gain
Beispiel #37
0
def solve(a, b, solver):

    return ca.solve(a, b, solver)
Beispiel #38
0
    def __init__(self, inertial_frame_id='world'):
        Vehicle.__init__(self, inertial_frame_id)

        # Declaring state variables
        ## Generalized position vector
        self.eta = casadi.SX.sym('eta', 6)
        ## Generalized velocity vector
        self.nu = casadi.SX.sym('nu', 6)

        # Build the Coriolis matrix
        self.CMatrix = casadi.SX.zeros(6, 6)

        S_12 = -cross_product_operator(
            casadi.mtimes(self._Mtotal[0:3, 0:3], self.nu[0:3]) +
            casadi.mtimes(self._Mtotal[0:3, 3:6], self.nu[3:6]))
        S_22 = -cross_product_operator(
            casadi.mtimes(self._Mtotal[3:6, 0:3], self.nu[0:3]) +
            casadi.mtimes(self._Mtotal[3:6, 3:6], self.nu[3:6]))

        self.CMatrix[0:3, 3:6] = S_12
        self.CMatrix[3:6, 0:3] = S_12
        self.CMatrix[3:6, 3:6] = S_22

        # Build the damping matrix (linear and nonlinear elements)
        self.DMatrix = -casadi.diag(self._linear_damping)
        self.DMatrix -= casadi.diag(self._linear_damping_forward_speed)
        self.DMatrix -= casadi.diag(self._quad_damping * self.nu)

        # Build the restoring forces vectors wrt the BODY frame
        Rx = np.array(
            [[1, 0, 0],
             [0, casadi.cos(self.eta[3]), -1 * casadi.sin(self.eta[3])],
             [0, casadi.sin(self.eta[3]),
              casadi.cos(self.eta[3])]])
        Ry = np.array(
            [[casadi.cos(self.eta[4]), 0,
              casadi.sin(self.eta[4])], [0, 1, 0],
             [-1 * casadi.sin(self.eta[4]), 0,
              casadi.cos(self.eta[4])]])
        Rz = np.array(
            [[casadi.cos(self.eta[5]), -1 * casadi.sin(self.eta[5]), 0],
             [casadi.sin(self.eta[5]),
              casadi.cos(self.eta[5]), 0], [0, 0, 1]])

        R_n_to_b = casadi.transpose(casadi.mtimes(Rz, casadi.mtimes(Ry, Rx)))

        if inertial_frame_id == 'world_ned':
            Fg = casadi.SX([0, 0, -self.mass * self.gravity])
            Fb = casadi.SX([0, 0, self.volume * self.gravity * self.density])
        else:
            Fg = casadi.SX([0, 0, self.mass * self.gravity])
            Fb = casadi.SX([0, 0, -self.volume * self.gravity * self.density])

        self.gVec = casadi.SX.zeros(6)

        self.gVec[0:3] = -1 * casadi.mtimes(R_n_to_b, Fg + Fb)
        self.gVec[3:6] = -1 * casadi.mtimes(
            R_n_to_b,
            casadi.cross(self._cog, Fg) + casadi.cross(self._cob, Fb))

        # Build Jacobian
        T = 1 / casadi.cos(self.eta[4]) * np.array(
            [[
                0,
                casadi.sin(self.eta[3]) * casadi.sin(self.eta[4]),
                casadi.cos(self.eta[3]) * casadi.sin(self.eta[4])
            ],
             [
                 0,
                 casadi.cos(self.eta[3]) * casadi.cos(self.eta[4]),
                 -casadi.cos(self.eta[4]) * casadi.sin(self.eta[3])
             ], [0, casadi.sin(self.eta[3]),
                 casadi.cos(self.eta[3])]])

        self.eta_dot = casadi.vertcat(
            casadi.mtimes(casadi.transpose(R_n_to_b), self.nu[0:3]),
            casadi.mtimes(T, self.nu[3::]))

        self.u = casadi.SX.sym('u', 6)

        self.nu_dot = casadi.solve(
            self._Mtotal, self.u - casadi.mtimes(self.CMatrix, self.nu) -
            casadi.mtimes(self.DMatrix, self.nu) - self.gVec)
Beispiel #39
0
    def _create_augmented_model_and_p_update(self, model):
        """

        :type model: SystemModel
        """
        aug_model = SystemModel("aug_linearized_EKF_model")
        aug_model.include_state(self.model.x_sym)
        aug_model.include_state(self.model.y_sym)
        aug_model.include_control(self.model.u_sym)
        aug_model.include_parameter(self.model.p_sym)
        aug_model.include_theta(self.model.theta_sym)

        # remove u_par (self.model.u_par model.u_par)
        all_sym = list(self.model.all_sym)

        # Mean
        a_func = Function("A_matrix", all_sym,
                          [jacobian(self.model.ode, self.model.x_sym)])
        b_func = Function("B_matrix", all_sym,
                          [jacobian(self.model.ode, self.model.y_sym)])
        c_func = Function("C_matrix", all_sym,
                          [jacobian(self.model.alg, self.model.x_sym)])
        d_func = Function("D_matrix", all_sym,
                          [jacobian(self.model.alg, self.model.y_sym)])

        x_lin = aug_model.create_parameter("x_lin", self.model.n_x)
        y_lin = aug_model.create_parameter("y_lin", self.model.n_y)

        all_sym[1] = x_lin
        all_sym[2] = y_lin

        a_matrix = a_func(*all_sym)
        b_matrix = b_func(*all_sym)
        c_matrix = c_func(*all_sym)
        d_matrix = d_func(*all_sym)

        a_aug_matrix = vertcat(
            horzcat(a_matrix, b_matrix),
            horzcat(
                mtimes(-solve(d_matrix, c_matrix), a_matrix),
                mtimes(-solve(d_matrix, c_matrix), b_matrix),
            ),
        )

        x_aug = vertcat(model.x_sym, model.y_sym)
        aug_model.include_equations(ode=[mtimes(a_aug_matrix, x_aug)])

        # Covariance
        gamma = vertcat(DM.eye(self.model.n_x), -solve(d_matrix, c_matrix))

        trans_matrix_sym = SX.sym("trans_matrix", a_aug_matrix.shape)
        p_matrix_sym = SX.sym("p_matrix",
                              (model.n_x + model.n_y, model.n_x + model.n_y))
        gamma_matrix_sym = SX.sym("gamma_matrix", gamma.shape)
        q_matrix_sym = SX.sym("q_matrix", (self.model.n_x, self.model.n_x))

        p_kpp = mtimes(trans_matrix_sym,
                       mtimes(p_matrix_sym, trans_matrix_sym.T)) + mtimes(
                           gamma_matrix_sym,
                           mtimes(q_matrix_sym, gamma_matrix_sym.T))

        a_aug_matrix_func = Function("trans_matrix", all_sym, [a_aug_matrix])
        gamma_func = Function("gamma", all_sym, [gamma])
        p_update_func = Function(
            "p_update",
            [trans_matrix_sym, p_matrix_sym, gamma_matrix_sym, q_matrix_sym],
            [p_kpp],
        )

        return aug_model, a_aug_matrix_func, gamma_func, p_update_func
    def _estimate_standard_ukf(self, t_k, y_k, u_k):
        if not self._checked:
            self._check()
            self._checked = True
        if not self._types_fixed:
            self._fix_types()
            self._types_fixed = True

        x_mean = self.x_mean
        x_cov = self.p_k

        # obtain the weights
        sigma_points, weights_m, weights_c = self._get_sigma_points_and_weights(
            x_mean, x_cov)

        # Obtain the unscented transformation points via simulation
        simulation_results = []
        x_ut_list = []
        y_ut_list = []
        x_aug_ut_list = []
        meas_ut_list = []
        for i in range(self.n_sigma_points):
            x_0_i = sigma_points[i]
            simulation_results_i = self.model.simulate(x_0=x_0_i,
                                                       t_0=self.t,
                                                       t_f=self.t + self.t_s,
                                                       u=u_k,
                                                       p=self.p,
                                                       theta=self.theta,
                                                       y_0=self.y_guess)
            simulation_results.append(simulation_results_i)
            x_ut_list.append(simulation_results_i.final_condition()[0])
            y_ut_list.append(simulation_results[i].final_condition()[1])
            x_aug_ut_list.append(vertcat(x_ut_list[-1], y_ut_list[-1]))

            meas_ut_list.append(
                self._get_measurement_from_prediction(x_ut_list[i],
                                                      y_ut_list[i], u_k))

        # Obtain the means
        x_aug_pred = sum([
            weights_m[i] * x_aug_ut_list[i] for i in range(self.n_sigma_points)
        ])
        x_pred = x_aug_pred[:self.model.n_x]
        meas_pred = sum([
            weights_m[i] * meas_ut_list[i] for i in range(self.n_sigma_points)
        ])

        # Compute the covariances
        cov_x_aug_pred = sum([
            weights_c[i] * mtimes((x_aug_ut_list[i] - x_aug_pred),
                                  (x_aug_ut_list[i] - x_aug_pred).T)
            for i in range(self.n_sigma_points)
        ])
        cov_x_aug_pred[:self.model.n_x, :self.model.n_x] += self.r_v

        cov_meas_pred = sum([
            weights_c[i] * mtimes((meas_ut_list[i] - meas_pred),
                                  (meas_ut_list[i] - meas_pred).T)
            for i in range(self.n_sigma_points)
        ]) + self.r_n

        cov_xmeas_pred = sum([
            weights_c[i] * mtimes((x_aug_ut_list[i] - x_aug_pred),
                                  (meas_ut_list[i] - meas_pred).T)
            for i in range(self.n_sigma_points)
        ])

        # Calculate the gain
        k_gain = solve(cov_meas_pred.T, cov_xmeas_pred.T).T

        # Correct prediction of the state estimation
        x_mean = x_aug_pred + mtimes(k_gain, (y_k - meas_pred))
        meas_corr = self._get_measurement_from_prediction(
            x_mean[:self.model.n_x], x_mean[self.model.n_x:], u_k)
        print('Predicted state: {}'.format(x_pred))
        print('Prediction error: {}'.format(y_k - meas_pred))
        print('Correction: {}'.format(mtimes(k_gain, (y_k - meas_pred))))
        print('Corrected state: {}'.format(x_mean))
        print('Measurement: {}'.format(y_k))
        print('Corrected Meas.: {}'.format(meas_corr))

        # Correct covariance prediction
        cov_x_aug = cov_x_aug_pred - mtimes(k_gain,
                                            mtimes(cov_meas_pred, k_gain.T))

        self.x_mean = x_mean
        self.p_k = cov_x_aug

        # Save variables in local object
        self._x_aug = x_mean
        self.x_mean = x_mean[:self.model.n_x]
        self._p_k_aug = cov_x_aug_pred
        self.p_k = cov_x_aug[:self.model.n_x, :self.model.n_x]

        # Save in the data set
        self.dataset.insert_data('x', t_k, self.x_mean)
        self.dataset.insert_data('y', t_k, x_mean[self.model.n_x:])
        self.dataset.insert_data('meas', t_k, meas_corr)
        self.dataset.insert_data('P', t_k, vec(self.p_k))
        self.dataset.insert_data('P_y', t_k, cov_x_aug[self.model.n_x:,
                                                       self.model.n_x:])

        return x_mean, cov_x_aug
Beispiel #41
0
    def __init__(self, NR=4, debug=False, quatnorm=False):
        """
    Keyword arguments:
      NR    -- the number of rotors
      debug -- wether to print out debug info
      quatnorm -- add the quaternion norm to the DAE rhs
    """

        # ----------- system states and their derivatives ----
        pos = struct_symSX(["x", "y", "z"
                            ])  # rigid body centre of mass position [m]   {0}
        v = struct_symSX(
            ["vx", "vy",
             "vz"])  # rigid body centre of mass position velocity [m/s] {0}

        NR = 4  # Number of rotors

        states = struct_symSX([
            entry("p", struct=pos),
            entry("v", struct=v),
            entry("q", shape=4),  # quaternions  {0} -> {1}
            entry("w",
                  shape=3),  # rigid body angular velocity w_101 [rad/s] {1}
            entry(
                "r", shape=NR
            )  # spin speed of rotor, wrt to platform. [rad/s] Should be positive!
            # The signs are such that positive means lift generating, regardless of spin direction.
        ])

        pos, v, q, w, r = states[...]

        # ------------------------------------------------

        dist = struct_symSX([
            entry("Faer", shape=NR),  # Disturbance on aerodynamic forcing [N]
            entry("Caer", shape=NR)  # Disturbance on aerodynamic torques [Nm]
        ])

        # ----------------- Controls ---------------------
        controls = struct_symSX([
            entry("CR", shape=NR)  # [Nm]
            # Torques of the motors that drive the rotors, acting from platform on propeller
            # The torque signs are always positive when putting energy in the propellor,
            # regardless of spin direction.
            #
        ])

        CR = controls["CR"]

        # ------------------------------------------------

        # ----------------  Temporary symbols --------------
        F = ssym("F", 3)  # Forces acting on the platform in {1} [N]
        C = ssym("C", 3)  # Torques acting on the platform in {1} [Nm]

        rotors_Faer = [
            ssym("Faer_%d" % i, 3, 1) for i in range(NR)
        ]  # Placeholder for aerodynamic force acting on propeller {1} [N]
        rotors_Caer = [
            ssym("Caer_%d" % i, 3, 1) for i in range(NR)
        ]  # Placeholder for aerodynamic torques acting on propeller {1} [Nm]

        # ---------------------------------------------------

        # ----------------- Parameters ---------------------

        rotor_model = struct_symSX([
            "c",  # c          Cord length [m]
            "R",  # R          Radius of propeller [m]
            "CL_alpha",  # CL_alpha   Lift coefficient [-]
            "alpha_0",  # alpha_0
            "CD_alpha",  # CD_alpha   Drag coefficient [-]
            "CD_i",  # CD_i       Induced drag coefficient [-]  
        ])

        p = struct_symSX([
            entry("rotors_model", repeat=NR, struct=rotor_model
                  ),  # Parameters that describe the rotor model
            entry("rotors_I", repeat=NR,
                  shape=sp_diag(3)),  # Inertias of rotors [kg.m^2]
            entry(
                "rotors_spin", repeat=NR
            ),  # Direction of spin from each rotor. 1 means rotation around positive z.
            entry("rotors_p", repeat=NR,
                  shape=3),  # position of rotors in {1} [m],
            entry("I", sym=casadi.diag(
                ssym("[Ix,Iy,Iz]"))),  # Inertia of rigid body [kg.m^2]
            "m",  # Mass of the whole system [kg]
            "g",  # gravity [m/s^2]
            "rho",  # Air density [kg/m^3]
        ])

        I, m, g, rho = p[["I", "m", "g", "rho"]]

        # --------------------------------------------------

        # ----------------- Parameters fillin's ---------------------

        p_ = p()
        p_["rotors_spin"] = [1, -1, 1, -1]

        p_["rotors_model", :, {}] = {
            "c": 0.01,
            "R": 0.127,
            "CL_alpha": 6.0,
            "alpha_0": 0.15,
            "CD_alpha": 0.02,
            "CD_i": 0.05
        }  # c          Cord length [m]

        p_["m"] = 0.5  # [kg]
        p_["g"] = 9.81  # [N/kg]
        p_["rho"] = 1.225  # [kg/m^3]

        L = 0.25

        I_max = p_["m"] * L**2  # Inertia of a point mass at a distance L
        I_ref = I_max / 5

        p_["I"] = casadi.diag([I_ref / 2, I_ref / 2, I_ref])  # [N.m^2]

        p_["rotors_p", 0] = DMatrix([L, 0, 0])
        p_["rotors_p", 1] = DMatrix([0, L, 0])
        p_["rotors_p", 2] = DMatrix([-L, 0, 0])
        p_["rotors_p", 3] = DMatrix([0, -L, 0])

        for i in range(NR):
            R_ = p_["rotors_model", i, "R"]  #  Radius of propeller [m]
            m_ = 0.01  # Mass of a propeller [kg]
            I_max = m_ * R_**2  # Inertia of a point mass
            I_ref = I_max / 5
            p_["rotors_I", i] = casadi.diag([I_ref / 2, I_ref / 2, I_ref])

        if debug:
            print p.vecNZcat()

        dist_ = dist(0)

        # ----------------- Scaling ---------------------

        scaling_states = states(1)
        scaling_controls = controls(1)

        scaling_states["r"] = 500
        scaling_controls["CR"] = 0.005

        scaling_dist = dist()

        scaling_dist["Faer"] = float(p_["m"] * p_["g"] / NR)
        scaling_dist["Caer"] = 0.0026

        # ----------- Frames ------------------
        T_10 = mul(tr(*pos), Tquat(*q))
        T_01 = kin_inv(T_10)
        R_10 = T2R(T_10)
        R_01 = R_10.T
        # -------------------------------------

        dstates = struct_symSX(states)

        dp, dv, dq, dw, dr = dstates[...]

        res = struct_SX(states)  # DAE residual hand side
        # ----------- Dynamics of the body ----
        res["p"] = v - dp
        # Newton, but transform the force F from {1} to {0}
        res["v"] = mul(R_10, F) - m * dv
        # Kinematics of the quaterion.
        res["q"] = mul(quatDynamics(*q), w) - dq
        # This is a trick by Sebastien Gros to stabilize the quaternion evolution equation
        res["q"] += -q * (sumAll(q**2) - 1)
        # Agular impulse H_1011
        H = mul(p["I"], w)  # Due to platform
        for i in range(NR):
            H += mul(
                p["rotors_I", i], w +
                vertcat([0, 0, p["rotors_spin", i] * r[i]]))  # Due to rotor i

        dH = mul(jacobian(H, w), dw) + mul(jacobian(H, q), dq) + mul(
            jacobian(H, r), dr) + casadi.cross(w, H)

        res["w"] = C - dH

        for i in range(NR):
            res["r", i] = CR[i] + p["rotors_spin", i] * rotors_Caer[i][2] - p[
                "rotors_I", i][2] * (dr[i] + dw[2])  # Dynamics of rotor i

        # ---------------------------------

        # Make a vector of f ?
        #if quatnorm:
        #    f = vertcat(f+[sumAll(q**2)-1])
        #else:
        #    f = vertcat(f)

        # ------------ Force model ------------

        Fg = mul(R_01, vertcat([0, 0, -g * m]))

        F_total = Fg + sum(rotors_Faer)  # Total force acting on the platform
        C_total = SX([0, 0, 0])  # Total torque acting on the platform

        for i in range(NR):
            C_total[:2] += rotors_Caer[
                i][:2]  # The x and y components propagate
            C_total[2] -= p["rotors_spin", i] * CR[
                i]  # the z compent moves through a serparate system
            C_total += casadi.cross(p["rotors_p", i],
                                    rotors_Faer[i])  # Torques due to thrust

        res = substitute(res, F, F_total)
        res = substitute(res, C, C_total)

        subs_before = []
        subs_after = []

        v_global = mul(R_01, v)
        u_z = SX([0, 0, 1])

        # Now fill in the aerodynamic forces
        for i in range(NR):
            c, R, CL_alpha, alpha_0, CD_alpha, CD_i = p["rotors_model", i, ...]
            #Bristeau P-jean, Martin P, Salaun E, Petit N. The role of propeller aerodynamics in the model of a quadrotor UAV. In: Proceedings of the European Control Conference 2009.; 2009:683-688.
            v_local = v_global + (casadi.cross(w, p["rotors_p", i])
                                  )  # Velocity at rotor i
            rotors_Faer_physics = (rho * c * R**3 * r[i]**2 * CL_alpha *
                                   (alpha_0 / 3.0 - v_local[2] /
                                    (2.0 * R * r[i]))) * u_z
            subs_before.append(rotors_Faer[i])
            subs_after.append(rotors_Faer_physics + dist["Faer", i])
            rotors_Caer_physics = -p["rotors_spin", i] * rho * c * R**4 * r[
                i]**2 * (CD_alpha / 4.0 + CD_i * alpha_0**2 *
                         (alpha_0 / 4.0 - 2.0 * v_local[2] /
                          (3.0 * r[i] * R)) - CL_alpha * v_local[2] /
                         (r[i] * R) * (alpha_0 / 3.0 - v_local[2] /
                                       (2.0 * r[i] * R))) * u_z
            subs_before.append(rotors_Caer[i])
            subs_after.append(rotors_Caer_physics + dist["Caer", i])

        res = substitute(res, veccat(subs_before), veccat(subs_after))

        # Make an explicit ode
        rhs = -casadi.solve(jacobian(res, dstates), substitute(
            res, dstates, 0))

        # --------------------------------------

        self.res_w = res
        self.res = substitute(res, dist, dist_)
        self.res_ = substitute(self.res, p, p_)

        resf = SXFunction([dstates, states, controls], [self.res_])
        resf.init()
        self.resf = resf

        self.rhs_w = rhs

        self.rhs = substitute(rhs, dist, dist_)

        self.rhs_ = substitute(self.rhs, p, p_)

        t = SX("t")
        # We end up with a DAE that captures the system dynamics
        dae = SXFunction(daeIn(t=t, x=states, p=controls),
                         daeOut(ode=self.rhs_))
        dae.init()

        self.dae = dae

        cdae = SXFunction(controldaeIn(t=t, x=states, u=controls, p=p),
                          daeOut(ode=self.rhs))
        cdae.init()
        self.cdae = cdae

        self.states = states
        self.dstates = dstates
        self.p = p
        self.p_ = p_
        self.controls = controls
        self.NR = NR
        self.w = dist
        self.w_ = dist_
        self.t = t

        self.states_ = states()
        self.dstates_ = states()
        self.controls_ = controls()

        self.scaling_states = scaling_states
        self.scaling_controls = scaling_controls
        self.scaling_dist = scaling_dist
Beispiel #42
0
def build_gp(invK, X, hyper, alpha, chol, meanFunc='zero'):
    """ Build Gaussian Process function
        Copyright (c) 2018, Helge-André Langåker

    # Arguments
        invK: Array with the inverse covariance matrices of size (Ny x N x N),
            with Ny number of outputs from the GP and N number of training points.
        X: Training data matrix with inputs of size (N x Nx), with Nx number of
            inputs to the GP.
        alpha: Training data matrix with invK time outputs of size (Ny x N).
        hyper: Array with hyperparame|ters [ell_1 .. ell_Nx sf sn].

    # Returns
        mean:     GP mean casadi function [mean(z)]
        var:      GP variance casadi function [var(z)]
        covar:    GP covariance casadi function [covar(z) = diag(var(z))]
        mean_jac: Casadi jacobian of the GP mean function [jac(z)]
    """


    Ny = len(invK)
    X = ca.SX(X)
    N, Nx = ca.SX.size(X)

    mean  = ca.SX.zeros(Ny, 1)
    var   = ca.SX.zeros(Ny, 1)

    # Casadi symbols
    x_s        = ca.SX.sym('x', Nx)
    z_s        = ca.SX.sym('z', Nx)
    m_s        = ca.SX.sym('m')
    ell_s      = ca.SX.sym('ell', Nx)
    sf2_s      = ca.SX.sym('sf2')
    X_s        = ca.SX.sym('X', N, Nx)
    ks_s       = ca.SX.sym('ks', N)
    v_s       = ca.SX.sym('v', N)
    kss_s      = ca.SX.sym('kss')
    alpha_s    = ca.SX.sym('alpha', N)

    covSE = ca.Function('covSE', [x_s, z_s, ell_s, sf2_s],
                          [covSEard(x_s, z_s, ell_s, sf2_s)])

    ks = ca.SX.zeros(N, 1)
    for i in range(N):
        ks[i] = covSE(X_s[i, :], z_s, ell_s, sf2_s)
    ks_func = ca.Function('ks', [X_s, z_s, ell_s, sf2_s], [ks])

    mean_i_func = ca.Function('mean', [ks_s, alpha_s, m_s],
                            [ca.mtimes(ks_s.T, alpha_s) + m_s])

    L_s = ca.SX.sym('L', ca.Sparsity.lower(N))
    v_func = ca.Function('v', [L_s, ks_s], [ca.solve(L_s, ks_s)])

    var_i_func  = ca.Function('var', [v_s, kss_s,],
                            [kss_s - v_s.T @ v_s])

    for output in range(Ny):
        ell      = ca.SX(hyper[output, 0:Nx])
        sf2      = ca.SX(hyper[output, Nx]**2)
        alpha_a  = ca.SX(alpha[output])
        ks       = ks_func(X_s, z_s, ell, sf2)
        v        = v_func(chol[output], ks)
        m = get_mean_function(ca.MX(hyper[output, :]), z_s, func=meanFunc)
        mean[output] = mean_i_func(ks, alpha_a, m(z_s))
        var[output]  = var_i_func(v, sf2)


    mean_temp  = ca.Function('mean_temp', [z_s, X_s], [mean])
    var_temp   = ca.Function('var_temp',  [z_s, X_s], [var])

    mean_func  = ca.Function('mean', [z_s], [mean_temp(z_s, X)])
    covar_func = ca.Function('var',  [z_s], [ca.diag(var_temp(z_s, X))])
    var_func = ca.Function('var',  [z_s], [var_temp(z_s, X)])

    mean_jac_z = ca.Function('mean_jac_z', [z_s],
                                      [ca.jacobian(mean_func(z_s), z_s)])

    return mean_func, var_func, covar_func, mean_jac_z