%% *** Robot (kinematic) model parameters ***


clear all;
close all;



%==================================================
% Part 1: Definitions   
%==================================================
%
%==================================================
%
% Part 1-A  :Robot Parameters
%
%==================================================

link(1) = 10.0;      %Link 1 lenght %% in cm
link(2) = 10.0;      %Link 2 lenght 

l(1) = link(1); 
l(2) = link(2);

lc(1) = l(1)/2;
lc(2) = l(2)/2;

m1 = 1.0;		 % 10.0, defining the mass for link 1	
m2 = 1.0;      % 10.0, defining the mass for link 2

I_1 = m1*(l(1)^2)/12;    %defining the inertia for link 1
I_2 = m2*(l(2)^2)/12;    %defining the inertia for link 2

%==================================================


%==================================================
%
% Part 1-B  : Desired Task / Motion Profile
%
%==================================================

%% *** sampling period ***
%% *** for the robot motion, kinematic simulation:
dt = 0.001; %dt = 0.001; i.e. 1 msec) 

%% *** Create (or load from file) reference signals ***
%% *** DESIRED MOTION PROFILE - TASK SPACE ***
Tf=2.0; 	% 10sec duration of motion
t=0:dt:Tf;

%xd0,td0,yd1: initial/final end-point position --> desired task-space trajectory (STRAIGHT LINE Pd0 --> Pd1)
xd0 = 15.0;	xd1 =  15.0;
yd0 = 4.00; yd1 = -4.00;

% Example of desired trajectory : linear segment (x0,y0)-->(x1,y1); Time duration: Tf;
disp('Initialising Desired Task-Space Trajectory (Motion Profile) ...'); %%
disp(' '); 

xd(1) = xd0;
yd(1) = yd0;

lambda_x = (xd1-xd0)/Tf;
lambda_y = (yd1-yd0)/Tf;
kmax=Tf/dt + 1;

for k=2:kmax;
   xd(k) = xd(k-1) + lambda_x*dt;
   yd(k) = yd(k-1) + lambda_y*dt;
end

xd(1) = xd0;
yd(1) = yd0;
lambda_x = (xd1-xd0)/Tf;
lambda_y = (yd1-yd0)/Tf;
kmax=Tf/dt + 1;

for k=2:kmax;
   xd(k) = xd(k-1) + lambda_x*dt;
   yd(k) = yd(k-1) + lambda_y*dt;
end
pd(:,1) = xd';
pd(:,2) = yd';
%==================================================

%===============================================
%  Obstacle Obst --> External Force Fe
%  [x_obst(1),y_obst(1)]  <---> [x_obst(2),y_obst(2)]
%                (angle: th_obst)
%===============================================
   dx_obst = 10; 
   dx_obst2 = dx_obst/2;
   th_obst = pi/4;
   tan_th_obst = tan(th_obst);
   sin_th_obst = sin(th_obst);
   cos_th_obst = cos(th_obst);
   la = tan_th_obst;
   la2 = la^2;
   y_obst(1) = -dx_obst2*tan_th_obst;		
   x_obst(1) = xd0 - dx_obst2;
   x_obst(2) = xd0 + dx_obst2;
   y_obst(2) = -y_obst(1); 
   
   Ke=90000;
%===============================================
%===============================================
   
   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%================================================================   
%================================================================   
%% ****** KINEMATIC SIMULATION - Main loop ******
disp('Kinematic Simulation ...'); %%
disp(' '); %%

%% ***** INVESRE KINEMATICS  -->  DESIRED MOTION - JOINT SPACE *****
%% compute the reference joint-motion vectors:
%% {qd(k,i), i=1,...,n (num of degrees of freedom), with k=1,..., kmax,}
%% and reference joint (angular) velocities {qd_1(k,i)}
rd2 = xd(:).^2 + yd(:).^2;
%%qd(:,2) = acos( (rd2(:)-l(1)^2-l(2)^2)./(2*l(1)*l(2)) ); %% 1st solution: elbow down
qd(:,2) = -acos( (rd2(:)-l(1)^2-l(2)^2)./(2*l(1)*l(2)) ); %% 2nd solution: elbow up
s2 = sin(qd(:,2));
qd(:,1) = atan2(yd(:),xd(:)) - asin(l(2)*s2(:)./sqrt(rd2(:)));



