Exemple #1
0
    def create_adjoint_states(self):
        lamb = SX.sym(self.name + '_lamb', self.model.n_x)
        nu = SX.sym(self.name + '_nu', self.model.n_y)
        self.eta = SX.sym(self.name + '_eta', self.n_h_final)

        self.H = self.L + dot(lamb, self.model.ode) + dot(nu, self.model.alg)

        l_dot = -gradient(self.H, self.model.x)
        alg_eq = gradient(self.H, self.model.y)

        self.include_state(lamb, l_dot, suppress=True)
        self.model.has_adjoint_variables = True

        self.include_algebraic(nu, alg_eq)

        self.h_final = vertcat(
            self.h_final,
            self.model.lamb_sym - gradient(self.V, self.model.x_sys_sym) -
            mtimes(jacobian(self.h_final, self.model.x_sys_sym).T, self.eta))
Exemple #2
0
def build_moment_functions(n, mgf):
    """ Builds a dictionary of functions

    Args:
        n ([type]): [description]
        mgf ([type]): [description]

    Returns:
        [type]: [description]
    """
    moment_functions = dict()
    for i in range(n):
        moment_fun = casadi.gradient(moment_fun, t)
        moment_functions[i + 1] = casadi.Function('g' + str(i + 1), [t],
                                                  [moment_fun])
    return moment_functions
Exemple #3
0
    def nllh_casf(self, grad=True, hess=False):
        """
        Creates a casadi function computing the negative log likelihood (without const terms) of the data
        dependent on hyperparameters v.
        :param grad: Whether the function should compute the gradient, too
        :param hess: Whether the function should compute the hessian, too
        :return: A casadi function taking a value of v as input and returning the neg log likelihood, gradient,
                 and hessian is desired
        """
        vshape = np.atleast_2d(self.v).shape
        v = cas.MX.sym("v", vshape[0], vshape[1])
        phi_x = self.phi_cas(self.x, v)

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

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

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

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

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

        f = cas.Function("f_mu", [v], res)
        return f
    def _convert(self, symbol, t, y, y_dot, inputs):
        """ See :meth:`CasadiConverter.convert()`. """
        if isinstance(
            symbol,
            (
                pybamm.Scalar,
                pybamm.Array,
                pybamm.Time,
                pybamm.InputParameter,
                pybamm.ExternalVariable,
            ),
        ):
            return casadi.MX(symbol.evaluate(t, y, y_dot, inputs))

        elif isinstance(symbol, pybamm.StateVector):
            if y is None:
                raise ValueError("Must provide a 'y' for converting state vectors")
            return casadi.vertcat(*[y[y_slice] for y_slice in symbol.y_slices])

        elif isinstance(symbol, pybamm.StateVectorDot):
            if y_dot is None:
                raise ValueError("Must provide a 'y_dot' for converting state vectors")
            return casadi.vertcat(*[y_dot[y_slice] for y_slice in symbol.y_slices])

        elif isinstance(symbol, pybamm.BinaryOperator):
            left, right = symbol.children
            # process children
            converted_left = self.convert(left, t, y, y_dot, inputs)
            converted_right = self.convert(right, t, y, y_dot, inputs)

            if isinstance(symbol, pybamm.Minimum):
                return casadi.fmin(converted_left, converted_right)
            if isinstance(symbol, pybamm.Maximum):
                return casadi.fmax(converted_left, converted_right)

            # _binary_evaluate defined in derived classes for specific rules
            return symbol._binary_evaluate(converted_left, converted_right)

        elif isinstance(symbol, pybamm.UnaryOperator):
            converted_child = self.convert(symbol.child, t, y, y_dot, inputs)
            if isinstance(symbol, pybamm.AbsoluteValue):
                return casadi.fabs(converted_child)
            return symbol._unary_evaluate(converted_child)

        elif isinstance(symbol, pybamm.Function):
            converted_children = [
                self.convert(child, t, y, y_dot, inputs) for child in symbol.children
            ]
            # Special functions
            if symbol.function == np.min:
                return casadi.mmin(*converted_children)
            elif symbol.function == np.max:
                return casadi.mmax(*converted_children)
            elif symbol.function == np.abs:
                return casadi.fabs(*converted_children)
            elif symbol.function == np.sqrt:
                return casadi.sqrt(*converted_children)
            elif symbol.function == np.sin:
                return casadi.sin(*converted_children)
            elif symbol.function == np.arcsinh:
                return casadi.arcsinh(*converted_children)
            elif symbol.function == np.arccosh:
                return casadi.arccosh(*converted_children)
            elif symbol.function == np.tanh:
                return casadi.tanh(*converted_children)
            elif symbol.function == np.cosh:
                return casadi.cosh(*converted_children)
            elif symbol.function == np.sinh:
                return casadi.sinh(*converted_children)
            elif symbol.function == np.cos:
                return casadi.cos(*converted_children)
            elif symbol.function == np.exp:
                return casadi.exp(*converted_children)
            elif symbol.function == np.log:
                return casadi.log(*converted_children)
            elif symbol.function == np.sign:
                return casadi.sign(*converted_children)
            elif isinstance(symbol.function, (PchipInterpolator, CubicSpline)):
                return casadi.interpolant("LUT", "bspline", [symbol.x], symbol.y)(
                    *converted_children
                )
            elif symbol.function.__name__.startswith("elementwise_grad_of_"):
                differentiating_child_idx = int(symbol.function.__name__[-1])
                # Create dummy symbolic variables in order to differentiate using CasADi
                dummy_vars = [
                    casadi.MX.sym("y_" + str(i)) for i in range(len(converted_children))
                ]
                func_diff = casadi.gradient(
                    symbol.differentiated_function(*dummy_vars),
                    dummy_vars[differentiating_child_idx],
                )
                # Create function and evaluate it using the children
                casadi_func_diff = casadi.Function("func_diff", dummy_vars, [func_diff])
                return casadi_func_diff(*converted_children)
            # Other functions
            else:
                return symbol._function_evaluate(converted_children)
        elif isinstance(symbol, pybamm.Concatenation):
            converted_children = [
                self.convert(child, t, y, y_dot, inputs) for child in symbol.children
            ]
            if isinstance(symbol, (pybamm.NumpyConcatenation, pybamm.SparseStack)):
                return casadi.vertcat(*converted_children)
            # DomainConcatenation specifies a particular ordering for the concatenation,
            # which we must follow
            elif isinstance(symbol, pybamm.DomainConcatenation):
                slice_starts = []
                all_child_vectors = []
                for i in range(symbol.secondary_dimensions_npts):
                    child_vectors = []
                    for child_var, slices in zip(
                        converted_children, symbol._children_slices
                    ):
                        for child_dom, child_slice in slices.items():
                            slice_starts.append(symbol._slices[child_dom][i].start)
                            child_vectors.append(
                                child_var[child_slice[i].start : child_slice[i].stop]
                            )
                    all_child_vectors.extend(
                        [v for _, v in sorted(zip(slice_starts, child_vectors))]
                    )
                return casadi.vertcat(*all_child_vectors)

        else:
            raise TypeError(
                """
                Cannot convert symbol of type '{}' to CasADi. Symbols must all be
                'linear algebra' at this stage.
                """.format(
                    type(symbol)
                )
            )
