%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Control of Redundant manipulator (planar 3 dof) %%%%
close all;
clear all;
%clear q*; clear x*; clear y*; clear H*;

kc = 20;  %%gain for control of redundant degrees of freedom (joints' null space)
%kc= 0;   %%no consideration of redundacies for collision avoidance
%kc= 20;


l(1) = 1.0;  %% in dm
l(2) = 1.0;
l(3) = 0.3;


%% *** 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=1.0; 	%  duration of motion (in secs)
t=0:dt:Tf;


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



%%Initial configuration
qd1 = 20*pi/180; qd2 = 30*pi/180; qd3 = 20*pi/180;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%1st subtask: desired trajectory
%xd0,yd0, xd1, yd1: initial/final end-point position
s1 = sin(qd1); s12 = sin(qd1+qd2); s123 = sin(qd1+qd2+qd3);
c1 = cos(qd1); c12 = cos(qd1+qd2); c123 = cos(qd1+qd2+qd3);
xd0=l(1)*c1+l(2)*c12+l(3)*c123;
yd0=l(1)*s1+l(2)*s12+l(3)*s123;

xd1=xd0;
yd1=0;

lambda_x = (xd1-xd0)/Tf;
lambda_y = (yd1-yd0)/Tf;
xd(1) = xd0;
yd(1) = yd0;
xd_(1)=lambda_x;
yd_(1)=lambda_y;

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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%2nd subtask: reference trajectory
qr1 = 45*pi/180; qr2 = -70*pi/180; qr3 = 0;
p2d = [qr1;qr2;qr3];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% ****** 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)}


%%% Inverse geometric model %%%%
%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
%ss2 = sin(qd(:,2));
%qd(:,1) = atan2(yd(:),xd(:)) - asin(l(2)*ss2(:)./sqrt(rd2(:)));


tt=0; tk=1;
qd(tk,1)=qd1; qd(tk,2)=qd2; qd(tk,3)=qd3;
while (tt<=Tf)
   
   %% Compute Jacobian
   s1 = sin(qd(tk,1)); s12 = sin(qd(tk,1)+qd(tk,2)); s123 = sin(qd(tk,1)+qd(tk,2)+qd(tk,3));
   c1 = cos(qd(tk,1)); c12 = cos(qd(tk,1)+qd(tk,2)); c123 = cos(qd(tk,1)+qd(tk,2)+qd(tk,3));
	Jac1(1,3) = -l(3)*s123;
   Jac1(1,2) = Jac1(1,3) - l(2)*s12;
   Jac1(1,1) = Jac1(1,2) - l(1)*s1;
	Jac1(2,3) = l(3)*c123;
   Jac1(2,2) = Jac1(2,3) + l(2)*c12;
   Jac1(2,1) = Jac1(2,2) + l(1)*c1;
   
   %%Pseudo-inverse computation of Jacobian matrix Jac1
   Jac1_psinv = Jac1'*inv(Jac1*Jac1');
   
   %Subtask 1
   task1=Jac1_psinv*[xd_(tk);yd_(tk)];
   
   %Subtask 1
   %%p2d=...
   p2=qd(tk,:)';
   H2=eye(3)*kc;
   task2 = (eye(3)-Jac1_psinv*Jac1)*H2*(p2d-p2);
   
   %angular velocity
   qd_(tk,:) = task1'+task2';
   
   %% numerical integration --> kinematic simulation
   qd(tk+1,1) = qd(tk,1) + dt*qd_(tk,1);
   qd(tk+1,2) = qd(tk,2) + dt*qd_(tk,2);
   qd(tk+1,3) = qd(tk,3) + dt*qd_(tk,3);
   
   
   tk=tk+1;
   tt=tt+dt;
end



%% ***** 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 = xd1 + l(2)*cos( qd(:,1)+qd(:,2) );  
yd2 = yd1 + l(2)*sin( qd(:,1)+qd(:,2) );

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

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



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

fig1 = figure;

subplot(2,2,1);
plot(t,xd);
ylabel('xd (cm)');
xlabel('time t (sec)');


subplot(2,2,2);
plot(t,yd);
ylabel('yd (cm)');
xlabel('time t (sec)');


subplot(2,2,3);
plot(t,qd(:,1));
ylabel('qd1 (rad)');
xlabel('time t (sec)');


subplot(2,2,4);
plot(t,qd(:,2));
ylabel('qd2 (rad)');
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([-0.5 2.5 -1.5 1.5]) %%set xy plot axes (caution: square axes, i.e. dx=dy)
axis on
hold on

xlabel('x (cm)');
ylabel('y (cm)');
%plot(xd,yd,'rs');
plot(xd,yd,'r:');
dtk=100; %% plot robot position every dtk samples, to animate its motion


plot([0],[0],'o');

%% OBSTACLE
fill([0.4,1.1,1.1,0.4],[0.0,0.0,-0.4,-0.4],[0.8,0.3,0.3]);

for tk=1:dtk:kmax,

   %%%
	pause(0.5);	%% pause motion to view successive robot configurations
   plot([0,xd1(tk)],[0,yd1(tk)]);					
   plot([xd1(tk)],[yd1(tk)],'o');
   plot([xd1(tk),xd2(tk)],[yd1(tk),yd2(tk)]);
   plot([xd2(tk)],[yd2(tk)],'o');
   plot([xd2(tk),xd3(tk)],[yd2(tk),yd3(tk)]);
   plot([xd3(tk)],[yd3(tk)],'y*');
   
   plot([xd(tk)],[yd(tk)],'g+');

end





















