% TOGAC45_AMBF_CartesianAdmittanceTestWController.m

% !!!!This script is best used when run in sections!!!!
% It is not reccommended to simply run this script as is. Initializatoin of
% the Galen_robot is a finicky process. Ensure you have the correct IP
% address or ROS initialization steps updated for your machine in the
% galen_interace.m file before running. HINT: running sections is easy.
% Instead of pressing the run section button in the editor, simply place
% and click your cursor in the section you want to run, then type cntrl +
% enter

% This script will execute the admittance controller for the open AMBF
% environment for the Galen Robot. It features a control scheme compatible
% with a gamepad or joystick and has a gui that also allows for control of
% the robot

% to run the system, run section 1 until you have successfully read the
% joint states and the force sensor data at the E35 joint
% if it fails to run or if you're waiting for the loop to continue because
% it's hung and subscribing or receiving then cancel the loop and try
% again. If this does not eventually work, then update your ROS_MASTER_URI
% or your ROS_IP in either the galen_interface file or on your linux
% machine. You may need to do a restart of your device.

% After running section 1, run section 2 to start the gui and joystick
% control.
% After running section 2, you may run any of the section 3 commands to
% preemtively move the robot. If you don't want gravity on, don't forget to
% click in your AMBF window and press 2

% You can then run section 4 to control the robot

% Modified 05-2-2023
% Created by Brevin Banks

%% Section 1

galen_robot = galen_interface();  % initiate the robot interface

%% Section 2
SwitchPro = vrjoystick(1); % Initiate the use of joystick or gamepad, ensure one is connected either by bluetooth or direct cable.
% Matlab already detects the controller present as long as there is a
% compatible gamepad. This function requires the Simulink 3D Animation
% Library

APP = GalenControlApp() % create the GUI window and the controls options for interacting with the robot in the simulation

%%% Section 3
%% Section 3.1
galen_center_vel_based(galen_robot) % Home the robot
%% Section 3.2
uppose = single([.35,.35,.35,0,0,0]); % Home the galen robot partially above the midpoints
galen_center_vel_based(galen_robot,uppose)
%% Section 3.3
galen_robot.get_current_joints(); % center the galen robot rotational joints
current_position = galen_robot.extract_galen_pose();
uppose = single([current_position(1),current_position(2),current_position(3),0,0,0]);
galen_center_vel_based(galen_robot,uppose)

%% Section 4 
% Admittance control loop

Appcmd=1; % Bool to keep the loop running
Executing_run = false; % Bool to check if the loop should stop running

