% barcol_augm_script.m
%
%
%  振り子棒とカラーがばねで相互に連結した系の3次元運動の解析
%
%
% 呼び出される関数
% barcol_augm_ode.m
% tilde.m
%
%
%　　プログラム全体の説明
%
%  ・支点をボールジョイントで拘束された棒に、カラーが拘束された系
%  ・回転姿勢にオイラー角φθψを使用
%  ・SI単位(メートル、キログラム、秒)を使用
%  ・z軸の負の方向に重力が働く
%  ・棒の支点位置は原点
%
% Copyright (C) 2007 清水 信行, 曽我部 潔, 井上 剛志 All Rights Reserved.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　　プログラム中の記号の説明
%
%  rG     :全系の質量中心の位置ベクトル[rG1' rG2']'
%  rG1    :棒　　の質量中心の位置
%  rG2    :カラーの質量中心の位置
%  vG     :質量中心の速度
%  thOG   :全系の姿勢を表すベクトル[φ1 θ1 ψ1 φ2 θ2 ψ2]'
%  th1OG  :棒　　の姿勢を表すオイラー角φ1θ1ψ1（zxz変換）
%  th2OG  :カラーの姿勢を表すオイラー角φ2θ2ψ2（zxz変換）
%  AOG1   :棒　　の回転マトリックス
%  AOG2   :カラーの回転マトリックス
%  omOGd  :角速度(一般化速度) 
%  om1OGd :棒　　の角速度(一般化速度) 
%  om2OGd :カラーの角速度(一般化速度) 
%  DthOG  :thOGの時間微分
%  Dth1OG :棒　　のth1OGの時間微分
%  Dth2OG :カラーのth2OGの時間微分
%  DOmega :omOGdの時間微分
%
%  M      :全系　の質量マトリックス
%  M1     :棒　　の質量マトリックス
%  M2     :カラーの質量マトリックス
%  Jd     :各々の座標から見た各々の剛体の質量中心まわりの
%            慣性モーメントのマトリックス
%  Jd1    :棒に固定された座標から見た棒の質量中心まわりの
%            慣性モーメントのマトリックス
%  Jd2    :カラーに固定された座標から見たカラー質量中心まわりの
%            慣性モーメントマトリックス
%  invJd  :Jdの逆マトリックス
%  g      :重力加速度
%  c      :支点の回転減衰係数マトリックス
%  fa1    :棒　　の質量中心に作用する外力=fg1+fs1
%  fa2    :カラーの質量中心に作用する外力=fg2+fs2
%  fg1    :棒　　の質量中心に作用する重力
%  fg2    :カラーの質量中心に作用する重力
%  fs1    :棒　　に作用するばね力
%  fs2    :カラーに作用するばね力
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　■　(a) MATLABプログラムの前処理
%
%　　　ワークスペース内の関数や変数を全てクリア
%
clear all;
close all;
clear functions;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　■　(b) パラメータの入力
%
%  ★棒とカラーの形状パラメータ　　　　（入力）★
%  
%  棒は、端点Cで座標原点Oに拘束
%  ばねは、カラーの端点Dと円柱棒の質量中心G1を接続
%
R1 =0.01;                     %  円柱棒の直径  [m]
L1 =0.1;                      %  円柱棒の長さ  [m]
R2i=0.01;                     %  カラー内径    [m]
R2o=0.015;                    %  カラー外径    [m]
L2 =0.02;                     %  カラー長さ    [m]
Lk0=0.01;                     %  ばねの自然長  [m]
rou=7800.0;                   %  鉄の密度　　  [kg/m^3]
k  =200.0;                    %  ばね定数      [N/m]
g  =9.81;                     %  重力加速度    [m/sec^2]
%
%
m1 =(R1^2*pi*L1)*rou;         % 円柱棒の質量   [kg]
m2 =(R2o^2-R2i^2)*pi*L2*rou;  % カラーの質量   [g]
%
%    質量中心まわりの慣性マトリックス (基表10.1より)
J1d=diag([m1*(3*R1^2+L1^2)/12.0 m1*(3*R1^2+L1^2)/12 m1*R1^2/2.0]);
%    カラーの質量中心まわりの慣性マトリックス (基表10.1より)
J2d=diag([m2*(3*(R2o^2+R2i^2)+L2^2)/12.0 m2*(3*(R2o^2+R2i^2)+L2^2)/12.0 m2*(R2o^2+R2i^2)/2.0]);
%
LG1C=L1/2.0;                  % 棒の質量中心G1と端点Cの距離
LG1A=L1/2.0;                  % 棒の質量中心G1と端点Aの距離
rG1d=[0; 0; LG1C];            % 端点Cから棒の質量中心G1の相対位置
rG1Cd=[0; 0; -LG1C];          % 棒の質量中心G1から端点Cの相対位置
rG1Ad=[0; 0;  LG1A];          % 棒の質量中心G1から端点Aの相対位置
%
LG2D=L2/2.0;                  % カラーの質量中心G2と端点Dの距離
LG2B=L2/2.0;                  % カラーの質量中心G2と端点Bの距離
rG2Dd=[0; 0; -LG2D];          % カラーの質量中心G2から端点Dの相対位置
rG2Bd=[0; 0;  LG2B];          % カラーの質量中心G2から端点Bの相対位置
%
a1d=rG1Cd;
a2d=rG2Dd;
%
b1xd=[1 0 0]';
b2yd=[0 1 0]';
%
%  ★ 棒とカラーの動的パラメータ　　　　（入力）★
%
c1x=0.0; %  x1'''軸まわりの角速度成分に対する質量中心まわりの回転に関する減衰係数[Nm/(rad/sec)]
c1y=0.0; %  y1'''軸まわりの　　　〃
c1z=0.0; %  z1'''軸まわりの　　　〃
%
c2x=0.0; %  x2'''軸まわりの角速度成分に対する質量中心まわりの回転に関する減衰係数
c2y=0.0; %  y2'''軸まわりの　　　〃
c2z=0.0; %  z2'''軸まわりの　　　〃
%
%  ★棒とカラーの初期オイラー角，初期オイラー角の時間微分　（入力）★
%
fai10  =0.5*pi;    % 棒の初期オイラー角φ0
theta10=-0.7*pi;  % 　　　　　〃　　　θ0
psi10  =0.0;       % 　　　　　〃　　　ψ0
fai20  =0.5*pi;    % カラーの初期オイラー角φ0
theta20=-0.7*pi;  % 　　　　　　〃　　　　θ0
psi20  =0.0;       % 　　　　　　〃　　　　ψ0
%
dfai10  =2.0*pi;   %     棒の初期オイラー角 dφ/dt
dtheta10=0.0;      %     　　　　　〃　　　 dθ/dt
dpsi10  =0.0;      %     　　　　　〃　　　 dψ/dt
dfai20  =2.0*pi;   % カラーの初期オイラー角 dφ/dt
dtheta20=0.0;      % 　　　　　　〃　　　 　dθ/dt
dpsi20  =0.0;      % 　　　　　　〃　　　 　dψ/dt
%
% ★ばねの初期伸び量　（入力）★
%
% ばねがカラー長さL2のDspringini(%)縮んだ状態からスタート
%
Dspringini=30;
%
%  ★シミュレーション時間　　　　（入力）★
%
ts=0;              % 計算開始時間[sec]
dt=0.01;           % 計算結果出力刻み[sec]
te=2.00;           % 計算終了時間[sec]
%
%  ★バウムガルテの安定化法　　　（入力）★
%
Alpha=200;
Beta=200;
%
%  ★アニメーション用  棒とカラーの形状設定（入力）★ (100倍してcmに変換)
%
Barshape =[0,R1*100,R1*100,0];
[XX1,YY1,ZZ1]=cylinder(Barshape,32);
ZZ1(2,:)=0.0;
ZZ1(3,:)=L1*100;
ZZ1(4,:)=L1*100;
%
Springshape =[0,(R1*1.2)*100,(R1*1.2)*100,0];
[XXs,YYs,ZZs]=cylinder(Springshape,32);
ZZs(1,:)=rG1d(3)*100;
ZZs(2,:)=rG1d(3)*100;
ZZs(3,:)=(rG1d(3)+Lk0)*100;
ZZs(4,:)=(rG1d(3)+Lk0)*100;
%
Collarshape =[0,R2o*100,R2o*100,0];
[XX2,YY2,ZZ2]=cylinder(Collarshape,32);
ZZ2(1,:)=-L2/2*100;
ZZ2(2,:)=-L2/2*100;
ZZ2(3,:)=L2/2*100;
ZZ2(4,:)=L2/2*100;
%
[iend,jend]=size(XX1);
%
%
%  パラメータの入力終了
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　■　(c) 運動方程式中の定数の決定
%
% 棒とカラーの質量中心に作用する外力（重力のみ）
fg=[0 ; 0; -m1*g; 0 ;0 ; -m2*g]; 
%
M =diag([m1 m1 m1 m2 m2 m2]);
Jd=[J1d zeros(3:3);zeros(3:3) J2d];
%
% 全一般化座標に関する質量マトリックス （式(4.81)）
%
MM=[M zeros(6:6);zeros(6:6) Jd];
%
invMM=inv(MM);
invM=inv(M);                     % M の逆マトリックス
invJd=inv(Jd);                   % Jdの逆マトリックス
rG1Cdtilde=tilde(rG1Cd);         % rG1Cdから作った歪み対称マトリックス
rG1Adtilde=tilde(rG1Ad);         % rG1Adから作った歪み対称マトリックス
rG2Bdtilde=tilde(rG2Bd);         % rG2Bdから作った歪み対称マトリックス
rG2Ddtilde=tilde(rG2Dd);         % rG2Ddから作った歪み対称マトリックス
%
a1dtilde  =tilde(a1d);           % a1dから作った歪み対称マトリックス
a2dtilde  =tilde(a2d);           % a2dから作った歪み対称マトリックス
%
b1xdtilde=tilde(b1xd);
b2ydtilde=tilde(b2yd);
%
% 質量中心まわりの回転に関する減衰係数マトリックス
c1=[c1x  0 0; 0 c1y 0; 0  0 c1z];
c2=[c2x  0 0; 0 c2y 0; 0  0 c2z];
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　■　(d) 状態変数の初期状態の計算
%
% 式(4.36)
rG1=[	 LG1C*sin(theta10)*sin(fai10);
	-LG1C*sin(theta10)*cos(fai10);
	 LG1C*cos(theta10)];