Exemple #5
0
# 适用于矩阵或稀疏模式
# y = ca.SX.sym('y',10,1)
# print(y.shape)

# .size1() => 行数
# .size2() => 列数
# .numel() => 元素个数 == .size1() * .size2()
# .sparisity() => 稀疏模式
'''
    Part 7: 微分计算
'''
# 微分计算可以从前向和反向分别计算,分别对应得到jacobian vector和jacobian-transposed vector

#1. jacobian
A = ca.SX.sym('A', 3, 2)
x = ca.SX.sym('x', 2)
print('A:', A, ' x: ', x, 'Ax: ', ca.mtimes(A, x))
print('J:', ca.jacobian(ca.mtimes(A, x), x))

print(ca.dot(A, A))
print(ca.gradient(ca.dot(A, A), A))

[H, g] = ca.hessian(ca.dot(x, x), x)  #hessian()会同时返回梯度和hessian矩阵
print('H:', H)
print('g:', g)

v = ca.SX.sym('v', 2)
f = ca.mtimes(A, x)

print(ca.jtimes(f, x, v))  # jtimes = jacobian function * vector
Exemple #6
0
            con.T - 157.29) @ R @ (con - 157.29)
        g[(k + 1) * 3:(k + 2) * 3] = Z[:, k + 1] - (f(st, con) * T + Z[:, k])
    st = Z[:, N_pred]
    print(st)
    obj = obj + (st - P[3:6]).T @ P_f @ (st - P[3:6])
    # create a dense matrix of state constraints
    #g = SX.zeros(3 * (N_pred + 1), 1)
    for i in range(Lambda.size(2)):
        L = L + Lambda[:, i].T @ g[3 * i:3 * (i + 1)]
    L = L + obj
    print(L)
    #hinkreigen phi und Solution S
    phi = SX.sym('phi')
    S = SX.sym('S')

    phi = gradient(L, Lambda[:, 0])
    S = Lambda[:, 0]
    print("phi", phi)

    for i in range(N_pred):
        phi = vertcat(phi, gradient(L, Z[:, i]))
        phi = vertcat(phi, gradient(L, V[:, i]))
        phi = vertcat(phi, gradient(L, Lambda[:, i + 1]))
        S = vertcat(S, Z[:, i])
        S = vertcat(S, V[:, i])
        S = vertcat(S, Lambda[:, i + 1])

    phi = vertcat(phi, gradient(L, Z[:, N_pred]))
    S = vertcat(S, Z[:, N_pred])
    phi_func = Function("phi_func", [P, S], [phi])
    print(phi_func)