%% ***** FORWARD KINEMATICS  JOINT MOTION -->  CARTESIAN POSITIONS *****
%%(xd1, yd1) : cartesian position of the 1st link's local reference frame
xd1 = l(1)*cos(qd(:,1));  
yd1 = l(1)*sin(qd(:,1));

%%(xd2, yd2) : cartesian position of the 2nd link's local reference frame
xd2 = l(1)*cos( qd(:,1) ) + l(2)*cos( qd(:,1)+qd(:,2) );  
yd2 = l(1)*sin( qd(:,1) ) + l(2)*sin( qd(:,1)+qd(:,2) );


%%%%%%%%%%%%%%%%%%%%%%%%%

tt=0; tk=1;
%qd(tk,1)=qd1; qd(tk,2)=qd2; qd(tk,3)=qd3;

%%% q_(tk,1,2): current joint positions (joint 1,2) at time tk
q_(1,1) = qd(1,1);
q_(1,2) = qd(1,2);
%%% dq_(:,1,2): current joint velocities (joint 1,2) at time tk
dq_(1,1) = 0;
dq_(1,2) = 0;

%================================================
% %%%% Main Dynamic Simulation Loop %%%%%
while (tt<=Tf)  %%%% Main Dynamic Simulation Loop
   
   tt
   
   c1 = cos(q_(tk,1));
   c2 = cos(q_(tk,2));
   c12 = cos(q_(tk,1)+q_(tk,2));
   s1 = sin(q_(tk,1));
   s2 = sin(q_(tk,2));
   s12 = sin(q_(tk,1)+q_(tk,2));
   
   %===============================================
   % Robot Dynamic Model - Joint space Q
   % T = M(Q)ddQ + B + G(Q) - Jt*Fe
   %===============================================
   %% Gravity Terms G[:]
   g2 = m2*lc(2)*c12;
   g1 = g2 + m1*lc(1)*c1 + m2*l(1)*c1;
   G = [g1;g2]*9.84;

   %% Robot Inertia Matrix M[:,:]
   M22 = I_2 + m2*lc(2)^2;
   M12 = M22 + m2*(l(1)*lc(2)*c2); %%=M21
   M21 = M12;
   M11 = M22 + I_1 + m1*lc(1)^2 + m2*( l(1)^2 + 2*m2*l(1)*lc(2)*c2 );
	M = [M11,M12;M21,M22];
   
   %% Robot dynamic model terms B[:,:]
   h_112 = -m2*l(1)*lc(2)*s2; %=h_122=-h_211
   h_122= h_112;
   h_211 = -h_122;
	B_2 = h_211*dq_(tk,1)*dq_(tk,1);
   B_1 = ( 2*h_112*dq_(tk,1) + h_122*dq_(tk,2) )*dq_(tk,2);
   B = [B_1;B_2];
   %=========================================
   
   %========================================
   %% Compute Jacobian
	Jac(1,2) = -l(2)*s12;
   Jac(1,1) = Jac(1,2) - l(1)*s1;
	Jac(2,2) = l(2)*c12;
   Jac(2,1) = Jac(2,2) + l(1)*c1;
   
   dJac(1,2) = - l(2)*c12*(dq_(tk,1) + dq_(tk,2));
   dJac(1,1) = dJac(1,2) - l(1)*c1*dq_(tk,1);
   dJac(2,2) = - l(2)*s12*(dq_(tk,1) + dq_(tk,2));
   dJac(2,1) = dJac(2,2) - l(1)*s1*dq_(tk,1);
   %=========================================
   
   %========================================
   %% ***** FORWARD KINEMATICS: ACTUAL JOINT MOTION -->  ACTUAL CARTESIAN POSITIONS *****
	%%(x_1, y_1) : actual cartesian position of the 1st link's local reference frame
	x_1(tk) = l(1)*cos(q_(tk,1));  
	y_1(tk) = l(1)*sin(q_(tk,1));

	%%(x_2, y_2) : actual cartesian position of the 2nd link's local reference frame
	x_2(tk) = l(1)*cos( q_(tk,1) ) + l(2)*cos( q_(tk,1)+q_(tk,2) );  
   y_2(tk) = l(1)*sin( q_(tk,1) ) + l(2)*sin( q_(tk,1)+q_(tk,2) );
   
   %% Current end-effector position vector
   p_ = [x_2(tk);y_2(tk)];
   %% Current end-effector velocity vector
   dp_ = Jac*dq_(tk,:)';
   %========================================
   
   %===============================================
   % Robot Dynamic Model parameters - Task space P
   % Fa = Mt(q)*ddp + ht  - Fe
   % T = Jac'*Fa
   %===============================================
   M_inv = inv(M);
   Jac_Minv = Jac*M_inv;
   Mt = inv( Jac_Minv*Jac' );
   ht = (Mt*Jac_Minv)*(B+G) + Mt*dJac*dq_(tk,:)';
   %===============================================
   
   %===============================================
   %  Obstacle Obst --> External Force Fe
   %  x_obst(1),y_obst(1)  <---> x_obst(2),y_obst(2)
   %===============================================
   dp_obst_x = x_2(tk)- x_obst(1);
   dp_obst_y = y_2(tk)- y_obst(1);

   if ( tan_th_obst*dp_obst_x > dp_obst_y )
      %contact point
      %xc = ( x_2(tk) + la*( y_2(tk)- y_obst(1) ) + la2*x_obst(1) )/(1+la2);
      %yc = ( y_obst(1) + la*( x_2(tk)- x_obst(1) ) + la2*y_2(tk) )/(1+la2);
      %dxc = (xc-x_2(tk));
      %dyc = (yc-y_2(tk));
      
      dpc_proj = dp_obst_x*cos_th_obst + dp_obst_y*sin_th_obst;
      dxc = dpc_proj*cos_th_obst;
      dyc = dpc_proj*sin_th_obst;
      d_obst = sqrt( dxc^2 + dyc^2 );
      xc(tk) = x_obst(1) + dxc;
      yc(tk) = y_obst(1) + dyc;
      
      nc_x = xc(tk) - x_2(tk);  %% "penetration" normal to the contact point of the obstacle
      nc_y = yc(tk) - y_2(tk);
      
      Fe(tk,1) = Ke*nc_x; %Fex
      Fe(tk,2) = Ke*nc_y; %Fey
    else
      Fe(tk,1) = 0; %Fex
      Fe(tk,2) = 0; %Fey
    end
    %=================================================
    
    
   %===============================================
   % T = .... Control law
   %===============================================
   
   % Desired Impedance parameters,
   % inertia, damping and stiffness matrices: Md, Bd, Kd
   Md(1,1) = 1.0;
   Md(2,2) = 1.0;
   Md_inv(1,1) = 1/Md(1,1);
   Md_inv(2,2) = 1/Md(2,2);
   
   K1=50;	%eigenfrequency: omega_0
   Kd(1,1) = Md(1,1)*(K1^2);
	Kd(2,2) = Kd(1,1);

	Bd(1,1) = 2*1.5*K1*Md(1,1);  %damping coefficient: zeta=1.5
   Bd(2,2) = Bd(1,1);
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   
   % Calculating the errors of the controller 
   % regarding velocity/position
   % at the TASK SPACE 
   err_p(tk,1) =  pd(tk,1) -  p_(1);
   err_p(tk,2) =  pd(tk,2) -  p_(2);
    
   dpd(tk,1)=0;	dpd(tk,2)=0;   
   err_dp(tk,1) =  dpd(tk,1) -  dp_(1);
   err_dp(tk,2) =  dpd(tk,2) -  dp_(2);
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   
   %%% Control signal u
   Fd = [0;0];
   u(:,tk) = Md_inv*( Kd*err_p(tk,:)' + Bd*err_dp(tk,:)' - (Fd - Fe(tk,:)') );
    
   %%% Control law
   Fa = Mt*u(:,tk) + ht - Fe(tk,:)';
   T = Jac'*Fa ; 
   Tau(:,tk) = T;
   %===============================================
   %===============================================

   
   %===============================================
	% T = M(Q)ddQ + B + G(Q) - Jt*Fe
	%===============================================
   % %%%%%%%%%%% DYNAMIC SIMULATION %%%%%%%%%%%%% %
   %===============================================
	% ddQ = inv(M(Q))*[T  - B - G(Q) + Jt*Fe]
	%===============================================

	DDQ = inv(M)*(T  - B - G + Jac'*Fe(tk,:)'); %%*****************************
   
   tt=tt+dt;
   if (tt<=Tf)
      q_(tk+1,1) =  q_(tk,1) + dq_(tk,1)*dt;
      q_(tk+1,2) =  q_(tk,2) + dq_(tk,2)*dt;
      
      dq_(tk+1,1) = dq_(tk,1) + DDQ(1,1)*dt;
      dq_(tk+1,2) = dq_(tk,2) + DDQ(2,1)*dt;
   end
   
	%%%%%%%%%%%%%%%%%%%%%%%%%%

   tk=tk+1;
   
end
% %%%% End of Main Dynamic Simulation Loop %%%%
%================================================




%% *** SAVE and PLOT output data ***
%%** use functions plot(...)
save;  %% --> save data to 'matlab.mat' file

close all
fig1 = figure;
subplot(2,2,1);
plot(t,xd,':');
hold on;
plot(t,x_2);
ylabel('x, xd (cm)');
xlabel('time t (sec)');

subplot(2,2,2);
plot(t,yd,':');
hold on;
plot(t,y_2);
ylabel('y, yd (cm)');
xlabel('time t (sec)');

subplot(2,2,3);
plot(t,qd(:,1),':');
hold on;
plot(t,q_(:,1));
ylabel('q1 (rad)');
xlabel('time t (sec)');

subplot(2,2,4);
plot(t,qd(:,2),':');
hold on;
plot(t,q_(:,2));
ylabel('qd2 (rad)');
xlabel('time t (sec)');


fig3 = figure;

%subplot(2,1,1)
%axis([13 16 -1.5 1.5])
%hold on
%plot(xc,yc);
%ylabel('xc,yc (grf)');
%xlabel('time t (sec)');

%subplot(2,1,2)
plot(t,sqrt(Fe(:,1).^2+Fe(:,2).^2),'r');
hold on;
plot(t,Fe(:,2),'b');
plot(t,Fe(:,1),'g');
ylabel('Fex,Fey (grf)');
xlabel('time t (sec)');


%%*** stick diagram --> animate robot motion ... (**optional**)
%% within a for (or while) loop, use periodic plot(...) functions to draw the geometry (current pos) 
%% of the robot, and thus animate its motion ...

fig2 = figure;
axis([-1 21 -11 11]) %%set xy plot axes (caution: square axes, i.e. dx=dy)
axis on
hold on

xlabel('x (cm)');
ylabel('y (cm)');

plot(x_obst,y_obst,'rs');
fill([x_obst(1),x_obst(2),x_obst(2)], [y_obst(1),y_obst(1),y_obst(2)], [1.0,0.8,0.6]); %% obstacle

plot(xd,yd,'r'); %% desired robot position

dtk=(Tf/10)*1000; %% plot robot position every dtk samples, to animate its motion
plot([0],[0],'o');
for tk=1:dtk:kmax,
   %%%
	pause(0.1);	%% pause motion to view successive robot configurations
   plot([0,x_1(tk)],[0,y_1(tk)]);					plot([x_1(tk)],[y_1(tk)],'o');
   plot([x_1(tk),x_2(tk)],[y_1(tk),y_2(tk)]);	plot([x_2(tk)],[y_2(tk)],'b*');
   plot([xd(tk)],[yd(tk)],'r+');

end

