% TwoLink.m
% 
% Kinematic analysis of Two Link mechanism
% 
% Copyright (C) 2007 清水 信行, 曽我部 潔 All Rights Reserved.

% =================================================================================
% プログラムの使用条件
% 1. 本プログラムおよびデータを商用で，頒布してはならない．
% 2. 本プログラムおよびデータを著作者の許可なく営利目的で利用してはならない．
% 3. 本プログラムおよびデータの改変は，営利目的でない限り自由である．
% 4. 本プログラムおよびデータを使用することにより生じた損害などについては，著者，
%    プロクラム著作者，(社)日本機械学会，および(株)コロナ社は一切の責任を負わない．
% 5. 本プログラムの著作権は，本書編著者ならびにプログラム著作者に帰す．
% =================================================================================

% Setting of parameters
x1O = -0.1; y1O = 0;
x1A =  0.1; y1A = 0;
x2A = -0.3; y2A = 0;
x2B =  0.3; y2B = 0;

% Initialization
close; % Close the figure window
A_data = []; % Initialize the Animation data
G_data = []; % Initialize the Graphics data

% Initial state of the mechanism
q = [0.1 0 0 0.5 0 0]';

% Time counter
Ts = 0; Te = 10; dt = 0.025

for t = Ts:dt:Te
	
	% Displacement analysis
	iteration = 0; iconv = 1;
	while(iteration <= 20) & (iconv == 1) 
		sq3 = sin(q(3)); cq3 = cos(q(3)); sq6 = sin(q(6)); cq6 = cos(q(6)); 
		Fq = [1 0 -(x1O*sq3+y1O*cq3)  0  0  0
		      0 1  (x1O*cq3-y1O*sq3)  0  0  0
		      1 0 -(x1A*sq3+y1A*cq3) -1  0  (x2A*sq6+y2A*cq6)
		      0 1  (x1A*cq3-y1A*sq3)  0 -1 -(x2A*cq6-y2A*sq6)
		      0 0  0                  0  1  (x2B*cq6-y2B*sq6) 
		      0 0  1                  0  0  0 ]; 
		F = [q(1)+(x1O*cq3-y1O*sq3)
		     q(2)+(x1O*sq3+y1O*cq3)
		     q(1)+(x1A*cq3-y1A*sq3)-(q(4)+(x2A*cq6-y2A*sq6))
		     q(2)+(x1A*sq3+y1A*cq3)-(q(2)+(x2A*sq6+y2A*cq6))
		     q(5)+(x2B*sq6+y2B*cq6)
		     q(3)-0.5*t^2 ];
		dq = -Fq\F;
		q = q+dq;
		iconv = 0;
		for i = 1:3
			if(abs(dq(i)>0.001)) iconv = 1; end
		end
		iteration = iteration+1;
	end
		
	% Velocity analysis
	sq3 = sin(q(3)); cq3 = cos(q(3)); sq6 = sin(q(6)); cq6 = cos(q(6)); 
	F = [q(1)+(x1O*cq3-y1O*sq3)
	     q(2)+(x1O*sq3+y1O*cq3)
	     q(1)+(x1A*cq3-y1A*sq3)-(q(4)+(x2A*cq6-y2A*sq6))
	     q(2)+(x1A*sq3+y1A*cq3)-(q(2)+(x2A*sq6+y2A*cq6))
	     q(5)+(x2B*sq6+y2B*cq6)
	     q(3)-0.5*t^2 ];
	v = [ 0 0 0 0 0 t ]';
	qd = Fq\v;  
	
	% Acceleration analysis
	gam = [(x1O*cq3-y1O*sq3)*qd(3)^2
	       (x1O*sq3+y1O*cq3)*qd(3)^2
	       (x1A*cq3-y1A*sq3)*qd(3)^2-(x2A*cq6-y2A*sq6)*qd(6)^2
	       (x1A*sq3+y1A*cq3)*qd(3)^2-(x2A*sq6+y2A*cq6)*qd(6)^2
	       (x2B*sq6+y2B*cq6)*qd(6)^2
	       1 ];
	qdd = Fq\gam;
	
	% Animation
	plot([0,0],[0,0])
	axis square
	xmin = -0.4; xmax = 1.0; ymin = -0.7; ymax = 0.7;
	axis([xmin,xmax,ymin,ymax])
	hold on
	
	temp = [t];
	fprintf('t = %f\n',t)
	
	nae = 1; % Number of Animation element = 1
	xL = q(1)+(x1O*cq3-y1O*sq3); yL = q(2)+(x1O*sq3+y1O*cq3);
	xR = q(1)+(x1A*cq3-y1A*sq3); yR = q(2)+(x1A*sq3+y1A*cq3);
	plot([xL,xR],[yL,yR],'linewidth',0.5);
	temp = [temp,[xL,yL,xR,yR]];
	
	nae = 2; % Number of Animation element = 2
	xL = q(1)+(x1A*cq3-y1A*sq3); yL = q(2)+(x1A*sq3+y1A*cq3);
	xR = q(4)+(x2B*cq6-y2B*sq6); yR = q(5)+(x2B*sq6+y2B*cq6);
	plot([xL,xR],[yL,yR],'linewidth',0.5);
	temp = [temp,[xL,yL,xR,yR]];
	
	drawnow
	hold off
	set(1,'doublebuffer','on')
	
	% Output of the Animation data
	A_data = [A_data;temp];
	
	% Output of the Graphics data for the point of interest
	dis = q(4)+(x2B*cq6-y2B*sq6); yP = 0;
	vel = qd(4)-(x2B*sq6+y2B*cq6)*qd(6);
	acc = qdd(4)-(x2B*cq6-y2B*sq6)*qd(6)^2;
	temp = [t,dis,vel,acc];
	G_data = [G_data;temp];
end
