import numpy as np
import matplotlib.pyplot as plt
from pendulum import Pendulum
from scipy import io

'''
This is a script that computes the region of attraction for proving the asymptotic stability 
of the Variable-Length Pendulum (VLP) problem. 

The name 'adaptive' originated from the fact that the dense of grid points is adaptively distributed over the state 
space. 

=====================================
Author  :  <NAME>
Date    :  Jul. 23, 2021
Location:  UC San Diego, La Jolla, CA
=====================================
'''

# 1. Set up the starting points on the grid, computes the trajectory generated by VLP.
#    To see if the


def execute_pendulum_control(wave, attributes):
    vary_length_pendulum = Pendulum(wave, attributes)
    vary_length_pendulum.main()
    return vary_length_pendulum


def asymptotic_stable_judger(vary_length_pendulum):
    threshold = 1e-10
    signal = np.vstack((vary_length_pendulum.asym_control_phi, vary_length_pendulum.asym_control_dphi))
    left_x_crossers = np.empty(shape=[2, 0])

    # Step 1: find the points close to the y-axis from x left plane
    # Since the time step has been set to be .02, re-set the threshold to be 0.1
    while threshold <= 1e-1:
        left_x_crossers = signal[:, (abs(signal[1, :]) < threshold) & (signal[0, :] < 0)]
        # y-axis too aggressive, set the threshold larger
        if left_x_crossers.shape[1] <= 5:
            threshold *= 10
        else:
            # print('already found enough points near y axis, break')
            break
        # print('threshold reaches bound, no enough points near y axis found')

    # Step 2: clean the data points
    # criterion 1: though trajectory shrinking, for each slice there might be multiple points considered due to the
    # numerical issue
    if left_x_crossers.shape[1] <= 1:
        # only one point close to x-axis, not converging.
        roa = False
        slow_converge_rate = False

    else:
        delete_list = []
        query_point = left_x_crossers[0, 0]
        for i in range(1, left_x_crossers.shape[1]):
            if abs(query_point - left_x_crossers[0, i]) * 100 < 1:
                delete_list.append(i)
            else:
                query_point = left_x_crossers[0, i]
        left_x_crossers_cleaned = np.delete(left_x_crossers, delete_list, 1)
        if left_x_crossers_cleaned.shape[1] <= 1:
            roa = False
            slow_converge_rate = True
            # trajectory passing through x-axis through the same position, not converging/converging very slow due to
            # the numerical issue

        else:
            # condition 1: consider the x-value of the point crossing the x-axis at the left plane, if value increasing,
            # the trajectory is converging
            decrease_indicator = np.sign(np.diff(left_x_crossers_cleaned[0, :]))
            last_first_diff = left_x_crossers[0, -1] * 10 - left_x_crossers[0, 1]

            # if indicator all positive, inside ROA true
            if (decrease_indicator > 0).all() or last_first_diff > 0:
                roa = True
                slow_converge_rate = False

            else:
                roa = False
                slow_converge_rate = False

    return roa, slow_converge_rate


# Define the attributes for the variable-length pendulum problem
d = 10
dt = 0.02
g = 9.8
l0 = 1
m = 1
delta = .05

attributes = {
    'm': m,
    'max_t': d,
    'dt': dt,
    'constrain_L': True,
    'save_data': False,
    'plot': False,
    'save_fig': False,
    'show_fig': True,
    'asymptotic_mode': True,
    'delta_asymptotic_const': delta,
    'adaptive_mode': False,
    'delta_adaptive_const': delta,
    'l0': l0,
    'Ldotmax': 5,
    'Ldotmin': -5,
    'Lmax': 1.5,
    'Lmin': 0.5,
    'g': g,
}


def roa_indicator_adaptive_grid(upper_bnd, lower_bnd, fine_trigger, attributes):
    size = 40 if fine_trigger else 10
    x, y = np.meshgrid(np.linspace(lower_bnd[0], upper_bnd[0], size), np.linspace(lower_bnd[1], upper_bnd[1], size))

    roa_indicator = np.zeros(x.shape)
    converge_slow_indicator = np.zeros(x.shape)

    for i in range(x.shape[0]):
        print(f'i = {i}')
        for j in range(y.shape[1]):

            phi = x[i, j] * np.ones(1)
            dphi = y[i, j] * np.ones(1)
            wave = {'phi': phi, 'dphi': dphi}

            vary_length_pendulum = execute_pendulum_control(wave, attributes)
            roa_indicator[i, j], converge_slow_indicator[i, j] = asymptotic_stable_judger(vary_length_pendulum)
    final_indicator = np.maximum(roa_indicator, converge_slow_indicator)
    return x, y, final_indicator