%
LG20d=(LG1C+Lk0+LG2D)+(Dspringini/100)*L2;
rG2=[	 LG20d*sin(theta20)*sin(fai20);
	-LG20d*sin(theta20)*cos(fai20);
	 LG20d*cos(theta20)];
%
rG=[rG1; rG2] ;
%
% 式(4.36)
vG1=[	 LG1C*cos(theta10)*sin(fai10)*dtheta10+LG1C*sin(theta10)*cos(fai10)*dfai10;
	-LG1C*cos(theta10)*cos(fai10)*dtheta10+LG1C*sin(theta10)*sin(fai10)*dfai10;
	-LG1C*sin(theta10)*dtheta10];
%
vG2=[	 LG20d*cos(theta20)*sin(fai20)*dtheta20+LG20d*sin(theta20)*cos(fai20)*dfai20;
	-LG20d*cos(theta20)*cos(fai20)*dtheta20+LG20d*sin(theta20)*sin(fai20)*dfai20;
	-LG20d*sin(theta20)*dtheta20];
%
vG=[vG1; vG2] ;
%
% 初期回転姿勢（オイラー角φθψ）
thOG=[fai10; theta10; psi10; fai20; theta20; psi20];
%
% オイラー角の時間微分
DthOG=[dfai10; dtheta10; dpsi10; dfai20; dtheta20; dpsi20]; 
%
% 角速度をオイラー角の時間微分で表す．      基式(5.53)
%
Ged=[	sin(theta10)*sin(psi10)  cos(psi10)   0 0 0 0;
	sin(theta10)*cos(psi10) -sin(psi10)   0 0 0 0;
	cos(theta10)             0            1 0 0 0;
	0 0 0 sin(theta20)*sin(psi20)  cos(psi20)   0;
	0 0 0 sin(theta20)*cos(psi20) -sin(psi20)   0;
	0 0 0 cos(theta20)             0            1 ];
