예제 #1
0
파일: matrix.py 프로젝트: casadi/casadi
  def test_diag_sparse(self):
    self.message("diag sparse")

    for n in [[0,1,0,0,2,3,4,5,6,0],[1,2,3,0],[0,1,2,3]]:
      d = DM(n)
      D = DM(n)
      d = sparsify(d)
      m = c.diag(d)
      M = sparsify(c.diag(D))

      self.checkarray(m.sparsity().colind(),M.sparsity().colind())
      self.checkarray(m.sparsity().row(),M.sparsity().row())
예제 #2
0
파일: matrix.py 프로젝트: pstjohn/casadi
  def test_diag_sparse(self):
    self.message("diag sparse")

    for n in [[0,1,0,0,2,3,4,5,6,0],[1,2,3,0],[0,1,2,3]]:
      d = DM(n)
      D = DM(n)
      d = sparsify(d)
      m = c.diag(d)
      M = sparsify(c.diag(D))

      self.checkarray(m.sparsity().colind(),M.sparsity().colind())
      self.checkarray(m.sparsity().row(),M.sparsity().row())
예제 #3
0
파일: matrix.py 프로젝트: Snkrnryn/casadi
 def test_diag_sparse(self):
   self.message("diag sparse")
   
   for n in [[0,1,0,0,2,3,4,5,6,0],[1,2,3,0],[0,1,2,3]]:
     d = DMatrix(n)
     D = DMatrix(n)
     makeSparse(d)
     m = c.diag(d)
     M = c.diag(D)
     makeSparse(M)
     
     self.checkarray(m.sparsity().rowind(),M.sparsity().rowind())
     self.checkarray(m.sparsity().col(),M.sparsity().col())
예제 #4
0
    def test_diag_sparse(self):
        self.message("diag sparse")

        for n in [[0, 1, 0, 0, 2, 3, 4, 5, 6, 0], [1, 2, 3, 0], [0, 1, 2, 3]]:
            d = DMatrix(n)
            D = DMatrix(n)
            makeSparse(d)
            m = c.diag(d)
            M = c.diag(D)
            makeSparse(M)

            self.checkarray(m.sparsity().rowind(), M.sparsity().rowind())
            self.checkarray(m.sparsity().col(), M.sparsity().col())
예제 #5
0
  def test_cholesky(self):
    numpy.random.seed(0)
    n = 10
    L = self.randDMatrix(n,n,sparsity=0.2) +  1.5*c.diag(range(1,n+1))
    L = L[Sparsity.lower(n)]
    M = mul(L,L.T)
    b = self.randDMatrix(n,1)
    
    M.sparsity().spy()

    S = LinearSolver("S", "csparsecholesky",M.sparsity())
    S.setInput(M)
    S.prepare()
    
    self.checkarray(M,M.T)
    
    C = S.getFactorization()
    self.checkarray(mul(C,C.T),M)
    self.checkarray(C,L)
    
    print C
    
    S.getFactorizationSparsity().spy()

    C = solve(M,b,"csparsecholesky")
    self.checkarray(mul(M,C),b)
예제 #6
0
def FIM_t(xpdot, b, criterion):
    '''
    computes FIM at a given time.
    b is the bonary variable which selects or not the time point
    '''
    n_x = 2
    n_theta = 4
    FIM_sample = np.zeros([n_theta, n_theta])
    FIM_0 = cad.inv((((10.0 - 0.001)**2) / 12) * cad.SX.eye(n_theta))
    FIM_sample += FIM_0
    for i in range(np.shape(xpdot)[0] - 1):
        xp_r = cad.reshape(xpdot[i + 1], (n_x, n_theta))
        #    vv = np.zeros([ntheta[0], ntheta[0], 1 + N])
        #    for i in range(0, 1 + N):
        FIM_sample += b[i] * xp_r.T @ np.linalg.inv(
            np.array([[0.01, 0], [0, 0.05]])
        ) @ xp_r  # + np.linalg.inv(np.array([[0.01, 0, 0, 0], [0, 0.05, 0, 0], [0, 0, 1, 0], [0, 0, 0, 0.2]]))
#    FIM  = solve(FIM1, SX.eye(FIM1.size1()))
#   [Q, R] = qr(FIM1.expand())

    if criterion == 'D_optimal':
        #        objective = -cad.log(cad.det(FIM_sample) + 1e-10)
        objective = -2 * cad.sum1(cad.log(cad.diag(
            cad.chol(FIM_sample))))  # by Cholesky factorisation


#        objective = -cad.log((cad.det(FIM_sample)**2))
#        objective = -cad.det(FIM_sample)
    elif criterion == 'A_optimal':
        objective = -cad.log(cad.trace(FIM_sample) + 1e-10)
    return objective
예제 #7
0
    def linearized_quad_dynamics(self):
        """
        Jacobian J matrix of the linearized dynamics specified in the function quad_dynamics. J[i, j] corresponds to
        the partial derivative of f_i(x) wrt x(j).

        :return: a CasADi symbolic function that calculates the 13 x 13 Jacobian matrix of the linearized simplified
        quadrotor dynamics
        """

        jac = cs.MX(self.state_dim, self.state_dim)

        # Position derivatives
        jac[0:3, 7:10] = cs.diag(cs.MX.ones(3))

        # Angle derivatives
        jac[3:7, 3:7] = skew_symmetric(self.r) / 2
        jac[3, 10:] = 1 / 2 * cs.horzcat(-self.q[1], -self.q[2], -self.q[3])
        jac[4, 10:] = 1 / 2 * cs.horzcat(self.q[0], -self.q[3], self.q[2])
        jac[5, 10:] = 1 / 2 * cs.horzcat(self.q[3], self.q[0], -self.q[1])
        jac[6, 10:] = 1 / 2 * cs.horzcat(-self.q[2], self.q[1], self.q[0])

        # Velocity derivatives
        a_u = (self.u[0] + self.u[1] + self.u[2] + self.u[3]) * self.quad.max_thrust / self.quad.mass
        jac[7, 3:7] = 2 * cs.horzcat(a_u * self.q[2], a_u * self.q[3], a_u * self.q[0], a_u * self.q[1])
        jac[8, 3:7] = 2 * cs.horzcat(-a_u * self.q[1], -a_u * self.q[0], a_u * self.q[3], a_u * self.q[2])
        jac[9, 3:7] = 2 * cs.horzcat(0, -2 * a_u * self.q[1], -2 * a_u * self.q[1], 0)

        # Rate derivatives
        jac[10, 10:] = (self.quad.J[1] - self.quad.J[2]) / self.quad.J[0] * cs.horzcat(0, self.r[2], self.r[1])
        jac[11, 10:] = (self.quad.J[2] - self.quad.J[0]) / self.quad.J[1] * cs.horzcat(self.r[2], 0, self.r[0])
        jac[12, 10:] = (self.quad.J[0] - self.quad.J[1]) / self.quad.J[2] * cs.horzcat(self.r[1], self.r[0], 0)

        return cs.Function('J', [self.x, self.u], [jac])
예제 #8
0
    def test_general_convex_sparse(self):
        self.message("Convex sparse QP with solvers: " +
                     str([qpsol for qpsol, options, aux_options in qpsols]))
        H = c.diag([2, 1, 0.2, 0.7, 1.3])

        H[1, 2] = 0.1
        H[2, 1] = 0.1

        G = DM([-2, -6, 1, 0, 0])
        A = DM([[1, 0, 0.1, 0.7, -1], [0.1, 2, -0.3, 4, 0.1]])
        A = sparsify(A)

        LBA = DM([-inf])
        UBA = DM([2, 2])

        LBX = DM([0] * 5)
        UBX = DM([inf] * 5)

        for qpsol, qp_options, aux_options in qpsols:
            self.message("general_convex: " + str(qpsol))

            solver = casadi.qpsol("mysolver", qpsol, {
                'h': H.sparsity(),
                'a': A.sparsity()
            }, qp_options)

            try:
                less_digits = aux_options["less_digits"]
            except:
                less_digits = 0

            solver_in = {}
            solver_in["h"] = H
            solver_in["g"] = G
            solver_in["a"] = A
            solver_in["lbx"] = LBX
            solver_in["ubx"] = UBX
            solver_in["lba"] = LBA
            solver_in["uba"] = UBA

            solver_out = solver(**solver_in)

            self.checkarray(solver_out["x"],
                            DM([0.873908, 0.95630465, 0, 0, 0]),
                            str(qpsol),
                            digits=max(1, 6 - less_digits))

            self.checkarray(solver_out["lam_x"],
                            DM([0, 0, -0.339076, -10.0873907, -0.252185]),
                            6,
                            str(qpsol),
                            digits=max(1, 6 - less_digits))

            self.checkarray(solver_out["lam_a"],
                            DM([0, 2.52184767]),
                            str(qpsol),
                            digits=max(1, 6 - less_digits))

            self.assertAlmostEqual(solver_out["cost"][0], -6.264669320767,
                                   max(1, 6 - less_digits), str(qpsol))
예제 #9
0
  def test_cholesky(self):
    numpy.random.seed(0)
    n = 10
    L = self.randDM(n,n,sparsity=0.2) +  1.5*c.diag(list(range(1,n+1)))
    L = L[Sparsity.lower(n)]
    M = mtimes(L,L.T)
    b = self.randDM(n,1)
    
    M.sparsity().spy()

    S = casadi.linsol("S", "csparsecholesky", M.sparsity(), 1)
    S_in = [0]*S.n_in();S_in[0]=M
    S.linsol_prepare()
    
    self.checkarray(M,M.T)
    
    C = S.linsol_cholesky()
    self.checkarray(mtimes(C,C.T),M)
    self.checkarray(C,L)
    
    print(C)
    
    S.linsol_cholesky_sparsity().spy()

    C = solve(M,b,"csparsecholesky")
    self.checkarray(mtimes(M,C),b)
예제 #10
0
    def test_cholesky(self):
        numpy.random.seed(0)
        n = 10
        L = self.randDM(n, n,
                        sparsity=0.2) + 1.5 * c.diag(list(range(1, n + 1)))
        L = L[Sparsity.lower(n)]
        M = mtimes(L, L.T)
        b = self.randDM(n, 1)

        M.sparsity().spy()

        S = casadi.Linsol("S", "csparsecholesky")
        S_in = [0] * S.n_in()
        S_in[0] = M
        S.linsol_prepare()

        self.checkarray(M, M.T)

        C = S.linsol_cholesky()
        self.checkarray(mtimes(C, C.T), M)
        self.checkarray(C, L)

        print(C)

        S.linsol_cholesky_sparsity().spy()

        C = solve(M, b, "csparsecholesky")
        self.checkarray(mtimes(M, C), b)
예제 #11
0
    def _predict_sym(self, x_test, return_std=False, return_cov=False):
        """
        Computes the GP posterior mean and covariance at a given a test sample using CasADi symbolics.
        :param x_test: vector of size mx1, where m is the number of features used for the GP prediction

        :return: the posterior mean (scalar) and covariance (scalar).
        """

        k_s = self.kernel(self._x_train_cs, x_test.T)

        # Posterior mean value
        mu_s = cs.mtimes(k_s.T, self._K_inv_y_cs) + self.y_mean

        if not return_std and not return_cov:
            return {'mu': mu_s}

        k_ss = self.kernel(x_test, x_test) + 1e-8 * cs.MX.eye(x_test.shape[1])

        # Posterior covariance
        cov_s = k_ss - cs.mtimes(cs.mtimes(k_s.T, self._K_inv_cs), k_s)
        cov_s = cs.diag(cov_s)

        if return_std:
            return {'mu': mu_s, 'std': np.sqrt(cov_s)}

        return {'mu': mu_s, 'cov': cov_s}
예제 #12
0
    def test_cholesky(self):
        numpy.random.seed(0)
        n = 10
        L = self.randDMatrix(n, n,
                             sparsity=0.2) + 1.5 * c.diag(range(1, n + 1))
        L = L[Sparsity.tril(n)]
        M = mul(L, L.T)
        b = self.randDMatrix(n, 1)

        M.sparsity().spy()

        S = LinearSolver("csparsecholesky", M.sparsity())

        S.init()
        S.setInput(M)
        S.prepare()

        self.checkarray(M, M.T)

        C = S.getFactorization()
        self.checkarray(mul(C, C.T), M)
        self.checkarray(C, L)

        print C

        S.getFactorizationSparsity().spy()

        C = solve(M, b, "csparsecholesky")
        self.checkarray(mul(M, C), b)