# plotting -
limit = 3

fig = plt.figure(figsize=[16, 9])
plt.gca().set_aspect('equal', adjustable='box')
# Show the major grid lines with dark grey lines
plt.grid(b=True, which='major', color='#666666', linestyle='-')

# Show the minor grid lines with very faint and almost transparent grey lines
plt.minorticks_on()
plt.grid(b=True, which='minor', color='#999999', linestyle='-', alpha=0.2)


# the first two block are region of interest, fine grid
lower = np.array([[-1.75, -2.5], [-2.5, -2], [-2.5, 1], [1.75, 1], [-2.5, -2.5]])
upper = np.array([[2.5, -2], [2.5, 1], [1.75, 2.5], [2.5, 2.5], [-1.75, -2]])
fine_trigger = [0, 0, 0, 1, 1]


# for storing the data
coordinates = np.empty(shape=[2, 0])
labels = np.empty(shape=[1, ])

# plot the ROA: gray - stable, black - unstable
for ii in range(lower.shape[0]):
    print(f'========= ii = {ii} =========')
    x, y, indicator = roa_indicator_adaptive_grid(upper[ii, :], lower[ii, :], fine_trigger[ii], attributes)
    mu = plt.contourf(x, y, indicator, levels=[0, 0.5, 1], colors=('black', 'gray'))
    coordinates = np.hstack((coordinates, np.vstack((x.flatten(), y.flatten()))))
    labels = np.hstack((labels, indicator.flatten()))


# store the data
data = {'coordinates': coordinates,
        'labels': labels}
io.savemat('training_set.mat', data)

plt.xlabel(r'$\phi(t)$', size=20)
plt.ylabel(r'$\dot{\phi}(t)$', size=20, rotation=0)


# plot the boundary of region of interest
width = 2.5
bnds = np.array(([-width, width, width, -width, -width],[-width, -width, width, width, -width]))
for i in range(bnds.shape[1]-1):
    plt.plot(bnds[:, i], bnds[:, i+1], 'k')

# plot the trajectory of system states, intial point start from a unstable region
phi_not_stable1 = np.array([-2.22368421])
dphi_not_stable1 = np.array([-2.5])
wave = {'phi': phi_not_stable1, 'dphi': dphi_not_stable1}
vary_length_pendulum = execute_pendulum_control(wave, attributes)
plt.scatter(vary_length_pendulum.asym_control_phi[0], vary_length_pendulum.asym_control_dphi[0], c='b', marker='s', s=2)
plt.plot(vary_length_pendulum.asym_control_phi, vary_length_pendulum.asym_control_dphi, 'r--')


# plot the stable point
phi_not_stable1 = np.array([-1])
dphi_not_stable1 = np.array([-1])
wave = {'phi': phi_not_stable1, 'dphi': dphi_not_stable1}
vary_length_pendulum = execute_pendulum_control(wave, attributes)
plt.scatter(vary_length_pendulum.asym_control_phi[0], vary_length_pendulum.asym_control_dphi[0], c='b', marker='s', s=2)
plt.plot(vary_length_pendulum.asym_control_phi, vary_length_pendulum.asym_control_dphi, 'r--')

# plot the zoomed in unstable region
limit = 3
plt.ylim(-3, 3)
plt.xlim(-5.3, 5.3)
# plt.show()
ax_unstable = plt.axes([.2, .5, .1, .15])
ax_unstable.contourf(x, y, indicator, levels=[0, 0.5, 1], colors=('black', 'gray'))
ax_unstable.set_xlim(-2.5, -1.75)
ax_unstable.set_ylim(-2.5, -2)

ax_unstable.grid()

plt.savefig('adaptive_ROA.png', format='png', dpi=300)
plt.close(fig)





import numpy as np
import matplotlib.pyplot as plt
from pendulum import Pendulum
from scipy import io

'''
This is a script that computes the region of attraction for proving the asymptotic stability 
of the Variable-Length Pendulum (VLP) problem. 

The name 'adaptive' originated from the fact that the dense of grid points is adaptively distributed over the state 
space. 

=====================================
Author  :  <NAME>
Date    :  Jul. 23, 2021
Location:  UC San Diego, La Jolla, CA
=====================================
'''

# 1. Set up the starting points on the grid, computes the trajectory generated by VLP.
#    To see if the


def execute_pendulum_control(wave, attributes):
    vary_length_pendulum = Pendulum(wave, attributes)
    vary_length_pendulum.main()
    return vary_length_pendulum


