% TOGAC32_AMBF_AdmittanceTest.m
% This file uses the galen_interface.m class to initiate the ROS nodes,
% pubs, and subs needed to control the Galen Robot AMBF model.
% These admittance tests contains the application of simulated human
% impedance in the worst case (kh = 401) and the simulated environment impedance
% (ke = 16599) and also the case where the environment stiffness is
% actually caused by physical contacts in the ambf simulated space
% The response is observed in the AMBF simulation and figures are output to
% show the activity of the robot for the given time interval
% To use galen_interface, ensure ROS is running by running roscore in a terminal and then
% open the ambf model in a separate terminal
% Instructions on how to install the AMBF application and how to use the
% robot with ambf are found in:

% TOGAC25_AMBF_IN_AMBFforGalen.pdf

% Instructions for using matalb with ambf are found in:

% TOGAC29_AMBF_IN_ConnectMatlab2AMBF.pdf

% Created by Brevin Banks
% Modified April 1 2023


galen_robot = galen_interface(); % initiate the robot interface

%%
center_galen_robot(galen_robot) % Center the robot at 0.21m if necessary
% the above command only works upon the AMBF environment creation. For some
% unknown reason the AMBF simulation struggles with switching between
% velocity and position control. To use this function again, restart the
% AMBF application. It may also be necessary to turn of gravity to keep the
% robot in the center after running the command.

%%

galen_center_vel_based(galen_robot)
%%
tool_place_galen_robot(galen_robot) % To zero off the robot at its current pose (stop oscilations) if necessary
% the above command only works upon the AMBF environment creation. For some
% unknown reason the AMBF simulation struggles with switching between
% velocity and position control. To use this function again, restart the
% AMBF application. It may also be necessary to turn of gravity to keep the
% robot at the desired pose after running the command.

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

% Initialize in and out Velocity and history
Vref = 0;
Vref_prev = 0;
Vref_prev_prev = 0;
Vout = 0;
Vout_prev = 0;
Vout_prev_prev = 0;

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

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

% Initialize history variables
rext = [];
rf = [];
ri = [];
rv = [];
cv = [];


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

% Initialize impedance force
Fh = 0;


% Desired Admittance values
b_ad = 450;
m_ad = 20;

% rate control object will hold the loop at 200Hz
r = rateControl(200);


while(elapsed_time<10)

    % Observe the time progression
    elapsed_time(k+1) = r.TotalElapsedTime;
    Ts = elapsed_time(k+1)-elapsed_time(k);

    % Input types, step or sin
    Fext= 5; %*sin(elapsed_time(k+1));
    % 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
    Vref = (-Vref_prev*(b_ad-((2/Ts)*m_ad)) + Fnet + Fnet_prev)/(b_ad + (2/Ts)*m_ad);



    % 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;

    % Ensure velocity is single type and apply to three carriage joints
    ref_velocity = single([Vref,Vref,Vref,0,0,0]);
    galen_robot.move_galen(ref_velocity);

    % Grab galen join variables
    current_velocity = galen_robot.get_current_joints();
    Vout = current_velocity;

    Fh_prev = Fh;
    % Apply impedance if desired
    if Impede ==true
        % Impedance response Only takes in worst case scenario (Fe = ke + kh)
        Fh = Vout(1)*1.002+Vout_prev(1)*1.002+Fh_prev;

    else
        Fh = 0;
    end

    % Record the history of inputs and outputs for a single carriage
    rext(k) = Fext;
    rf(k) = Fnet;
    ri(k) = Fh;
    rv(k) = Vref;
    cv(k) = Vout(1);

    k = k+1;
    numMisses = waitfor(r); % Hold the loop if it's faster than 200Hz
end

% Display the corrected sample rate and error in desired sampling time
disp(r)
stats = statistics(r)

%%
% Plot the figures for applied desired admittance values
if ishandle(2)
    close 2
end