예제 #13
0
    def test_general_convex_sparse(self):
        self.message("Convex sparse QP with solvers: " +
                     str([qpsolver for qpsolver, options in qpsolvers]))
        H = c.diag([2, 1, 0.2, 0.7, 1.3])

        H[1, 2] = 0.1
        H[2, 1] = 0.1

        G = DMatrix([-2, -6, 1, 0, 0])
        A = DMatrix([[1, 0, 0.1, 0.7, -1], [0.1, 2, -0.3, 4, 0.1]])
        A = sparse(A)

        LBA = DMatrix([-inf])
        UBA = DMatrix([2, 2])

        LBX = DMatrix([0] * 5)
        UBX = DMatrix([inf] * 5)

        options = {"mutol": 1e-12, "artol": 1e-12, "tol": 1e-12}

        for qpsolver, qp_options in qpsolvers:
            self.message("general_convex: " + str(qpsolver))

            solver = QpSolver(qpsolver, qpStruct(h=H.sparsity(),
                                                 a=A.sparsity()))
            for key, val in options.iteritems():
                if solver.hasOption(key):
                    solver.setOption(key, val)
            solver.setOption(qp_options)
            solver.init()

            solver.setInput(H, "h")
            solver.setInput(G, "g")
            solver.setInput(A, "a")
            solver.setInput(LBX, "lbx")
            solver.setInput(UBX, "ubx")
            solver.setInput(LBA, "lba")
            solver.setInput(UBA, "uba")

            solver.evaluate()

            self.checkarray(solver.getOutput(),
                            DMatrix([0.873908, 0.95630465, 0, 0, 0]),
                            str(qpsolver),
                            digits=6)

            self.checkarray(solver.getOutput("lam_x"),
                            DMatrix([0, 0, -0.339076, -10.0873907, -0.252185]),
                            6,
                            str(qpsolver),
                            digits=6)

            self.checkarray(solver.getOutput("lam_a"),
                            DMatrix([0, 2.52184767]),
                            str(qpsolver),
                            digits=6)

            self.assertAlmostEqual(
                solver.getOutput("cost")[0], -6.264669320767, 6, str(qpsolver))
예제 #14
0
def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3):
    '''
    take the dae that has x/z/u/p added to it already and return
    the states added to it and return mass matrix and rhs of the dae residual
    '''

    R_b2n = dae['R_n2b'].T
    r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']])
    r_b2bridle_b = C.veccat([0,0,conf['zt']])
    v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']])

    r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b)

    mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0)
    mm01 = C.SXMatrix(3,3)
    mm10 = mm01.T
    mm02 = r_n2bridle_n
    mm20 = mm02.T
    J = C.vertcat([C.horzcat([ conf['j1'],          0, conf['j31']]),
                   C.horzcat([          0, conf['j2'],           0]),
                   C.horzcat([conf['j31'],          0,  conf['j3']])])
    mm11 = J
    mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n))
    mm21 = mm12.T
    mm22 = C.SXMatrix(1,1)

    mm = C.vertcat([C.horzcat([mm00,mm01,mm02]),
                    C.horzcat([mm10,mm11,mm12]),
                    C.horzcat([mm20,mm21,mm22])])

    # right hand side
    rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)])
    rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b']))

    # last element of RHS
    R_n2b = dae['R_n2b']
    w_bn_b = dae['w_bn_b']
    grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b))
    tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \
          C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b)
    rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr']

    rhs = C.veccat([rhs0,rhs1,rhs2])

    c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2)
    v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b))
    cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr']

    a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']])
    dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']])
    a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b)))
    cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \
            dae['dr']**2 - dae['r']*dae['ddr']

    dae['c'] = c
    dae['cdot'] = cdot
    dae['cddot'] = cddot

    return (mm, rhs)
예제 #15
0
    def _setup_process_noise_covariance_and_weightings(self):

        r_w = np.ones(self.nx)

        r_w[self.x_index["T_hts"][0]] = 2.0
        r_w[self.x_index["T_hts"][1]] = 2.0
        r_w[self.x_index["T_hts"][2]] = 2.0
        r_w[self.x_index["T_hts"][3]] = 2.0

        r_w[self.x_index["T_lts"][0]] = 2.0
        r_w[self.x_index["T_lts"][1]] = 2.0

        r_w[self.x_index["T_fpsc"]] = 1.0
        r_w[self.x_index["T_fpsc_s"]] = 2.0

        r_w[self.x_index["T_vtsc"]] = 1.0
        r_w[self.x_index["T_vtsc_s"]] = 2.0

        r_w[self.x_index["T_pscf"]] = 1.0
        r_w[self.x_index["T_pscr"]] = 1.0

        r_w[self.x_index["T_shx_psc"][0]] = 3.0
        r_w[self.x_index["T_shx_psc"][1]] = 1.0
        r_w[self.x_index["T_shx_psc"][2]] = 1.0
        r_w[self.x_index["T_shx_psc"][3]] = 3.0

        r_w[self.x_index["T_shx_ssc"][0]] = 3.0
        r_w[self.x_index["T_shx_ssc"][1]] = 1.0
        r_w[self.x_index["T_shx_ssc"][2]] = 1.0
        r_w[self.x_index["T_shx_ssc"][3]] = 3.0

        r_w[self.x_index["T_fcu_a"]] = 3.0
        r_w[self.x_index["T_fcu_w"]] = 2.0

        r_w[self.x_index["T_r_c"][0]] = 2.0
        r_w[self.x_index["T_r_c"][1]] = 2.0
        r_w[self.x_index["T_r_c"][2]] = 2.0

        r_w[self.x_index["T_r_a"][0]] = 2.0
        r_w[self.x_index["T_r_a"][1]] = 2.0
        r_w[self.x_index["T_r_a"][2]] = 2.0

        r_w[self.x_aux_index["dT_amb"]] = 2.0
        r_w[self.x_aux_index["dI_vtsc"]] = 10.0
        r_w[self.x_aux_index["dI_fpsc"]] = 10.0

        for x_aux_idx in self.x_aux_index["Qdot_n_c"]:

            r_w[x_aux_idx]  = 5.0

        for x_aux_idx in self.x_aux_index["Qdot_n_a"]:

            r_w[x_aux_idx]  = 5.0

        r_w[self.x_aux_index["dalpha_vtsc"]] = 1.0
        r_w[self.x_aux_index["dalpha_fpsc"]] = 1.0

        self.R_w = ca.diag(r_w)
        self.W_w = ca.inv(self.R_w)
예제 #16
0
def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3):
    '''
    take the dae that has x/z/u/p added to it already and return
    the states added to it and return mass matrix and rhs of the dae residual
    '''

    R_b2n = dae['R_n2b'].T
    r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']])
    r_b2bridle_b = C.veccat([0,0,conf['zt']])
    v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']])

    r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b)

    mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0)
    mm01 = C.SXMatrix(3,3)
    mm10 = mm01.T
    mm02 = r_n2bridle_n
    mm20 = mm02.T
    J = C.blockcat([[ conf['j1'],          0, conf['j31']],
                    [          0, conf['j2'],           0],
                    [conf['j31'],          0,  conf['j3']]])
    mm11 = J
    mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n))
    mm21 = mm12.T
    mm22 = C.SXMatrix(1,1)

    mm = C.blockcat([[mm00,mm01,mm02],
                     [mm10,mm11,mm12],
                     [mm20,mm21,mm22]])

    # right hand side
    rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)])
    rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b']))

    # last element of RHS
    R_n2b = dae['R_n2b']
    w_bn_b = dae['w_bn_b']
    grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b))
    tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \
          C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b)
    rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr']

    rhs = C.veccat([rhs0,rhs1,rhs2])

    c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2)
    v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b))
    cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr']

    a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']])
    dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']])
    a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b)))
    cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \
            dae['dr']**2 - dae['r']*dae['ddr']

    dae['c'] = c
    dae['cdot'] = cdot
    dae['cddot'] = cddot

    return (mm, rhs)
예제 #17
0
 def get_regularised_cost_expr(self):
     slack_var = self.skill_spec.slack_var
     if slack_var is not None:
         slack_H = cs.diag(self.weight_shifter + self.slack_var_weights)
         slack_cost = cs.mtimes(cs.mtimes(slack_var.T, slack_H), slack_var)
     else:
         slack_cost = 0.0
     return self.weight_shifter * self.cost_expression + slack_cost
예제 #18
0
    def test_general_convex_sparse(self):
        self.message("Convex sparse QP with solvers: " +
                     str([qpsolver for qpsolver, options in qpsolvers]))
        H = c.diag([2, 1, 0.2, 0.7, 1.3])

        H[1, 2] = 0.1
        H[2, 1] = 0.1

        G = DMatrix([-2, -6, 1, 0, 0])
        A = DMatrix([[1, 0, 0.1, 0.7, -1], [0.1, 2, -0.3, 4, 0.1]])
        makeSparse(A)

        LBA = DMatrix([-inf])
        UBA = DMatrix([2, 2])

        LBX = DMatrix([0] * 5)
        UBX = DMatrix([inf] * 5)

        options = {"mutol": 1e-12, "artol": 1e-12, "tol": 1e-12}

        for qpsolver, qp_options in qpsolvers:
            self.message("general_convex: " + str(qpsolver))

            solver = qpsolver(H.sparsity(), A.sparsity())
            for key, val in options.iteritems():
                if solver.hasOption(key):
                    solver.setOption(key, val)
            solver.setOption(qp_options)
            solver.init()

            solver.input(QP_H).set(H)
            solver.input(QP_G).set(G)
            solver.input(QP_A).set(A)
            solver.input(QP_LBX).set(LBX)
            solver.input(QP_UBX).set(UBX)
            solver.input(QP_LBA).set(LBA)
            solver.input(QP_UBA).set(UBA)

            solver.solve()

            self.checkarray(solver.output(),
                            DMatrix([0.873908, 0.95630465, 0, 0, 0]),
                            str(qpsolver),
                            digits=6)

            self.checkarray(solver.output(QP_LAMBDA_X),
                            DMatrix([0, 0, -0.339076, -10.0873907, -0.252185]),
                            6,
                            str(qpsolver),
                            digits=6)

            self.checkarray(solver.output(QP_LAMBDA_A),
                            DMatrix([0, 2.52184767]),
                            str(qpsolver),
                            digits=6)

            self.assertAlmostEqual(
                solver.output(QP_COST)[0], -6.264669320767, 6, str(qpsolver))
예제 #19
0
    def get_cost_integrand_function(self):
        """Returns a casadi function for the discretized integrand of
        the cost expression integrated one timestep. For the rectangle
        method, this just amounts to timing by the timestep.

        As with the other controllers, the cost is affected by the
        weight shifter, giving a regularised cost with the slack
        variables.
        """
        # Setup new symbols needed
        dt = self.timestep
        # Setup skill_spec symbols
        time_var = self.skill_spec.time_var
        robot_var = self.skill_spec.robot_var
        list_vars = [time_var, robot_var]
        list_names = ["time_var", "robot_var"]
        robot_vel_var = self.skill_spec.robot_vel_var
        cntrl_vars = [robot_vel_var]
        cntrl_names = ["robot_vel_var"]
        virtual_var = self.skill_spec.virtual_var
        if virtual_var is not None:
            list_vars += [virtual_var]
            list_names += ["virtual_var"]
            virtual_vel_var = self.skill_spec.virtual_vel_var
            cntrl_vars += [virtual_vel_var]
            cntrl_names += ["virtual_vel_var"]
        slack_var = self.skill_spec.slack_var
        if slack_var is not None:
            list_vars += [slack_var]
            list_names += ["slack_var"]
        # Full symbol list same way as in other controllers
        list_vars += cntrl_vars
        list_names += cntrl_names

        # Expression for the cost with regularisation:
        if slack_var is not None:
            slack_H = cs.diag(self.weight_shifter + self.slack_var_weights)
            slack_cost = cs.mtimes(cs.mtimes(slack_var.T, slack_H), slack_var)
            regularised_cost = self.weight_shifter*self.cost_expression
            regularised_cost += slack_cost
        else:
            regularised_cost = self.cost_expression
        # Choose integration method
        if self.options["cost_integration_method"].lower() == "rectangle":
            cost_integrand = regularised_cost*dt
        elif self.options["cost_integration_method"].lower() == "trapezoidal":
            # Trapezoidal rule
            raise NotImplementedError("Trapezoidal rule integration not"
                                      + " implemented.")
        elif self.options["cost_integration_method"].lower() == "simpson":
            # Simpson rule
            raise NotImplementedError("Simpson rule integration not"
                                      + " implemented.")
        else:
            raise NotImplementedError(self.options["cost_integration_method"]
                                      + " is not a known integration method.")
        return cs.Function("fc_k", list_vars, [cost_integrand],
                           list_names, ["cost_integrand"])
