% doublepend_augm_script.m
%
%
%  ２重振り子の3次元運動の解析(拡大法)
%
% 呼び出される関数
% doublepend_augm_ode.m
% tilde.m
%
%　　プログラム全体の説明
%
%  ・支点,結合点をボールジョイントで拘束された2重振り子
%  ・回転姿勢にオイラー角φθψを使用
%  ・SI単位(メートル、キログラム、秒)を使用
%  ・z軸の負の方向に重力が働く
%　・振り子の支点は原点とする
%
% Copyright (C) 2007 清水 信行, 曽我部 潔, 竹原 昭一郎 All Rights Reserved.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　　プログラム中の記号の説明(式()に記載)
%
%  rG1    :全体枠から見た質量中心の位置
%  thOG   :剛体の姿勢を表すオイラー角φθψ（zxz変換）
%  AOG    :座標変換マトリックス
%  omOGd  :物体固定枠成分表示における角速度
%  DthOG  :thOGの時間微分（一般化速度）
%  DOmega :物体固定枠成分表示におけるomOGdの時間微分
%  M      :剛体の質量マトリックス
%  LGC    :剛体の質量中心Gと拘束点Cとの間の距離
%  Jd     :剛体に固定された基準枠から見た剛体の質量中心まわりの慣性マトリックス
%  invJd  :Jdの逆マトリックス
%  cJd    :剛体に固定された基準枠から見た剛体の質量中心まわりの慣性マトリックス
%  invcJd :cJdの逆マトリックス
%  g      :重力加速度
%  c      :質量中心まわりの回転に関する減衰係数マトリックス
%  nd     :回転運動に対する減衰トルクに起因する外トルク
%  fa     :剛体の質量中心に作用する外力（重力のみ）
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　■　(a) MATLABプログラムの前処理
%
%　　　ワークスペース内の関数や変数を全てクリア
clear all;
clear functions;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　■　(b) パラメータの入力
%
%  ★振り子の形状パラメータ　　　　（入力）★
%
r1=0.2;                % 剛体１の質量部分の半径[m]
h1=1.0;                % 剛体１の質量部分の長さ[m]
m1=r1^2*pi*h1;         % 剛体１の質量[kg]
LGC1=0.5;              % 剛体１の重心までの長さ[m]
Jd1=[m1*(r1^2/4+(2*LGC1)^2/12) m1*(r1^2/4+(2*LGC1)^2/12) m1*r1^2];
                       % 質量中心まわりの慣性マトリックス[kg m^2]
%
r2=0.2;                % 剛体２の質量部分の半径[m]
h2=1.0;                % 剛体２の質量部分の長さ[m]
m2=r2^2*pi*h2;         % 剛体２の質量[kg]
LGC2=0.5;              % 剛体２の重心までの長さ[m]
Jd2=[m2*(r2^2/4+(2*LGC2)^2/12) m2*(r2^2/4+(2*LGC2)^2/12) m2*r2^2];
                       % 質量中心まわりの慣性マトリックス[kg m^2]
