% TOGAC44_AMBF_CartesianAdmittanceTest.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. 

% 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
% control.
% After running section 1, you may run any of the section 2 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 3 to control the robot

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

%% Section 1
galen_robot = galen_interface();  % initiate the robot interface

%%% Section 2
%% Section 2.1
galen_center_vel_based(galen_robot) % Home the robot
%% Section 2.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 2.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 3
% Assign A Step Force Input With Admittanc Control
galen_robot.set_control_type('Velocity');  % setup velocity control

% Step Input
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 = true; % 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];

% b_ad = [150;155;165;150;350;0];
% m_ad = [20;20;20;20;20;0];


% b_ad = [600;600;600;600;600;0];
% m_ad = [40;40;40;40;40;0];

% Initialize a vector to capture the change in time
time_vec = [];

desRate = 200;
des_t = 10;  %simulate for this many seconds
t_sig = 2.5; % Sigmoid rise time
traj = linspace(0,t_sig,t_sig*2*desRate);
traj2 = linspace(t_sig,des_t,(des_t-t_sig)*2*desRate);
traj_time = [traj traj2];
sig1 = 1./(1+exp(-traj*6+6));
sig2 = ones(1,length(traj2));
amplitude = -10;
sig = [sig1*amplitude sig2*amplitude];


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


wc = 5; % Filter cutt off frequency
J = galen_robot.galen_jacob();
while(current_sim_time<des_t)
    %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);

    zext = traj_interp(current_sim_time,sig,traj_time);

    Fext = single([0.0,0.0, zext,0.0,0.0,0.0]'); % Step input reference 


    % 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

    % 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();
    % Increase iterator and record current time
    i = i+1;
    time_vec(i) = current_sim_time;

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



%% Section 4
% 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



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

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


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

    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>7000
        break
    end

end

end