Exemple #7
0
    def makeSolver(self):
        # make sure all bounds are set
        (xuMissing,pMissing) = self._boundMap.getMissing()
        msg = []
        for name in xuMissing:
            msg.append("you forgot to set a bound on \""+name+"\" at timesteps: "+str(xuMissing[name]))
        for name in pMissing:
            msg.append("you forgot to set a bound on \""+name+"\"")
        if len(msg)>0:
            raise ValueError('\n'.join(msg))

        # constraints:
        constraints = self._constraints._g
        constraintLbgs = self._constraints._glb
        constraintUbgs = self._constraints._gub

        g = [self._setupDynamicsConstraints()]
        g = []
        h = []
        hlbs = []
        hubs = []
        for k in range(len(constraints)):
            lb = constraintLbgs[k]
            ub = constraintUbgs[k]
            if all(lb==ub):
                g.append(constraints[k]-lb) # constrain to be zero
            else:
                h.append(constraints[k])
                hlbs.append(lb)
                hubs.append(ub)
        g = C.veccat(g)

        h = C.veccat(h)
        hlbs = C.veccat(hlbs)
        hubs = C.veccat(hubs)

        # design vars
        V = self._dvMap.vectorize()

        # gradient of arbitraryObj
        if hasattr(self,'_obj'):
            arbitraryObj = self._obj
        else:
            arbitraryObj = 0
        gradF = C.gradient(arbitraryObj,V)
        
        # hessian of lagrangian:
        J = 0
        for gnf in self._gaussNewtonObjF:
            J += C.jacobian(gnf,V)
        hessL = C.mul(J.T,J) + C.jacobian(gradF,V)
        
        # equality constraint jacobian
        jacobG = C.jacobian(g,V)

        # inequality constraint jacobian
        jacobH = C.jacobian(h,V)

        # function which generates everything needed
        masterFun = C.MXFunction([V],[hessL, gradF, g, jacobG, h, jacobH])
        masterFun.init()

        class JorisError(Exception): pass
        raise JorisError('JORIS, please read the following comment')
