예제 #1
0
def create_plan_fc(b0, N_sim):
    # Degrees of freedom for the optimizer
    V = cat.struct_symSX([
            (
                cat.entry('X',repeat=N_sim+1,struct=belief),
                cat.entry('U',repeat=N_sim,struct=control)
            )
        ])
    
    # Objective function
    m_bN = V['X',N_sim,'m',ca.veccat,['x_b','y_b']]
    m_cN = V['X',N_sim,'m',ca.veccat,['x_c','y_c']]
    dm_bc = m_bN - m_cN
    J = 1e2 * ca.mul(dm_bc.T, dm_bc) # m_cN -> m_bN
    J += 1e-1 * ca.trace(V['X',N_sim,'S']) # Sigma -> 0
    # Regularize controls
    J += 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
    
    # Multiple shooting constraints and running costs
    g = []
    for k in range(N_sim):
        # Multiple shooting
        [x_next] = BF([V['X',k], V['U',k]])
        g.append(x_next - V['X',k+1])
        
        # Penalize uncertainty
        J += 1e-1 * ca.trace(V['X',k,'S']) * dt
    g = ca.vertcat(g)
    
    # log-probability, doesn't work with full collocation
    #Sb = V['X',N_sim,'S',['x_b','y_b'], ['x_b','y_b']]
    #J += ca.mul([ dm_bc.T, ca.inv(Sb + 1e-8 * ca.SX.eye(2)), dm_bc ]) + \
    #     ca.log(ca.det(Sb + 1e-8 * ca.SX.eye(2)))
    
    # Formulate the NLP
    nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
    
    # Create solver
    opts = {}
    opts['linear_solver'] = 'ma97'
    #opts['hessian_approximation'] = 'limited-memory'
    solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
    
    # Define box constraints
    lbx = V(-ca.inf)
    ubx = V(ca.inf)
    
    # 0 <= v <= v_max
    lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
    # -w_max <= w <= w_max
    lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
    
    # m(t=0) = m0
    lbx['X',0,'m'] = ubx['X',0,'m'] = b0['m']
    # S(t=0) = S0
    lbx['X',0,'S'] = ubx['X',0,'S'] = b0['S']
    
    # Solve the NLP
    sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
    return V(sol['x'])
예제 #2
0
 def construct_parameters(self):
     entries = []
     for label, child in self.children.items():
         entries_child = []
         for name, par in child._parameters.items():
             entries_child.append(entry(name, shape=par.shape))
         entries.append(entry(label, struct=struct(entries_child)))
     self._par_struct = struct(entries)
     return struct_symMX(self._par_struct)
예제 #3
0
 def construct_parameters(self):
     entries = []
     for label, child in self.children.items():
         entries_child = []
         for name, par in child._parameters.items():
             entries_child.append(entry(name, shape=par.shape))
         entries.append(entry(label, struct=struct(entries_child)))
     self._par_struct = struct(entries)
     return struct_symMX(self._par_struct)
예제 #4
0
def setup_nlp_p_fix(V, model):

    # fixed system parameters
    p_fix = cas.struct_symSX([(
        cas.entry('ref', struct=V),  # tracking reference for cost function
        cas.entry('weights',
                  struct=model.variables)  # weights for cost function
    )])

    return p_fix
예제 #5
0
파일: planner.py 프로젝트: b4be1/easy_catch
    def create_belief_plan(cls, model, warm_start=False,
                           x0=0, lam_x0=0, lam_g0=0):
        # Degrees of freedom for the optimizer
        V = cat.struct_symSX([
            (
                cat.entry('X', repeat=model.n+1, struct=model.x),
                cat.entry('U', repeat=model.n, struct=model.u)
            )
        ])

        # Box constraints
        [lbx, ubx] = cls._create_box_constraints(model, V)

        # Non-linear constraints
        [g, lbg, ubg] = cls._create_belief_nonlinear_constraints(model, V)

        # Objective function
        J = cls._create_belief_objective_function(model, V)

        # Formulate non-linear problem
        nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J, g=g))
        op = {# Linear solver
              #'linear_solver':              'ma57',
              # Warm start
              # 'warm_start_init_point':      'yes',
              # Termination
              'max_iter':                   1500,
              'tol':                        1e-6,
              'constr_viol_tol':            1e-5,
              'compl_inf_tol':              1e-4,
              # Acceptable termination
              'acceptable_tol':             1e-3,
              'acceptable_iter':            5,
              'acceptable_obj_change_tol':  1e-2,
              # NLP
              # 'fixed_variable_treatment':   'make_constraint',
              # Quasi-Newton
              'hessian_approximation':      'limited-memory',
              'limited_memory_max_history': 5,
              'limited_memory_max_skipping': 1}

        if warm_start:
            op['warm_start_init_point'] = 'yes'
            op['fixed_variable_treatment'] = 'make_constraint'

        # Initialize solver
        solver = ca.NlpSolver('solver', 'ipopt', nlp, op)

        # Solve
        if warm_start:
            sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg,
                         lam_x0=lam_x0, lam_g0=lam_g0)
        else:
            sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)
        return V(sol['x']), sol['lam_x'], sol['lam_g']
예제 #6
0
def make_stage_constraint_struct(model):

    # make entry list to check if not empty
    entry_list = [cas.entry('collocation', shape =model.dynamics(model.variables, model.parameters).size())]
    if list(model.constraints.keys()):  # check if not empty
        entry_list.append(cas.entry('path_constraints', struct = model.constraints))

    # stage constraints structure -- necessary for interleaving
    stage_constraints = cas.struct_symSX(entry_list)

    return stage_constraints
예제 #7
0
def setup_nlp_p(V, model):

    cost = setup_nlp_cost()
    p_fix = setup_nlp_p_fix(V, model)

    P = cas.struct_symMX([
        cas.entry('p', struct=p_fix),
        cas.entry('cost', struct=cost),
        cas.entry('theta0', struct=model.parameters_dict['theta0'])
    ])

    return P
