コード例 #1
0
    state = dynamics.state

    x_hist = []
    t_hist = []

    while (t0 < params.tf):
        tp = t0 + params.t_plot
        while t0 < tp:
            x_hist.append(state.copy())
            x_ref = np.array([[
                5 * np.cos(0.2 * (t0 + i * params.dt)),
                5 * np.sin(0.2 * (t0 + i * params.dt)),
                -10 - (t0 + i * params.dt) * 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0
            ] for i in range(params.T + 1)]).T
            u = controller.calculateControl(x_ref, state)
            state = dynamics.updateState(u + u_eq)
            # state = dynamics.updateState(u)
            t0 += params.dt
            if isinstance(controller, LNMPC):
                A, B = dynamics.get_SS(state)
                controller.A = A
                controller.B = B
        if t0 > 8.0:
            # x_ref = np.array([-xr for i in range(params.T+1)]).T
            debug = 1
        t = state[:3]
        ang = state[6:9]
        R_b_from_i = Rotation.from_euler('ZYX',
                                         [ang[2], ang[1], ang[0]]).as_dcm()
        # viewer.update(t, R_b_from_i)
        data_view.update(state, x_ref[cmd_idx, 0], params.t_plot)