Exemple #8
0
    def makeSolver(self,endTime,traj=None):
        # make sure all bounds are set
        (xMissing,pMissing) = self._boundMap.getMissing()
        msg = []
        for name in xMissing:
            msg.append("you forgot to set a bound on \""+name+"\" at timesteps: "+str(xMissing[name]))
        for name in pMissing:
            msg.append("you forgot to set a bound on \""+name+"\"")
        if len(msg)>0:
            raise ValueError('\n'.join(msg))

        # constraints:
        g   = self._constraints.getG()
        glb = self._constraints.getLb()
        gub = self._constraints.getUb()

        gDyn = self._setupDynamicsConstraints(endTime,traj)
        gDynLb = gDynUb = [C.DMatrix.zeros(gg.shape) for gg in gDyn]
        
        g = C.veccat([g]+gDyn)
        glb = C.veccat([glb]+gDynLb)
        gub = C.veccat([gub]+gDynUb)

        self.glb = glb
        self.gub = gub

        # design vars
        V = self._dvMap.vectorize()

        # gradient of arbitraryObj
        if hasattr(self,'_obj'):
            arbitraryObj = self._obj
        else:
            arbitraryObj = 0
        gradF = C.gradient(arbitraryObj,V)
        
        # hessian of lagrangian:
        Js = [C.jacobian(gnf,V) for gnf in self._gaussNewtonObjF]
        gradFgns = [C.mul(J.T,F) for (F,J) in zip(self._gaussNewtonObjF, Js)]
        gaussNewtonHess = sum([C.mul(J.T,J) for J in Js])
        hessL = gaussNewtonHess + C.jacobian(gradF,V)
        
        gradF += sum(gradFgns)
        
        # equality/inequality constraint jacobian
        gfcn = C.MXFunction([V,self._U],[g])
        gfcn.init()
        jacobG = gfcn.jacobian(0,0)
        jacobG.init()

        # function which generates everything needed
        f = sum([f_*f_ for f_ in self._gaussNewtonObjF])
        if hasattr(self,'_obj'):
            f += self._obj
        
        self.masterFun = C.MXFunction([V,self._U],[hessL, gradF, g, jacobG.call([V,self._U])[0], f])
        self.masterFun.init()

#        self.qp = C.CplexSolver(hessL.sparsity(),jacobG.output(0).sparsity())
        self.qp = C.NLPQPSolver(hessL.sparsity(),jacobG.output(0).sparsity())
        self.qp.setOption('nlp_solver',C.IpoptSolver)
        self.qp.setOption('nlp_solver_options',{'print_level':0,'print_time':False})
        self.qp.init()
Exemple #9
0
    delta_Q = SX([[5, 0, 0], [0, 5, 0], [0, 0, 5]])
    K = reshape(K, 1, 3)
    X = SX.sym('X', 3, 1)

    psi = f(X, -K @ (X - state_r) + u_r) - ((phi_c) @ (X - state_r) + state_r)

    V_inf = (X - state_r).T @ P_f @ (X - state_r)
    Dis = norm_2(-K @ (X - state_r) + u_r[0] - 255) / sqrt(K[0]**2 + K[1]**2 +
                                                           K[2]**2)
    print("dis", Dis)
    print("K", K, K.shape)
    obj = Dis
    OPT_variables = reshape(X, 3, 1)

    g = SX.zeros(2, 1)
    g[0] = K @ gradient(V_inf, X) / (norm_2(K) * norm_2(gradient(V_inf, X)))
    g[1] = -K @ (X - state_r) + u_r[0]
    nlp_prob = {'f': obj, 'x': OPT_variables, 'g': g}
    opts = {}
    opts['print_time'] = False
    opts['ipopt'] = {
        'max_iter': 1000,
        'print_level': 3,
        #'acceptable_tol': 1e-8,
        #'acceptable_obj_change_tol': 1e-6
    }
    # print(opts)
    solver = nlpsol("solver", "ipopt", nlp_prob, opts)
    nl = {}
    nl['lbx'] = [0 for i in range(3)]
    nl['ubx'] = [0 for i in range(3)]
Exemple #10
0

def build_moment_functions(n, mgf):
    """ Builds a dictionary of functions

    Args:
        n ([type]): [description]
        mgf ([type]): [description]

    Returns:
        [type]: [description]
    """
    moment_functions = dict()
    for i in range(n):
        moment_fun = casadi.gradient(moment_fun, t)
        moment_functions[i + 1] = casadi.Function('g' + str(i + 1), [t],
                                                  [moment_fun])
    return moment_functions


t = casadi.MX.sym('t')
mu = casadi.MX.sym('mu')
sigmasq = casadi.MX.sym('sigmasq')
a = casadi.MX.sym('a')
b = casadi.MX.sym('b')
foo = truncated_normal_mgf(t, mu, sigmasq, a, b)

for i in range(8):
    foo = casadi.gradient(foo, t)
    fun = casadi.Function('g' + str(i + 1), [t, mu, sigmasq, a, b], [foo])