예제 #8
0
    def generate_parameterization_settings(self, options):

        [
            periodic, initial_conditions, param_initial_conditions,
            param_terminal_conditions, terminal_inequalities,
            integral_constraints
        ] = operation.get_operation_conditions(options)

        xi = cas.struct_symMX([(cas.entry('xi_0'), cas.entry('xi_f'))])
        xi_bounds = {}

        xi_bounds['xi_0'] = [0.0, 0.0]
        xi_bounds['xi_f'] = [0.0, 0.0]

        V_pickle_initial = None
        plot_dict_pickle_initial = None
        V_pickle_terminal = None
        plot_dict_pickle_terminal = None

        if param_initial_conditions:
            xi_bounds['xi_0'] = [0.0, 1.0]
            if options['trajectory']['type'] == 'compromised_landing':
                xi_0 = options['landing']['xi_0_initial']
                xi_bounds['xi_0'] = [xi_0, xi_0]
            V_pickle_initial, plot_dict_pickle_initial = self.__get_V_pickle(
                options, 'initial')

        if param_terminal_conditions:
            xi_bounds['xi_f'] = [0.0, 1.0]
            V_pickle_terminal, plot_dict_pickle_terminal = self.__get_V_pickle(
                options, 'terminal')

        if param_terminal_conditions and param_initial_conditions:
            for theta in struct_op.subkeys(V_pickle_initial, 'theta'):
                diff = V_pickle_terminal['theta',
                                         theta] - V_pickle_initial['theta',
                                                                   theta]
                if theta != 't_f':
                    if (float(diff) != 0.0):
                        raise ValueError(
                            'Parameters of initial and terminal trajectory are not identical.'
                        )

        xi_dict = {}
        xi_dict['V_pickle_initial'] = V_pickle_initial
        xi_dict['plot_dict_pickle_initial'] = plot_dict_pickle_initial
        xi_dict['V_pickle_terminal'] = V_pickle_terminal
        xi_dict['plot_dict_pickle_terminal'] = plot_dict_pickle_terminal
        xi_dict['xi'] = xi
        xi_dict['xi_bounds'] = xi_bounds
        self.__xi_dict = xi_dict

        return None
예제 #9
0
파일: admm.py 프로젝트: jgillis/omg-tools
def _create_struct_from_dict(dictionary):
    entries = []
    for key, data in dictionary.items():
        if isinstance(data, dict):
            stru = _create_struct_from_dict(data)
            entries.append(entry(str(key), struct=stru))
        else:
            if isinstance(data, list):
                sh = len(data)
            else:
                sh = data.shape
            entries.append(entry(key, shape=sh))
    return struct(entries)
예제 #10
0
def get_phase_fix_theta(variables_dict):

    entry_list = []
    for name in list(variables_dict['theta'].keys()):
        if name == 't_f':
            entry_list.append(cas.entry('t_f', shape=(2, 1)))
        else:
            entry_list.append(
                cas.entry(name, shape=variables_dict['theta'][name].shape))

    theta = cas.struct_symSX(entry_list)

    return theta
예제 #11
0
def _create_struct_from_dict(dictionary):
    entries = []
    for key, data in dictionary.items():
        if isinstance(data, dict):
            stru = _create_struct_from_dict(data)
            entries.append(entry(str(key), struct=stru))
        else:
            if isinstance(data, list):
                sh = len(data)
            else:
                sh = data.shape
            entries.append(entry(key, shape=sh))
    return struct(entries)
예제 #12
0
def create_simple_plan(x0, N_sim):
    # Degrees of freedom for the optimizer
    V = cat.struct_symSX([
            (
                cat.entry('X',repeat=N_sim+1,struct=state),
                cat.entry('U',repeat=N_sim,struct=control)
            )
        ])

    # Objective function
    x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
    x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
    dx_bc = x_bN - x_cN
    J = 1e1 * ca.mul(dx_bc.T, dx_bc) # x_cN -> x_bN
    
    # Regularize controls
    J += 1e-1 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang

    # Multiple shooting constraints and non-linear control constraints
    g = []
    for k in range(N_sim):
        # Multiple shooting
        [x_next] = F([V['X',k], V['U',k], dt])
        g.append(x_next - V['X',k+1])        
    g = ca.vertcat(g)

    # Formulate the NLP
    nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))

    opts = {}
    #opts['hessian_approximation'] = 'limited-memory'
    opts['linear_solver'] = 'ma57'
    # Create a solver
    solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
    
    # Define box constraints
    lbx = V(-ca.inf)
    ubx = V(ca.inf)
    
    # 0 <= v <= v_max
    lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
    # -w_max <= w <= w_max
    lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
    
    # m(t=0) = m0
    lbx['X',0] = ubx['X',0] = x0
    
    # Solve the NLP
    sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
    return V(sol['x'])
예제 #13
0
    def get_collocation_variables_struct(self, variables_dict, u_param):

        entry_list = [
            cas.entry('xd', struct=variables_dict['xd']),
            cas.entry('xa', struct=variables_dict['xa'])
        ]

        if 'xl' in list(variables_dict.keys()):
            entry_list += [cas.entry('xl', struct=variables_dict['xl'])]

        if u_param == 'poly':
            entry_list += [cas.entry('u', struct=variables_dict['u'])]

        return cas.struct_symMX(entry_list)
예제 #14
0
    def get_collocation_variables_struct(self, variables_dict):

        if 'xl' in list(variables_dict.keys()):
            coll_var = cas.struct_symSX([
                cas.entry('xd', struct=variables_dict['xd']),
                cas.entry('xa', struct=variables_dict['xa']),
                cas.entry('xl', struct=variables_dict['xl'])
            ])
        else:
            coll_var = cas.struct_symSX([
                cas.entry('xd', struct=variables_dict['xd']),
                cas.entry('xa', struct=variables_dict['xa']),
            ])

        return coll_var
예제 #15
0
def generate_variable_struct(variable_list):

    structs = {}
    for name in list(variable_list.keys()):
        structs[name] = cas.struct_symSX([
            cas.entry(variable_list[name][i][0],
                      shape=variable_list[name][i][1])
            for i in range(len(variable_list[name]))
        ])

    variable_struct = cas.struct_symSX([
        cas.entry(name, struct=structs[name])
        for name in list(variable_list.keys())
    ])

    return variable_struct, structs