예제 #20
0
  def test_cholesky2(self):
    random.seed(0)
    n = 10
    L = c.diag(range(n))
    M = mul(L,L.T)

    print L
    S = CSparseCholesky(M.sparsity())
    

    S.init()
    S.getFactorizationSparsity().spy()
예제 #21
0
  def test_cholesky(self):
    random.seed(1)
    n = 10
    L = self.randDMatrix(n,n,sparsity=0.2) +  c.diag(range(n))
    M = mul(L,L.T)

    S = CSparseCholesky(M.sparsity())

    S.init()
    S.getFactorizationSparsity().spy()

    S.setInput(M,0)
예제 #22
0
    def test_badscaling(self):
        #return
        self.message("Badly scaled problem")
        N = 50
        H = c.diag(list(range(1, N + 1)))
        x0 = DM(list(range(N)))

        G = -1.0 * mtimes(H, x0)

        A = DM(0, N)

        LBX = DM([-1000] * N)
        UBX = DM([1000] * N)

        for conic, qp_options, aux_options in conics:
            if not aux_options["quadratic"]: continue
            if 'cplex' in str(conic):
                continue
            if 'worhp' in str(
                    conic
            ):  # works but occasionaly throws segfaults, ulimit on travis?
                continue
            solver = casadi.conic("mysolver", conic, {
                'h': H.sparsity(),
                'a': A.sparsity()
            }, qp_options)

            try:
                less_digits = aux_options["less_digits"]
            except:
                less_digits = 0

            solver_in = {}
            solver_in["h"] = H
            solver_in["g"] = G
            solver_in["a"] = A
            solver_in["lbx"] = LBX
            solver_in["ubx"] = UBX

            solver_out = solver(**solver_in)

            self.checkarray(solver_out["x"],
                            x0,
                            str(conic) + str(qp_options),
                            digits=max(1, 2 - less_digits))
            self.assertAlmostEqual(solver_out["cost"][0],
                                   -0.5 * mtimes([x0.T, H, x0]),
                                   max(1, 3 - less_digits), str(conic))
            if aux_options["dual"]:
                self.checkarray(solver_out["lam_x"],
                                DM.zeros(N, 1),
                                str(conic),
                                digits=max(1, 4 - less_digits))
예제 #23
0
파일: qpsolver.py 프로젝트: mzanon/casadi
  def test_general_convex_sparse(self):
    self.message("Convex sparse QP with solvers: " + str([qpsolver for qpsolver,options,aux_options in qpsolvers]))
    H = c.diag([2,1,0.2,0.7,1.3])

    H[1,2]=0.1
    H[2,1]=0.1
    
    G = DMatrix([-2,-6,1,0,0])
    A =  DMatrix([[1, 0,0.1,0.7,-1],[0.1, 2,-0.3,4,0.1]])
    A = sparsify(A)
    
    LBA = DMatrix([-inf])
    UBA = DMatrix([2, 2])

    LBX = DMatrix([0]*5)
    UBX = DMatrix([inf]*5)

    options = { "mutol": 1e-12, "artol": 1e-12, "tol":1e-12}
      
    for qpsolver, qp_options, aux_options in qpsolvers:
      self.message("general_convex: " + str(qpsolver))

      solver = QpSolver(qpsolver,qpStruct(h=H.sparsity(),a=A.sparsity()))
      for key, val in options.iteritems():
        if solver.hasOption(key):
           solver.setOption(key,val)
      solver.setOption(qp_options)
      solver.init()

      try:
        less_digits=aux_options["less_digits"]
      except:
        less_digits=0

      solver.setInput(H,"h")
      solver.setInput(G,"g")
      solver.setInput(A,"a")
      solver.setInput(LBX,"lbx")
      solver.setInput(UBX,"ubx")
      solver.setInput(LBA,"lba")
      solver.setInput(UBA,"uba")

      solver.evaluate()

      self.checkarray(solver.getOutput(),DMatrix([0.873908,0.95630465,0,0,0]),str(qpsolver),digits=max(1,6-less_digits))
      
      self.checkarray(solver.getOutput("lam_x"),DMatrix([0,0,-0.339076,-10.0873907,-0.252185]),6,str(qpsolver),digits=max(1,6-less_digits))

      self.checkarray(solver.getOutput("lam_a"),DMatrix([0,2.52184767]),str(qpsolver),digits=max(1,6-less_digits))

      self.assertAlmostEqual(solver.getOutput("cost")[0],-6.264669320767,max(1,6-less_digits),str(qpsolver))
예제 #24
0
    def _terminalObj(self):
        
        # estado terminal
        XN = self.X_pred[-1]  # terminal state
        XdN = XN[self.nxs:self.nxs+self.nxd]
        Vt = 0
 
        # Adição do custo terminal
        # Q terminal
        Q_lyap = [email protected]@[email protected]@self.sys.F
        Q_bar = solve_discrete_lyapunov(self.sys.F, Q_lyap, method='bilinear')
        Vt = csd.dot(XdN**2, csd.diag(Q_bar))
        self.Q_bar = Q_bar
        return Vt
예제 #25
0
    def test_expm(self):
        # Test for eye
        correct_res = diag(exp(DM.ones(3)))
        a = expm(DM.eye(3))
        self.assertAlmostEqual(norm_fro(a - correct_res), 0, 3)

        # Test for -magic(3) (compared with MATLAB solution)
        a = DM([[-8, -1, -6], [-3, -5, -7], [-4, -9, -2]])

        correct_res = DM(
            [[3.646628887990924, 32.404567030885005, -36.051195612973601],
             [5.022261973341555, 44.720086474306093, -49.742348141745325],
             [-8.668890555430160, -77.124653199288772, 85.793544060621244]])
        self.assertAlmostEqual(norm_fro(expm(a) - correct_res), 0, 2)
예제 #26
0
    def test_badscaling(self):
        #return
        self.message("Badly scaled problem")
        N = 50
        H = c.diag(range(1, N + 1))
        x0 = DMatrix(range(N))

        G = -1.0 * mul(H, x0)

        A = DMatrix(0, N)

        LBX = DMatrix([-1000] * N)
        UBX = DMatrix([1000] * N)

        for qpsolver, qp_options, aux_options in qpsolvers:
            if 'cplex' in str(qpsolver):
                continue
            if 'worhp' in str(
                    qpsolver
            ):  # works but occasionaly throws segfaults, ulimit on travis?
                continue
            solver = QpSolver("mysolver", qpsolver, {
                'h': H.sparsity(),
                'a': A.sparsity()
            }, qp_options)

            try:
                less_digits = aux_options["less_digits"]
            except:
                less_digits = 0

            solver.setInput(H, "h")
            solver.setInput(G, "g")
            solver.setInput(A, "a")
            solver.setInput(LBX, "lbx")
            solver.setInput(UBX, "ubx")

            solver.evaluate()

            self.checkarray(solver.getOutput(),
                            x0,
                            str(qpsolver) + str(qp_options),
                            digits=max(1, 2 - less_digits))
            self.assertAlmostEqual(
                solver.getOutput("cost")[0], -0.5 * mul([x0.T, H, x0]),
                max(1, 3 - less_digits), str(qpsolver))
            self.checkarray(solver.getOutput("lam_x"),
                            DMatrix.zeros(N, 1),
                            str(qpsolver),
                            digits=max(1, 4 - less_digits))
예제 #27
0
 def get_obs_cov_func(x):
     pos = x[:2]
     Nt = (x.shape[0] - 2) // 2
     R_nominal = 0.001 * cs.MX.eye(2 * Nt)
     dists = cs.MX.zeros(2 * Nt)
     for ii in range(Nt):
         pos_target_ii = x[2 * (ii + 1):2 * (ii + 2)]
         dist_ii = cs.sqrt(cs.sum1((pos - pos_target_ii)**2))
         dists[2 * ii] = dist_ii
         dists[2 * ii + 1] = dist_ii
     R_scale = 0.01 * cs.diag(dists)
     R = R_nominal + R_scale
     obs_cov = cs.Function('obs_cov', [x], [R], ['x'], ['R'])
     return obs_cov
예제 #28
0
  def test_cholesky2(self):
    numpy.random.seed(0)
    n = 10
    L = c.diag(list(range(1,n+1)))
    M = mtimes(L,L.T)

    print(L)
    S = casadi.linsol("S", "csparsecholesky", M.sparsity(), 1)
    
    S.linsol_cholesky_sparsity().spy()
    S_in = [0]*S.n_in();S_in[0]=M
    S.linsol_prepare()

    C = S.linsol_cholesky()
    self.checkarray(mtimes(C,C.T),M)
예제 #29
0
    def __init__(self, model, **kwargs):
        self.cost = {
            "Q": diag([10, 0.1, 0.1, 0.1]),
            "R": 0.1,
            "Qv": diag([100, 100, 0, 0]),
            "x_ref": DM([pi, 0, 0, 0]),
        }
        self.state_constraints = False
        self.control_constraints = False

        OptimalControlProblem.__init__(self, model, obj=self.cost)

        self.t_f = 5
        self.x_0 = [pi / 6, 0, 0, 0]

        for (k, v) in kwargs.items():
            setattr(self, k, v)

        if self.state_constraints:
            self.x_max[2] = 10
            self.x_min[2] = -10
        if self.control_constraints:
            self.u_max[0] = 2
            self.u_min[0] = -2
예제 #30
0
    def test_cholesky2(self):
        numpy.random.seed(0)
        n = 10
        L = c.diag(range(1, n + 1))
        M = mul(L, L.T)

        print L
        S = LinearSolver("S", "csparsecholesky", M.sparsity())

        S.getFactorizationSparsity().spy()
        S.setInput(M)
        S.prepare()

        C = S.getFactorization()
        self.checkarray(mul(C, C.T), M)
예제 #31
0
    def get_cost_expr(self):
        """Returns a casadi expression describing the cost.

        Return:
             H for min_opt_var opt_var^T*H*opt_var"""
        nvirt = self.skill_spec.n_virtual_var
        nslack = self.skill_spec.n_slack_var
        mu = self.weight_shifter
        opt_weights = [mu * self.robot_var_weights]
        if nvirt > 0:
            opt_weights += [mu * self.virtual_var_weights]
        if nslack > 0:
            opt_weights += [mu + self.slack_var_weights]
        H = cs.diag(cs.vertcat(*opt_weights))
        return H
예제 #32
0
  def test_cholesky2(self):
    numpy.random.seed(0)
    n = 10
    L = c.diag(range(1,n+1))
    M = mul(L,L.T)

    print L
    S = LinearSolver("S", "csparsecholesky", M.sparsity())
    
    S.getFactorizationSparsity().spy()
    S.setInput(M)
    S.prepare()

    C = S.getFactorization()
    self.checkarray(mul(C,C.T),M)
예제 #33
0
    def test_cholesky2(self):
        numpy.random.seed(0)
        n = 10
        L = c.diag(list(range(1, n + 1)))
        M = mtimes(L, L.T)

        print(L)
        S = casadi.linsol("S", "csparsecholesky", M.sparsity(), 0)

        S.linsol_cholesky_sparsity().spy()
        S_in = [0] * S.n_in()
        S_in[0] = M
        S.linsol_prepare()

        C = S.linsol_cholesky()
        self.checkarray(mtimes(C, C.T), M)
예제 #34
0
    def test_badscaling(self):
        #return
        self.message("Badly scaled problem")
        N = 50
        H = c.diag(range(1, N + 1))
        x0 = DMatrix(range(N))

        G = -1.0 * mul(H, x0)

        A = DMatrix.sparse(0, N)

        LBX = DMatrix([-1000] * N)
        UBX = DMatrix([1000] * N)

        options = {"mutol": 1e-12, "artol": 1e-12, "tol": 1e-12}

        for qpsolver, qp_options in qpsolvers:
            if 'cplex' in str(qpsolver):
                continue
            solver = QpSolver(qpsolver, qpStruct(h=H.sparsity(),
                                                 a=A.sparsity()))
            for key, val in options.iteritems():
                if solver.hasOption(key):
                    solver.setOption(key, val)

            solver.setOption(qp_options)
            solver.init()

            solver.setInput(H, "h")
            solver.setInput(G, "g")
            solver.setInput(A, "a")
            solver.setInput(LBX, "lbx")
            solver.setInput(UBX, "ubx")

            solver.evaluate()

            self.checkarray(solver.getOutput(),
                            x0,
                            str(qpsolver) + str(qp_options),
                            digits=2)
            self.assertAlmostEqual(
                solver.getOutput("cost")[0], -0.5 * mul([x0.T, H, x0]), 3,
                str(qpsolver))
            self.checkarray(solver.getOutput("lam_x"),
                            DMatrix.zeros(N, 1),
                            str(qpsolver),
                            digits=4)