%
omOGd=Ged*DthOG;               % 初期角速度
%
y0=[rG; vG; thOG; omOGd];     % 状態変数初期値
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ■　(f) 微分方程式の積分計算
%
[tt,YY]=ode45(@barcol_augm_ode,[ts:dt:te],y0,[],fg,c1,c2,MM,invMM,Jd,invJd,k,Lk0,a1d,a2d,b1xd,b2yd,rG2Dd,a1dtilde,a2dtilde,b1xdtilde,b2ydtilde,Alpha,Beta);
%
%  ・コマの運動の計算
%  ・出力ttは、tsからteまでdtキザミの時間が入った列マトリックス（n行1列）
%  ・出力YYは、各時間に対応した棒とカラーの質量中心位置，質量中心の速度，オイラー角,角速度を含むマトリックス（n行24列）
%
%  []にはオプションを入れることもできる．たとえば，相対誤差（1.0e-3）と絶対誤差（1.0e-6）
%　あるいは，イベント検出（なにか条件を与えると，それが発生した時点で終了）
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ■　(h) グラフ出力処理
%
ntt=length(tt);     % length：ベクトルの長さ　⇒　この場合は出力時間の数
%
%  計算結果出力（棒の質量中心の時刻歴：図4.13(a)）
%
figure(1);          %  	ウィンドウ作成
set(1,'Position',[5 440 500 250]);
plot(tt,YY(:,1),'-k',tt,YY(:,2),':k',tt,YY(:,3),'--k');
%			時間－棒の質量中心X,Y,Z座標を描画
xlabel('Time (s)')
ylabel('Displacement x1,y1,z1 (m)')
legend('棒の質量中心のx位置','質量中心のy位置','質量中心のz位置')
%
%  計算結果出力（カラーの質量中心の時刻歴：図4.13(b)）
%
figure(2);		%	ウィンドウ作成
set(2,'Position',[515 440 500 250]);
plot(tt,YY(:,4),'-k',tt,YY(:,5),':k',tt,YY(:,6),'--k');
%			時間－カラーの質量中心X,Y,Z座標を描画
xlabel('Time (s)')
ylabel('Displacement x2,y2,z2 (m)')
legend('カラーの質量中心のx位置','質量中心のy位置','質量中心のz位置')
%
%  計算結果出力（オイラー角φ1,θ1,ψ1の時刻歴：図4.13(c)）
%
figure(3);		%	ウィンドウ作成
set(3,'Position',[5 110 500 250]);
plot(tt,YY(:,13),'-k',tt,YY(:,14),':k',tt,YY(:,15),'--k');
xlabel('Time (s)')
ylabel('Euler angle φ1,θ1,ψ1 (rad)')
legend('オイラー角φ1','オイラー角θ1','オイラー角ψ1')
%			時間－オイラー角φ,θ,ψを描画
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ■　(i) こまの運動のアニメーション
%
% アニメ用出力，4つのウインドウの視点を表示する
%
figure(4);          %  棒とカラーの配置のウィンドウ作成
set(4,'Position',[515 35 500 500]);
%
subplot(221);
hold on;
surf(XX1,YY1,ZZ1);
surf(XX2,YY2,ZZ2+(rG1d(3)+Lk0+L2/2)*100);
surf(XXs,YYs,ZZs);
view([45 60 25]);
axis([-10 10 -10 10 -10 10]);%
xlabel('x')
ylabel('y')
zlabel('z')
%
subplot(222);
hold on;
surf(XX1,YY1,ZZ1);
surf(XX2,YY2,ZZ2+(rG1d(3)+Lk0+L2/2)*100);
surf(XXs,YYs,ZZs);
view([0 90 0]);
axis([-10 10 -10 10 -10 10]);%
xlabel('x')
ylabel('y')
zlabel('z')
%
subplot(223);
hold on;
surf(XX1,YY1,ZZ1);
surf(XX2,YY2,ZZ2+(rG1d(3)+Lk0+L2/2)*100);
surf(XXs,YYs,ZZs);
view([90 0 0]);
axis([-10 10 -10 10 -10 10]);%
xlabel('x')
ylabel('y')
zlabel('z')
%
subplot(224);
hold on;
surf(XX1,YY1,ZZ1);
surf(XX2,YY2,ZZ2+(rG1d(3)+Lk0+L2/2)*100);
surf(XXs,YYs,ZZs);
view([180 90]);
axis([-10 10 -10 10 -10 10]);%
xlabel('x')
ylabel('y')
zlabel('z')
%
%  アニメ用出力（図4.12,4.14），4つの視点から見たコマの運動を表示する
%
figure(5);
set(5,'Position',[225 55 600 600]);
axis([-10 10 -10 10 -10 10]);%
grid on;%
%
for itt=1:ntt
	%  回転マトリックス(式(4.35), 基式(4.50))
	Cos11=cos(YY(itt,13));         % cos(φ)
	Cos12=cos(YY(itt,14));         % cos(θ)
	Cos13=cos(YY(itt,15));         % cos(ψ)
	Sin11=sin(YY(itt,13));         % sin(φ)
	Sin12=sin(YY(itt,14));         % sin(θ)
	Sin13=sin(YY(itt,15));         % sin(ψ)
