def DP_lqr():
    """Double Pendulum upright LQR stabilization example"""
    p = DoublePendulum(DOUBLE_PENDULUM_PARAMS)

    upside_state = np.array([pi, 0.0, pi, 0.0])
    # upside_state with noise in position
    perturbation = np.array([1.0, 0.0, 1.0, 0.0]) * np.random.normal(0.0, 0.1, (4,))
    initial_state = upside_state + perturbation
    print('Starting perturbed by %s' % (perturbation,))
    # 20 seconds
    time_range = np.arange(0.0, 10.0, 0.01)

    # LQR controller focused on upside
    lqr = LQR(p, upside_state, 0)
    p.set_controller(lqr.get())

    # simulation and visualization
    y = p.simulate(initial_state, time_range, visualize=True)
Beispiel #2
0
def DP_lqr():
    """Double Pendulum upright LQR stabilization example"""
    p = DoublePendulum(DOUBLE_PENDULUM_PARAMS)

    upside_state = np.array([pi, 0.0, pi, 0.0])
    # upside_state with noise in position
    perturbation = np.array([1.0, 0.0, 1.0, 0.0]) * np.random.normal(
        0.0, 0.1, (4, ))
    initial_state = upside_state + perturbation
    print('Starting perturbed by %s' % (perturbation, ))
    # 20 seconds
    time_range = np.arange(0.0, 10.0, 0.01)

    # LQR controller focused on upside
    lqr = LQR(p, upside_state, 0)
    p.set_controller(lqr.get())

    # simulation and visualization
    y = p.simulate(initial_state, time_range, visualize=True)
Beispiel #3
0
def swingup_example(aggressive=True, debug=False):
    """Double Pendulum upright LQR stabilization example"""
    p = DoublePendulum(DOUBLE_PENDULUM_PARAMS)

    downside_state = np.array([0.0, 0.0, 0.0, 0.0])
    upside_state = np.array([pi, 0.0, pi, 0.0])
    # upside_state with noise in position

    initial_state = downside_state + np.array([1.0, 0.0, 1.0, 0.0
                                               ]) * np.random.normal(0.0, 0.1)
    print('Starting from %s' % (initial_state, ))

    # 20 seconds
    time_range = np.arange(0.0, 10, 0.05)

    # Energy controller
    energy_ctrl = EnergyStabilization(p, upside_state, 0)

    k = [0.1, 0.025, 0.025]
    max_lqr_score = 19.0

    # Erection controller
    normalize_angle = lambda angle: angle % (2 * pi) - pi

    def angle_controller(state, t):
        t1 = state[0]
        t2 = state[2]
        diff = t1 - t2
        if abs(diff) > pi:
            new_abs = 2 * pi - abs(diff)
            diff = new_abs * np.sign(diff)
        return diff

    straight_ctrl = Controller(angle_controller)
    straight_dot_ctrl = Controller(lambda state, t: -(state[3] - state[1]))

    swingup_controller = MixedController()
    swingup_controller.add(k[0], energy_ctrl)
    swingup_controller.add(k[1], straight_ctrl)
    swingup_controller.add(k[2], straight_dot_ctrl)

    # LQR controller focused on upside
    lqr = LQR(p, upside_state, 0)
    swingup_controller.add_lqr(lqr, max_lqr_score)

    # Set the mixed controller
    p.set_controller(swingup_controller.get())

    # Just the energy
    #energy_ctrl = EnergyStabilization(p, upside_state, 0, coefficient=0.05)
    #p.set_controller(energy_ctrl.get())

    # simulation and visualization
    y = p.simulate(initial_state, time_range, visualize=False)
    if debug:
        total_energy = []
        desired_energy = []
        angle_diff = []
        lqr_score = []
        lqr_required_score = []
        angle_vel = []
        for i in range(y.shape[0]):
            yi = y[i]
            total_energy.append(p.energy(yi, 0))
            desired_energy.append(p.energy(upside_state, 0))
            angle_diff.append(abs(angle_controller(yi, 0)))
            lqr_score.append(min(100.0, lqr.score(yi, 0)[0, 0]))
            lqr_required_score.append(max_lqr_score)
            angle_vel.append(abs(yi[3] - yi[1]))
        plt.figure()
        plt.plot(time_range, desired_energy, color='blue')
        plt.plot(time_range, total_energy, color='red')

        plt.figure()
        plt.plot(time_range, lqr_score, color='black')
        plt.plot(time_range, lqr_required_score, color='red')
        plt.figure()
        plt.plot(time_range, angle_vel, color="red")
        plt.plot(time_range, angle_diff, color='green')

    return p.animation(y, time_range)
def swingup_example(aggressive=True, debug=False):
    """Double Pendulum upright LQR stabilization example"""
    p = DoublePendulum(DOUBLE_PENDULUM_PARAMS)

    downside_state = np.array([0.0, 0.0, 0.0, 0.0])
    upside_state = np.array([pi, 0.0, pi, 0.0])
    # upside_state with noise in position

    initial_state = downside_state + np.array([1.0, 0.0, 1.0, 0.0]) * np.random.normal(0.0, 0.1)
    print('Starting from %s' % (initial_state,))

    # 20 seconds
    time_range = np.arange(0.0, 10, 0.05)

    # Energy controller
    energy_ctrl = EnergyStabilization(p, upside_state, 0)

    k = [0.1, 0.025, 0.025]
    max_lqr_score = 19.0

    # Erection controller
    normalize_angle = lambda angle : angle % (2*pi) - pi

    def angle_controller(state, t):
        t1 = state[0]
        t2 = state[2]
        diff = t1-t2
        if abs(diff) > pi:
            new_abs = 2*pi - abs(diff)
            diff = new_abs * np.sign(diff)
        return diff

    straight_ctrl = Controller(angle_controller)
    straight_dot_ctrl = Controller(lambda state, t: -(state[3]-state[1]) )

    swingup_controller = MixedController()
    swingup_controller.add(k[0], energy_ctrl)
    swingup_controller.add(k[1], straight_ctrl)
    swingup_controller.add(k[2], straight_dot_ctrl)

    # LQR controller focused on upside
    lqr = LQR(p, upside_state, 0)
    swingup_controller.add_lqr(lqr, max_lqr_score)

    # Set the mixed controller
    p.set_controller(swingup_controller.get())

    # Just the energy
    #energy_ctrl = EnergyStabilization(p, upside_state, 0, coefficient=0.05)
    #p.set_controller(energy_ctrl.get())



    # simulation and visualization
    y = p.simulate(initial_state, time_range, visualize=False)
    if debug:
        total_energy = []
        desired_energy = []
        angle_diff = []
        lqr_score = []
        lqr_required_score = []
        angle_vel = []
        for i in range(y.shape[0]):
            yi = y[i]
            total_energy.append(p.energy(yi,0))
            desired_energy.append(p.energy(upside_state,0))
            angle_diff.append(abs(angle_controller(yi, 0)))
            lqr_score.append(min(100.0, lqr.score(yi,0)[0,0]))
            lqr_required_score.append(max_lqr_score)
            angle_vel.append(abs(yi[3]-yi[1]))
        plt.figure()
        plt.plot(time_range, desired_energy, color='blue')
        plt.plot(time_range, total_energy, color='red')


        plt.figure()
        plt.plot(time_range, lqr_score, color='black')
        plt.plot(time_range, lqr_required_score, color='red')
        plt.figure()
        plt.plot(time_range, angle_vel, color="red")
        plt.plot(time_range, angle_diff, color='green')

    return p.animation(y, time_range)