Exemple #11
0
def qp_solve(prob, obj, p_init, x_init, y_init, lam_opt, mu_opt, case):
    """
    QP solver for path-following algorithm
    inputs: prob - problem description
            obj - problem equations
            p_init - initial parameter
            x_init - initial primal variable
            y_init - initial dual variable
            lam_opt - Lagrange multipliers of equality and active constraints
            mu_opt - Lagrange multipliers of inequality constraints
    outputs: y - solution primal variable
            qp_val - objective function value
            qp_exit - return status of QP solver
            deriv - derivatives of the problem
            k_zero_tilde - active set index
            k_plus_tilde - inactive set index
            grad - gradient of objective function
    """
    print 'Current point x:', x_init
    #Importing problem to be solved
    nx, np, neq, niq, name = prob()
    x, p, f, f_fun, con, conf, ubx, lbx, ubg, lbg = obj(
        x_init, y_init, p_init, neq, niq, nx, np)

    #Deteriming constraint types
    eq_con_ind = array([])  #indices of equality constraints
    iq_con_ind = array([])  #indices of inequality constraints
    eq_con = array([])  #equality constraints
    iq_con = array([])  #inequality constraints

    for i in range(0, len(lbg[0])):
        if lbg[0, i] == 0:
            eq_con = vertcat(eq_con, con[i])
            eq_con_ind = append(eq_con_ind, i)
        elif lbg[0, i] < 0:
            iq_con = vertcat(iq_con, con[i])
            iq_con_ind = append(iq_con_ind, i)
#    print 'Equality Constraint:', eq_con
#    print 'Inequality Constraint:', iq_con

#    if case == 'pure-predictor':

#       return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt
    if case == 'predictor-corrector':
        #Evaluating constraints at current iteration point
        con_vals = conf(x_init, p_init)
        #Determining which inequality constraints are active
        k_plus_tilde = array([])  #active constraint
        k_zero_tilde = array([])  #inactive constraint
        tol = 10e-5  #tolerance
        for i in range(0, len(iq_con_ind)):
            if ubg[0, i] - tol <= con_vals[i] and con_vals[i] <= ubg[0,
                                                                     i] + tol:
                k_plus_tilde = append(k_plus_tilde, i)
            else:
                k_zero_tilde = append(k_zero_tilde, i)