예제 #35
0
파일: conic.py 프로젝트: casadi/casadi
  def test_general_convex_sparse(self):
    self.message("Convex sparse QP with solvers: " + str([conic for conic,options,aux_options in conics]))
    H = c.diag([2,1,0.2,0.7,1.3])

    H[1,2]=0.1
    H[2,1]=0.1

    G = DM([-2,-6,1,0,0])
    A =  DM([[1, 0,0.1,0.7,-1],[0.1, 2,-0.3,4,0.1]])
    A = sparsify(A)

    LBA = DM([-inf])
    UBA = DM([2, 2])

    LBX = DM([0]*5)
    UBX = DM([inf]*5)


    for conic, qp_options, aux_options in conics:
      if not aux_options["quadratic"]: continue
      self.message("general_convex: " + str(conic))

      solver = casadi.conic("mysolver",conic,{'h':H.sparsity(),'a':A.sparsity()},qp_options)

      try:
        less_digits=aux_options["less_digits"]
      except:
        less_digits=0

      solver_in = {}
      solver_in["h"]=H
      solver_in["g"]=G
      solver_in["a"]=A
      solver_in["lbx"]=LBX
      solver_in["ubx"]=UBX
      solver_in["lba"]=LBA
      solver_in["uba"]=UBA

      solver_out = solver(**solver_in)

      self.checkarray(solver_out["x"],DM([0.873908,0.95630465,0,0,0]),str(conic),digits=max(1,6-less_digits))

      if aux_options["dual"]: self.checkarray(solver_out["lam_x"],DM([0,0,-0.339076,-10.0873907,-0.252185]),6,str(conic),digits=max(1,6-less_digits))

      if aux_options["dual"]: self.checkarray(solver_out["lam_a"],DM([0,2.52184767]),str(conic),digits=max(1,6-less_digits))

      self.assertAlmostEqual(solver_out["cost"][0],-6.264669320767,max(1,6-less_digits),str(conic))
예제 #36
0
    def test_badscaling(self):
        #return
        self.message("Badly scaled problem")
        N = 50
        H = c.diag(range(1, N + 1))
        x0 = DMatrix(range(N))

        G = -1.0 * mul(H, x0)

        A = DMatrix(0, N)

        LBX = DMatrix([-1000] * N)
        UBX = DMatrix([1000] * N)

        options = {"mutol": 1e-12, "artol": 1e-12, "tol": 1e-12}

        for qpsolver, qp_options in qpsolvers:
            if 'Cplex' in str(qpsolver):
                continue
            solver = qpsolver(H.sparsity(), A.sparsity())
            for key, val in options.iteritems():
                if solver.hasOption(key):
                    solver.setOption(key, val)

            solver.setOption(qp_options)
            solver.init()

            solver.input(QP_H).set(H)
            solver.input(QP_G).set(G)
            solver.input(QP_A).set(A)
            solver.input(QP_LBX).set(LBX)
            solver.input(QP_UBX).set(UBX)

            solver.solve()

            self.checkarray(solver.output(),
                            x0,
                            str(qpsolver) + str(qp_options),
                            digits=2)
            self.assertAlmostEqual(
                solver.output(QP_COST)[0], -0.5 * mul([x0.T, H, x0]), 3,
                str(qpsolver))
            self.checkarray(solver.output(QP_LAMBDA_X),
                            DMatrix.zeros(N, 1),
                            str(qpsolver),
                            digits=4)
예제 #37
0
    def generate_model(self, configuration):
        """ Logic to construct a model, and smooth representation on BSpline basis """
        self.n = configuration['grid_size']
        self.K = configuration['basis_number']
        self.s = configuration['model_form']['state']
        n_ps = configuration['model_form']['parameters']

        # setup fine time grid
        self.ts = ca.MX.sym("t", self.n, 1)
        self.observation_times = np.linspace(*configuration['time_span'][:2],
                                             self.n)

        # determine knots and build basis functions
        if configuration['knot_function'] is None:
            knots = casbasis.choose_knots(self.observation_times, self.K - 2)
        else:
            knots = configuration['knot_function'](self.observation_times,
                                                   self.K - 2,
                                                   configuration['dataset'])
        self.basis_fns = casbasis.basis_functions(knots)
        self.basis = ca.vcat([b(self.ts) for b in self.basis_fns]).reshape(
            (self.n, self.K))

        self.tssx = ca.SX.sym("t", self.n, 1)

        # define basis matrix and gradient matrix
        phi = ca.Function('phi', [self.ts], [self.basis])
        self.phi = np.array(phi(self.observation_times))

        bjac = ca.vcat([
            ca.diag(ca.jacobian(self.basis[:, i], self.ts))
            for i in range(self.K)
        ]).reshape((self.n, self.K))
        self.basis_jacobian = np.array(
            ca.Function('bjac', [self.ts], [bjac])(self.observation_times))

        # create the objects that define the smooth, model parameters
        self.cs = [ca.SX.sym("c_" + str(i), self.K, 1) for i in range(self.s)]
        self.xs = [self.phi @ ci for ci in self.cs]
        self.xdash = self.basis_jacobian @ ca.hcat(self.cs)
        self.ps = [ca.SX.sym("p_" + str(i)) for i in range(n_ps)]

        # model function derived from input model function
        self.model = ca.Function(
            "model", [self.tssx, *self.cs, *self.ps],
            [ca.hcat(configuration['model'](self.tssx, self.xs, self.ps))])
예제 #38
0
파일: qpsolver.py 프로젝트: kozatt/casadi
  def test_general_convex_sparse(self):
    self.message("Convex sparse QP with solvers: " + str([qpsolver for qpsolver,options in qpsolvers]))
    H = c.diag([2,1,0.2,0.7,1.3])

    H[1,2]=0.1
    H[2,1]=0.1
    
    G = DMatrix([-2,-6,1,0,0])
    A =  DMatrix([[1, 0,0.1,0.7,-1],[0.1, 2,-0.3,4,0.1]])
    makeSparse(A)
    
    LBA = DMatrix([-inf])
    UBA = DMatrix([2, 2])

    LBX = DMatrix([0]*5)
    UBX = DMatrix([inf]*5)

    options = { "mutol": 1e-12, "artol": 1e-12, "tol":1e-12}
      
    for qpsolver, qp_options in qpsolvers:
      self.message("general_convex: " + str(qpsolver))

      solver = qpsolver(H.sparsity(),A.sparsity())
      for key, val in options.iteritems():
        if solver.hasOption(key):
           solver.setOption(key,val)
      solver.setOption(qp_options)
      solver.init()

      solver.input(QP_H).set(H)
      solver.input(QP_G).set(G)
      solver.input(QP_A).set(A)
      solver.input(QP_LBX).set(LBX)
      solver.input(QP_UBX).set(UBX)
      solver.input(QP_LBA).set(LBA)
      solver.input(QP_UBA).set(UBA)

      solver.solve()
      
      self.checkarray(solver.output(),DMatrix([0.873908,0.95630465,0,0,0]),str(qpsolver),digits=6)
      
      self.checkarray(solver.output(QP_LAMBDA_X),DMatrix([0,0,-0.339076,-10.0873907,-0.252185]),6,str(qpsolver),digits=6)

      self.checkarray(solver.output(QP_LAMBDA_A),DMatrix([0,2.52184767]),str(qpsolver),digits=6)

      self.assertAlmostEqual(solver.output(QP_COST)[0],-6.264669320767,6,str(qpsolver))
예제 #39
0
파일: qpsolver.py 프로젝트: mzanon/casadi
  def test_badscaling(self):
    #return
    self.message("Badly scaled problem")
    N = 50
    H = c.diag(range(1,N+1))
    x0 = DMatrix(range(N))
    
    G = -1.0*mul(H,x0)

    A =  DMatrix(0,N)

    LBX = DMatrix([-1000]*N)
    UBX = DMatrix([1000]*N)


    options = {"mutol": 1e-12, "artol": 1e-12, "tol":1e-12}
      
    for qpsolver, qp_options, aux_options in qpsolvers:
      if 'cplex' in str(qpsolver):
        continue
      solver = QpSolver(qpsolver,qpStruct(h=H.sparsity(),a=A.sparsity()))
      for key, val in options.iteritems():
        if solver.hasOption(key):
           solver.setOption(key,val)
           
      solver.setOption(qp_options)
      solver.init()

      try:
        less_digits=aux_options["less_digits"]
      except:
        less_digits=0

      solver.setInput(H,"h")
      solver.setInput(G,"g")
      solver.setInput(A,"a")
      solver.setInput(LBX,"lbx")
      solver.setInput(UBX,"ubx")

      solver.evaluate()

      self.checkarray(solver.getOutput(),x0,str(qpsolver)+str(qp_options),digits=max(1,2-less_digits))
      self.assertAlmostEqual(solver.getOutput("cost")[0],-0.5*mul([x0.T,H,x0]),max(1,3-less_digits),str(qpsolver))
      self.checkarray(solver.getOutput("lam_x"),DMatrix.zeros(N,1),str(qpsolver),digits=max(1,4-less_digits))
예제 #40
0
def diag(v, k=0):
    """
    Extract a diagonal or construct a diagonal array.

    See syntax here: https://numpy.org/doc/stable/reference/generated/numpy.diag.html
    """
    if not is_casadi_type(v):
        return _onp.diag(v, k=k)

    else:
        if k != 0:
            raise NotImplementedError("Should be super possible, just haven't had the need yet.")

        if 1 in v.shape:
            return _cas.diag(v)
        elif v.shape[0] == v.shape[1]:
            raise NotImplementedError("Should be super possible, just haven't had the need yet.")
        else:
            raise ValueError("Cannot return the diagonal of a non-square matrix.")
예제 #41
0
파일: conic.py 프로젝트: casadi/casadi
  def test_badscaling(self):
    #return
    self.message("Badly scaled problem")
    N = 50
    H = c.diag(list(range(1,N+1)))
    x0 = DM(list(range(N)))

    G = -1.0*mtimes(H,x0)

    A =  DM(0,N)

    LBX = DM([-1000]*N)
    UBX = DM([1000]*N)

    for conic, qp_options, aux_options in conics:
      if not aux_options["quadratic"]: continue
      if 'cplex' in str(conic):
        continue
      if 'worhp' in str(conic): # works but occasionaly throws segfaults, ulimit on travis?
        continue
      solver = casadi.conic("mysolver",conic,{'h':H.sparsity(),'a':A.sparsity()},qp_options)

      try:
        less_digits=aux_options["less_digits"]
      except:
        less_digits=0

      solver_in = {}
      solver_in["h"]=H
      solver_in["g"]=G
      solver_in["a"]=A
      solver_in["lbx"]=LBX
      solver_in["ubx"]=UBX

      solver_out = solver(**solver_in)

      self.checkarray(solver_out["x"],x0,str(conic)+str(qp_options),digits=max(1,2-less_digits))
      self.assertAlmostEqual(solver_out["cost"][0],-0.5*mtimes([x0.T,H,x0]),max(1,3-less_digits),str(conic))
      if aux_options["dual"]: self.checkarray(solver_out["lam_x"],DM.zeros(N,1),str(conic),digits=max(1,4-less_digits))
예제 #42
0
파일: qpsolver.py 프로젝트: kozatt/casadi
  def test_badscaling(self):
    #return
    self.message("Badly scaled problem")
    N = 50
    H = c.diag(range(1,N+1))
    x0 = DMatrix(range(N))
    
    G = -1.0*mul(H,x0)

    A =  DMatrix(0,N)

    LBX = DMatrix([-1000]*N)
    UBX = DMatrix([1000]*N)


    options = {"mutol": 1e-12, "artol": 1e-12, "tol":1e-12}
      
    for qpsolver, qp_options in qpsolvers:
      if 'Cplex' in str(qpsolver):
        continue
      solver = qpsolver(H.sparsity(),A.sparsity())
      for key, val in options.iteritems():
        if solver.hasOption(key):
           solver.setOption(key,val)
           
      solver.setOption(qp_options)
      solver.init()

      solver.input(QP_H).set(H)
      solver.input(QP_G).set(G)
      solver.input(QP_A).set(A)
      solver.input(QP_LBX).set(LBX)
      solver.input(QP_UBX).set(UBX)

      solver.solve()

      self.checkarray(solver.output(),x0,str(qpsolver)+str(qp_options),digits=2)
      self.assertAlmostEqual(solver.output(QP_COST)[0],-0.5*mul([x0.T,H,x0]),3,str(qpsolver))
      self.checkarray(solver.output(QP_LAMBDA_X),DMatrix.zeros(N,1),str(qpsolver),digits=4)