예제 #16
0
파일: system.py 프로젝트: wuyou33/awebox
def generate_system_parameters(options):

    # extract parametric options
    parametric_options = options['params']

    parameters_dict = {}
    # generate nested casadi structure for system parameters
    parameters_dict['theta0'] = struct_op.generate_nested_dict_struct(parametric_options)
    parameters_dict['phi'] = generate_optimization_parameters()

    parameters = cas.struct_symSX([
        cas.entry('theta0', struct= parameters_dict['theta0']),
        cas.entry('phi', struct= parameters_dict['phi'])]
    )

    return parameters, parameters_dict
예제 #17
0
def get_component_cost_structure(component_costs):

    list_of_entries = []
    for name in list(component_costs.keys()):
        list_of_entries += [cas.entry(name)]

    component_cost_struct = cas.struct_symMX(list_of_entries)

    return component_cost_struct
예제 #18
0
파일: planner.py 프로젝트: b4be1/easy_catch
    def create_plan(cls, model, warm_start=False,
                    x0=0, lam_x0=0, lam_g0=0):
        # Degrees of freedom for the optimizer
        V = cat.struct_symSX([
            (
                cat.entry('X', repeat=model.n+1, struct=model.x),
                cat.entry('U', repeat=model.n, struct=model.u)
            )
        ])

        # Box constraints
        [lbx, ubx] = cls._create_box_constraints(model, V)

        # Force the catcher to always look forward
        # lbx['U', :, 'theta'] = ubx['U', :, 'theta'] = 0

        # Non-linear constraints
        [g, lbg, ubg] = cls._create_nonlinear_constraints(model, V)

        # Objective function
        J = cls._create_objective_function(model, V, warm_start)

        # Formulate non-linear problem
        nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J, g=g))
        op = {# Linear solver
              #'linear_solver':              'ma57',
              # Acceptable termination
              'acceptable_iter':            5}

        if warm_start:
            op['warm_start_init_point'] = 'yes'
            op['fixed_variable_treatment'] = 'make_constraint'

        # Initialize solver
        solver = ca.NlpSolver('solver', 'ipopt', nlp, op)

        # Solve
        if warm_start:
            sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg,
                         lam_x0=lam_x0, lam_g0=lam_g0)
        else:
            sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)
        return V(sol['x']), sol['lam_x'], sol['lam_g']
예제 #19
0
def gen(NX, NU, M):
    """gen Generates a casadi struct containing the symbolic optimization parameters
    x_cur (the initial state), x_ref (the reference states) and u_cur (the reference controls).
    x_cur is a <NX>x<1> vector, x_ref is a <NX>x<M+1> matrix, u_ref is a <NU>x<M> matrix

    :param NX: Number of reference state variables
    :param NU: Number of reference control variables
    :param M: Prediction horizon length

    :returns: A casadi struct_symSX object
    """

    params = struct_symSX([
        entry('x_cur', shape=NX),
        entry('x_ref', shape=(NX, M + 1)),
        entry('u_ref', shape=(NU, M))
    ])

    return params
예제 #20
0
    def get_structure(self, cstr_type):

        cstr_list = self.get_list(cstr_type)

        entry_list = []
        for cstr in cstr_list:
            joined_name = cstr.name
            local = cas.entry(joined_name, shape=cstr.expr.shape)
            entry_list.append(local)

        return cas.struct_symSX(entry_list)
예제 #21
0
def setup_integral_output_structure(nlp_numerics_options, integral_outputs):

    nk = nlp_numerics_options['n_k']

    entry_tuple = ()

    # interval outputs
    entry_tuple += (cas.entry('int_out',
                              repeat=[nk + 1],
                              struct=integral_outputs), )

    if nlp_numerics_options['discretization'] == 'direct_collocation':
        d = nlp_numerics_options['collocation']['d']
        entry_tuple += (cas.entry('coll_int_out',
                                  repeat=[nk, d],
                                  struct=integral_outputs), )

    Integral_outputs_struct = cas.struct_symMX([entry_tuple])

    return Integral_outputs_struct
예제 #22
0
def setup_output_structure(nlp_numerics_options, model_outputs, form_outputs):
    # create outputs
    # n_o_t_e: !!! outputs are defined at nodes where both state and algebraic variables
    # are defined. In the radau case, algebraic variables are not defined on the interval
    # nodes, note however that algebraic variables might be discontinuous so the same
    # holds for the outputs!!!
    nk = nlp_numerics_options['n_k']

    entry_tuple = ()
    if nlp_numerics_options['discretization'] == 'direct_collocation':

        # extract collocation parameters
        d = nlp_numerics_options['collocation']['d']
        scheme = nlp_numerics_options['collocation']['scheme']

        if scheme != 'radau':

            # define outputs on interval and collocation nodes
            entry_tuple += (
                cas.entry('outputs', repeat=[nk], struct=model_outputs),
                cas.entry('coll_outputs', repeat=[nk, d],
                          struct=model_outputs),
            )

        else:

            # define outputs on collocation nodes
            entry_tuple += (cas.entry('coll_outputs',
                                      repeat=[nk, d],
                                      struct=model_outputs), )

    elif nlp_numerics_options['discretization'] == 'multiple_shooting':

        # define outputs on interval nodes
        entry_tuple += (cas.entry('outputs', repeat=[nk],
                                  struct=model_outputs), )

    Outputs = cas.struct_symSX([entry_tuple] +
                               [cas.entry('final', struct=form_outputs)])

    return Outputs
예제 #23
0
def gen(NX, NU, M, stateBounds=None, controlBounds=None):
    """gen Generates a casadi struct containing the symbolic optimization variables required
    for direct multiple shooting. x is a <NX>x<M+1> matrix, u is a <NU>x<M> matrix.

    :param NX: Number of state variables
    :param NU: Number of control variables
    :param M: Prediction horizon length

    :returns: A casadi struct_symSX object
    """

    # decision (free) variables
    variables = struct_symSX(
        [entry('x', shape=(NX, M + 1)),
         entry('u', shape=(NU, M))])

    # symbolic bounds
    bx = ca.SX.sym('bx', NX)
    bu = ca.SX.sym('bu', NU)

    # bounds struct, must be identical to variables struct in dimensions and keys
    bounds = struct_SX([
        entry('x', expr=ca.repmat(bx, 1, M + 1)),
        entry('u', expr=ca.repmat(bu, 1, M))
    ])
    boundsFun = ca.Function('varBoundsFun', [bx, bu], [bounds.cat])

    if stateBounds is None:
        stateBounds = np.multiply(np.ones((2, NX)),
                                  np.array((-np.inf, np.inf), ndmin=2).T)

    if controlBounds is None:
        controlBounds = np.multiply(np.ones((2, NU)),
                                    np.array((-np.inf, np.inf), ndmin=2).T)

    lbw = boundsFun(stateBounds[0, :], controlBounds[0, :])
    ubw = boundsFun(stateBounds[1, :], controlBounds[1, :])

    return variables, lbw, ubw