while(Appcmd==1)

    galen_robot.set_control_type('Velocity');  % setup velocity control

    % Grab the current galen data
    current_velocity = galen_robot.get_current_joints();
    current_position = galen_robot.extract_galen_pose();

    % initialize all data storing vectors. Start at zero
    Vref = [0;0;0;0;0;0];
    Vref_prev = [0;0;0;0;0;0];
    Vref_prev_prev = [0;0;0;0;0;0];
    Vout = [0;0;0;0;0;0];
    Vout_prev = [0;0;0;0;0;0];
    Vout_prev_prev = [0;0;0;0;0;0];

    % caputre time and iteration history
    elapsed_time = [0];
    k = 1;

    % Initialize net and external forces
    Fnet_prev = [0;0;0;0;0;0];
    Fext = [0;0;0;0;0;0];
    Fext_prev = [0;0;0;0;0;0];
    Fext_prev_prev = [0;0;0;0;0;0];

    % Initialize iteratior count, cv - output data of robot, rv - desired input
    % for robot

    i=1;
    rext = [];
    rf = [];
    cv = [];
    rv = [];
    ri = [];
    rqd = [];
    cqd = [];
    pout = [];

    % Impedance From humnan h and envrionment e
    Impede = false; % Boolean to apply impedance
    mh = 0;
    bh = 0;
    kh = 401;
    ke = 0;%16599;


    % Initialize impedance force
    Fh = [0;0;0;0;0;0];
    Fhin = [0;0;0;0;0;0];
    Fh_prev_prev = [0;0;0;0;0;0];
    Fh_prev = [0;0;0;0;0;0];

    % Unstable Admittance for Env contact, stable for human
    % casestab = " (Unstable)";
    % b_ad = [50;50;20;50;50;0];
    % m_ad = [20;20;20;20;20;0];

    % Desired Admittance values Stable
    casestab = " (Stable)"; %stable if no simulated ke
    b_ad = [150;170;170;170;270;0];
    m_ad = [20;20;20;20;20;0];


    % Read the desired admittance values from the GUI
    appmads = [APP.Xm_adEditField.Value,APP.Ym_adEditField.Value,APP.Zm_adEditField.Value,APP.Wxm_adEditField.Value,APP.Wym_adEditField.Value];
    appbads = [APP.Xb_adEditField.Value,APP.Yb_adEditField.Value,APP.Zb_adEditField.Value,APP.Wxb_adEditField.Value,APP.Wyb_adEditField.Value];

    % Initialize a vector to capture the change in time
    time_vec = [];
    ref_cmd = [0,0,0,0,0];
    joy = [];
    desRate = 200;
    des_t = APP.RunTimeLengthsEditField.Value;  %simulate for this many seconds



    r = rosrate(desRate);% rate control object will hold the loop at 200Hz
    current_sim_time = 0; % initialize time at 0
    reset(r) % clear any previously stored rate statistics

    run_time_complete = false;

    wc = 5; % Filter cutt off frequency

    J = galen_robot.galen_jacob(); % grab the jacobian of the inital position

    while(run_time_complete==false)

        % Retrieve App info
        % Grab simulated impedance information
        mh = APP.mhkgEditField.Value;
        bh = APP.bhNsmEditField.Value;
        kh = APP.khNmEditField.Value;
        ke = APP.keNmEditField.Value;

        % Set the desired admittance gains
        for adval = 1:5
            m_ad(adval) = appmads(adval);
            b_ad(adval) = appbads(adval);
        end

        %Grab the current sampling point in time relative to total time
        current_sim_time = r.TotalElapsedTime;
        elapsed_time(i+1) = r.TotalElapsedTime;
        Ts = elapsed_time(i+1)-elapsed_time(i);

        % if the GUI impedance bool or gamepad button is selected then tell the robot to apply
        % the desired impedance values
        if strcmp(APP.ApplyImpedanceSwitch.Value,'On') || button(SwitchPro,1)==1
            Impede = true;
            APP.ImpedanceLamp.Color = [0 1 0];
        else
            Impede = false;
            APP.ApplyImpedanceSwitch.Value = 'Off';
            APP.ImpedanceLamp.Color = [0 0 0];
        end
        % if impedance is turned off get rid of the force impedance
        if Impede==false
            Fh = Fh*0;
        end


        %Read the game controller input
        joy(1,k) = -axis(SwitchPro,2);
        joy(2,k) = -axis(SwitchPro,1);
        L2 = button(SwitchPro,7);
        R2 = button(SwitchPro,8);
        if L2 == 0 && R2 == 0
            joy(3,k) = 0;
        elseif L2==1 && R2==1
            joy(3,k) = 0;
        elseif L2==1 && R2==0
            joy(3,k) = -0.6;
        elseif L2==0 && R2==1
            joy(3,k) = 0.6;
        end
        joy(4,k) = -axis(SwitchPro,3);
        joy(5,k) = axis(SwitchPro,4);

        % Convert the input to a referece command
        for stick_num=1:5
            joycmd = joy(stick_num,k);
            if joycmd>0.2 || joycmd<-0.2
                if abs(joycmd)>0.6
                    joycmd = 0.6*sign(joycmd);
                end
                if stick_num ==4
                    ref_cmd(stick_num) = ref_cmd(stick_num) + joycmd;
                    if abs(ref_cmd(stick_num))>50
                        ref_cmd(stick_num) = sign(ref_cmd(stick_num))*50;
                    end
                else
                    ref_cmd(stick_num) = ref_cmd(stick_num) + joycmd;
                    if abs(ref_cmd(stick_num))>20
                        ref_cmd(stick_num) = sign(ref_cmd(stick_num))*20;
                    end
                end
            else
                joycmd = 0;
                ref_cmd(stick_num) = joycmd;
            end
        end
        
        % Check the force control bool in the GUI. This tells it to either
        % read input from the gamepad or only force data from the GUI
        if strcmp(APP.ForceControlSwitch.Value,'Assign force by Gamepad')
            Fext= single([ref_cmd,0.0]'); % Assign game controller input to the force input
            % Update Input Force State
            APP.FxEditField.Value = double(Fext(1));
            APP.FyEditField.Value = double(Fext(2));
            APP.FzEditField.Value = double(Fext(3));
            APP.FWxEditField.Value = double(Fext(4));
            APP.FWyEditField.Value = double(Fext(5));
        else
            Fext(1) = single(APP.FxEditField.Value);
            Fext(2) = single(APP.FyEditField.Value);
            Fext(3) = single(APP.FzEditField.Value);
            Fext(4) = single(APP.FWxEditField.Value);
            Fext(5) = single(APP.FWyEditField.Value);
        end

        % Find net force, Input-impedance
        Fnet = Fext-Fh;

        % Record Velocity in history
        Vref_prev_prev = Vref_prev;
        Vref_prev = Vref;

        % Admittance controller tustin transform application
        for j=1:5
            Vref(j) = (-Vref_prev(j)*(b_ad(j)-((2/Ts)*m_ad(j))) + Fnet(j) + Fnet_prev(j))/(b_ad(j) + (2/Ts)*m_ad(j));
        end
        Vref(6)=0;
        qdot_ref = J\Vref;
        qdot_ref = single([qdot_ref(1);qdot_ref(2);qdot_ref(3);qdot_ref(4);0;qdot_ref(5)]);



        % Record Velocity out and force history
        Vout_prev_prev = Vout_prev;
        Vout_prev = Vout;

        Fnet_prev = Fnet;

        Fext_prev_prev = Fext_prev;
        Fext_prev = Fext;

        % Move the galen robot
        galen_robot.move_galen(qdot_ref);

        % Retrieve the current robot joints
        qdot_out = galen_robot.get_current_joints();
        J = galen_robot.galen_jacob();

        qdot_out =[qdot_out(1);qdot_out(2);qdot_out(3);qdot_out(4);qdot_out(6)];
        current_velocity = J*qdot_out;
        Vout = current_velocity;

        Fh_prev_prev = Fh_prev;
        Fh_prev = Fh;

        % Apply impedance if desired
        if Impede ==true
            % Impedance response Only takes in worst case scenario (Fe = ke + kh)
            for j=1:5
                Fh(j) = Vout(j)*((2*mh/Ts)+bh+((kh+ke)*Ts/2))+Vout_prev(j)*((-4*mh/Ts)+(Ts*(kh+kh)))+Vout_prev_prev(j)*((2*mh/Ts)-bh+((kh+ke)*Ts/2))+Fh_prev_prev(j);

            end
            Fh(6)= 0;
        else
            Fh = [0;0;0;0;0;0];
        end

        % Add the environment contact impedance
        if Impede==true
            Fe = -galen_robot.read_E35_wrench();
            Fh = Fh+Fe;
        end
        % Find the error between desired and output
        error = norm(current_velocity-qdot_ref);

        %Store input and output history
        rext(:,i) = Fext;
        rf(:,i) = Fnet;
        ri(:,i) = Fh;
        cv(:,i) = current_velocity;
        rv(:,i) = Vref;
        rqd(:,i) = qdot_ref;
        cqd(:,i) = qdot_out;
        pout(:,i) = galen_robot.extract_galen_pose();

        % Update App state
        APP.XEditField.Value = double(current_velocity(1));
        APP.YEditField.Value = double(current_velocity(2));
        APP.ZEditField.Value = double(current_velocity(3));
        APP.WxEditField.Value = double(current_velocity(4));
        APP.WyEditField.Value = double(current_velocity(5));


        % Increase iterator and record current time
        i = i+1;
        time_vec(i) = current_sim_time;



        % Check if the GUI wants to exit the loop to center the robot
        if strcmp(APP.CenterSwitch.Value,'On')== 1
            break;
        end
        % Check if the GUI wants to exit the loop to home the robot
        if strcmp(APP.HomeSwitch.Value,'On')== 1
            break;
        end
        % Check if the GUI wants to exit the to output the test results
        if strcmp(APP.StopAndOutputDataSwitch.Value,'On')== 1
            APP.StopAndOutputDataSwitch.Value = 'Off';
            break;
        end
        % Check if the GUI is running a time fixed test and if time is up
        if Executing_run==false
            if strcmp(APP.ExecuteRunSwitch.Value,'Go')==1
                Executing_run=true;
                APP.ExecuteRunSwitch.Value = 'Off';
                APP.CenterSwitch.Value = 'On';
                break;
            end
        else
            if r.TotalElapsedTime>des_t
                run_time_complete=true;
            end
        end

        % Hold the loop if the sampling time is under 200Hz
        waitfor(r);

    end

    % Output the resulting total loop time and quality of the sampling rate
    stats = statistics(r)
    disp(r)
    time_elapsed = r.TotalElapsedTime;
    Appcmd = 0; % Tell the loop not to start again

    % check if the robot should center itself and jump back into the loop
    if strcmp(APP.CenterSwitch.Value,'On') == 1
        galen_robot.get_current_joints();
        current_position = galen_robot.extract_galen_pose();
        CarAvg = (current_position(2)+current_position(2)+current_position(3))/3;
        uppose = single([CarAvg,CarAvg,CarAvg,0,0,0]);
        galen_center_vel_based(galen_robot,uppose)
        APP.CenterSwitch.Value = 'Off';
        Appcmd = 1;
    end
   % check if the robot should home itself and jump back into the loop
    if strcmp(APP.HomeSwitch.Value,'On') == 1
        galen_center_vel_based(galen_robot);
        APP.HomeSwitch.Value = 'Off';
        Appcmd = 1;
    end
end

% Section 5

% Plot the current values store in cv and rv. This should theoretically be
% the 3 different carriage joint values, and they should all be nearly
% identical if not for noise between sampling times and robot model
% innacuracies

t = time_vec(1:end-1); % not true seconds, Just a count for every existing sample.
% Update t above to be the true total loop time for the collected sample to
% get an accurate figure for the repsonse time

rv(1:3,:) = rv(1:3,:)*1000; % convert m to mm
cv(1:3,:) = cv(1:3,:)*1000; % convert m to mm


% plot all the cartesian velocities
close all
figure(1)
subplot(5,1,1)
hold on
plot(t,rv(1,:))
plot(t,cv(1,:))
legend('X Vref','X Vout')
ylabel('mm/s')

subplot(5,1,2)
hold on
plot(t,rv(2,:))
plot(t,cv(2,:))
legend('Y Vref','Y Vout')
ylabel('mm/s')

subplot(5,1,3)
hold on
plot(t,rv(3,:))
plot(t,cv(3,:))
legend('Z Vref','Z Vout')
ylabel('mm/s')

subplot(5,1,4)
hold on
plot(t,rv(4,:))
plot(t,cv(4,:))
legend('wx Vref','wx Vout')
ylabel('rad/s')

subplot(5,1,5)
hold on
plot(t,rv(5,:))
plot(t,cv(5,:))
legend('wy Vref','wy Vout')
xlabel('Time (s)')
ylabel('rad/s')


% plot the joint velocities

figure(2)
subplot(5,1,1)
hold on
plot(t,rqd(1,:))
plot(t,cqd(1,:))
legend('Car1 Vref','Car1 Vout')
ylabel('mm/s')

subplot(5,1,2)
hold on
plot(t,rqd(2,:))
plot(t,cqd(2,:))
legend('Car2 Vref','Car2 Vout')
ylabel('mm/s')

subplot(5,1,3)
hold on
plot(t,rqd(3,:))
plot(t,cqd(3,:))
legend('Car3 Vref','Car3 Vout')
ylabel('mm/s')

subplot(5,1,4)
hold on
plot(t,rqd(4,:))
plot(t,cqd(4,:))
legend('Roll Vref','Roll Vout')
ylabel('rad/s')

subplot(5,1,5)
hold on
plot(t,rqd(6,:))
plot(t,cqd(5,:))
legend('Tilt Vref','Tilt Vout')
xlabel('Time (s)')
ylabel('rad/s')

% plot the input force, impedance, and net force
figure(3)
for j=1:5
    subplot(5,1,j)
    hold on
    plot(elapsed_time(1:end-1),rext(j,:),'m')
    plot(elapsed_time(1:end-1),rf(j,:),'g')
    plot(elapsed_time(1:end-1),ri(j,:),'k')
    legend('Input Force: Fext','Net Force','Equiv Impedance')
    if j<4
        ylabel('F N')
    else
        ylabel('T Nm')
    end
    xlabel('Time s')
    madstring = string(m_ad(j));
    badstring = string(b_ad(j));
    switch(j)
        case 1
            ftyp = "X";

        case 2
            ftyp = "Y";

        case 3
            ftyp = "Z";

        case 4
            ftyp = "Wx";

        case 5
            ftyp = "Wy";

    end
    title(ftyp +" Interaction Forces with des m_{ad} = " + madstring +" des b_{ad} = " + badstring + casestab)
    if j==1
        subtitle('Sigmoid trajectory force of -10 N with Sim Object Contact and Worst Case Arm Impedance')
    end
end
mpout = pout*1000;

% Plot the output joint positions
figure(4)

subplot(5,1,1)
hold on
plot(t,mpout(1,:))
legend('Car1 Pout')
ylabel('mm/s')
title('Current Joint States')

subplot(5,1,2)
hold on
plot(t,mpout(2,:))
legend('Car2 Pout')
ylabel('mm/s')

subplot(5,1,3)
hold on
plot(t,mpout(3,:))
legend('Car3 Pout')
ylabel('mm/s')

subplot(5,1,4)
hold on
plot(t,pout(4,:))
legend('Roll Pout')
ylabel('rad/s')

subplot(5,1,5)
hold on
plot(t,pout(6,:))
legend('Tilt Pout')
xlabel('Time (s)')
ylabel('rad/s')

CartPout = [];
% convert the joint positions to cartesian positions
for k=1:length(pout(1,:))
    pfk = [pout(1:4,k);(pout(6,k))];
    TF = galen_robot.galen_fk(pfk);
    Eulout = rotm2eul(TF(1:3,1:3),'ZYX');
    Rx(k) = Eulout(3);
    Ry(k) = Eulout(2);
    Rz(k) = Eulout(1);
    CartPout(:,k) = [TF(1:3,4);Ry(k);Rx(k)];
end


CartPout(1:3,:) = CartPout(1:3,:)*1000;

% Plot the ouptut cartesian positions
figure(5)
subplot(5,1,1)
hold on
plot(t,CartPout(1,:))
legend('X Pout')
ylabel('mm/s')
title('Position of End effector relative to world frame')

subplot(5,1,2)
hold on
plot(t,CartPout(2,:))
legend('Y Pout')
ylabel('mm/s')

subplot(5,1,3)
hold on
plot(t,CartPout(3,:))
legend('Z Pout')
ylabel('mm/s')

subplot(5,1,4)
hold on
plot(t,CartPout(4,:))
legend('wx Pout')
ylabel('rad/s')

subplot(5,1,5)
hold on
plot(t,CartPout(5,:))
legend('wy Pout')
xlabel('Time (s)')
ylabel('rad/s')

%%

% Center the robot
% Will move the robot to the indicated pose in other_pose, or the home
% position if no input is given
% Input:
%   galen_robot - the robot object from galen_interface()
%   other_pose - desired centered pose
% Note: this function will not use any optimization of trajectories and
% will drive the galen hard towards its goal poses

function galen_center_vel_based(galen_robot,other_pose)

galen_robot.set_control_type('Velocity');  % setup velocity control
if nargin<2
    ref_pose = single([0.210,0.210,0.210,0.0,0.0,0.0]'); %Home pose
else
    ref_pose = other_pose'; %Home pose
end
galen_robot.get_current_joints();
current_position = galen_robot.extract_galen_pose();

% Initialize iteratior count, cv - output data of robot, rv - desired input
% for robot
i=1;
% Initialize a vector to capture the change in time
time_vec = [];


ref_velocity = single([0.0,0.0,0.0,0.0,0.0,0.0]');



r = rosrate(200);% rate control object will hold the loop at 200Hz
current_sim_time = 0; % initialize time at 0
reset(r) % clear any previously stored rate statistics

sendout = false;

while(sendout==false || i<50)
    %Grab the current sampling point in time relative to total time
    current_sim_time = r.TotalElapsedTime;


    for k = 1:6
        if k~=5
            if abs(current_position(k)-ref_pose(k))>0.002
                if current_position(k)>ref_pose(k)
                    ref_velocity(k) = ref_velocity(k)-single(0.002);
                elseif current_position(k)<ref_pose(k)
                    ref_velocity(k) = ref_velocity(k)+single(0.002);
                end
            else
                ref_velocity(k) = ref_velocity(k)/2;
            end
            if abs(ref_velocity(k))>0.05
                ref_velocity(k)= sign(ref_velocity(k))*0.05;
            end
        end
    end

    % Move the galen robot
    galen_robot.move_galen(ref_velocity);

    % Retrieve the current robot joints
    galen_robot.get_current_joints();
    current_position = galen_robot.extract_galen_pose();

    % Increase iterator and record current time
    i = i+1;

    norm(current_position-ref_pose);
    norm(ref_velocity);
    i;
    if norm(current_position-ref_pose)<0.003 && norm(ref_velocity)<0.003
        if sendout == false
            i = 0;
        end
        sendout = true;

    end

    % Hold the loop if the sampling time is under 200Hz
    waitfor(r);
    if i>700
        break
    end

end

end