% TOGAC28_AMBF_Matlab2AMBFControl_tests.m
% This file will run basic control inputs on the galen robot for the ambf
% simulation. This contects matlab with ambf using the galen_interface
% class. 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

% 3 main control types are avaliable. Chirp, Linear Trajectory, Sine, and Step inputs can be
% used. After the control loop sections, you can plot rv and cv, the output
% results of the 3 carraige joints on the galen robot. The control here is
% currently designed only to output the desired velocity for the 3 carriage
% joints, lumping the galen robot as a single mass. The carriage joints should have
% identical response.

% Created by Brevin Banks
% Modified March 29 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.

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

% Initialize the error for iteration
error = 1;

%Generate Chirp
sim_time_window = (0:1/200:50); % 50 second window with 200Hz sampling
f0 =   0.001; % Starting frequency 1000 Hz
f1 = 1; % ending frequency 1 Hz
Fs = 1/mean(diff(sim_time_window));
x = chirp(sim_time_window,f0,sim_time_window(end),f1,'linear',-90)*.05; % generate chirp magnitude 0.05

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

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

while(current_sim_time<51) % for ~50 seconds run the loop
    %Grab the current sampling point in time relative to total time
    current_sim_time = r.TotalElapsedTime;

    % find the closest point to time in the chirp function
    [ d, ix ] = min( abs( sim_time_window-current_sim_time) );
    if ix <2
        ix =2; % can't interpolate starter position without more than 2 samples
    elseif ix>10000
        ix = 10000; % can't interpolate end position without if more than 10000 samples
    end
    %interpolate to get the value of the chirp function
    Cinput = x(ix-1)+(current_sim_time-sim_time_window(ix-1))*(((x(ix+1) - x(ix-1))/(sim_time_window(ix+1) - sim_time_window(ix-1))));
    if current_sim_time>50 % can't interpolate at the end conditions. Just assign 0
        Cinput = 0;
    end

    % Assign the desired velocity for 3 carriage joints
    ref_velocity = single([Cinput,Cinput,Cinput,0,0,0]);

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

    % Retrieve the current robot joints
    current_velocity = galen_robot.get_current_joints();
    % Find the error between desired and output
    error = norm(current_velocity-ref_velocity);

    %Store input and output history
    cv(1:3,i) = current_velocity(1:3);
    rv(1:3,i) = ref_velocity(1:3);

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

% Plot the shape of the Velocity input response
figure(2)
plot(sim_time_window,x)
title('Chirp Input on all 3 carriage joints')
xlabel('Time (s)')
ylabel('Velocity input (m/s)')


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

% Trajectory Input
current_velocity = galen_robot.get_current_joints();
% Create a trajectory rather than applying a specifc reference
ref_velocity = [.05,.05,.05,0,0,0]'; % desired velocity
des_traj = lin_trajectory(current_velocity,ref_velocity);

% Initialize iteratior count, cv - output data of robot, rv - desired input
% for robot
i=1;
cv = [];
rv = [];

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

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

for k = 1:length(des_traj)

    % Move the galen robot according to the desired trajectory
    galen_robot.move_galen(des_traj(:,k));

    % Store the the current velocity
    cv(1:3,i) = current_velocity(1:3);

    % Grab the current velocity
    current_velocity = galen_robot.get_current_joints();

    % Find the error between desired and output
    error = norm(current_velocity-ref_velocity);

    %Store input velocity history
    rv(1:3,i) = ref_velocity(1:3);

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

%% Assign A Step Input

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

% Step Input
current_velocity = galen_robot.get_current_joints();


% Initialize iteratior count, cv - output data of robot, rv - desired input
% for robot
i=1;
cv = [];
rv = [];

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

ref_velocity = single([.05,.05,.05,0,0,0]'); % Step input reference 0.05 magnitude
    
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

% Initialize iteratior count, cv - output data of robot, rv - desired input
% for robot
i=1;
cv = [];
rv = [];

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

ref_velocity = single([.05,.05,.05,0,0,0]'); % Step input reference 0.05 magnitude
    
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


while(current_sim_time<51)
    %Grab the current sampling point in time relative to total time
    current_sim_time = r.TotalElapsedTime;

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

    % Retrieve the current robot joints
    current_velocity = galen_robot.get_current_joints();
    % Find the error between desired and output
    error = norm(current_velocity-ref_velocity);

    %Store input and output history
    cv(1:3,i) = current_velocity(1:3);
    rv(1:3,i) = ref_velocity(1:3);

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


%% Apply Sine input on the robot

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

t = 0:pi/36:2*pi;%discretized input to have a period over 2pi separated by 36 parts
timer = 1;% Initialize starter time variable

r = rosrate(200);
current_sim_time =0;
reset(r)

while(current_sim_time<51)
    if timer>length(t)-1
        timer = 1;
    end
    upsign = 0.01*sin(t(timer));
    timer = 1 + timer;
    ref_velocity = single([upsign,upsign,upsign,0,0,0]');
     % Move the galen robot
    galen_robot.move_galen(ref_velocity);

    % Retrieve the current robot joints
    current_velocity = galen_robot.get_current_joints();
    % Find the error between desired and output
    error = norm(current_velocity-ref_velocity);

    %Store input and output history
    cv(1:3,i) = current_velocity(1:3);
    rv(1:3,i) = ref_velocity(1:3);

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



%%

% 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 = 1:length(cv); % 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

close all
figure(1)
subplot(3,1,1)
hold on
plot(t,rv(1,:))
plot(t,cv(1,:))
legend('Car1 Vref','Car1 Vout')


subplot(3,1,2)
hold on
plot(t,rv(2,:))
plot(t,cv(2,:))
legend('Car2 Vref','Car2 Vout')


subplot(3,1,3)
hold on
plot(t,rv(3,:))
plot(t,cv(3,:))
legend('Car3 Vref','Car3 Vout')



%%


% Generate a trajectory from a given initial joint angle set to the goal
% joint angle set. The trajectory is a straight line between the 2 points
% in joint space. This function takes the two joint sets and creates the
% joint trajectory by parameterizing a line between the two points and
% discreteizes the distance between them by steps of 1/200.
% Input:
%   q_init - a 6x1 vector of desired joint states
%   q_goal - a 6x1 vector of desired joint states
% Output:
%   traj - a vector of joint values to give the robot for each time step 1/200
function traj = lin_trajectory(q_init,q_goal)
T = 1/200;
q_goal = reshape(q_goal,size(q_init));

lambda = round(norm(q_goal-q_init)/(2*T));
if lambda < 10
    lambda = 10;
end
if lambda > 200
    lambda = 200;
end
m = (q_goal - q_init)./lambda;
traj = zeros(length(q_init),lambda);
for t=1:lambda
    traj(:,t) = m.*t + q_init;
end
traj = [q_init, traj, q_goal];
end


% 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