예제 #43
0
파일: qpsolver.py 프로젝트: BrechtBa/casadi
  def test_badscaling(self):
    #return
    self.message("Badly scaled problem")
    N = 50
    H = c.diag(range(1,N+1))
    x0 = DMatrix(range(N))
    
    G = -1.0*mul(H,x0)

    A =  DMatrix(0,N)

    LBX = DMatrix([-1000]*N)
    UBX = DMatrix([1000]*N)

    for qpsolver, qp_options, aux_options in qpsolvers:
      if 'cplex' in str(qpsolver):
        continue
      if 'worhp' in str(qpsolver): # works but occasionaly throws segfaults, ulimit on travis?
        continue
      solver = QpSolver("mysolver",qpsolver,{'h':H.sparsity(),'a':A.sparsity()},qp_options)

      try:
        less_digits=aux_options["less_digits"]
      except:
        less_digits=0

      solver.setInput(H,"h")
      solver.setInput(G,"g")
      solver.setInput(A,"a")
      solver.setInput(LBX,"lbx")
      solver.setInput(UBX,"ubx")

      solver.evaluate()

      self.checkarray(solver.getOutput(),x0,str(qpsolver)+str(qp_options),digits=max(1,2-less_digits))
      self.assertAlmostEqual(solver.getOutput("cost")[0],-0.5*mul([x0.T,H,x0]),max(1,3-less_digits),str(qpsolver))
      self.checkarray(solver.getOutput("lam_x"),DMatrix.zeros(N,1),str(qpsolver),digits=max(1,4-less_digits))
예제 #44
0
def loadGPModel(name, model, xscaler, yscaler, kernel='RBF'):
    """ GP mean and variance as casadi.SX variable
    """
    X = model.X_train_
    x = cs.SX.sym('x', 1, X.shape[1])

    # mean
    if kernel == 'RBF':
        K1 = CasadiRBF(x, X, model)
        K2 = CasadiConstant(x, X, model)
        K = K1 + K2
    elif kernel == 'Matern':
        K = CasadiMatern(x, X, model)
    else:
        raise NotImplementedError

    y_mu = cs.mtimes(K, model.alpha_) + model._y_train_mean
    y_mu = y_mu * yscaler.scale_ + yscaler.mean_

    # variance
    L_inv = solve_triangular(model.L_.T,np.eye(model.L_.shape[0]))
    K_inv = L_inv.dot(L_inv.T)

    if kernel == 'RBF':
        K1_ = CasadiRBF(x, x, model)
        K2_ = CasadiConstant(x, x, model)
        K_ = K1_ + K2_
    elif kernel == 'Matern':
        K_ = CasadiMatern(x, x, model)

    y_var = cs.diag(K_) - cs.sum2(cs.mtimes(K, K_inv)*K)
    y_var = cs.fmax(y_var, 0)
    y_std = cs.sqrt(y_var)
    y_std *= yscaler.scale_

    gpmodel = cs.Function(name, [x], [y_mu, y_std])
    return gpmodel
예제 #45
0
    def __constraint(self, mean, covar, H, quantile, ub, lb, eps):
        """ Build up chance constraint vectors
        """

        r = ca.SX.sym('r')
        mean_s = ca.SX.sym('mean', ca.MX.size(mean))
        S_s = ca.SX.sym('S', ca.MX.size(covar))
        H_s = ca.SX.sym('H', 1, ca.MX.size2(H))
        S = covar
        con_func = ca.Function('con', [mean_s, S_s, H_s, r],
                               [H_s @ mean_s + r * H_s @ ca.diag(S_s)])

        con = []
        con_lb = []
        con_ub = []
        for i in range(ca.MX.size1(mean)):
            con.append(con_func(mean, S, H[i, :], quantile[i]) - eps[i])
            con_ub.append(ub[i])
            con_lb.append(-np.inf)
            con.append(con_func(mean, S, H[i, :], -quantile[i]) + eps[i])
            con_ub.append(np.inf)
            con_lb.append(lb[i])
        cons = dict(con=con, con_lb=con_lb, con_ub=con_ub)
        return cons
예제 #46
0
파일: ad.py 프로젝트: casadi/casadi
  def test_MX(self):

    x = MX.sym("x",2)
    y = MX.sym("y",2,2)

    f1 = Function("f1", [x,y],[x+y[0,0],mtimes(y,x)])

    f2 = Function("f2", [x,y],[mtimes(MX.zeros(0,2),x)])

    f3 = Function("f3", [x,y],[MX.zeros(0,0),mtimes(y,x)])

    f4 = Function("f4", [x,y],[MX.zeros(0,2),mtimes(y,x)])

    ndir = 2

    in1 = [x,y]
    v1 = [DM([1.1,1.3]),DM([[0.7,1.5],[2.1,0.9]])]

    w=x[:]
    w[1]*=2

    w2=x[:]
    w2[1]*=x[0]

    ww=x[:]
    ww[[0,1]]*=x

    wwf=x[:]
    wwf[[1,0]]*=x

    wwr=x[:]
    wwr[[0,0,1,1]]*=2

    yy=y[:,:]

    yy[:,0] = x

    yy2=y[:,:]

    yy2[:,0] = x**2

    yyy=y[:,:]

    yyy[[1,0],0] = x

    yyy2=y[:,:]

    yyy2[[1,0],0] = x**2


    def remove_first(x):
      ret = DM(x)
      if ret.numel()>0:
        ret[0,0] = DM(1,1)
        return ret.sparsity()
      else:
        return ret.sparsity()

    def remove_last(x):
      ret = DM(x)
      if ret.nnz()>0:
        ret[ret.sparsity().row()[-1],ret.sparsity().get_col()[-1]] = DM(1,1)
        return ret.sparsity()
      else:
        return x

    spmods = [lambda x: x , remove_first, remove_last]

    # TODO: sparse seeding

    for inputs,values,out, jac in [
          (in1,v1,x,DM.eye(2)),
          (in1,v1,x.T,DM.eye(2)),
          (in1,v1,x**2,2*c.diag(x)),
          (in1,v1,(x**2).attachAssert(True),2*c.diag(x)),
          (in1,v1,(x**2).T,2*c.diag(x)),
          (in1,v1,c.reshape(x,(1,2)),DM.eye(2)),
          (in1,v1,c.reshape(x**2,(1,2)),2*c.diag(x)),
          (in1,v1,x+y.nz[0],DM.eye(2)),
          (in1,v1,x+y[0,0],DM.eye(2)),
          (in1,v1,x+x,2*DM.eye(2)),
          (in1,v1,x**2+x,2*c.diag(x)+DM.eye(2)),
          (in1,v1,x*x,2*c.diag(x)),
          (in1,v1,x*y.nz[0],DM.eye(2)*y.nz[0]),
          (in1,v1,x*y[0,0],DM.eye(2)*y[0,0]),
          (in1,v1,x[0],DM.eye(2)[0,:]),
          (in1,v1,(x**2)[0],horzcat(*[2*x[0],MX(1,1)])),
          (in1,v1,x[0]+x[1],DM.ones(1,2)),
          (in1,v1,sin(repmat(x**2,1,3)),repmat(cos(c.diag(x**2))*2*c.diag(x),3,1)),
          (in1,v1,sin(repsum((x**2).T,1,2)),cos(x[0]**2+x[1]**2)*2*x.T),
          (in1,v1,vertcat(*[x[1],x[0]]),sparsify(DM([[0,1],[1,0]]))),
          (in1,v1,vertsplit(x,[0,1,2])[1],sparsify(DM([[0,1]]))),
          (in1,v1,vertcat(*[x[1]**2,x[0]**2]),blockcat([[MX(1,1),2*x[1]],[2*x[0],MX(1,1)]])),
          (in1,v1,vertsplit(x**2,[0,1,2])[1],blockcat([[MX(1,1),2*x[1]]])),
          (in1,v1,vertsplit(x**2,[0,1,2])[1]**3,blockcat([[MX(1,1),6*x[1]**5]])),
          (in1,v1,horzcat(*[x[1],x[0]]).T,sparsify(DM([[0,1],[1,0]]))),
          (in1,v1,horzcat(*[x[1]**2,x[0]**2]).T,blockcat([[MX(1,1),2*x[1]],[2*x[0],MX(1,1)]])),
          (in1,v1,diagcat(*[x[1]**2,y,x[0]**2]),
            blockcat(  [[MX(1,1),2*x[1]]] + ([[MX(1,1),MX(1,1)]]*14)  + [[2*x[0],MX(1,1)]] )
          ),
          (in1,v1,horzcat(*[x[1]**2,x[0]**2]).T,blockcat([[MX(1,1),2*x[1]],[2*x[0],MX(1,1)]])),
          (in1,v1,x[[0,1]],sparsify(DM([[1,0],[0,1]]))),
          (in1,v1,(x**2)[[0,1]],2*c.diag(x)),
          (in1,v1,x[[0,0,1,1]],sparsify(DM([[1,0],[1,0],[0,1],[0,1]]))),
          (in1,v1,(x**2)[[0,0,1,1]],blockcat([[2*x[0],MX(1,1)],[2*x[0],MX(1,1)],[MX(1,1),2*x[1]],[MX(1,1),2*x[1]]])),
          (in1,v1,wwr,sparsify(DM([[2,0],[0,2]]))),
          (in1,v1,x[[1,0]],sparsify(DM([[0,1],[1,0]]))),
          (in1,v1,x[[1,0],0],sparsify(DM([[0,1],[1,0]]))),
          (in1,v1,w,sparsify(DM([[1,0],[0,2]]))),
          (in1,v1,w2,blockcat([[1,MX(1,1)],[x[1],x[0]]])),
          (in1,v1,ww,2*c.diag(x)),
          (in1,v1,wwf,vertcat(*[x[[1,0]].T,x[[1,0]].T])),
          (in1,v1,yy[:,0],DM.eye(2)),
          (in1,v1,yy2[:,0],2*c.diag(x)),
          (in1,v1,yyy[:,0],sparsify(DM([[0,1],[1,0]]))),
          (in1,v1,mtimes(y,x),y),
          (in1,v1,mtimes(x.T,y.T),y),
          (in1,v1,mac(y,x,DM.zeros(Sparsity.triplet(2,1,[1],[0]))),y[Sparsity.triplet(2,2,[1,1],[0,1])]),
          (in1,v1,mac(x.T,y.T,DM.zeros(Sparsity.triplet(2,1,[1],[0]).T)),y[Sparsity.triplet(2,2,[1,1],[0,1])]),
          (in1,v1,mtimes(y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])],x),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]),
          (in1,v1,mtimes(x.T,y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])].T),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]),
          (in1,v1,mtimes(y,x**2),y*2*vertcat(*[x.T,x.T])),
          (in1,v1,sin(x),c.diag(cos(x))),
          (in1,v1,sin(x**2),c.diag(cos(x**2)*2*x)),
          (in1,v1,x*y[:,0],c.diag(y[:,0])),
          (in1,v1,x*y.nz[[0,1]],c.diag(y.nz[[0,1]])),
          (in1,v1,x*y.nz[[1,0]],c.diag(y.nz[[1,0]])),
          (in1,v1,x*y[[0,1],0],c.diag(y[[0,1],0])),
          (in1,v1,x*y[[1,0],0],c.diag(y[[1,0],0])),
          (in1,v1,c.dot(x,x),(2*x).T),
          (in1,v1,c.dot(x**2,x),(3*x**2).T),
          #(in1,v1,c.det(horzcat(*[x,DM([1,2])])),DM([-1,2])), not implemented
          (in1,v1,f1.call(in1)[1],y),
          (in1,v1,f1.call([x**2,y])[1],y*2*vertcat(*[x.T,x.T])),
          (in1,v1,f2.call(in1)[0],DM.zeros(0,2)),
          (in1,v1,f2(x**2,y),DM.zeros(0,2)),
          (in1,v1,f3.call(in1)[0],DM.zeros(0,2)),
          (in1,v1,f3.call([x**2,y])[0],DM.zeros(0,2)),
          (in1,v1,f4.call(in1)[0],DM.zeros(0,2)),
          (in1,v1,f4.call([x**2,y])[0],DM.zeros(0,2)),
          #(in1,v1,f1([x**2,[]])[1],DM.zeros(2,2)),
          #(in1,v1,f1([[],y])[1],DM.zeros(2,2)),
          (in1,v1,vertcat(*[x,DM(0,1)]),DM.eye(2)),
          (in1,v1,project(x**2, sparsify(DM([0,1])).sparsity()),blockcat([[MX(1,1),MX(1,1)],[MX(1,1),2*x[1]]])),
          (in1,v1,c.dot(x,y[:,0]),y[:,0].T),
          (in1,v1,x.nz[IM([[1,0]])].T*y.nz[IM([[0,2]])],blockcat([[MX(1,1),y.nz[0]],[y.nz[2],MX(1,1)]])),
          (in1,v1,x.nz[c.diag([1,0])]*y.nz[c.diag([0,2])],blockcat([[MX(1,1),y.nz[0]],[MX(1,1),MX(1,1)],[MX(1,1),MX(1,1)],[y.nz[2],MX(1,1)]])),
     ]:
      print(out)
      fun = Function("fun", inputs,[out,jac])
      funsx = fun.expand("expand_fun")
      fun_ad = [Function("fun", inputs,[out,jac], {'ad_weight':w, 'ad_weight_sp':w}) for w in [0,1]]
      funsx_ad = [f.expand('expand_'+f.name()) for f in fun_ad]

      fun_out = fun.call(values)
      funsx_out = funsx.call(values)

      self.checkarray(fun_out[0],funsx_out[0])
      self.checkarray(fun_out[1],funsx_out[1])

      self.check_codegen(fun,inputs=values)

      J_ = fun_out[1]

      def vec(l):
        ret = []
        for i in l:
          ret.extend(i)
        return ret

      storage2 = {}
      storage = {}

      vf_mx = None

      for f in [fun, fun.expand('expand_'+fun.name())]:
        d1 = f.forward(ndir)
        d2 = f.reverse(ndir)

        num_in = f.n_in()
        num_out = f.n_out()

        # evalThings
        for sym in [MX.sym, SX.sym]:
          if f.is_a('MXFunction') and sym==SX.sym: continue
          if f.is_a('SXFunction') and sym==MX.sym: continue

          # dense
          for spmod,spmod2 in itertools.product(spmods,repeat=2):
            fseeds = [[sym("f",spmod(f.sparsity_in(i))) for i in range(f.n_in())]  for d in range(ndir)]
            aseeds = [[sym("a",spmod2(f.sparsity_out(i)))  for i in range(f.n_out())] for d in range(ndir)]
            inputss = [sym("i",f.sparsity_in(i)) for i in range(f.n_in())]

            res = f.call(inputss,True)
            fwdsens = forward(res,inputss,fseeds,dict(always_inline=True))
            adjsens = reverse(res,inputss,aseeds,dict(always_inline=True))

            fseed = [DM(fseeds[d][0].sparsity(),random.random(fseeds[d][0].nnz())) for d in range(ndir) ]
            aseed = [DM(aseeds[d][0].sparsity(),random.random(aseeds[d][0].nnz())) for d in range(ndir) ]
            vf = Function("vf", inputss+vec([fseeds[i]+aseeds[i] for i in range(ndir)]),list(res) + vec([list(fwdsens[i])+list(adjsens[i]) for i in range(ndir)]))

            vf_in = list(values)
            offset = len(inputss)

            for d in range(ndir):
              vf_in.append(fseed[d])
              for i in range(len(values)-1):
                vf_in.append(0)

              vf_in.append(aseed[d])
              vf_in.append(0)

            vf_out = vf.call(vf_in)
            self.check_codegen(vf,inputs=vf_in)

            offset = len(res)
            for d in range(ndir):
              seed = array(fseed[d].T).ravel()
              sens = array(vf_out[offset+0].T).ravel()
              offset+=len(inputss)
              self.checkarray(sens,mtimes(J_,seed),"eval Fwd %d %s" % (d,str(type(f))+str(sym)))

              seed = array(aseed[d].T).ravel()
              sens = array(vf_out[offset+0].T).ravel()
              offset+=len(inputss)

              self.checkarray(sens,mtimes(J_.T,seed),"eval Adj %d %s" % (d,str([vf_out[i] for i in range(vf.n_out())])))


            assert(offset==vf.n_out())

            # Complete random seeding
            random.seed(1)
            vf_in = []
            for i in range(vf.n_in()):
              vf_in.append(DM(vf.sparsity_in(i),random.random(vf.nnz_in(i))))

            vf_out = vf.call(vf_in)
            self.check_codegen(vf,inputs=vf_in)
            storagekey = (spmod,spmod2)
            if not(storagekey in storage):
              storage[storagekey] = []
            storage[storagekey].append(vf_out)

            # Added to make sure that the same seeds are used for SX and MX
            if sym is MX.sym:
              vf_mx = vf

          # Second order sensitivities
          for sym2 in [MX.sym, SX.sym]:

            if vf.is_a('MXFunction') and sym2==SX.sym: continue
            if vf.is_a('MXFunction') and sym2==MX.sym: continue

            for spmod_2,spmod2_2 in itertools.product(spmods,repeat=2):
              fseeds2 = [[sym2("f",vf_mx.sparsity_in(i)) for i in range(vf.n_in())] for d in range(ndir)]
              aseeds2 = [[sym2("a",vf_mx.sparsity_out(i))  for i in range(vf.n_out()) ] for d in range(ndir)]
              inputss2 = [sym2("i",vf_mx.sparsity_in(i)) for i in range(vf.n_in())]

              res2 = vf.call(inputss2,True)
              fwdsens2 = forward(res2,inputss2,fseeds2,dict(always_inline=True))
              adjsens2 = reverse(res2,inputss2,aseeds2,dict(always_inline=True))

              vf2 = Function("vf2", inputss2+vec([fseeds2[i]+aseeds2[i] for i in range(ndir)]),list(res2) + vec([list(fwdsens2[i])+list(adjsens2[i]) for i in range(ndir)]))

              random.seed(1)
              vf2_in = []
              for i in range(vf2.n_in()):
                vf2_in.append(DM(vf2.sparsity_in(i),random.random(vf2.nnz_in(i))))

              vf2_out = vf2.call(vf2_in)
              self.check_codegen(vf2,inputs=vf2_in)
              storagekey = (spmod,spmod2)
              if not(storagekey in storage2):
                storage2[storagekey] = []
              storage2[storagekey].append(vf2_out)

      # Remainder of eval testing
      for store,order in [(storage,"first-order"),(storage2,"second-order")]:
        for stk,st in list(store.items()):
          for i in range(len(st)-1):
            for k,(a,b) in enumerate(zip(st[0],st[i+1])):
              if b.numel()==0 and sparsify(a).nnz()==0: continue
              if a.numel()==0 and sparsify(b).nnz()==0: continue
              self.checkarray(sparsify(a),sparsify(b),("%s, output(%d)" % (order,k)))

      for expand in [False, True]:
        #  jacobian()
        for mode in ["forward","reverse"]:
          ind = 0 if mode=='forward' else 1
          f = fun_ad[ind] if expand  else funsx_ad[ind]

          Jf=f.jacobian_old(0,0)
          Jf_out = Jf.call(values)

          self.check_codegen(Jf,inputs=values)
          self.checkarray(Jf_out[0],J_)
          self.checkarray(DM.ones(Jf.sparsity_out(0)),DM.ones(J_.sparsity()),str(out)+str(mode))
          self.checkarray(DM.ones(f.sparsity_jac(0, 0)),DM.ones(J_.sparsity()))

      # Scalarized
      if out.is_empty(): continue
      s_i  = out.sparsity().row()[0]
      s_j  = out.sparsity().get_col()[0]
      s_k = s_i*out.size2()+s_j
      H_ = None

      for expand in [False, True]:
        for mode in ["forward","reverse"]:
          w = 0 if mode=='forward' else 1
          f = Function("fun", inputs,[out[s_i,s_j],jac[s_k,:].T], {'ad_weight':w, 'ad_weight_sp':w})
          if expand: f=f.expand('expand_'+f.name())
          f_out = f.call(values)
          J_ = f_out[1]

          Hf=f.hessian_old(0, 0)
          Hf_out = Hf.call(values)
          self.check_codegen(Hf,inputs=values)
          if H_ is None:
            H_ = Hf_out[0]
          self.checkarray(Hf_out[0],H_,failmessage=("mode: %s" % mode))