%
	Cos21=cos(YY(itt,16));         % cos(φ)
	Cos22=cos(YY(itt,17));         % cos(θ)
	Cos23=cos(YY(itt,18));         % cos(ψ)
	Sin21=sin(YY(itt,16));         % sin(φ)
	Sin22=sin(YY(itt,17));         % sin(θ)
	Sin23=sin(YY(itt,18));         % sin(ψ)
%
	AOG1=[	Cos11*Cos13-Sin11*Cos12*Sin13 -Cos11*Sin13-Sin11*Cos12*Cos13  Sin11*Sin12;%
		Sin11*Cos13+Cos11*Cos12*Sin13 -Sin11*Sin13+Cos11*Cos12*Cos13 -Cos11*Sin12;%
		Sin12*Sin13                    Sin12*Cos13                    Cos12      ];
%
	AOG2=[	Cos21*Cos23-Sin21*Cos22*Sin23 -Cos21*Sin23-Sin21*Cos22*Cos23  Sin21*Sin22 ;%
		Sin21*Cos23+Cos21*Cos22*Sin23 -Sin21*Sin23+Cos21*Cos22*Cos23 -Cos21*Sin22 ;%
		Sin22*Sin23                    Sin22*Cos23                    Cos22       ];
%
	rG2d=AOG2'*[YY(itt,4); YY(itt,5); YY(itt,6)];
