% TOGAC15_MSD_ArvidAdmitConDesign.m

% This script attempts to use the description of an admittance controller
% with the values from on pages 1422 to 1431 of the Arvid Admittance
% control paper.
% This contains the Arvid Paper Replication results for the Ya admittance
% controller bode plot response

% Created by Brevin Banks
% Modified 3/7/2023

% Keemink AQ, van der Kooij H, Stienen AH.
% Admittance control for physical human–robot interaction. 
% The International Journal of Robotics Research. 2018;37(11):1421-1444. doi:10.1177/0278364918768950
% the following controller is based on the fundemantal controller in the
% paper highlighted above

% Controller parameters. mass, gains, stiffness, damping coefficents,
% controller values for PID
mv = 2;
kr = 1;
mr = 10;
mps = 2;
br = 5;
kp = 100;
ki = 2000;

% Initial force
Fext = 1;
Fdist = 0;

% Transfer function formulations
s = tf('s');
Yv = tf([1],[mv 0]); % Addmitance controller tf
Yr = tf([1],[mr br]); % dynamics tf
Zps = tf([mps 0],[1]); % Impedance from robot inertia
C = kr*tf([kp ki],[1 0]); % Controller tf
Yabar = (Yr*(1 + C*Yv*kr))/(Yr*(Zps + C*(kr + Yv*Zps*kr)) + 1); % Closed loop tf from Fext to V
Yapi = tf([1],[mr 0]);

% Plot of the results
close all
figure('name','Bodes')
hold on
bode(Yv,'r',Yr,'g',Yabar,'b',Yapi,'b--',{10^-2,10^2})

legend('$Yv$','$Y{r}$','$\bar{Y}{\alpha}$','$^{\pi}Y{\alpha}$','Interpreter','Latex')
title({'\bf Bode Plots for Admittance Controller';'\rm Yv is the admittance controller. $\bar{Y}{\alpha}$ is the apperent CL dynamics';'$^{\pi}Y{\alpha}$ is the passive dynamics (inertia only). $Y{r}$ is the System Dynamics'},'Interpreter','Latex')