def asymptotic_stable_judger(vary_length_pendulum):
    threshold = 1e-10
    signal = np.vstack((vary_length_pendulum.asym_control_phi, vary_length_pendulum.asym_control_dphi))
    left_x_crossers = np.empty(shape=[2, 0])

    # Step 1: find the points close to the y-axis from x left plane
    # Since the time step has been set to be .02, re-set the threshold to be 0.1
    while threshold <= 1e-1:
        left_x_crossers = signal[:, (abs(signal[1, :]) < threshold) & (signal[0, :] < 0)]
        # y-axis too aggressive, set the threshold larger
        if left_x_crossers.shape[1] <= 5:
            threshold *= 10
        else:
            # print('already found enough points near y axis, break')
            break
        # print('threshold reaches bound, no enough points near y axis found')

    # Step 2: clean the data points
    # criterion 1: though trajectory shrinking, for each slice there might be multiple points considered due to the
    # numerical issue
    if left_x_crossers.shape[1] <= 1:
        # only one point close to x-axis, not converging.
        roa = False
        slow_converge_rate = False

    else:
        delete_list = []
        query_point = left_x_crossers[0, 0]
        for i in range(1, left_x_crossers.shape[1]):
            if abs(query_point - left_x_crossers[0, i]) * 100 < 1:
                delete_list.append(i)
            else:
                query_point = left_x_crossers[0, i]
        left_x_crossers_cleaned = np.delete(left_x_crossers, delete_list, 1)
        if left_x_crossers_cleaned.shape[1] <= 1:
            roa = False
            slow_converge_rate = True
            # trajectory passing through x-axis through the same position, not converging/converging very slow due to
            # the numerical issue

        else:
            # condition 1: consider the x-value of the point crossing the x-axis at the left plane, if value increasing,
            # the trajectory is converging
            decrease_indicator = np.sign(np.diff(left_x_crossers_cleaned[0, :]))
            last_first_diff = left_x_crossers[0, -1] * 10 - left_x_crossers[0, 1]

            # if indicator all positive, inside ROA true
            if (decrease_indicator > 0).all() or last_first_diff > 0:
                roa = True
                slow_converge_rate = False

            else:
                roa = False
                slow_converge_rate = False

    return roa, slow_converge_rate


# Define the attributes for the variable-length pendulum problem
d = 10
dt = 0.02
g = 9.8
l0 = 1
m = 1
delta = .05

attributes = {
    'm': m,
    'max_t': d,
    'dt': dt,
    'constrain_L': True,
    'save_data': False,
    'plot': False,
    'save_fig': False,
    'show_fig': True,
    'asymptotic_mode': True,
    'delta_asymptotic_const': delta,
    'adaptive_mode': False,
    'delta_adaptive_const': delta,
    'l0': l0,
    'Ldotmax': 5,
    'Ldotmin': -5,
    'Lmax': 1.5,
    'Lmin': 0.5,
    'g': g,
}


def roa_indicator_adaptive_grid(upper_bnd, lower_bnd, fine_trigger, attributes):
    size = 40 if fine_trigger else 10
    x, y = np.meshgrid(np.linspace(lower_bnd[0], upper_bnd[0], size), np.linspace(lower_bnd[1], upper_bnd[1], size))

    roa_indicator = np.zeros(x.shape)
    converge_slow_indicator = np.zeros(x.shape)

    for i in range(x.shape[0]):
        print(f'i = {i}')
        for j in range(y.shape[1]):

            phi = x[i, j] * np.ones(1)
            dphi = y[i, j] * np.ones(1)
            wave = {'phi': phi, 'dphi': dphi}

            vary_length_pendulum = execute_pendulum_control(wave, attributes)
            roa_indicator[i, j], converge_slow_indicator[i, j] = asymptotic_stable_judger(vary_length_pendulum)
    final_indicator = np.maximum(roa_indicator, converge_slow_indicator)
    return x, y, final_indicator


# plotting -
limit = 3

fig = plt.figure(figsize=[16, 9])
plt.gca().set_aspect('equal', adjustable='box')
# Show the major grid lines with dark grey lines
plt.grid(b=True, which='major', color='#666666', linestyle='-')

# Show the minor grid lines with very faint and almost transparent grey lines
plt.minorticks_on()
plt.grid(b=True, which='minor', color='#999999', linestyle='-', alpha=0.2)


# the first two block are region of interest, fine grid
lower = np.array([[-1.75, -2.5], [-2.5, -2], [-2.5, 1], [1.75, 1], [-2.5, -2.5]])
upper = np.array([[2.5, -2], [2.5, 1], [1.75, 2.5], [2.5, 2.5], [-1.75, -2]])
fine_trigger = [0, 0, 0, 1, 1]