%
%  ★振り子の動的パラメータ　　　　（入力）★
%
cx1=0;  %  剛体１に関するx'''軸まわりの角速度成分に対する質量中心まわりの回転に関する減衰係数 [Nm/(rad/sec)]
cy1=0;  %  剛体１に関するy'''軸まわりの　　　〃
cz1=0;  %  剛体１に関するy'''軸まわりの　　　〃
cx2=0;  %  剛体２に関するx'''軸まわりの角速度成分に対する質量中心まわりの回転に関する減衰係数 [Nm/(rad/sec)]
cy2=0;  %  剛体１に関するy'''軸まわりの　　　〃
cz2=0;  %  剛体１に関するy'''軸まわりの　　　〃
%
%  ★振り子の初期オイラー角，初期オイラー角の時間微分　　　　（入力）★
%
%
fai01=pi/8;            % 剛体１に関する初期オイラー角φ0[rad]
theta01=pi/5;          % 　　　〃　　　　　　　　　　θ0[rad]
psi01=0;               % 　　　〃　　　　　　　　　　ψ0[rad]
%
dfai01=0;              % 剛体１に関する初期オイラー角の時間微分 dφ/dt[rad/s]
dtheta01=0;            % 　　　〃　　　 　　　　　　　　　　　　dθ/dt[rad/s]
dpsi01=0;              % 　　　〃　　　 　　　　　　　　　　　　dψ/dt[rad/s]
%
fai02=pi/8;            % 剛体２に関する初期オイラー角φ0[rad]
theta02=-pi/4;         % 　　　〃　　　　　　　　　　θ0[rad]
psi02=0;               % 　　　〃　　　　　　　　　　ψ0[rad]
%
dfai02=0;              % 剛体２に関する初期オイラー角の時間微分 dφ/dt[rad/s]
dtheta02=0;            % 　　　〃　　　 　　　　　　　　　　　　dθ/dt[rad/s]
dpsi02=0;              % 　　　〃　　　 　　　　　　　　　　　　dψ/dt[rad/s]
%
%  ★事例のパラメータ★
%
%fai01=0;               % 剛体１に関する初期オイラー角φ0[rad]
%theta01=pi/5;          % 　　　〃　　　　　　　　　　θ0[rad]
%psi01=0;               % 　　　〃　　　　　　　　　　ψ0[rad]
%
%dfai01=0;              % 剛体１に関する初期オイラー角の時間微分 dφ/dt[rad/s]
%dtheta01=0;            % 　　　〃　　　 　　　　　　　　　　　　dθ/dt[rad/s]
%dpsi01=0;              % 　　　〃　　　 　　　　　　　　　　　　dψ/dt[rad/s]
%
%fai02=0;               % 剛体２に関する初期オイラー角φ0[rad]
%theta02=pi/5;          % 　　　〃　　　　　　　　　　θ0[rad]
%psi02=0;               % 　　　〃　　　　　　　　　　ψ0[rad]
%
%dfai02=0;              % 剛体２に関する初期オイラー角の時間微分 dφ/dt[rad/s]
%dtheta02=0;            % 　　　〃　　　 　　　　　　　　　　　　dθ/dt[rad/s]
%dpsi02=400/60*2*pi;    % 　　　〃　　　 　　　　　　　　　　　　dψ/dt[rad/s]
%
%
%  ★シミュレーション時間　　　　（入力）★
%
ts=0;                  % 計算開始時間[s]
dt=0.01;               % 計算結果出力刻み[s]
te=4;                  % 計算終了時間[s]
%
%  ★バウムガルテの安定化法　　　　（入力）★
%
Alpha=10; 
Beta=10;
%
%  ★アニメーション用振り子の形状設定（入力）★
%
Topshape1=[0,r1,r1,0,0,r2,r2,0];
[XXX,YYY,ZZZ]=cylinder(Topshape1,12);
ZZZ(2,:)=-LGC1/4;
ZZZ(3,:)=-2*LGC1+LGC1/4;
ZZZ(4,:)=-2*LGC1;
ZZZ(5,:)=-2*LGC1;
ZZZ(6,:)=-2*LGC1-LGC1/4;
ZZZ(7,:)=-2*LGC1-2*LGC2+LGC1/4;
ZZZ(8,:)=-2*LGC1-2*LGC2;
[iend,jend]=size(XXX);
%
%  パラメータの入力終了
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　■　(c) 運動方程式中の定数の決定
%
g=9.81;                        % 重力加速度[m/s^2]
fa1=[0; 0; -m1*g];             % 剛体１の質量中心に作用する外力（重力のみ）
fa2=[0; 0; -m2*g];             % 剛体２の質量中心に作用する外力（重力のみ）
M1=[m1 m1 m1];                 % 剛体１の質量マトリックス
M2=[m2 m2 m2];                 % 剛体２の質量マトリックス
M=diag([M1 M2 Jd1 Jd2]);       % 質量マトリクス
Jd1=diag([Jd1]);               % 剛体１の質量中心まわりの慣性マトリックス
Jd2=diag([Jd2]);               % 剛体２の質量中心まわりの慣性マトリックス
invM=inv(M);                   % Mの逆マトリックス
rGCd1=[0;0;LGC1];              % 剛体１の座標系G１で見た剛体の支点C１の位置
                               % (-rGCd1: 剛体１の座標系G１で見た剛体の拘束点C２の位置)
