main.m

%%%%%%%%%%%%%%%%%%%%
% Creates the EOM and simulates the EOM for a given initial condition
%%%%%%%%%%%%%%%%%%%%
clc         % Comment this line when running AtomatedValidation
clear       % Comment this line when running AtomatedValidation
% this code has access to grep, you can use it to see where a variable is
% used: grep -ir 'F_bar' *.m   (this will search for variable F_bar)
beep off
close all
set(0, 'DefaultTextInterpreter', 'latex');
set(0, 'DefaultAxesTickLabelInterpreter', 'latex');
set(0, 'DefaultLegendInterpreter', 'latex');
addpath('Multibody_core\')
addpath('OnStartup\')
addpath('./Plotting/')
if ~exist('./MatlabFunc/', 'dir') % Check if the folder does not exist
    mkdir('./MatlabFunc/'); % Create the folder
end
addpath('./MatlabFunc/')

%% inputs
%description (for more detail, see the python documentation)
% user defines the input needed to construct the kinematics system at the
% initial condition. Mainly, this is done by defining each joint, its
% parent body, its child, and the vectors that connect the parent's CG to
% joint and Joint to child CG. If you have all of this information, you can
% construct  the initial configuration of the system
% Then, the velocity transformation matrix (R) and essenetial equations of
% motion are developed using reduced-coordinates, see https://www.sciencedirect.com/science/article/pii/002074629090046C
% Applied forces and springs and dampers are also defined in the input file
% (see the python description for more info)
% Comment this line when running AtomatedValidation
run('Important_Examples\e1_Double_pendulum.m') % Imports an example
Configuration       = struct2table(data);
[~,IDsortedJoints]  = sort(data.Joints(:,2));
Configuration       = Configuration(IDsortedJoints,:); % sorting joints ascendingly 
if sum(diff(IDsortedJoints)==1)<(length(IDsortedJoints)-1)% if the children joints are not defined in ascending order in data.Joints 
    fprintf('-------------------------------------------------------------------------------------')
    fprintf('\nit is a good practice to define the children joints in ascending order in data.Joints file (i.e., define body 2 before defining body 3) \n ')
    fprintf('\nSorted Configuration Table:\n');
    fprintf('-------------------------------------------------------------------------------------')
    disp(Configuration);
end
data = table2struct(Configuration,'ToScalar',1);

%% Finding Positons, Velocities and Accelerations
NBodies = size(data.Joints,1); % number of bodies
% Find the symbolic coordinates and the column on R of each sym coordinate
tic
[Q,QD,QDD,NDoF,Body_col] = coordinate_finder(data);
% adding the initial condition for the floating joints based on the table
Floats = find(data.Type=='F');
if isa(data.ParentCGtoJoint, 'sym')
Symbols = symvar(data.ParentCGtoJoint);
for i=1:size(Floats)
    current_XZcoordinates = Body_col{Floats(i)}(1:2);
    if  ~any(has(data.ParentCGtoJoint(Floats(i),:), Symbols)) % in case the float is not symbolic 
        ic(current_XZcoordinates,:) = data.ParentCGtoJoint(Floats(i),:);
    else
        disp('If the initial positions of the floats are defined symbolically, you must manually provide the correct initial values ("ic" in the example) when simulating the system.')
    end
end
end


% Finding position of joints and CGs, velocity transformation matrix, and its derivatve for bodies connected to the ground:
[Pos,JointLoc,R,RD,R_track] =  ...
    pos_vel_acc_finder_grounded_joint(data,Body_col,NDoF,Reference_frame_Oriigin,Q,QD,QDD,NBodies);
% Finding position of joints and CGs, velocity transformation matrix, and its derivatve for other bodies:
[Pos,JointLoc,R,RD,R_track,pris_body_to_parent_map] =  ...
    pos_vel_acc_finder(data,Body_col,NDoF,Reference_frame_Oriigin,Q,QD,QDD,NBodies,Pos,JointLoc,R,RD,R_track);

Pos         = simplify(Pos);
JointLoc    = simplify(JointLoc);
R           = simplify(R);
RD          = simplify(RD);
Vel         = R*QD;
Acc         = R*QDD+RD*QD;

dummy = toc;
fprintf('\nProblem initialization finished in:\t\t %3.2f seconds \n',dummy)

%% Parsing  points and forces, and systems energy
[Points_All,Force_All,SpringPotentialEnergy]    = ...
    PointsForce_finder(data,NBodies,Pos,Q,QD,Initial_Points,Force,JointLoc,pris_body_to_parent_map,Body_col,Vel);

% Systems mechanical energy
[BodiesEnergy,M,m,J] = systemsEnergy(NBodies,Q,QD,R,Points_All.CG,g,gVec);

% Total Energy
Energy = BodiesEnergy + SpringPotentialEnergy;

%% Checking R and RD by comparing to analytical (uncommenting this will slow down the code)
% tic
% [R_analytical,RD_analytical,R_diff_error,RD_diff_error] = Analytical_R_RD_Jacobian(Pos,Q,QD,R,RD);
% 
% dummy = toc;
% fprintf('R and RD matrices check finished in:\t %3.2f seconds \n',dummy)

%% Storing the EOM in reduced form
% the EOM can be written as ReducedM*QDD= Right_side or QDD= ReducedM\Right_side
ReducedM          = simplify(R'*M*R);
Fgravity          = g * reshape(m' .*gVec'.* [0 -1 0]',[],1);
ForceExternal     = cell2sym(Force_All.CG)+cell2sym(Force_All.PointsBd)+cell2sym(Force_All.TensionDamper)+cell2sym(Force_All.TensionSpring)+cell2sym(Force_All.TorsionDamper)+cell2sym(Force_All.TorsionSpring);
ForceAllCombined  = reshape(ForceExternal',[],1) + Fgravity;
Right_side_1      = -R'*M*simplify(RD*QD);
Right_side_2      = R'*ForceAllCombined;
Right_side        = Right_side_2+Right_side_1;

%% Create all necessary matlab functions
tic
mainSymVars = [Q;QD;ForcesPointsSym;BodyDataSym]; % all the symbolic variables used in the work, TODO: make consistent with numerical values
% Convert empty entries to NaN
Points_All.JO(cellfun(@isempty, Points_All.JO)) = {[nan,nan]};

% Function for trajectory variables
matlabFunction(Pos,"File","MatlabFunc\PosFunc","Vars",mainSymVars);
matlabFunction(JointLoc,"File","MatlabFunc\JointLocFunc","Vars",mainSymVars);
matlabFunction(R,"File","MatlabFunc\RFunc","Vars",mainSymVars);
matlabFunction(RD,"File","MatlabFunc\RDFunc","Vars",mainSymVars);
matlabFunction(Vel,"File","MatlabFunc\VelFunc","Vars",mainSymVars);
matlabFunction(Acc,"File","MatlabFunc\AccFunc","Vars",[mainSymVars;QDD]);
matlabFunction(ForceAllCombined,"File","MatlabFunc\ForceFunc","Vars",[mainSymVars;m;J]);

% Functions for points
matlabFunction(cell2sym(Points_All.BD),"File","MatlabFunc\Points_All_BD_CelltoSymFun","Vars",mainSymVars);
matlabFunction(Points_All.CG,"File","MatlabFunc\Points_all_CG","Vars",mainSymVars);
matlabFunction(Points_All.GR,"File","MatlabFunc\Points_all_GR","Vars",mainSymVars);
matlabFunction(cell2sym(Points_All.JO),"File","MatlabFunc\Points_all_JO","Vars",mainSymVars);

% Functions for EOM
matlabFunction(Right_side,"File","MatlabFunc\Right_side_func","Vars",[mainSymVars;m;J]);
matlabFunction(ReducedM,"File","MatlabFunc\ReducedM_func","Vars",[mainSymVars;m;J]);

% Function for Energy
matlabFunction(Energy,"File","MatlabFunc\totalEnergy","Vars",[mainSymVars;m;J]);

dummy = toc;
fprintf('Matlab Functions writting finished in:\t %3.2f seconds \n',dummy)

%% running the simulation (ODE)
% Gather all numeric evaluation of symbolic variables
% Display the variables that require initial conditions
ic_vars     = [Q;QD]';
fprintf('\nThe order of initial conditions (ic) needed for ODE45 are \n %s\n', char(ic_vars));
mainNumVars = [ic;ForcesPointsNum;BodyDataNum];
% Find the index of variable t in mainSymVars to update mainNumVars in ODE
t_update    = has(mainSymVars,t);
% Integration options and evaluation
opts        = odeset('RelTol',1e-8,'AbsTol',1e-8);
tic
[time,y]    = ode15s(@(t,y) RigidBIntegrator(t,y,mainNumVars,m0,J0,t_update), tspan, ic, opts);

dummy = toc;
fprintf('ODE integration of %4.1f seconds finished in:\t %3.2f seconds \n',tspan(end),dummy)

%% Post Processing
% Plot the energy
Em_num = zeros(numel(time),1);
for i = 1:numel(time)
    % Update the variables that depend on time
    QQD_vec                         = y(i,:);
    mainNumVars(1:numel(QQD_vec))   = QQD_vec;
    mainNumVars(t_update)           = time(i);
    energy_inputs                   = num2cell([mainNumVars;m0;J0]);
    Em_num(i)                       = totalEnergy(energy_inputs{:});
end

figure('Name','Energy of the System','NumberTitle','off','Position',[10,10,1920,1080]);
plot(time,Em_num)
title('Energy of the system')
xlabel('time [s]')
ylabel('$E_m$')
PlotUtil(0)
legend('off')
% figuring out the bounds of X and Z (to adjust the plotting frame)
NumBodied               = length(m);
frame_tol               = 2; % [min_x-frame_tol min_z-frame_tol max_x-frame_tol max_z-frame_tol] % defines the frame boumdary
[MinX,MaxX,MinZ,MaxZ]   = Bound_finder(frame_tol,NumBodied,time,ForcesPointsNum,BodyDataNum,y);

% plotting the system's initial configuration
figure('Position',[10,10,1920,1080]);
InitialConfig = 1; % keep as 1
hold on;
axis equal;
grid on;
xlabel('X');
ylabel('Z');
CurrentFrame = animation_plot(time(1),y(1,:),mainNumVars,t_update,data, PointsTable, ForceTable, NBodies,mainSymVars,MinX,MaxX,MinZ,MaxZ,InitialConfig);
title('System''s initial condition ')
pause(0.1)

%% Saving animation
% TODO add 
if animation_on
    if SaveMovieOn
        filename = 'Equinox'; % Output filename
        vid = VideoWriter(filename, 'MPEG-4');
        vid.FrameRate = 5;
        open(vid);
    end
    figure('Position',[10,10,1920,1080]);
    InitialConfig = 0; % keep as 0
    hold on;
    axis equal;
    grid on;
    xlabel('X');
    ylabel('Z');
    title('Animation')
    xlim([MinX MaxX])
    ylim([MinZ MaxZ])
    for i=1:floor(length(time)/plotTstep)
        ID       = plotTstep * (i-1) + 1;
        CurrentFrame = animation_plot(time(ID),y(ID,:),mainNumVars,t_update,data, PointsTable, ForceTable, NBodies,mainSymVars,MinX,MaxX,MinZ,MaxZ,InitialConfig);
        if SaveMovieOn
            writeVideo(vid, CurrentFrame);
        end
    end
end
if SaveMovieOn
    close(vid)
end
close(gcf)
% TODO, when you write the code manual, talk about GREP feature
% TODO allow the user to use XY or XZ axis. If they want to do XY, we need
% disp('TODO: add a check that goes over the size of Force.TorsionDamper/Spring Force.LinearDamper/Spring  and if the size is not correct, it gives an error')
% TODO, give a message that energy is only calculated for linear springs