%
	ZZs(3,:)=(rG2d(3)-LG2D)*100;
	ZZs(4,:)=(rG2d(3)-LG2D)*100;
%
	for i=1:iend
		for j=1:jend
%			棒の現在の配置を計算
			RRR1=(AOG1*[XX1(i,j); YY1(i,j); ZZ1(i,j)])';
%			ばねの現在の配置を計算
			RRRs=(AOG2*[XXs(i,j); YYs(i,j); ZZs(i,j)])';
%			カラーの現在の配置を計算
			RRR2=(AOG2*[XX2(i,j)+rG2d(1)*100; YY2(i,j)+rG2d(2)*100; ZZ2(i,j)+rG2d(3)*100])';
%
			XXXX1(i,j)=RRR1(1);
			YYYY1(i,j)=RRR1(2);
			ZZZZ1(i,j)=RRR1(3);
			XXXXs(i,j)=RRRs(1);
			YYYYs(i,j)=RRRs(2);
			ZZZZs(i,j)=RRRs(3);
			XXXX2(i,j)=RRR2(1);
			YYYY2(i,j)=RRR2(2);
			ZZZZ2(i,j)=RRR2(3);
		end
	end
%
%
	figure(5);
	hold off;
	subplot(221);
	surf(XXXX1,YYYY1,ZZZZ1);
	hold on;
	surf(XXXX2,YYYY2,ZZZZ2);
	surf(XXXXs,YYYYs,ZZZZs);
	view([45 60 25]);
	axis([-10 10 -10 10 -10 10]);
	xlabel('x')
	ylabel('y')
	zlabel('z')
%
	hold off;
	subplot(222);
	surf(XXXX1,YYYY1,ZZZZ1);
	hold on;
	surf(XXXX2,YYYY2,ZZZZ2);
	surf(XXXXs,YYYYs,ZZZZs);
	view([0 90 0]);
	axis([-10 10 -10 10 -10 10]);
	xlabel('x')
	ylabel('y')
	zlabel('z')
%
	hold off;
	subplot(223);
	surf(XXXX1,YYYY1,ZZZZ1);
	hold on;
	surf(XXXX2,YYYY2,ZZZZ2);
	surf(XXXXs,YYYYs,ZZZZs);
	view([90 0 0]);
	axis([-10 10 -10 10 -10 10]);
	xlabel('x')
	ylabel('y')
	zlabel('z')
%
	hold off;
	subplot(224);
	surf(XXXX1,YYYY1,ZZZZ1);
	hold on;
	surf(XXXX2,YYYY2,ZZZZ2);
	surf(XXXXs,YYYYs,ZZZZs);
	view([180 90]);
	axis([-10 10 -10 10 -10 10]);
	xlabel('x')
	ylabel('y')
	zlabel('z')
%
	drawnow;
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ■　(g) 微分方程式の定義
%
% ファンクション M ファイル    barcol_augm_ode.m に記述
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ■　(e) 関数tildeの定義
%
% ファンクション M ファイル    tilde.m           に記述
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