figure(2)
subplot(2,1,1)
hold on
plot(elapsed_time(1:end-1),rext,'m')
plot(elapsed_time(1:end-1),rf,'g')
plot(elapsed_time(1:end-1),ri,'k')
legend('Input Force: Fext','Net Force','Equiv Impedance')
ylabel('Force N')
xlabel('Time s')
title('Interaction Forces with des m_{ad} = 20   des b_{ad} = 15 (Unstable)')
subtitle('Step force of -5 N with Feq = Vout*(401+env)/s (Real Contact Case)')
subplot(2,1,2)
hold on
plot(elapsed_time(1:end-1),rv,'b')
plot(elapsed_time(1:end-1),cv,'r')
legend('Vref','Vout')
ylabel('Velocity m/s')
xlabel({'Time s','Contact Case (mh=0,bh=0,kh=401,with sim ke). Data captured at 200 Hz and Unstable'})
title('Velocity of Carriage 1 through 3')






%Center the robot
% This command only works upon the AMBF environment creation. For some
% unknown reason the AMBF simulation struggles with switching between
% velocity and position control. To use this function again, restart the
% AMBF application. It may also be necessary to turn of gravity to keep the
% robot in the center after running the command.
% Input:
%   galen_robot - the robot object from galen_interface()
function center_galen_robot(galen_robot)
galen_robot.set_control_type('Position'); % set up position control

error = 1; % initialize error variable
steadier = 0; % observe if the change in position is small
const_err = 0; % Observe if the error is not changing
error_prev = 0; % capture error history
start_pose = single([0.21, 0.21, 0.21, 0 ,0 ,0]); % home position
current_pose = galen_robot.get_current_joints();
d_pose = 1;
while(error>0.07 || steadier< 5) % Check that the robot has achieved the desired pose
    prev_pose = current_pose;
    galen_robot.move_galen(start_pose);
    pause(1/200); % keep sampling frequency below 200Hz
    current_pose = galen_robot.get_current_joints();
    error = norm(start_pose(1:3) - current_pose(1:3)); %calculate error
    d_pose = mean(current_pose(1:3)-prev_pose(1:3)); % check that the pose is changing
    if abs(d_pose) < 0.0001 % if the robot stops moving over time, iterate to kill the process
        steadier = steadier+1;
    else
        steadier = 0;
    end
    if abs(mean(error-error_prev))<0.001% if the error stops changin over time, iterate to kill the process
        const_err = const_err+1;
    else
        const_err = 0;
    end
    if const_err>10
        break;
    end
    error_prev = error;
end
end




% Hold the robot
% To zero off the robot at its current pose (stop oscilations) if necessary
% this command only works upon the AMBF environment creation. For some
% unknown reason the AMBF simulation struggles with switching between
% velocity and position control. To use this function again, restart the
% AMBF application. It may also be necessary to turn of gravity to keep the
% robot at the desired pose after running the command.
% Input:
%   galen_robot - the robot object from galen_interface()
function tool_place_galen_robot(galen_robot)
galen_robot.set_control_type('Position');% set up position control

error = 1; % initialize error variable
steadier = 0;% observe if the change in position is small
const_err = 0;% Observe if the error is not changing
error_prev = 0;% capture error history
current_pose = galen_robot.get_current_joints();
start_pose = single([current_pose(1), current_pose(2), current_pose(3), 0 ,0 ,0]);  % desired hold positon

d_pose = 1;
while(error>0.07 || steadier< 5) % Check that the robot has achieved the desired pose
    prev_pose = current_pose;
    galen_robot.move_galen(start_pose);
    pause(1/200); % keep sampling frequency below 200Hz
    current_pose = galen_robot.get_current_joints();
    error = norm(start_pose(1:3) - current_pose(1:3));%calculate error
    d_pose = mean(current_pose(1:3)-prev_pose(1:3));% check that the pose is changing
    if abs(d_pose) < 0.0001% if the robot stops moving over time, iterate to kill the process
        steadier = steadier+1;
    else
        steadier = 0;
    end
    if abs(mean(error-error_prev))<0.001% if the error stops changin over time, iterate to kill the process
        const_err = const_err+1;
    else
        const_err = 0;
    end
    if const_err>10
        break;
    end
    error_prev = error;
end
end




function galen_center_vel_based(galen_robot)

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


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_pose = single([0.210,0.210,0.210,0.0,0.0,0.0]'); %Home pose
    
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.001
                if current_position(k)>ref_pose(k)
                ref_velocity(k) = ref_velocity(k)-single(0.001);
                elseif current_position(k)<ref_pose(k)
                ref_velocity(k) = ref_velocity(k)+single(0.001);
                end
            else
                ref_velocity(k) = ref_velocity(k)/5;
            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;
    

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

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

end