function [Fq, RHS] = const(Body, Joint, q, dq, t, flag);
% function [Fq, RHS] = const(Body, Joint, q, dq, t, flag);
% 
% 拘束条件式とヤコビ行列の評価
% 
% 呼び出されるプログラム 
%  rvlt.m : 回転ジョイント拘束条件の評価
%  tran.m : 並進ジョイント拘束条件の評価
%  grnd.m : グランド拘束条件の評価
%  smpl.m : 単純拘束条件の評価
%  drvr.m : 駆動拘束条件の評価
% 
% 引数の説明：
%  flag==1: Constraint equation
%  flag==2: Right-hand-side of velocity equations
%  flag==3: Right-hand-side of acceleration equations
% 
% Copyright (C) 2007 清水 信行, 曽我部 潔, 椎葉 太一 All Rights Reserved.

[Fq_RJ, RHS_RJ] = rvlt(Body, Joint, q, dq, flag);
[Fq_TJ, RHS_TJ] = tran(Body, Joint, q, dq, flag);
[Fq_GR, RHS_GR] = grnd(Body, Joint, q, dq, flag);
[Fq_SM, RHS_SM] = smpl(Body, Joint, q, dq, flag);
[Fq_DR, RHS_DR] = drvr(Body, Joint, q, dq, t, flag);

Fq  = [ Fq_RJ;  Fq_TJ;  Fq_GR;  Fq_SM;  Fq_DR ];
RHS = [ RHS_RJ; RHS_TJ; RHS_GR; RHS_SM; RHS_DR ];

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