function g_output = baumgarte(Body, Joint, q, dq, t, g_input)
% function g_output = baumgarte(Body, Joint, q, dq, t, g_input)
% 
% Baumgarteの安定化法
%  プログラム中の安定化パラメータalphaおよびbetaは適宜調整のこと
%  時間を陽に含む拘束条件を扱っている場合は、速度方程式の箇所を修正すること
% 
% Copyright (C) 2007 清水 信行, 曽我部 潔, 椎葉 太一 All Rights Reserved.

dq_column = zeros(3*Body.Number,1);

% 安定化パラメータ
alpha = 10;
beta  = 10;

% 拘束条件式
[Fq, phi] = const(Body, Joint, q, dq, t, 1);

% 速度方程式
% [Fq, nu]  = const(Body, Joint, q, dq, t, 2); % 時間を陽に含む拘束条件を扱っている場合はこちらを利用
nu = zeros(Joint.m, 1); % 時間を陽に含まない拘束条件のみ扱う場合はこちらを利用

for i=1:Body.Number
	for j=1:3
		dq_column(3*(i-1)+j,1) = dq(i,j);
	end
end

% Stabilization method by Baumgarte
g_output = g_input - 2*alpha*(Fq*dq_column - nu) - beta^2*phi;

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