tester.asv

function tester()
    clf;
    clear all;
    LW = 10; LH = 7.5; LG = 3;
    m = 1; Ic = (1/12)*(LH^2+LW^2);
    g = 1; k = 20; k_list = [2.5*k, 3.5*k,  2*k, 1.5*k,  0.5*k, 0.5*k,  2*k, 2.5*k];
    l0 = 1.5*LG;
    Pbl_box = [-LW;-LH]/2;
    Pbr_box = [LW;-LH]/2;
    Ptl_box = [-LW;LH]/2;
    Ptr_box = [LW;LH]/2;
    boundary_pts = [Pbl_box,Pbr_box,Ptr_box,Ptl_box,Pbl_box];
    Pbl1_world = Pbl_box + [-LG/4;-LG]*1.5;
    Pbl2_world = Pbl_box + [LG;-LG]*1.5;
    Pbr1_world = Pbr_box + [LG;-LG/4]*1.5;
    Pbr2_world = Pbr_box + [LG;LG]*1.5;
    Ptl1_world = Ptl_box + [-LG;LG/4]*1.5;
    Ptl2_world = Ptl_box + [-LG;-LG]*1.5;
    Ptr1_world = Ptr_box + [LG/4;LG]*1.5;
    Ptr2_world = Ptr_box + [-LG;LG]*1.5;
    P_world = [Pbl1_world,Pbl2_world,Pbr1_world,Pbr2_world, Ptl1_world, Ptl2_world, Ptr1_world, Ptr2_world];
    P_box = [Pbl_box,Pbl_box,Pbr_box,Pbr_box, Ptl_box, Ptl_box, Ptr_box, Ptr_box];
    %define system parameters
    box_params = struct();
    box_params.m = m;
    box_params.I = Ic;
    box_params.g = g;
    box_params.k_list = k_list;
    box_params.l0_list = l0*ones(size(P_world,2));
    box_params.P_world = P_world;
    box_params.P_box = P_box;
    box_params.boundary_pts = boundary_pts;

    Fehlberg = struct();
    Fehlberg.C = [0, 1/4, 3/8, 12/13, 1, 1/2];
    Fehlberg.B = [16/135, 0, 6656/12825, 28561/56430, -9/50, 2/55;...
    25/216, 0, 1408/2565, 2197/4104, -1/5, 0];
    Fehlberg.A = [0,0,0,0,0,0;...
    1/4, 0,0,0,0,0;...
    3/32, 9/32, 0,0,0,0;...
    1932/2197, -7200/2197, 7296/2197, 0,0,0;...
    439/216, -8, 3680/513, -845/4104, 0,0;...
    -8/27, 2, -3544/2565, 1859/4104, -11/40, 0];
    h_ref = 2.8;
    error_desired = 10^-6;
    p=5;

    %load the system parameters into the rate function
    %via an anonymous function
    my_rate_func = @(V_in) box_rate_func(0,V_in,box_params);

    x0 =0;       %your code here
    y0 =0;       %your code here
    theta0 = 0;   %your code here
    vx0 = 0;      %your code here
    vy0 = 0;     %your code here
    vtheta0 = 0;   %your code here
    V0 = [x0;y0;theta0;vx0;vy0;vtheta0];
    tspan = [0 5];    %your code here

    solver_params=struct();
    solver_params.dx_tol=1e-14;

    
    
    [Veq,~,~,J_approx] = multi_newton_solver_generalized(my_rate_func,V0,solver_params);
    
    my_rate_func = @(t_in, V_in) box_rate_func(t_in,V_in,box_params);

    my_linear_rate=@(t_in, V_in)J_approx*(V_in-Veq);

    dx0 = 1;%your code here
    dy0 = 1;%your code here
    dtheta0 = 1;%your code here
    vx0 = 1;%your code here
    vy0 = 1;%your code here
    vtheta0 = 1;%your code here
    %small number used to scale initial perturbation
    epsilon = 0.067; %your code here
    V0 = Veq + epsilon*[dx0;dy0;dtheta0;vx0;vy0;vtheta0];
    tspan = [0 5]; %your code here
    %run the integration of nonlinear system
    % [tlist_nonlinear,Vlist_nonlinear] =...
    % your_integrator(my_rate_func,tspan,V0,...);
    %run the integration of linear system
    % [tlist_linear,Vlist_linear] =...
    % your_integrator(my_linear_rate,tspan,V0,...);

    %run the integration
    [t_list_nonlinear,V_list_nonlinear,~, ~, ~, ~] = explicit_RK_variable_step_integration(my_rate_func,tspan,V0,h_ref,Fehlberg, p, error_desired);
    [t_list_linear,V_list_linear,~, ~, ~, ~] = explicit_RK_variable_step_integration(my_linear_rate,tspan,V0,h_ref,Fehlberg, p, error_desired);
    %[t_list,X_list] = ode45(my_rate_func,tspan,V0);

    delta_x_list_nonlinear=V_list_nonlinear(1,:)-Veq(1);
    delta_y_list_nonlinear=V_list_nonlinear(2,:)-Veq(2);
    delta_theta_list_nonlinear=V_list_nonlinear(3,:)-Veq(3);
    delta_x_list_linear=V_list_linear(1,:)-Veq(1);
    delta_y_list_linear=V_list_linear(2,:)-Veq(2);
    delta_theta_list_linear=V_list_linear(3,:)-Veq(3);

    %{
    figure(1)

    hold on
    plot(t_list_linear,delta_x_list_linear,"r", "DisplayName","\Deltax linear")
    plot(t_list_linear,delta_y_list_linear,"b", "DisplayName","\Deltay linear")
    plot(t_list_linear,delta_theta_list_linear,"g", "DisplayName","\Delta\theta linear")
    plot(t_list_nonlinear,delta_x_list_nonlinear,"r--", "DisplayName","\Deltax nonlinear")
    plot(t_list_nonlinear,delta_y_list_nonlinear,"b--", "DisplayName","\Deltay nonlinear")
    plot(t_list_nonlinear,delta_theta_list_nonlinear,"g--", "DisplayName","\Delta\theta nonlinear")
    legend("Location","best")
    xlim([0 5])
    xlabel("time")
    ylabel("displacement")
    title("linear vs non linear from tiny pertubation \epsilon=0.0067")
    %}

    Q   = -J_approx(4:6, 1:3);
    [U_modes, lambda] = eig(Q);                      
    w_n = real(diag(lambda));                 
    [omega_n, idx] = sort(sqrt(max(w_n,0)));
    U_modes = U_modes(:,idx);

    %x_modal = Veq(1)+epsilon*U_modes(1)*cos(omega_n*t_list_nonlinear);
    %y_modal = Veq(2)+epsilon*U_modes(2)*cos(omega_n*t_list_nonlinear);
    %theta_modal = Veq(3)+epsilon*U_modes(3)*cos(omega_n*t_list_nonlinear);

    delta_x_modal = epsilon*U_modes(1)*cos(omega_n*t_list_nonlinear);
    delta_y_modal = epsilon*U_modes(2)*cos(omega_n*t_list_nonlinear);
    delta_theta_modal = epsilon*U_modes(3)*cos(omega_n*t_list_nonlinear);

    figure()
    
    hold on
    plot(t_list_nonlinear,delta_x_modal(3,:),'r',"DisplayName","\Deltax modal")
    plot(t_list_nonlinear,delta_y_modal(3,:),'b',"DisplayName","\Deltay modal")
    plot(t_list_nonlinear,delta_theta_modal(3,:),'g',"DisplayName","\Delta\theta modal")
    plot(t_list_nonlinear,delta_x_list_nonlinear,"r--", "DisplayName","\Deltax nonlinear")
    plot(t_list_nonlinear,delta_y_list_nonlinear,"b--", "DisplayName","\Deltay nonlinear")
    plot(t_list_nonlinear,delta_theta_list_nonlinear,"g--", "DisplayName","\Delta\theta nonlinear")
    legend("Location","best");
    xlabel("time")
    ylabel("displacement");
    xlim([0 5])
    U_modes(3)
    title("mode -0.0287")

    %animate_simulation(t_list_linear, V_list_linear, box_params)
end