예제 #24
0
파일: operation.py 프로젝트: wuyou33/awebox
def make_entry_list(eqs_dict, ineqs_dict):

    # make entry list for all non-empty dicts
    entry_list = []
    if eqs_dict:  # check if not empty

        # equality constraint struct
        eq_struct = cas.struct_symSX([
            cas.entry(name, shape=eqs_dict[name].size())
            for name in list(eqs_dict.keys())
        ])
        entry_list.append(cas.entry('equality', struct=eq_struct))

    if ineqs_dict:  # check if not empty

        # inequality constraint struct
        ineq_struct = cas.struct_symSX([
            cas.entry(name, shape=ineqs_dict[name].size())
            for name in list(ineqs_dict.keys())
        ])
        entry_list.append(cas.entry('inequality', struct=ineq_struct))

    return entry_list
예제 #25
0
def create_simple_plan(x0, F, dt, N_sim):
    # Degrees of freedom for the optimizer
    V = cat.struct_symSX([
            (
                cat.entry('X',repeat=N_sim+1,struct=state),
                cat.entry('U',repeat=N_sim,struct=control)
            )
        ])

    # Final position
    x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
    x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
    dx_bc = x_bN - x_cN
    
    # Final velocity
    v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
    v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
    dv_bc = v_bN - v_cN
    
    # Angle between gaze and ball velocity
    theta = V['X',N_sim,'theta']
    phi = V['X',N_sim,'phi']
    d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
    r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
예제 #26
0
def test_affine_constraint():
    var = struct_symSX([entry('x', shape=(2, 1))])
    linear_coefficient = np.eye(2) * 2
    affine_coefficient = np.ones((2, ))
    lower_bound = np.zeros((2, 1))
    upper_bound = np.ones((2, 1))

    constr = AffineConstraint.gen(var, linear_coefficient, affine_coefficient,
                                  lower_bound, upper_bound)

    g, lb, ub = constr[0]

    assert (lb == (lower_bound - affine_coefficient)).all()
    assert (ub == (upper_bound - affine_coefficient)).all()
    assert '@1=2, [(@1*x_0), (@1*x_1)]' == str(g)
    assert np.equal(Function('g_fun', [var['x']], [g])([2, 2]), [4, 4]).all()
예제 #27
0
파일: system.py 프로젝트: wuyou33/awebox
def generate_optimization_parameters():

    # variable system parameters
    p_dec = cas.struct_symSX([(
        cas.entry('gamma'), # force homotopy variable
        cas.entry('tau'),   # tether drag homotopy variable
        cas.entry('iota'),  # induction homotopy variable
        cas.entry('psi'),    # power homotopy variable
        cas.entry('eta'),  # nominal landing homotopy variable
        cas.entry('nu'),  # compromised landing homotopy variable
        cas.entry('upsilon'),  # transition homotopy variable
    )])

    optimization_parameters = p_dec

    return optimization_parameters
예제 #28
0
 def construct_constraints(self, variables, parameters):
     entries = []
     for child in self.children.values():
         for name, constraint in child._constraints.items():
             expression = self._substitute_symbols(
                 constraint[0], variables, parameters)
             entries.append(entry(child._add_label(name), expr=expression))
     self._con_struct = struct(entries)
     constraints = struct_MX(entries)
     self._lb, self._ub = constraints(0), constraints(0)
     self._constraint_shutdown = {}
     for child in self.children.values():
         for name, constraint in child._constraints.items():
             self._lb[child._add_label(name)] = constraint[1]
             self._ub[child._add_label(name)] = constraint[2]
             if constraint[3]:
                 self._constraint_shutdown[
                     child._add_label(name)] = constraint[3]
     return constraints, self._lb, self._ub
예제 #29
0
 def construct_constraints(self, variables, parameters):
     entries = []
     for child in self.children.values():
         for name, constraint in child._constraints.items():
             expression = self._substitute_symbols(constraint[0], variables,
                                                   parameters)
             entries.append(entry(child._add_label(name), expr=expression))
     self._con_struct = struct(entries)
     constraints = struct_MX(entries)
     self._lb, self._ub = constraints(0), constraints(0)
     self._constraint_shutdown = {}
     for child in self.children.values():
         for name, constraint in child._constraints.items():
             self._lb[child._add_label(name)] = constraint[1]
             self._ub[child._add_label(name)] = constraint[2]
             if constraint[3]:
                 self._constraint_shutdown[child._add_label(
                     name)] = constraint[3]
     return constraints, self._lb, self._ub