#        print 'Active constraints:', k_plus_tilde
#        print 'Inactive constraints:', k_zero_tilde
#        print 'Constraint values:', con_vals

        nk_pt = len(k_plus_tilde)  #number of active constraints
        nk_zt = len(k_zero_tilde)  #number of inactive constraints

        #Calculating Lagrangian
        lam = SX.sym('lam', neq)  #Lagrangian multiplier equality constraints
        mu = SX.sym('mu', niq)  #Lagrangian multiplier inequality constraints
        lag_f = f + mtimes(lam.T, eq_con) + mtimes(
            mu.T, iq_con)  #Lagrangian equation

        #Calculating derivatives
        g = gradient(f, x)  #Derivative of objective function
        g_fun = Function('g_fun', [x, p], [gradient(f, x)])
        H = 2 * jacobian(gradient(lag_f, x),
                         x)  #Second derivative of the Lagrangian
        H_fun = Function('H_fun', [x, p, lam, mu],
                         [jacobian(jacobian(lag_f, x), x)])

        if len(eq_con_ind) > 0:
            deq = jacobian(eq_con, x)  #Derivative of equality constraints
        else:
            deq = array([])
        if len(iq_con_ind) > 0:
            diq = jacobian(iq_con, x)  #Derivative of inequality constraints
        else:
            diq = array([])
        #Creating constraint matrices
        nc = niq + neq  #Total number of constraints
        if (niq > 0) and (neq > 0):  #Equality and inequality constraints
            #this part needs to be tested
            if (nk_zt > 0):  #Inactive constraints exist
                A = SX.zeros((nc, nx))
                print deq
                A[0, :] = deq  #A matrix
                lba = -1e16 * SX.zeros((nc, 1))
                lba[0, :] = -eq_con  #lower bound of A
                uba = 1e16 * SX.zeros((nc, 1))
                uba[0, :] = -eq_con  #upper bound of A
                for j in range(0, nk_pt):  #adding active constraints
                    A[neq + j + 1, :] = diq[int(k_plus_tilde[j]), :]
                    lba[neq + j + 1] = -iq_con[int(k_plus_tilde[j])]
                    uba[neq + j + 1] = -iq_con[int(k_plus_tilde[i])]
                for i in range(0, nk_zt):  #adding inactive constraints
                    A[neq + nk_pt + i + 1, :] = diq[int(k_zero_tilde[i]), :]
                    uba[neq + nk_pt + i + 1] = -iq_con[int(k_zero_tilde[i])]
                    #inactive constraints don't have lower bounds
            else:  #Active constraints only
                A = vertcat(deq, diq)
                lba = vertcat(-eq_con, -iq_con)
                uba = vertcat(-eq_con, -iq_con)
        elif (niq > 0) and (neq == 0):  #Inquality constraints
            if (nk_zt > 0):  #Inactive constraints exist
                A = SX.zeros((nc, nx))
                lba = -1e16 * SX.ones((nc, 1))
                uba = 1e16 * SX.ones((nc, 1))
                for j in range(0, nk_pt):  #adding active constraints
                    A[j, :] = diq[int(k_plus_tilde[j]), :]
                    lba[j] = -iq_con[int(k_plus_tilde[j])]
                    uba[j] = -iq_con[int(k_plus_tilde[j])]
                for i in range(0, nk_zt):  #adding inactive constraints
                    A[nk_pt + i, :] = diq[int(k_zero_tilde[i]), :]
                    uba[nk_pt + i] = -iq_con[int(k_zero_tilde[i])]
                    #inactive constraints don't have lower bounds
            else:
                raw_input()
                A = vertcat(deq, diq)
                lba = -iq_con
                uba = -iq_con
        elif (niq == 0) and (neq > 0):  #Equality constriants
            A = deq
            lba = -eq_con
            uba = -eq_con
        A_fun = Function('A_fun', [x, p], [A])
        lba_fun = Function('lba_fun', [x, p], [lba])
        uba_fun = Function('uba_fun', [x, p], [uba])
        #Checking that matrices are correct sizes and types
        if (H.size1() != nx) or (H.size2() != nx) or (H.is_dense() == 'False'):
            #H matrix should be a sparse (nxn) and symmetrical
            print(
                'WARNING: H matrix is not the correct dimensions or matrix type'
            )
        if (g.size1() != nx) or (g.size2() != 1) or g.is_dense() == 'True':
            #g matrix should be a dense (nx1)
            print(
                'WARNING: g matrix is not the correct dimensions or matrix type'
            )
        if (A.size1() !=
            (neq + niq)) or (A.size2() != nx) or (A.is_dense() == 'False'):
            #A should be a sparse (nc x n)
            print(
                'WARNING: A matrix is not the correct dimensions or matrix type'
            )
        if lba.size1() != (neq + niq) or (lba.size2() !=
                                          1) or lba.is_dense() == 'False':
            print(
                'WARNING: lba matrix is not the correct dimensions or matrix type'
            )
        if uba.size1() != (neq + niq) or (uba.size2() !=
                                          1) or uba.is_dense() == 'False':
            print(
                'WARNING: uba matrix is not the correct dimensions or matrix type'
            )

        #Evaluating QP matrices at optimal points
        H_opt = H_fun(x_init, p_init, lam_opt, mu_opt)
        g_opt = g_fun(x_init, p_init)
        A_opt = A_fun(x_init, p_init)
        lba_opt = lba_fun(x_init, p_init)
        uba_opt = uba_fun(x_init, p_init)

        #        print 'Lower bounds', lba_opt
        #        print 'Upper bounds', uba_opt
        #        print 'Bound matrix', A_opt

        #Defining QP structure
        qp = {}
        qp['h'] = H_opt.sparsity()
        qp['a'] = A_opt.sparsity()
        optimize = conic('optimize', 'qpoases', qp)
        optimal = optimize(h=H_opt,
                           g=g_opt,
                           a=A_opt,
                           lba=lba_opt,
                           uba=uba_opt,
                           x0=x_init)
        x_qpopt = optimal['x']
        if x_qpopt.shape == x_init.shape:
            qp_exit = 'optimal'
        else:
            qp_exit = ''
        lag_qpopt = optimal['lam_a']

        #Determing Lagrangian multipliers (lambda and mu)
        lam_qpopt = zeros(
            (nk_pt, 1))  #Lagrange multiplier of active constraints
        mu_qpopt = zeros(
            (nk_zt, 1))  #Lagrange multiplier of inactive constraints
        if nk_pt > 0:
            for j in range(0, len(k_plus_tilde)):
                lam_qpopt[j] = lag_qpopt[int(k_plus_tilde[j])]
        if nk_zt > 0:
            for k in range(0, len(k_zero_tilde)):
                print lag_qpopt[int(k_zero_tilde[k])]
        return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt
Exemple #12
0
    def makeSolver(self):
        # make sure all bounds are set
        (xuMissing, pMissing) = self._boundMap.getMissing()
        msg = []
        for name in xuMissing:
            msg.append("you forgot to set a bound on \"" + name +
                       "\" at timesteps: " + str(xuMissing[name]))
        for name in pMissing:
            msg.append("you forgot to set a bound on \"" + name + "\"")
        if len(msg) > 0:
            raise ValueError('\n'.join(msg))

        # constraints:
        constraints = self._constraints._g
        constraintLbgs = self._constraints._glb
        constraintUbgs = self._constraints._gub

        g = [self._setupDynamicsConstraints()]
        g = []
        h = []
        hlbs = []
        hubs = []
        for k in range(len(constraints)):
            lb = constraintLbgs[k]
            ub = constraintUbgs[k]
            if all(lb == ub):
                g.append(constraints[k] - lb)  # constrain to be zero
            else:
                h.append(constraints[k])
                hlbs.append(lb)
                hubs.append(ub)
        g = C.veccat(g)

        h = C.veccat(h)
        hlbs = C.veccat(hlbs)
        hubs = C.veccat(hubs)

        # design vars
        V = self._dvMap.vectorize()

        # gradient of arbitraryObj
        if hasattr(self, '_obj'):
            arbitraryObj = self._obj
        else:
            arbitraryObj = 0
        gradF = C.gradient(arbitraryObj, V)

        # hessian of lagrangian:
        J = 0
        for gnf in self._gaussNewtonObjF:
            J += C.jacobian(gnf, V)
        hessL = C.mul(J.T, J) + C.jacobian(gradF, V)

        # equality constraint jacobian
        jacobG = C.jacobian(g, V)

        # inequality constraint jacobian
        jacobH = C.jacobian(h, V)

        # function which generates everything needed
        masterFun = C.MXFunction([V], [hessL, gradF, g, jacobG, h, jacobH])
        masterFun.init()

        class JorisError(Exception):
            pass

        raise JorisError('JORIS, please read the following comment')
def solve_hanging_chain_qp(num_masses, use_contraints):
    m_i = 40.0 / num_masses
    D_i = 70.0 * num_masses
    g0 = 9.81
    zmin = 0.5  # ground

    x = []
    f = 0
    g = []

    lbx = []
    ubx = []
    lbg = []
    ubg = []

    y_start, z_start = -2, 1
    y_end, z_end = 2, 1

    # Loop over all chain elements
    y_prev = z_prev = None
    for i in range(0, num_masses):
        # Create variables for the (y_i, z_i) coordinates
        y_i = casadi.SX.sym('y_' + str(i))
        z_i = casadi.SX.sym('z_' + str(i))

        # Add to the list of variables
        x += [y_i, z_i]

        lbx += [-numpy.inf, zmin]
        ubx += [numpy.inf, numpy.inf]

        # Spring potential
        if i == 0:
            f += D_i / 2 * ((y_start - y_i)**2 + (z_start - z_i)**2)
        else:
            f += D_i / 2 * ((y_prev - y_i)**2 + (z_prev - z_i)**2)

        # Graviational potential
        f += g0 * m_i * z_i

        # Slanted ground constraints
        if use_contraints:
            g.append(z_i - 0.1 * y_i)
            lbg.append(0.5)
            ubg.append(numpy.inf)

        # Prepare for the next iteration
        y_prev = y_i
        z_prev = z_i

    f += D_i / 2 * ((y_i - y_end)**2 + (z_i - z_end)**2)

    num_variables = len(x)
    num_constraints = len(g)

    x = casadi.vertcat(*x)
    g = casadi.vertcat(*g)
    qp = {'x': x, 'f': f, 'g': g}
    solver = casadi.qpsol('solver', 'qpoases', qp)

    gradient_f = casadi.gradient(f, x)
    h = casadi.jacobian(gradient_f, x, {'symmetric': True})
    c = casadi.substitute(gradient_f, x, casadi.SX.zeros(x.sparsity()))
    a = casadi.jacobian(g, x)

    prob = casadi.Function("qp_eval", [x], [h, c, a])
    qp_items = prob(casadi.SX.sym('eval_args', x.shape))
    h_flat = numpy.asarray(casadi.DM(qp_items[0]))
    c_flat = numpy.asarray(casadi.DM(qp_items[1]))
    a_flat = numpy.asarray(casadi.DM(qp_items[2]))

    sol = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)

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

    return (num_variables, num_constraints, h_flat, c_flat, a_flat,
            numpy.asarray(lbx), numpy.asarray(ubx), numpy.asarray(lbg),
            numpy.asarray(ubg), x_opt, y_opt, f_opt)