rGCd2=[0;0;LGC2];              % 剛体２の座標系G２で見た剛体の拘束点C２の位置
s1tilde=tilde(rGCd1);          % rGCd1から作った歪み対称マトリックス
s12tilde=tilde(-rGCd1);        % -rGCd1から作った歪み対称マトリックス
s2tilde=tilde(rGCd2);          % rGCd2から作った歪み対称マトリックス
c1=diag([cx1 cy1 cz1]);        % 剛体１の質量中心まわりの回転に関する減衰係数マトリックス
c2=diag([cx2 cy2 cz2]);        % 剛体２の質量中心まわりの回転に関する減衰係数マトリックス
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　■　(d) 状態変数の初期状態の計算
%
rG1=-LGC1*[sin(theta01)*sin(fai01);-sin(theta01)*cos(fai01);cos(theta01)];
vG1=-LGC1*[cos(theta01)*sin(fai01)*dtheta01+sin(theta01)*cos(fai01)*dfai01;
          -cos(theta01)*cos(fai01)*dtheta01+sin(theta01)*sin(fai01)*dfai01;
          -sin(theta01)*dtheta01];
rG2=2*rG1...
      -LGC2*[sin(theta02)*sin(fai02);-sin(theta02)*cos(fai02);cos(theta02)];
vG2=2*vG1...
      -LGC2*[cos(theta02)*sin(fai02)*dtheta02+sin(theta02)*cos(fai02)*dfai02;
          -cos(theta02)*cos(fai02)*dtheta02+sin(theta02)*sin(fai02)*dfai02;
          -sin(theta02)*dtheta02];
%
thOG1=[fai01; theta01; psi01];     % 剛体１の初期回転姿勢（オイラー角φθψ）
thOG2=[fai02; theta02; psi02];     % 剛体２の初期回転姿勢（オイラー角φθψ）
thOG=[thOG1; thOG2];               % 初期回転姿勢（オイラー角φθψ）
DthOG1=[dfai01; dtheta01; dpsi01]; % 剛体１の初期オイラー角の時間微分
DthOG2=[dfai02; dtheta02; dpsi02]; % 剛体２の初期オイラー角の時間微分
DthOG=[DthOG1; DthOG2];            % 初期オイラー角の時間微分 
%
%角速度をオイラー角の時間微分で表す.　　　
%
Ged1=[sin(theta01)*sin(psi01)  cos(psi01)   0;
      sin(theta01)*cos(psi01) -sin(psi01)   0;
      cos(theta01)                 0        1];                        
Ged2=[sin(theta02)*sin(psi02)  cos(psi02)   0;
      sin(theta02)*cos(psi02) -sin(psi02)   0;
      cos(theta02)                 0        1];
%
omOGd1=Ged1*DthOG1;
omOGd2=Ged2*DthOG2;                % 初期角速度　式(4.14), 基式(5.52b)参照
%
y0=[rG1; rG2; vG1; vG2; thOG1; thOG2; omOGd1; omOGd2];% 状態変数の初期値
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ■　(f) 微分方程式の積分計算
%
[tt,YY]=ode45(@doublepend_augm_ode,[ts:dt:te],y0,[],fa1,fa2,c1,c2,M,invM,Jd1,Jd2,...
                   LGC1,LGC2,rGCd1,rGCd2,s1tilde,s12tilde,s2tilde,Alpha,Beta);