# for storing the data
coordinates = np.empty(shape=[2, 0])
labels = np.empty(shape=[1, ])

# plot the ROA: gray - stable, black - unstable
for ii in range(lower.shape[0]):
    print(f'========= ii = {ii} =========')
    x, y, indicator = roa_indicator_adaptive_grid(upper[ii, :], lower[ii, :], fine_trigger[ii], attributes)
    mu = plt.contourf(x, y, indicator, levels=[0, 0.5, 1], colors=('black', 'gray'))
    coordinates = np.hstack((coordinates, np.vstack((x.flatten(), y.flatten()))))
    labels = np.hstack((labels, indicator.flatten()))


# store the data
data = {'coordinates': coordinates,
        'labels': labels}
io.savemat('training_set.mat', data)

plt.xlabel(r'$\phi(t)$', size=20)
plt.ylabel(r'$\dot{\phi}(t)$', size=20, rotation=0)


# plot the boundary of region of interest
width = 2.5
bnds = np.array(([-width, width, width, -width, -width],[-width, -width, width, width, -width]))
for i in range(bnds.shape[1]-1):
    plt.plot(bnds[:, i], bnds[:, i+1], 'k')

# plot the trajectory of system states, intial point start from a unstable region
phi_not_stable1 = np.array([-2.22368421])
dphi_not_stable1 = np.array([-2.5])
wave = {'phi': phi_not_stable1, 'dphi': dphi_not_stable1}
vary_length_pendulum = execute_pendulum_control(wave, attributes)
plt.scatter(vary_length_pendulum.asym_control_phi[0], vary_length_pendulum.asym_control_dphi[0], c='b', marker='s', s=2)
plt.plot(vary_length_pendulum.asym_control_phi, vary_length_pendulum.asym_control_dphi, 'r--')


# plot the stable point
phi_not_stable1 = np.array([-1])
dphi_not_stable1 = np.array([-1])
wave = {'phi': phi_not_stable1, 'dphi': dphi_not_stable1}
vary_length_pendulum = execute_pendulum_control(wave, attributes)
plt.scatter(vary_length_pendulum.asym_control_phi[0], vary_length_pendulum.asym_control_dphi[0], c='b', marker='s', s=2)
plt.plot(vary_length_pendulum.asym_control_phi, vary_length_pendulum.asym_control_dphi, 'r--')

# plot the zoomed in unstable region
limit = 3
plt.ylim(-3, 3)
plt.xlim(-5.3, 5.3)
# plt.show()
ax_unstable = plt.axes([.2, .5, .1, .15])
ax_unstable.contourf(x, y, indicator, levels=[0, 0.5, 1], colors=('black', 'gray'))
ax_unstable.set_xlim(-2.5, -1.75)
ax_unstable.set_ylim(-2.5, -2)

ax_unstable.grid()

plt.savefig('adaptive_ROA.png', format='png', dpi=300)
plt.close(fig)





This is a script that computes the region of attraction for proving the asymptotic stability  of the Variable-Length Pendulum (VLP) problem.   The name 'adaptive' originated from the fact that the dense of grid points is adaptively distributed over the state  space.   ===================================== Author  :  <NAME> Date    :  Jul. 23, 2021 Location:  UC San Diego, La Jolla, CA ===================================== # 1. Set up the starting points on the grid, computes the trajectory generated by VLP. #    To see if the # Step 1: find the points close to the y-axis from x left plane # Since the time step has been set to be .02, re-set the threshold to be 0.1 # y-axis too aggressive, set the threshold larger # print('already found enough points near y axis, break') # print('threshold reaches bound, no enough points near y axis found') # Step 2: clean the data points # criterion 1: though trajectory shrinking, for each slice there might be multiple points considered due to the # numerical issue # only one point close to x-axis, not converging. # trajectory passing through x-axis through the same position, not converging/converging very slow due to # the numerical issue # condition 1: consider the x-value of the point crossing the x-axis at the left plane, if value increasing, # the trajectory is converging # if indicator all positive, inside ROA true # Define the attributes for the variable-length pendulum problem # plotting - # Show the major grid lines with dark grey lines # Show the minor grid lines with very faint and almost transparent grey lines # the first two block are region of interest, fine grid # for storing the data # plot the ROA: gray - stable, black - unstable # store the data # plot the boundary of region of interest # plot the trajectory of system states, intial point start from a unstable region # plot the stable point # plot the zoomed in unstable region # plt.show()