Exemple #14
0
    def makeSolver(self, endTime, traj=None):
        # make sure all bounds are set
        (xMissing, pMissing) = self._boundMap.getMissing()
        msg = []
        for name in xMissing:
            msg.append("you forgot to set a bound on \"" + name +
                       "\" at timesteps: " + str(xMissing[name]))
        for name in pMissing:
            msg.append("you forgot to set a bound on \"" + name + "\"")
        if len(msg) > 0:
            raise ValueError('\n'.join(msg))

        # constraints:
        g = self._constraints.getG()
        glb = self._constraints.getLb()
        gub = self._constraints.getUb()

        gDyn = self._setupDynamicsConstraints(endTime, traj)
        gDynLb = gDynUb = [C.DMatrix.zeros(gg.shape) for gg in gDyn]

        g = C.veccat([g] + gDyn)
        glb = C.veccat([glb] + gDynLb)
        gub = C.veccat([gub] + gDynUb)

        self.glb = glb
        self.gub = gub

        # design vars
        V = self._dvMap.vectorize()

        # gradient of arbitraryObj
        if hasattr(self, '_obj'):
            arbitraryObj = self._obj
        else:
            arbitraryObj = 0
        gradF = C.gradient(arbitraryObj, V)

        # hessian of lagrangian:
        Js = [C.jacobian(gnf, V) for gnf in self._gaussNewtonObjF]
        gradFgns = [C.mul(J.T, F) for (F, J) in zip(self._gaussNewtonObjF, Js)]
        gaussNewtonHess = sum([C.mul(J.T, J) for J in Js])
        hessL = gaussNewtonHess + C.jacobian(gradF, V)

        gradF += sum(gradFgns)

        # equality/inequality constraint jacobian
        gfcn = C.MXFunction([V, self._U], [g])
        gfcn.init()
        jacobG = gfcn.jacobian(0, 0)
        jacobG.init()

        # function which generates everything needed
        f = sum([f_ * f_ for f_ in self._gaussNewtonObjF])
        if hasattr(self, '_obj'):
            f += self._obj

        self.masterFun = C.MXFunction(
            [V, self._U],
            [hessL, gradF, g, jacobG.call([V, self._U])[0], f])
        self.masterFun.init()

        #        self.qp = C.CplexSolver(hessL.sparsity(),jacobG.output(0).sparsity())
        self.qp = C.NLPQPSolver(hessL.sparsity(), jacobG.output(0).sparsity())
        self.qp.setOption('nlp_solver', C.IpoptSolver)
        self.qp.setOption('nlp_solver_options', {
            'print_level': 0,
            'print_time': False
        })
        self.qp.init()
Exemple #15
0
c = (Q + F) / 2

dC = jtimes(C, q, dq)
dQ = jtimes(Q, q, dq)
dc = jtimes(c, q, dq)

# Modeling with Lagrange mechanics
E_kin = 0.5 * dtau**2 * (m * l**2 / 12) + 0.5 * ddelta**2 * (
    M * H**2 / 12) + 0.5 * M * sumsqr(dC) + 0.5 * m * sumsqr(dc)
E_pot = M * g * C[1] + m * g * c[1]

Lag = E_kin - E_pot

E_tot = E_kin + E_pot

Lag_q = gradient(Lag, q)
Lag_dq = gradient(Lag, dq)

rhs = solve(jacobian(Lag_dq, dq),
            vertcat(0, R, T) + Lag_q - jtimes(Lag_dq, q, dq), "symbolicqr")

ocp.set_der(alpha, dalpha)
ocp.set_der(beta, dbeta)
ocp.set_der(gamma, dgamma)
ocp.set_der(dalpha, rhs[0])
ocp.set_der(dbeta, rhs[1])
ocp.set_der(dgamma, rhs[2])

ocp.set_initial(gamma, pi / 2)
ocp.set_initial(R, Rn)