%
%  ・２重振り子の運動の計算
%  ・出力ttは、tsからteまでdt出力キザミの時間が入った列マトリックス（n行1列）
%  ・出力YYは、各時間に対応したこまのオイラー角,角速度を含むマトリックス（n行6列）
%
%  []にはオプションを入れることもできる．たとえば，相対誤差（1.0e-3）と絶対誤差（1.0e-6）
%　あるいは，イベント検出（なにか条件を与えると，それが発生した時点で終了）
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ■　(h) グラフ出力処理
%
ntt=length(tt);      %length：ベクトルの長さ　⇒　出力時間の数
%
%　計算結果出力（質量中心の時刻歴：図4.9）
%
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位置')
%
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位置')
%
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')
%
figure(4);          %  ウィンドウ作成
set(4,'Position',[515 110 500 250]);
plot(tt,YY(:,16),'-k',tt,YY(:,17),':k',tt,YY(:,18),'--k');
xlabel('Time (s)')
ylabel('Euler angle φ2,θ2,ψ2 (rad)')
legend('オイラー角φ2','オイラー角θ2','オイラー角ψ2')
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ■　(i) 2重振り子の運動のアニメーション
%
% アニメ用出力（図4.10），4つのウインドウの視点を表示する
%
figure(5);
set(5,'Position',[225 55 600 600]);
for itt=1:ntt
	%  回転マトリックス(式(4.35), 基式(4.50))
	Cos1=cos(YY(itt,13));%          cos(φ)
	Cos2=cos(YY(itt,14));%          cos(θ)
	Cos3=cos(YY(itt,15));%          cos(ψ)
	Sin1=sin(YY(itt,13));%          sin(φ)
	Sin2=sin(YY(itt,14));%          sin(θ)
	Sin3=sin(YY(itt,15));%          sin(ψ)
	AOG1=[Cos1*Cos3-Sin1*Cos2*Sin3  -Cos1*Sin3-Sin1*Cos2*Cos3   Sin1*Sin2;%
		Sin1*Cos3+Cos1*Cos2*Sin3  -Sin1*Sin3+Cos1*Cos2*Cos3  -Cos1*Sin2;%
		Sin2*Sin3                  Sin2*Cos3                  Cos2];
	Cos4=cos(YY(itt,16));%          cos(φ)
	Cos5=cos(YY(itt,17));%          cos(θ)
	Cos6=cos(YY(itt,18));%          cos(ψ)
	Sin4=sin(YY(itt,16));%          sin(φ)
	Sin5=sin(YY(itt,17));%          sin(θ)
	Sin6=sin(YY(itt,18));%          sin(ψ)
	AOG2=[Cos4*Cos6-Sin4*Cos5*Sin6  -Cos4*Sin6-Sin4*Cos5*Cos6   Sin4*Sin5;%
		Sin4*Cos6+Cos4*Cos5*Sin6  -Sin4*Sin6+Cos4*Cos5*Cos6  -Cos4*Sin5;%
		Sin5*Sin6                  Sin5*Cos6                  Cos5];
	for i=1:4
		for j=1:jend
		RRR=(AOG1*[XXX(i,j)-0;YYY(i,j)-0;ZZZ(i,j)-(-LGC1)])';
		XXXX(i,j)=RRR(1) + YY(itt,1);
		YYYY(i,j)=RRR(2) + YY(itt,2);
		ZZZZ(i,j)=RRR(3) + YY(itt,3);
		end
	end
	for i=5:iend
		for j=1:jend
		RRR=(AOG2*[XXX(i,j)-0;YYY(i,j)-0;ZZZ(i,j)-(-LGC2-2*LGC1)])';
		XXXX(i,j)=RRR(1) + YY(itt,4);
		YYYY(i,j)=RRR(2) + YY(itt,5);
		ZZZZ(i,j)=RRR(3) + YY(itt,6);
		end
	end
%
figure(5);
%
subplot(221);
	surf(XXXX,YYYY,ZZZZ);
	axis([-2*(LGC1+LGC2) 2*(LGC1+LGC2) -2*(LGC1+LGC2) 2*(LGC1+LGC2) -2*(LGC1+LGC2) 2*(LGC1+LGC2)]);%
	view([45 60 25]);
	xlabel('x');
	ylabel('y');
	zlabel('z');
%
subplot(222);
	surf(XXXX,YYYY,ZZZZ);
	axis([-2*(LGC1+LGC2) 2*(LGC1+LGC2) -2*(LGC1+LGC2) 2*(LGC1+LGC2) -2*(LGC1+LGC2) 2*(LGC1+LGC2)]);%
	view([0 90 0]);
	xlabel('x');
	ylabel('y');
	zlabel('z');   
%
	subplot(223);
	surf(XXXX,YYYY,ZZZZ);
	axis([-2*(LGC1+LGC2) 2*(LGC1+LGC2) -2*(LGC1+LGC2) 2*(LGC1+LGC2) -2*(LGC1+LGC2) 2*(LGC1+LGC2)]);%
	view([90 0 0]);
	xlabel('x');
	ylabel('y');
	zlabel('z');
%
	subplot(224);
	surf(XXXX,YYYY,ZZZZ);
	axis([-2*(LGC1+LGC2) 2*(LGC1+LGC2) -2*(LGC1+LGC2) 2*(LGC1+LGC2) -2*(LGC1+LGC2) 2*(LGC1+LGC2)]);%
	view([180 90]);
	xlabel('x');
	ylabel('y');
	zlabel('z');
%
	drawnow;%
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ■　(g) 微分方程式の定義
%
% ファンクション M ファイル    doublepend_augm_ode.m  に記述
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ■　(e) 関数tildeの定義
%
% ファンクション M ファイル    tilde.m                に記述
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