예제 #47
0

p_mean = pl.mean(p_test)
p_std = pl.std(p_test, ddof=0)

pe_test.compute_covariance_matrix()
pe_test.print_estimation_results()


# Generate report

print("\np_mean         = " + str(ca.DMatrix(p_mean)))
print("phat_last_exp  = " + str(ca.DMatrix(pe_test.estimated_parameters)))

print("\np_sd           = " + str(ca.DMatrix(p_std)))
print("sd_from_covmat = " + str(ca.diag(ca.sqrt(pe_test.covariance_matrix))))
print("beta           = " + str(pe_test.beta))

print("\ndelta_abs_sd   = " + str(ca.fabs(ca.DMatrix(p_std) - \
    ca.diag(ca.sqrt(pe_test.covariance_matrix)))))
print("delta_rel_sd   = " + str(ca.fabs(ca.DMatrix(p_std) - \
    ca.diag(ca.sqrt(pe_test.covariance_matrix))) / ca.DMatrix(p_std)))


fname = os.path.basename(__file__)[:-3] + ".rst"

report = open(fname, "w")
report.write( \
'''Concept test: covariance matrix computation
===========================================
예제 #48
0
파일: pecas.py 프로젝트: adbuerger/PECas
    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
예제 #49
0
파일: sx.py 프로젝트: kurtgeebelen/casadi
  def test_eig_symbolic(self):
    x = SX.sym("x",2,2)
    f = Function("f", [x],[eig_symbolic(x)])
    f_in = [0]*f.n_in();f_in[0]=DM([[2,0.1],[0.3,0.7]])
    f_out = f.call(f_in)
    self.checkarray(f_out[0],DM([0.67732,2.02268]),digits=5)


    x = SX.sym("x",2)
    f = Function("f", [x],[eig_symbolic(c.diag(x))])
    f_in = [0]*f.n_in();f_in[0]=DM([3,7])
    f_out = f.call(f_in)
    self.checkarray(f_out[0],f_in[0])


    x = SX.sym("x",5)
    f = Function("f", [x],[eig_symbolic(c.diag(x))])
    f_in = [0]*f.n_in();f_in[0]=DM([3,7,2,1,6])
    f_out = f.call(f_in)
    self.checkarray(f_out[0],f_in[0])

    x = SX.sym("x",2,2)
    y = SX.sym("y",2)
    f = Function("f", [x,y],[eig_symbolic(diagcat(*[x,c.diag(y)]))])
    f_in = [0]*f.n_in();f_in[0]=DM([[2,0.1],[0.3,0.7]])
    f_in[1]=[3,7]
    f_out = f.call(f_in)
    self.checkarray(f_out[0],DM([0.67732,2.02268,3,7]),digits=5)

    x = SX.sym("x",3,3)
    x[2,0] = 0
    x[1,0] = 0

    x = sparsify(x)

    e = eig_symbolic(x)

    f = Function("f", [x],[e])
    f_in = [0]*f.n_in();f_in[0]=DM(f.sparsity_in(0),list(range(1,8)))
    f_in[0].print_dense()
    f_out = f.call(f_in)
    self.checkarray(f_out[0],DM([1,-0.29150,10.29150]),digits=5)


    x = SX.sym("x",3,3)
    x[2,0] = 0
    x[1,0] = 0
    x[2,1] = 0

    x = sparsify(x)

    e = eig_symbolic(x)

    f = Function("f", [x],[e])
    f_in = [0]*f.n_in();f_in[0]=DM(f.sparsity_in(0),list(range(1,7)))
    f_in[0].print_dense()
    f_out = f.call(f_in)
    self.checkarray(f_out[0],DM([1,3,6]),digits=5)

    x = SX.sym("x",Sparsity.upper(5))

    f = Function("f", [x],[eig_symbolic(x)])
    fin = DM(x.sparsity(),0)
    fin[Sparsity.diag(5)] = c.diag(list(range(5)))
    self.checkarray(f(fin), DM(list(range(5))))
예제 #50
0
L.input().set(1)
L.evaluate()
Lend = DM(L.output())  # Le at the end of the control interval

dLm = numSample1D(dL,DM(tau_root).T)  # d-by-d

resf = SXFunction(customIO(t=model.t, x=states, dx= dstates, u=controls, p=model.p,w=model.w),[model.res_w])
resf.init()

def linear_combination(v,w):
  return sum([i*j for i,j in zip(v,w)])

### LPDE -- start

# Disturbances have a standard deviation of 5% with respect to their nominal value
Sigma = c.diag(vertcat([scaling_dist,scaling_states])**2)*analysis.noiserel

K = ssym("K",controls.size,states.size)

u = ssym("u",nu)
x = ssym("x",ns)
zj = [ ssym("z",ns) for i in range(d)]
z = vertcat(zj)

w = ssym("w",model.w.size)
G = zj[0]-x

delta = ssym("delta")


for j in range(1,d):
예제 #51
0
파일: sx.py 프로젝트: tmmsartor/casadi
  def test_eig_symbolic(self):
    x = SX.sym("x",2,2)
    f = SXFunction([x],[eig_symbolic(x)])
    f.init()
    f.setInput(DMatrix([[2,0.1],[0.3,0.7]]))
    f.evaluate()
    self.checkarray(f.output(),DMatrix([0.67732,2.02268]),digits=5)
    
    
    x = SX.sym("x",2)
    f = SXFunction([x],[eig_symbolic(c.diag(x))])
    f.init()
    f.setInput([3,7])
    f.evaluate()
    self.checkarray(f.output(),f.input())

    
    x = SX.sym("x",5)
    f = SXFunction([x],[eig_symbolic(c.diag(x))])
    f.init()
    f.setInput([3,7,2,1,6])
    f.evaluate()
    self.checkarray(f.output(),f.input())
    
    x = SX.sym("x",2,2)
    y = SX.sym("y",2)
    f = SXFunction([x,y],[eig_symbolic(blkdiag([x,c.diag(y)]))])
    f.init()
    f.setInput(DMatrix([[2,0.1],[0.3,0.7]]),0)
    f.setInput([3,7],1)
    f.evaluate()
    self.checkarray(f.output(),DMatrix([0.67732,2.02268,3,7]),digits=5)

    x = SX.sym("x",3,3)
    x[2,0] = 0
    x[1,0] = 0

    x = sparse(x)

    e = eig_symbolic(x)
    
    f = SXFunction([x],[e])
    f.init()
    f.setInput(range(1,8))
    f.input().printDense()
    f.evaluate()
    self.checkarray(f.output(),DMatrix([1,-0.29150,10.29150]),digits=5)
    
    
    x = SX.sym("x",3,3)
    x[2,0] = 0
    x[1,0] = 0
    x[2,1] = 0
    
    x = sparse(x)

    e = eig_symbolic(x)
    
    f = SXFunction([x],[e])
    f.init()
    f.setInput(range(1,7))
    f.input().printDense()
    f.evaluate()
    self.checkarray(f.output(),DMatrix([1,3,6]),digits=5)

    x = SX.sym("x",Sparsity.triu(5))
  
    f = SXFunction([x],[eig_symbolic(x)])
    f.init()
    f.setInput(6)
    f.input()[Sparsity.diag(5)] = c.diag(range(5))
    f.evaluate()
    self.checkarray(f.output(),DMatrix(range(5)))
예제 #52
0
파일: ad.py 프로젝트: ghorn/debian-casadi
  def test_MX(self):

    x = MX.sym("x",2)
    y = MX.sym("y",2,2)
    
    f1 = MXFunction([x,y],[x+y[0,0],mul(y,x)])
    f1.init()
    
    f2 = MXFunction([x,y],[mul(MX.zeros(0,2),x)])
    f2.init()

    f3 = MXFunction([x,y],[MX.zeros(0,0),mul(y,x)])
    f3.init()
    
    f4 = MXFunction([x,y],[MX.zeros(0,2),mul(y,x)])
    f4.init()
    
    ndir = 2
    
    in1 = [x,y]
    v1 = [DMatrix([1.1,1.3]),DMatrix([[0.7,1.5],[2.1,0.9]])]
    
    w=x[:]
    w[1]*=2

    w2=x[:]
    w2[1]*=x[0]
    
    ww=x[:]
    ww[[0,1]]*=x

    wwf=x[:]
    wwf[[1,0]]*=x
    
    wwr=x[:]
    wwr[[0,0,1,1]]*=2
    
    yy=y[:,:]
    
    yy[:,0] = x

    yy2=y[:,:]
    
    yy2[:,0] = x**2
    
    yyy=y[:,:]
    
    yyy[[1,0],0] = x

    yyy2=y[:,:]
    
    yyy2[[1,0],0] = x**2
    

    def remove_first(x):
      ret = DMatrix(x)
      if ret.numel()>0:
        ret[0,0] = DMatrix.sparse(1,1)
        return ret
      else:
        return ret

    def remove_last(x):
      ret = DMatrix(x)
      if ret.size()>0:
        ret[ret.sparsity().row()[-1],ret.sparsity().getCol()[-1]] = DMatrix.sparse(1,1)
        return ret
      else:
        return x
      
    spmods = [lambda x: x , remove_first, remove_last]
    
    # TODO: sparse seeding
    
    for inputs,values,out, jac in [
          (in1,v1,x,DMatrix.eye(2)),
          (in1,v1,x.T,DMatrix.eye(2)),
          (in1,v1,x**2,2*c.diag(x)),
          (in1,v1,(x**2).attachAssert(True),2*c.diag(x)),
          (in1,v1,(x**2).T,2*c.diag(x)),
          (in1,v1,c.reshape(x,(1,2)),DMatrix.eye(2)),
          (in1,v1,c.reshape(x**2,(1,2)),2*c.diag(x)),
          (in1,v1,x+y.nz[0],DMatrix.eye(2)),
          (in1,v1,x+y[0,0],DMatrix.eye(2)),
          (in1,v1,x+x,2*DMatrix.eye(2)),
          (in1,v1,x**2+x,2*c.diag(x)+DMatrix.eye(2)),
          (in1,v1,x*x,2*c.diag(x)),
          (in1,v1,x*y.nz[0],DMatrix.eye(2)*y.nz[0]),
          (in1,v1,x*y[0,0],DMatrix.eye(2)*y[0,0]),
          (in1,v1,x[0],DMatrix.eye(2)[0,:]),
          (in1,v1,(x**2)[0],horzcat([2*x[0],MX.sparse(1,1)])),
          (in1,v1,x[0]+x[1],DMatrix.ones(1,2)),
          (in1,v1,vertcat([x[1],x[0]]),sparse(DMatrix([[0,1],[1,0]]))),
          (in1,v1,vertsplit(x,[0,1,2])[1],sparse(DMatrix([[0,1]]))),
          (in1,v1,vertcat([x[1]**2,x[0]**2]),blockcat([[MX.sparse(1,1),2*x[1]],[2*x[0],MX.sparse(1,1)]])),
          (in1,v1,vertsplit(x**2,[0,1,2])[1],blockcat([[MX.sparse(1,1),2*x[1]]])),
          (in1,v1,vertsplit(x**2,[0,1,2])[1]**3,blockcat([[MX.sparse(1,1),6*x[1]**5]])),
          (in1,v1,horzcat([x[1],x[0]]).T,sparse(DMatrix([[0,1],[1,0]]))),
          (in1,v1,horzcat([x[1]**2,x[0]**2]).T,blockcat([[MX.sparse(1,1),2*x[1]],[2*x[0],MX.sparse(1,1)]])),
          (in1,v1,diagcat([x[1]**2,y,x[0]**2]),
            blockcat(  [[MX.sparse(1,1),2*x[1]]] + ([[MX.sparse(1,1),MX.sparse(1,1)]]*14)  + [[2*x[0],MX.sparse(1,1)]] )
          ),
          (in1,v1,horzcat([x[1]**2,x[0]**2]).T,blockcat([[MX.sparse(1,1),2*x[1]],[2*x[0],MX.sparse(1,1)]])),
          (in1,v1,x[[0,1]],sparse(DMatrix([[1,0],[0,1]]))),
          (in1,v1,(x**2)[[0,1]],2*c.diag(x)),
          (in1,v1,x[[0,0,1,1]],sparse(DMatrix([[1,0],[1,0],[0,1],[0,1]]))),
          (in1,v1,(x**2)[[0,0,1,1]],blockcat([[2*x[0],MX.sparse(1,1)],[2*x[0],MX.sparse(1,1)],[MX.sparse(1,1),2*x[1]],[MX.sparse(1,1),2*x[1]]])),
          (in1,v1,wwr,sparse(DMatrix([[2,0],[0,2]]))),
          (in1,v1,x[[1,0]],sparse(DMatrix([[0,1],[1,0]]))), 
          (in1,v1,x[[1,0],0],sparse(DMatrix([[0,1],[1,0]]))),
          (in1,v1,w,sparse(DMatrix([[1,0],[0,2]]))),
          (in1,v1,w2,blockcat([[1,MX.sparse(1,1)],[x[1],x[0]]])),
          (in1,v1,ww,2*c.diag(x)),
          (in1,v1,wwf,vertcat([x[[1,0]].T,x[[1,0]].T])),
          (in1,v1,yy[:,0],DMatrix.eye(2)),
          (in1,v1,yy2[:,0],2*c.diag(x)),
          (in1,v1,yyy[:,0],sparse(DMatrix([[0,1],[1,0]]))),
          (in1,v1,mul(y,x),y),
          (in1,v1,mul(x.T,y.T),y),
          (in1,v1,mul(y,x,DMatrix.zeros(Sparsity.triplet(2,1,[1],[0]))),y[Sparsity.triplet(2,2,[1,1],[0,1])]),
          (in1,v1,mul(x.T,y.T,DMatrix.zeros(Sparsity.triplet(2,1,[1],[0]).T)),y[Sparsity.triplet(2,2,[1,1],[0,1])]),
          (in1,v1,mul(y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])],x),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]),
          (in1,v1,mul(x.T,y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])].T),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]),
          (in1,v1,mul(y,x**2),y*2*vertcat([x.T,x.T])),
          (in1,v1,sin(x),c.diag(cos(x))),
          (in1,v1,sin(x**2),c.diag(cos(x**2)*2*x)),
          (in1,v1,x*y[:,0],c.diag(y[:,0])),
          (in1,v1,x*y.nz[[0,1]],c.diag(y.nz[[0,1]])),
          (in1,v1,x*y.nz[[1,0]],c.diag(y.nz[[1,0]])),
          (in1,v1,x*y[[0,1],0],c.diag(y[[0,1],0])),
          (in1,v1,x*y[[1,0],0],c.diag(y[[1,0],0])),
          (in1,v1,inner_prod(x,x),(2*x).T),
          (in1,v1,inner_prod(x**2,x),(3*x**2).T),
          #(in1,v1,c.det(horzcat([x,DMatrix([1,2])])),DMatrix([-1,2])), not implemented
          (in1,v1,f1.call(in1)[1],y),
          (in1,v1,f1.call([x**2,y])[1],y*2*vertcat([x.T,x.T])),
          (in1,v1,f2.call(in1)[0],DMatrix.zeros(0,2)),
          (in1,v1,f2.call([x**2,y])[0],DMatrix.zeros(0,2)),
          (in1,v1,f3.call(in1)[0],DMatrix.zeros(0,2)),
          (in1,v1,f3.call([x**2,y])[0],DMatrix.zeros(0,2)),
          (in1,v1,f4.call(in1)[0],DMatrix.zeros(0,2)),
          (in1,v1,f4.call([x**2,y])[0],DMatrix.zeros(0,2)),
          #(in1,v1,f1.call([x**2,[]])[1],DMatrix.zeros(2,2)),
          #(in1,v1,f1.call([[],y])[1],DMatrix.zeros(2,2)),
          (in1,v1,vertcat([x,DMatrix.sparse(0,1)]),DMatrix.eye(2)),
          (in1,v1,(x**2).setSparse(sparse(DMatrix([0,1])).sparsity()),blockcat([[MX.sparse(1,1),MX.sparse(1,1)],[MX.sparse(1,1),2*x[1]]])),
          (in1,v1,c.inner_prod(x,y[:,0]),y[:,0].T),
          (in1,v1,x.nz[IMatrix([[1,0]])]*y.nz[IMatrix([[0,2]])],blockcat([[MX.sparse(1,1),y.nz[0]],[y.nz[2],MX.sparse(1,1)]])),
          (in1,v1,x.nz[c.diag([1,0])]*y.nz[c.diag([0,2])],blockcat([[MX.sparse(1,1),y.nz[0]],[MX.sparse(1,1),MX.sparse(1,1)],[MX.sparse(1,1),MX.sparse(1,1)],[y.nz[2],MX.sparse(1,1)]])),
     ]:
      print out
      fun = MXFunction(inputs,[out,jac])
      fun.init()
      
      funsx = fun.expand()
      funsx.init()
      
      for i,v in enumerate(values):
        fun.setInput(v,i)
        funsx.setInput(v,i)
        
      fun.evaluate()
      funsx.evaluate()
      self.checkarray(fun.getOutput(0),funsx.getOutput(0))
      self.checkarray(fun.getOutput(1),funsx.getOutput(1))
      
      J_ = fun.getOutput(1)
      
      def vec(l):
        ret = []
        for i in l:
          ret.extend(i)
        return ret

      storage2 = {}
      storage = {}
      
      vf_mx = None
              
      for f in [fun,fun.expand()]:
        f.init()
        d = f.derivative(ndir,ndir)
        d.init()
        
        num_in = f.getNumInputs()
        num_out = f.getNumOutputs()

        """# Fwd and Adjoint AD
        for i,v in enumerate(values):
          f.setInput(v,i)
          d.setInput(v,i)
        
        for d in range(ndir):
          f.setInput(DMatrix(inputs[0].sparsity(),random.random(inputs[0].size())),num_in+d*num_in + d)
          f.setAdjSeed(DMatrix(out.sparsity(),random.random(out.size())),num_in+d*num_in + 0)
          f.setFwdSeed(0,1,d)
          f.setAdjSeed(0,1,d)
          
        f.evaluate()
        for d in range(ndir):
          seed = array(f.getFwdSeed(0,d)).ravel()
          sens = array(f.getFwdSens(0,d)).ravel()
          self.checkarray(sens,mul(J_,seed),"Fwd %d %s" % (d,str(type(f))))

          seed = array(f.getAdjSeed(0,d)).ravel()
          sens = array(f.getAdjSens(0,d)).ravel()
          self.checkarray(sens,mul(J_.T,seed),"Adj %d" %d)
        """
        
        # evalThings
        for sym, Function in [(MX.sym,MXFunction),(SX.sym,SXFunction)]:
          if isinstance(f, MXFunction) and Function is SXFunction: continue
          if isinstance(f, SXFunction) and Function is MXFunction: continue
          
          

          # dense
          for spmod,spmod2 in itertools.product(spmods,repeat=2):
            fseeds = [[sym("f",spmod(f.getInput(i)).sparsity()) for i in range(f.getNumInputs())]  for d in range(ndir)]
            aseeds = [[sym("a",spmod2(f.getOutput(i)).sparsity())  for i in range(f.getNumOutputs())] for d in range(ndir)]
            inputss = [sym("i",f.input(i).sparsity()) for i in range(f.getNumInputs())]
        
            with internalAPI():
              res,fwdsens,adjsens = f.callDerivative(inputss,fseeds,aseeds,True)
            
            fseed = [DMatrix(fseeds[d][0].sparsity(),random.random(fseeds[d][0].size())) for d in range(ndir) ]
            aseed = [DMatrix(aseeds[d][0].sparsity(),random.random(aseeds[d][0].size())) for d in range(ndir) ]
            vf = Function(inputss+vec([fseeds[i]+aseeds[i] for i in range(ndir)]),list(res) + vec([list(fwdsens[i])+list(adjsens[i]) for i in range(ndir)]))
            
            vf.init()
            
            for i,v in enumerate(values):
              vf.setInput(v,i)
            offset = len(inputss)
              
            for d in range(ndir):
              vf.setInput(fseed[d],offset+0)
              for i in range(len(values)-1):
                vf.setInput(0,offset+i+1)
                
              offset += len(inputss)

              vf.setInput(aseed[d],offset+0)
              vf.setInput(0,offset+1)
              offset+=2
              
            assert(offset==vf.getNumInputs())
            
            vf.evaluate()
              
            offset = len(res)
            for d in range(ndir):
              seed = array(fseed[d]).ravel()
              sens = array(vf.getOutput(offset+0)).ravel()
              offset+=len(inputss)
              self.checkarray(sens,mul(J_,seed),"eval Fwd %d %s" % (d,str(type(f))+str(sym)))

              seed = array(aseed[d]).ravel()
              sens = array(vf.getOutput(offset+0)).ravel()
              offset+=len(inputss)
              
              self.checkarray(sens,mul(J_.T,seed),"eval Adj %d %s" % (d,str([vf.getOutput(i) for i in range(vf.getNumOutputs())])))
          
          
            assert(offset==vf.getNumOutputs())
          
            # Complete random seeding
            random.seed(1)
            for i in range(vf.getNumInputs()):
              vf.setInput(DMatrix(vf.input(i).sparsity(),random.random(vf.input(i).size())),i)
            
            vf.evaluate()
            storagekey = (spmod,spmod2)
            if not(storagekey in storage):
              storage[storagekey] = []
            storage[storagekey].append([vf.getOutput(i) for i in range(vf.getNumOutputs())])
            
            # Added to make sure that the same seeds are used for SX and MX
            if Function is MXFunction:
              vf_mx = vf

          # Second order sensitivities
          for sym2, Function2 in [(MX.sym,MXFunction),(SX.sym,SXFunction)]:
          
            if isinstance(vf, MXFunction) and Function2 is SXFunction: continue
            if isinstance(vf, SXFunction) and Function2 is MXFunction: continue
            

            for spmod_2,spmod2_2 in itertools.product(spmods,repeat=2):
              fseeds2 = [[sym2("f",vf_mx.input(i).sparsity()) for i in range(vf.getNumInputs())] for d in range(ndir)]
              aseeds2 = [[sym2("a",vf_mx.output(i).sparsity())  for i in range(vf.getNumOutputs()) ] for d in range(ndir)]
              inputss2 = [sym2("i",vf_mx.input(i).sparsity()) for i in range(vf.getNumInputs())]
           
              with internalAPI():
                res2,fwdsens2,adjsens2 = vf.callDerivative(inputss2,fseeds2,aseeds2,True)

              vf2 = Function2(inputss2+vec([fseeds2[i]+aseeds2[i] for i in range(ndir)]),list(res2) + vec([list(fwdsens2[i])+list(adjsens2[i]) for i in range(ndir)]))
              vf2.init()
                
              random.seed(1)
              for i in range(vf2.getNumInputs()):
                vf2.setInput(DMatrix(vf2.input(i).sparsity(),random.random(vf2.input(i).size())),i)
              
              vf2.evaluate()
              storagekey = (spmod,spmod2)
              if not(storagekey in storage2):
                storage2[storagekey] = []
              storage2[storagekey].append([vf2.getOutput(i) for i in range(vf2.getNumOutputs())])

      # Remainder of eval testing
      for store,order in [(storage,"first-order"),(storage2,"second-order")]:
        for stk,st in store.items():
          for i in range(len(st)-1):
            for k,(a,b) in enumerate(zip(st[0],st[i+1])):
              if b.numel()==0 and sparse(a).size()==0: continue
              if a.numel()==0 and sparse(b).size()==0: continue
              self.checkarray(sparse(a),sparse(b),("%s, output(%d)" % (order,k))+str(vf2.getInput(0)))
              
      for f in [fun.expand(),fun]:
        #  jacobian()
        for mode in ["forward","reverse"]:
          f.setOption("ad_mode",mode)
          f.init()
          Jf=f.jacobian(0,0)
          Jf.init()
          for i,v in enumerate(values):
            Jf.setInput(v,i)
          Jf.evaluate()
          self.checkarray(Jf.getOutput(),J_)
          self.checkarray(DMatrix(Jf.output().sparsity(),1),DMatrix(J_.sparsity(),1),str(out)+str(mode))
          self.checkarray(DMatrix(f.jacSparsity(),1),DMatrix(J_.sparsity(),1))
                
      # Scalarized
      if out.isEmpty(): continue
      s_i  = out.sparsity().row()[0]
      s_j  = out.sparsity().getCol()[0]
      s_k = s_i*out.size2()+s_j
      fun = MXFunction(inputs,[out[s_i,s_j],jac[s_k,:].T])
      fun.init()
        
      for i,v in enumerate(values):
        fun.setInput(v,i)
        
        
      fun.evaluate()
      J_ = fun.getOutput(1)
      
      for f in [fun,fun.expand()]:
        #  gradient()
        for mode in ["forward","reverse"]:
          f.setOption("ad_mode",mode)
          f.init()
          Gf=f.gradient(0,0)
          Gf.init()
          for i,v in enumerate(values):
            Gf.setInput(v,i)
          Gf.evaluate()
          self.checkarray(Gf.getOutput(),J_,failmessage=("mode: %s" % mode))
          #self.checkarray(DMatrix(Gf.output().sparsity(),1),DMatrix(J_.sparsity(),1),str(mode)+str(out)+str(type(fun)))

      H_ = None
      
      for f in [fun,fun.expand()]:
        #  hessian()
        for mode in ["forward","reverse"]:
          f.setOption("ad_mode",mode)
          f.init()
          Hf=f.hessian(0,0)
          Hf.init()
          for i,v in enumerate(values):
            Hf.setInput(v,i)
          Hf.evaluate()
          if H_ is None:
            H_ = Hf.getOutput()
          self.checkarray(Hf.getOutput(),H_,failmessage=("mode: %s" % mode))
예제 #53
0
def diag(inputobj):

    return ca.diag(inputobj)
예제 #54
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)

        
예제 #55
0
for j, e in enumerate(p_true):

    p_mean.append(pl.mean([k[j] for k in p_test]))
    p_std.append(pl.std([k[j] for k in p_test], ddof = 0))

lsqpe_test.compute_covariance_matrix()


# Generate report

print("\np_mean         = " + str(ca.DMatrix(p_mean)))
print("phat_last_exp  = " + str(ca.DMatrix(lsqpe_test.phat)))

print("\np_sd           = " + str(ca.DMatrix(p_std)))
print("sd_from_covmat = " + str(ca.diag(ca.sqrt(lsqpe_test.Covp))))
print("beta           = " + str(lsqpe_test.beta))

print("\ndelta_abs_sd   = " + str(ca.fabs(ca.DMatrix(p_std) - \
    ca.diag(ca.sqrt(lsqpe_test.Covp)))))
print("delta_rel_sd   = " + str(ca.fabs(ca.DMatrix(p_std) - \
    ca.diag(ca.sqrt(lsqpe_test.Covp))) / ca.DMatrix(p_std)))


fname = os.path.basename(__file__)[:-3] + ".rst"

report = open(fname, "w")
report.write( \
'''Concept test: covariance matrix computation
===========================================
    V = ca.vertcat([P, EPS_U, X0])

    x_end = X0
    obj = [x_end - ydata[0,:].T]

    for k in range(int(N)):

        x_end = rk4(x0 = x_end, p = ca.vertcat([udata[k], EPS_U[k], P]))["xf"]
        obj.append(x_end - ydata_noise[k+1, :].T)

    r = ca.vertcat([ca.vertcat(obj), EPS_U])

    wv = (1.0 / sigma_y**2) * pl.ones(ydata.shape)
    weps_u = (1.0 / sigma_u**2) * pl.ones(udata.shape)

    Sigma_y_inv = ca.diag(ca.vec(wv))
    Sigma_u_inv = ca.diag(weps_u)

    Sigma = ca.blockcat(Sigma_y_inv, ca.DMatrix(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1]))),\
        ca.DMatrix(pl.zeros((Sigma_u_inv.shape[0], Sigma_y_inv.shape[1]))), Sigma_u_inv)

    nlp = ca.MXFunction("nlp", ca.nlpIn(x = V), ca.nlpOut(f = ca.mul([r.T, Sigma, r])))

    nlpsolver = ca.NlpSolver("nlpsolver", "ipopt", nlp)

    V0 = ca.vertcat([

            pl.ones(3), \
            pl.zeros(N), \
            ydata_noise[0,:].T
예제 #57
0
p_std = []

for j, e in enumerate(p_true):

    p_mean.append(pl.mean([k[j] for k in p_test]))
    p_std.append(pl.std([k[j] for k in p_test], ddof=0))

pe_test.compute_covariance_matrix()

# Generate report

print("\np_mean         = " + str(ca.DM(p_mean)))
print("phat_last_exp  = " + str(ca.DM(pe_test.estimated_parameters)))

print("\np_sd           = " + str(ca.DM(p_std)))
print("sd_from_covmat = " + str(ca.diag(ca.sqrt(pe_test.covariance_matrix))))
print("beta           = " + str(pe_test.beta))

print("\ndelta_abs_sd   = " + str(ca.fabs(ca.DM(p_std) - \
    ca.diag(ca.sqrt(pe_test.covariance_matrix)))))
print("delta_rel_sd   = " + str(ca.fabs(ca.DM(p_std) - \
    ca.diag(ca.sqrt(pe_test.covariance_matrix))) / ca.DM(p_std)))

fname = os.path.basename(__file__)[:-3] + ".rst"

report = open(fname, "w")
report.write( \
'''Concept test: covariance matrix computation
===========================================

Simulate system. Then: add gaussian noise N~(0, sigma^2), estimate,
예제 #58
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