예제 #30
0
    def build_integrator(self, options, model):
        print('Building integrator...')

        # get model variables
        variables = model.variables
        # construct the DAE variables
        x = cas.struct_SX([cas.entry('xd', expr = variables['xd'])]) # differential states
        z = cas.struct_SX([cas.entry('xddot', expr = variables['xddot']), # state derivatives
                       cas.entry('xa', expr = variables['xa']), # algebraic variables
                       cas.entry('xl', expr = variables['xl']), # lifted variables
                      ])
        p = cas.struct_SX([cas.entry('u', expr = variables['u']), # dae parameters
                       cas.entry('theta', expr = variables['theta']),
                       cas.entry('phi', expr = model.parameters)])

        # scale xddot with t_f
        time_scaled_variables = scale_xddot(variables)

        # model equations
        alg = model.dynamics(time_scaled_variables, model.parameters)
        ode = variables['xddot']

        # create dae
        dae = {'x': x.cat, 'z': z.cat, 'p': p.cat, 'alg': alg,'ode': ode}
        # system dynamics
        f = cas.Function('f', [x, z, p], [ode, alg], ['x', 'z', 'p'], ['ode', 'alg'])

        # create integrator and rootfinder
        if cas.sprank(cas.jacobian(alg,z)) < z.cat.size()[0]:  # check dae index
            raise ValueError('jacobian of dynamics is structurally rank-deficient: DAE is not of index 1!')
        else:
            # create integrator
            I = cas.integrator('I', options['integrator']['type'], dae, {'tf': 1.0 / self.__N_sim})
            # create rootfinder
            g = cas.Function('g',[z.cat,x.cat,p.cat],[alg])
            G = cas.rootfinder('G', 'newton', g, {'linear_solver': 'csparse'})
            self.__integrator = I
            self.__rootfinder = G
            self.__variables_dict = model.variables_dict
            self.__phi = model.parameters
            self.__dae = dae
            self.__f = f
            self.__x = x
            self.__z = z
            self.__p = p
예제 #31
0
def setup_nlp_p(V, model):

    cost = setup_nlp_cost()
    p_fix = setup_nlp_p_fix(V, model)

    use_vortex_linearization = 'lin' in model.parameters_dict.keys()
    if use_vortex_linearization:
        P = cas.struct_symMX([
            cas.entry('p', struct=p_fix),
            cas.entry('cost', struct=cost),
            cas.entry('theta0', struct=model.parameters_dict['theta0']),
            cas.entry('lin', struct=V)
        ])
    else:
        P = cas.struct_symMX([
            cas.entry('p', struct=p_fix),
            cas.entry('cost', struct=cost),
            cas.entry('theta0', struct=model.parameters_dict['theta0'])
        ])

    return P
예제 #32
0
def __get_sstruct(dictionary):
    """Convert a dictionary to a casadi struct with the same structure and entries for 0th, 1st and 2nd derivatives

    :type dictionary: dict
    :param dictionary: variables to be put into the struct

    :rtype: casadi.struct_symSX
    """

    struct_list = []

    for key in list(dictionary.keys()):
        if key[0] != 'd':
            struct_list += [(key, dictionary[key].shape)]

    sub_struct = ct.struct_symSX([
        ct.entry(struct_list[i][0], shape=struct_list[i][1])
        for i in range(len(struct_list))
    ])

    dsub_struct = ct.struct_symSX([
        ct.entry('d' + struct_list[i][0], shape=struct_list[i][1])
        for i in range(len(struct_list))
    ])

    ddsub_struct = ct.struct_symSX([
        ct.entry('dd' + struct_list[i][0], shape=struct_list[i][1])
        for i in range(len(struct_list))
    ])

    sstruct = ct.struct_symSX([
        ct.entry('var', struct=sub_struct),
        ct.entry('dvar', struct=dsub_struct),
        ct.entry('ddvar', struct=ddsub_struct)
    ])

    return sstruct
예제 #33
0
파일: model.py 프로젝트: b4be1/ball_catcher

# %% =========================================================================
#                               Variables
# ============================================================================

# State
state = cat.struct_symSX(["x_b", "y_b", "z_b", "vx_b", "vy_b", "vz_b", "x_c", "y_c", "phi"])

# Control
control = cat.struct_symSX(["v", "w", "psi"])

# Observation
observation = cat.struct_SX(
    [
        cat.entry("x_b", expr=state["x_b"]),
        cat.entry("y_b", expr=state["y_b"]),
        cat.entry("z_b", expr=state["z_b"]),
        cat.entry("x_c", expr=state["x_c"]),
        cat.entry("y_c", expr=state["y_c"]),
        cat.entry("phi", expr=state["phi"]),
    ]
)

# Belief state (mu, Sigma)
belief = cat.struct_symSX([cat.entry("m", struct=state), cat.entry("S", shapestruct=(state, state))])

# Extended belief (mu, Sigma, Lambda) for plotting
ext_belief = cat.struct_symSX(
    [
        cat.entry("m", struct=state),
예제 #34
0
파일: admm.py 프로젝트: jgillis/omg-tools
 def _construct_upd_z_nlp(self):
     # construct variables
     self._var_struct_updz = struct([entry('z_i', struct=self.q_i_struct),
                                     entry('z_ij', struct=self.q_ij_struct)])
     var = struct_symMX(self._var_struct_updz)
     z_i = self.q_i_struct(var['z_i'])
     z_ij = self.q_ij_struct(var['z_ij'])
     # construct parameters
     self._par_struct_updz = struct([entry('x_i', struct=self.q_i_struct),
                                     entry('x_j', struct=self.q_ij_struct),
                                     entry('l_i', struct=self.q_i_struct),
                                     entry('l_ij', struct=self.q_ij_struct),
                                     entry('t'), entry('T'), entry('rho'),
                                     entry('par', struct=self.par_struct)])
     par = struct_symMX(self._par_struct_updz)
     x_i, x_j = self.q_i_struct(par['x_i']), self.q_ij_struct(par['x_j'])
     l_i, l_ij = self.q_i_struct(par['l_i']), self.q_ij_struct(par['l_ij'])
     t, T, rho = par['t'], par['T'], par['rho']
     t0 = t/T
     # transform spline variables: only consider future piece of spline
     tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
     self._transform_spline([x_i, z_i, l_i], tf, self.q_i)
     self._transform_spline([x_j, z_ij, l_ij], tf, self.q_ij)
     # construct constraints
     constraints, lb, ub = [], [], []
     for con in self.constraints:
         c = con[0]
         for sym in symvar(c):
             for label, child in self.group.items():
                 if sym.getName() in child.symbol_dict:
                     name = child.symbol_dict[sym.getName()][1]
                     v = z_i[label, name]
                     ind = self.q_i[child][name]
                     sym2 = MX.zeros(sym.size())
                     sym2[ind] = v
                     sym2 = reshape(sym2, sym.shape)
                     c = substitute(c, sym, sym2)
                     break
             for nghb in self.q_ij.keys():
                 for label, child in nghb.group.items():
                     if sym.getName() in child.symbol_dict:
                         name = child.symbol_dict[sym.getName()][1]
                         v = z_ij[nghb.label, label, name]
                         ind = self.q_ij[nghb][child][name]
                         sym2 = MX.zeros(sym.size())
                         sym2[ind] = v
                         sym2 = reshape(sym2, sym.shape)
                         c = substitute(c, sym, sym2)
                         break
             for name, s in self.par_i.items():
                 if s.getName() == sym.getName():
                     c = substitute(c, sym, par['par', name])
         constraints.append(c)
         lb.append(con[1])
         ub.append(con[2])
     self.lb_updz, self.ub_updz = lb, ub
     # construct objective
     obj = 0.
     for child, q_i in self.q_i.items():
         for name in q_i.keys():
             x = x_i[child.label, name]
             z = z_i[child.label, name]
             l = l_i[child.label, name]
             obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
     for nghb in self.q_ij.keys():
         for child, q_ij in self.q_ij[nghb].items():
             for name in q_ij.keys():
                 x = x_j[str(nghb), child.label, name]
                 z = z_ij[str(nghb), child.label, name]
                 l = l_ij[str(nghb), child.label, name]
                 obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
     # construct problem
     prob, compile_time = self.father.create_nlp(var, par, obj,
                                                 constraints, self.options)
     self.problem_upd_z = prob
예제 #35
0
def create_plan_pc(b0, N_sim):
    # Degrees of freedom for the optimizer
    V = cat.struct_symSX([
            (
                cat.entry('X',repeat=N_sim+1,struct=state),
                cat.entry('U',repeat=N_sim,struct=control)
            )
        ])
    
    # Final means
    m_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
    m_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
    dm_bc = m_bN - m_cN
    
    # Regularize controls
    J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
    
    # Multiple shooting constraints and running costs
    bk = cat.struct_SX(belief)
    bk['S'] = b0['S']
    g = []
    lbg = []
    ubg = []
    for k in range(N_sim):
        # Belief propagation
        bk['m'] = V['X',k]
        [bk_next] = BF([ bk, V['U',k] ])
        bk_next = belief(bk_next) # simplify indexing
        
        # Multiple shooting
        g.append(bk_next['m'] - V['X',k+1])
        lbg.append(ca.DMatrix.zeros(nx))
        ubg.append(ca.DMatrix.zeros(nx))

        # Control constraints
        g.append(V['U',k,'v'] - a - b * ca.cos(V['U',k,'psi']))
        lbg.append(-ca.inf)
        ubg.append(ca.DMatrix([0]))
        
        # Advance time
        bk = bk_next
    g = ca.vertcat(g)
    lbg = ca.veccat(lbg)
    ubg = ca.veccat(ubg)
    
    # Simple cost
    J += 1e1 * (ca.mul(dm_bc.T, dm_bc) + ca.trace(bk_next['S']))
    
    # log-probability cost
    #Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']]
    #J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb)))
    
    # Formulate the NLP
    nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
    
    # Create solver
    opts = {}
    opts['linear_solver'] = 'ma97'
    #opts['hessian_approximation'] = 'limited-memory'
    solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
    
    # Define box constraints
    lbx = V(-ca.inf)
    ubx = V(ca.inf)
    
    # 0 <= v
    lbx['U',:,'v'] = 0
    # -w_max <= w <= w_max
    lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
    # -pi <= psi <= pi
    lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi
    
    # m(t=0) = m0
    lbx['X',0] = ubx['X',0] = b0['m']
    
    # Solve the NLP
    sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
    return V(sol['x'])
예제 #36
0
def shift_trajectory(state, u):
    ##########################################################
    ## state and u in form (state, N)
    state_ = np.concatenate((state.T[1:], state.T[-1:]))
    u_ = np.concatenate((u.T[1:], u.T[-1:]))
    return u_, state_


if __name__ == '__main__':
    T = 0.2  # sampling time [s]
    N = 100  # prediction horizon
    rob_diam = 0.3  # [m]
    v_max = 0.6
    omega_max = np.pi / 4.0

    states = ca_tools.struct_symSX([(ca_tools.entry('x'), ca_tools.entry('y'),
                                     ca_tools.entry('theta'))])
    x, y, theta = states[...]
    n_states = states.size

    controls = ca_tools.struct_symSX([(ca_tools.entry('v'),
                                       ca_tools.entry('omega'))])
    v, omega = controls[...]
    n_controls = controls.size

    ## rhs
    rhs = ca_tools.struct_SX(states)
    rhs['x'] = v * ca.cos(theta)
    rhs['y'] = v * ca.sin(theta)
    rhs['theta'] = omega
#ulb = np.array([0.01, 299])
#xlb = np.array([0.5, 0.8, 0])
#xub = np.array([2.5, 2.0, 400])
#<<ENDCHUNK>>

# Make optimizers.
x0 = np.array([.05 * cs, .75 * Ts, .5 * hs])
u0 = np.array([Tcs, Fs])
umax = np.array([.05 * Tcs, .15 * Fs])
dumax = .2 * umax
xlb = np.array([-np.inf, -np.inf, -np.inf])
xub = np.array([np.inf, np.inf, np.inf])

# Create variables struct.
var = ctools.struct_symSX([(
    ctools.entry("x", shape=(Nx, ), repeat=Nt + 1),
    ctools.entry("u", shape=(Nu, ), repeat=Nt),
    ctools.entry("Du", shape=(Nu, ), repeat=Nt),
)])
varlb = var(-np.inf)
varub = var(np.inf)
varguess = var(0)

# Create parameters struct.
par = ctools.struct_symSX([
    ctools.entry("x_sp", shape=(Nx, ), repeat=Nt + 1),
    ctools.entry("u_sp", shape=(Nu, ), repeat=Nt),
    ctools.entry("uprev", shape=(Nu, 1)),
    ctools.entry("Ad", repeat=Nt, shape=(Nx, Nx)),
    ctools.entry("Bd", repeat=Nt, shape=(Nx, Nu)),
    ctools.entry("fd", repeat=Nt, shape=(Nx, 1))
예제 #38
0
lh_x = l.hessian('state')           # l_xx
lg_u = l.gradient('control')        # l_u
lh_u = l.hessian('control')         # l_uu
lgj_ux = ca.SXFunction('l_ux_fun',  # l_ux
                       [state, control, dt_sym],
                       [ca.jacobian(l.grad('control'), state)])


# ----------------------------------------------------------------------------
#                               Optimization
# ----------------------------------------------------------------------------

# Degrees of freedom for the optimizer
V = cat.struct_symSX([
    (
        cat.entry('X', repeat=n_sim + 1, struct=state),
        cat.entry('U', repeat=n_sim, struct=control)
    )
])

# Multiple shooting constraints
g = []
for k in range(n_sim):
    g.append(V['X', k + 1] - F([V['X', k], V['U', k], dt])[0])
g = ca.vertcat(g)

# Objective
[final_cost] = lf([V['X', n_sim]])
J = final_cost

# Regularize controls
예제 #39
0
파일: objective.py 프로젝트: mg-meth/awebox
def get_costs_struct(V):

    costs_struct = cas.struct_symSX([
        cas.entry("tracking_cost"),
        cas.entry("ddq_regularisation_cost"),
        cas.entry("u_regularisation_cost"),
        cas.entry("fictitious_cost"),
        cas.entry("theta_regularisation_cost"),
        cas.entry("slack_cost")
    ] + [cas.entry(name + '_cost') for name in struct_op.subkeys(V, 'phi')] + [
        cas.entry("time_cost"),
        cas.entry("power_cost"),
        cas.entry("nominal_landing_cost"),
        cas.entry("transition_cost"),
        cas.entry("compromised_battery_cost"),
        cas.entry("tracking_problem_cost"),
        cas.entry("power_problem_cost"),
        cas.entry("general_problem_cost"),
        cas.entry("objective")
    ])

    return costs_struct
lfunc = (casadi.mtimes(x.T, x) + casadi.mtimes(u.T, u))
l = casadi.Function("l", [x, u], [lfunc])

Pffunc = casadi.mtimes(x.T, x)
Pf = casadi.Function("Pf", [x], [Pffunc])

# Bounds on u.
uub = 1
ulb = -.75

# Make optimizers.
x0 = np.array([0, 1])

# Create variables struct.
var = ctools.struct_symSX([(
    ctools.entry("x", shape=(Nx, ), repeat=Nt + 1),
    ctools.entry("xc", shape=(Nx, Nc), repeat=Nt),
    ctools.entry("u", shape=(Nu, ), repeat=Nt),
)])
varlb = var(-np.inf)
varub = var(np.inf)
varguess = var(0)

# Adjust the relevant constraints.
for t in range(Nt):
    varlb["u", t, :] = ulb
    varub["u", t, :] = uub

# Now build up constraints and objective.
obj = casadi.SX(0)
con = []
예제 #41
0
파일: optimize.py 프로젝트: psinha/rawesome
import casadi.tools as CT
import numpy
from LLT_solver import setupImplicitFunction
import geometry
import pylab

# geomRootChord = geomRoot[0]
# geomRootY     = geomRoot[1]
# geomRootAinc  = numpy.radians(geomRoot[2])
# geomTipChord  = geomTip[0]
# geomTipY      = geomTip[1]
# geomTipAinc   = numpy.radians(geomTip[2])
n = 50

dvs = CT.struct_ssym(['operAlpha',
                      CT.entry('chordLoc',shape=n),
                      CT.entry('aIncGeometricLoc',shape=n),
                      CT.entry('An',shape=n)])

thetaLoc = numpy.linspace(1,n,n)*numpy.pi/(2.0*n)
bref = 10.0
yLoc = 0.5*bref*numpy.cos(thetaLoc)
chordLoc = dvs['chordLoc']
aIncGeometricLoc = dvs['aIncGeometricLoc']

aeroCLaRoot = numpy.array([0.0, 0.0, 2*numpy.pi, 0.0])
aeroCLaTip  = numpy.array([0.0, 0.0, 2*numpy.pi, 0.0])
#aeroCLaRoot = numpy.array([ -28.2136, 16.4140, 0.9568,-0.4000])
#aeroCLaTip = numpy.array([ -28.2136, 16.4140, 0.9568, -0.4000])
clPolyLoc = numpy.zeros((4,n))    #setup lift slope curves for each station
for i in range(n):
예제 #42
0
def setup_nlp_cost():

    cost = cas.struct_symSX([(
        cas.entry('tracking'),
        cas.entry('regularisation'),
        cas.entry('ddq_regularisation'),
        cas.entry('gamma'),
        cas.entry('iota'),
        cas.entry('psi'),
        cas.entry('tau'),
        cas.entry('eta'),
        cas.entry('nu'),
        cas.entry('upsilon'),
        cas.entry('fictitious'),
        cas.entry('power'),
        cas.entry('t_f'),
        cas.entry('theta'),
        cas.entry('nominal_landing'),
        cas.entry('compromised_battery'),
        cas.entry('transition'),
    )])

    return cost
예제 #43
0

# %% =========================================================================
#                               Variables
# ============================================================================

# State
state = cat.struct_symSX(['x_b','y_b','vx_b','vy_b',
                          'x_c','y_c','phi'])

# Control
control = cat.struct_symSX(['v','w'])

# Observation
observation = cat.struct_SX([
            cat.entry('x_b', expr = state['x_b']),
            cat.entry('y_b', expr = state['y_b']),
            cat.entry('x_c', expr = state['x_c']),
            cat.entry('y_c', expr = state['y_c']),
            cat.entry('phi', expr = state['phi'])
        ])

# Belief state (mu, Sigma)
belief = cat.struct_symSX([
        cat.entry('m',struct=state),
        cat.entry('S',shapestruct=(state,state))
    ])

# Extended belief (mu, Sigma, Lambda) for plotting
ext_belief = cat.struct_symSX([
        cat.entry('m',struct=state),
예제 #44
0
def create_plan_pc(b0, BF, dt, N_sim):
    # Degrees of freedom for the optimizer
    V = cat.struct_symSX([
            (
                cat.entry('X',repeat=N_sim+1,struct=state),
                cat.entry('U',repeat=N_sim,struct=control)
            )
        ])
    
    # Final coordinate
    x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
    x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
    dx_bc = x_bN - x_cN
    
    # Final velocity
    v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
    v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
    dv_bc = v_bN - v_cN
    
    # Angle between gaze and ball velocity
    theta = V['X',N_sim,'theta']
    phi = V['X',N_sim,'phi']
    d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
    r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
    r_unit = r / (ca.norm_2(r) + 1e-2)
    d_gaze = d - r_unit
    
    
    # Regularize controls
    J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
    
    # Multiple shooting constraints and running costs
    bk = cat.struct_SX(belief)
    bk['S'] = b0['S']
    g, lbg, ubg = [], [], []
    for k in range(N_sim):
        # Belief propagation
        bk['m'] = V['X',k]
        [bk_next] = BF([ bk, V['U',k] ])
        bk_next = belief(bk_next) # simplify indexing
        
        # Multiple shooting
        g.append(bk_next['m'] - V['X',k+1])
        lbg.append(ca.DMatrix.zeros(nx))
        ubg.append(ca.DMatrix.zeros(nx))

        # Control constraints
        g.append(V['U',k,'F'] - a - b * ca.cos(V['U',k,'psi']))
        lbg.append(-ca.inf)
        ubg.append(ca.DMatrix([0]))
        
        # Advance time
        bk = bk_next
    g = ca.vertcat(g)
    lbg = ca.veccat(lbg)
    ubg = ca.veccat(ubg)
    
    # Simple cost
    J += 1e1 * ca.mul(dx_bc.T, dx_bc) # coordinate
    J += 1e0 * ca.mul(dv_bc.T, dv_bc) # velocity
    #J += 1e0 * ca.mul(d_gaze.T, d_gaze) # gaze antialigned with ball velocity
    J += 1e1 * ca.trace(bk_next['S']) # uncertainty
    
    # log-probability cost
    #Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']]
    #J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb)))
    
    # Formulate the NLP
    nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
    
    # Create solver
    opts = {}
    opts['linear_solver'] = 'ma97'
    #opts['hessian_approximation'] = 'limited-memory'
    solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
    
    # Define box constraints
    lbx = V(-ca.inf)
    ubx = V(ca.inf)
    
    # State constraints
    # catcher can look within the upper hemisphere
    lbx['X',:,'theta'] = 0; ubx['X',:,'theta'] = theta_max
    
    # Control constraints
    # 0 <= F
    lbx['U',:,'F'] = 0
    # -w_max <= w <= w_max
    lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
    # -pi <= psi <= pi
    lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi
    
    # m(t=0) = m0
    lbx['X',0] = ubx['X',0] = b0['m']
    
    # Take care of the time
    #lbx['X',:,'T'] = 0.5; ubx['X',:,'T'] = ca.inf
    # Simulation ends when the ball touches the ground
    #lbx['X',N_sim,'z_b'] = ubx['X',N_sim,'z_b'] = 0
    
    # Solve the NLP
    sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
    return V(sol['x'])
예제 #45
0
def setup_nlp_v(nlp_numerics_options, model, formulation, Collocation):

    # extract necessary inputs
    variables_dict = model.variables_dict
    nk = nlp_numerics_options['n_k']

    # check if phase fix and adjust theta accordingly
    if nlp_numerics_options['phase_fix']:
        theta = get_phase_fix_theta(variables_dict)
    else:
        theta = variables_dict['theta']

    # define interval struct entries for controls and states
    entry_tuple = (
        cas.entry('xd', repeat=[nk + 1], struct=variables_dict['xd']),
        cas.entry('u', repeat=[nk], struct=variables_dict['u']),
    )

    # add additional variables according to provided options
    if nlp_numerics_options['discretization'] == 'direct_collocation':

        # add algebraic variables at interval except for radau case
        if nlp_numerics_options['collocation']['scheme'] != 'radau':
            if nlp_numerics_options['lift_xddot']:
                entry_tuple += (
                    cas.entry(
                        'xddot', repeat=[nk], struct=variables_dict['xddot']
                    ),  # depends on implementation (e.g. not present for radau collocation)
                )
            if nlp_numerics_options['lift_xa']:
                entry_tuple += (
                    cas.entry('xa', repeat=[nk], struct=variables_dict['xa']),
                )  # depends on implementation (e.g. not present for radau collocation)
                if 'xl' in list(variables_dict.keys()):
                    entry_tuple += (
                        cas.entry('xl',
                                  repeat=[nk],
                                  struct=variables_dict['xl']),
                    )  # depends on implementation (e.g. not present for radau collocation)

        # add collocation node variables
        d = nlp_numerics_options['collocation'][
            'd']  # interpolating polynomial order
        coll_var = Collocation.get_collocation_variables_struct(variables_dict)
        entry_tuple += (cas.entry('coll_var', struct=coll_var, repeat=[nk,
                                                                       d]), )

    elif nlp_numerics_options['discretization'] == 'multiple_shooting':

        # add slack variables for inequalities
        if nlp_numerics_options[
                'slack_constraints'] == True and model.constraints_dict[
                    'inequality']:
            entry_tuple += (cas.entry(
                'us', repeat=[nk],
                struct=model.constraints_dict['inequality']), )

        # multiple shooting: add algebraic variables at interval if lifted
        if nlp_numerics_options['lift_xddot']:
            entry_tuple += (
                cas.entry(
                    'xddot', repeat=[nk], struct=variables_dict['xddot']
                ),  # depends on implementation (e.g. not present for radau collocation)
            )
        if nlp_numerics_options['lift_xa']:
            entry_tuple += (
                cas.entry('xa', repeat=[nk], struct=variables_dict['xa']),
            )  # depends on implementation (e.g. not present for radau collocation)
            if 'xl' in list(variables_dict.keys()):
                entry_tuple += (
                    cas.entry('xl', repeat=[nk], struct=variables_dict['xl']),
                )  # depends on implementation (e.g. not present for radau collocation)

    # add global entries
    entry_list = [entry_tuple]
    entry_list += [
        cas.entry('theta', struct=theta),
        cas.entry('phi', struct=model.parameters_dict['phi']),
        cas.entry('xi', struct=formulation.xi_dict['xi'])
    ]

    # generate structure
    V = cas.struct_symMX(entry_list)

    return V