forwardDynamics.C
src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C
ソースコード
/*---------------------------------------------------------------------------*\
========= |
\\ / F ield | OpenFOAM: The Open Source CFD Toolbox
\\ / O peration |
\\ / A nd | www.openfoam.com
\\/ M anipulation |
-------------------------------------------------------------------------------
Copyright (C) 2016 OpenFOAM Foundation
-------------------------------------------------------------------------------
License
This file is part of OpenFOAM.
OpenFOAM is free software: you can redistribute it and/or modify it
under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
for more details.
You should have received a copy of the GNU General Public License
along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>.
\*---------------------------------------------------------------------------*/
#include "rigidBodyModel.H"
#include "rigidBodyModelState.H"
#include "rigidBodyRestraint.H"
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::rigidBodyModel::applyRestraints
(
scalarField& tau,
Field<spatialVector>& fx,
const rigidBodyModelState& state
) const
{
if (restraints_.empty())
{
return;
}
forAll(restraints_, ri)
{
DebugInfo << "Restraint " << restraints_[ri].name();
// Accumulate the restraint forces
restraints_[ri].restrain(tau, fx, state);
}
}
void Foam::RBD::rigidBodyModel::forwardDynamics
(
rigidBodyModelState& state,
const scalarField& tau,
const Field<spatialVector>& fx
) const
{
const scalarField& q = state.q();
const scalarField& qDot = state.qDot();
scalarField& qDdot = state.qDdot();
DebugInFunction
<< "q = " << q << nl
<< "qDot = " << qDot << nl
<< "tau = " << tau << endl;
// Joint state returned by jcalc
joint::XSvc J;
v_[0] = Zero;
for (label i=1; i<nBodies(); i++)
{
const joint& jnt = joints()[i];
jnt.jcalc(J, q, qDot);
S_[i] = J.S;
S1_[i] = J.S1;
Xlambda_[i] = J.X & XT_[i];
const label lambdai = lambda_[i];
if (lambdai != 0)
{
X0_[i] = Xlambda_[i] & X0_[lambdai];
}
else
{
X0_[i] = Xlambda_[i];
}
v_[i] = (Xlambda_[i] & v_[lambdai]) + J.v;
c_[i] = J.c + (v_[i] ^ J.v);
IA_[i] = I(i);
pA_[i] = v_[i] ^* (I(i) & v_[i]);
if (fx.size())
{
pA_[i] -= *X0_[i] & fx[i];
}
}
for (label i=nBodies()-1; i>0; i--)
{
const joint& jnt = joints()[i];
const label qi = jnt.qIndex();
if (jnt.nDoF() == 1)
{
U1_[i] = IA_[i] & S1_[i];
Dinv_[i].xx() = 1/(S1_[i] && U1_[i]);
u_[i].x() = tau[qi] - (S1_[i] && pA_[i]);
const label lambdai = lambda_[i];
if (lambdai != 0)
{
const spatialTensor Ia
(
IA_[i] - (U1_[i]*(Dinv_[i].xx()*U1_[i]))
);
const spatialVector pa
(
pA_[i] + (Ia & c_[i]) + U1_[i]*(Dinv_[i].xx()*u_[i].x())
);
IA_[lambdai] +=
spatialTensor(Xlambda_[i].T())
& Ia
& spatialTensor(Xlambda_[i]);
pA_[lambdai] += Xlambda_[i].T() & pa;
}
}
else
{
U_[i] = IA_[i] & S_[i];
Dinv_[i] = (S_[i].T() & U_[i]).inv();
u_[i] = tau.block<vector>(qi) - (S_[i].T() & pA_[i]);
const label lambdai = lambda_[i];
if (lambdai != 0)
{
spatialTensor Ia
(
IA_[i]
- (U_[i] & Dinv_[i] & U_[i].T())
);
spatialVector pa
(
pA_[i]
+ (Ia & c_[i])
+ (U_[i] & Dinv_[i] & u_[i])
);
IA_[lambdai] +=
spatialTensor(Xlambda_[i].T())
& Ia
& spatialTensor(Xlambda_[i]);
pA_[lambdai] += Xlambda_[i].T() & pa;
}
}
}
a_[0] = spatialVector(Zero, -g_);
for (label i=1; i<nBodies(); i++)
{
const joint& jnt = joints()[i];
const label qi = jnt.qIndex();
a_[i] = (Xlambda_[i] & a_[lambda_[i]]) + c_[i];
if (jnt.nDoF() == 1)
{
qDdot[qi] = Dinv_[i].xx()*(u_[i].x() - (U1_[i] && a_[i]));
a_[i] += S1_[i]*qDdot[qi];
}
else
{
vector qDdoti(Dinv_[i] & (u_[i] - (U_[i].T() & a_[i])));
// Need to add mutable "block<vector>" to Field
qDdot[qi] = qDdoti.x();
qDdot[qi+1] = qDdoti.y();
qDdot[qi+2] = qDdoti.z();
a_[i] += (S_[i] & qDdoti);
}
}
DebugInfo
<< "qDdot = " << qDdot << nl
<< "a = " << a_ << endl;
}
void Foam::RBD::rigidBodyModel::forwardDynamicsCorrection
(
const rigidBodyModelState& state
) const
{
DebugInFunction << endl;
const scalarField& q = state.q();
const scalarField& qDot = state.qDot();
const scalarField& qDdot = state.qDdot();
// Joint state returned by jcalc
joint::XSvc J;
v_[0] = Zero;
a_[0] = spatialVector(Zero, -g_);
for (label i=1; i<nBodies(); i++)
{
const joint& jnt = joints()[i];
const label qi = jnt.qIndex();
jnt.jcalc(J, q, qDot);
S_[i] = J.S;
S1_[i] = J.S1;
Xlambda_[i] = J.X & XT_[i];
const label lambdai = lambda_[i];
if (lambdai != 0)
{
X0_[i] = Xlambda_[i] & X0_[lambdai];
}
else
{
X0_[i] = Xlambda_[i];
}
v_[i] = (Xlambda_[i] & v_[lambdai]) + J.v;
c_[i] = J.c + (v_[i] ^ J.v);
a_[i] = (Xlambda_[i] & a_[lambdai]) + c_[i];
if (jnt.nDoF() == 1)
{
a_[i] += S1_[i]*qDdot[qi];
}
else
{
a_[i] += S_[i] & qDdot.block<vector>(qi);
}
}
DebugInfo<< "a = " << a_ << endl;
}
// ************************************************************************* //
void Foam::RBD::rigidBodyModel::forwardDynamics
(
rigidBodyModelState& state,
const scalarField& tau,
const Field<spatialVector>& fx
) const
{
const scalarField& q = state.q();
n個の剛体の位置姿勢：（Px1, Py1, Pz1, Rx1, Ry1, Rz1, Px2,…..）
const scalarField& qDot = state.qDot();
n 個の剛体の速度：（vx1, vy1, vz1, ωx1, ωy1, ωz1, vx2,…..）
scalarField& qDdot = state.qDdot();
n 個の剛体の加速度：（ax1, ay1, az1, dωx1, dωy1, dωz1, ax2,…..）
joint::XSvc J;
ジョイントの状態から空間行列, ベクトルを変数Jに格納（J.X, J.S, J.v, J.c, ）
XSvcはX, S, v, cの4要素をまとめたクラス
- X : 空間座標変換行列
親から子へ座標変換を行う
- S : 運動部分空間ベクトル
ジョイントの自由度
- v : ジョイントの空間速度
親に対する子の相対的な速度（並進速度と角速度をまとめた6次元空間ベクトル）
ジョイントの速度$\dot{q}$に運動部分空間ベクトル$S$を乗算したベクトル（$v=S\dot{q}$）
- c : バイアス空間加速度
ジョイントが動くことによって生じる、コリオリ加速度や遠心加速度などの項（6次元ベクトル）
Xlambda_[i] = J.X & XT_[i];
J.X：参照剛体の座標系の変位
XT_[i]：親剛体から見た参照剛体の座標系の初期変位
これらの内積なので, Xlambda_[i]は親剛体から見た現在の参照剛体の座標系の変位.
ここでいう変位は並進と回転どちらも含む.
const label lambdai = lambda_[i];
親剛体を番号を出力する. 計算空間は0
剛体1に剛体2と3の二つが接続されている場合、
lambda_[0] = -1：計算空間は-1だが, おそらくこれはダミー
lambda_[1] = 0：計算空間(0)が親
lambda_[2] = 1：剛体1(1)が親
lambda_[3] = 1：剛体1(1)が親
if (lambdai != 0)
{
X0_[i] = Xlambda_[i] & X0_[lambdai];
}
else
{
X0_[i] = Xlambda_[i];
}
このif文は参照剛体の親が計算空間か否かを判定する.
親が計算空間ではない場合, 親剛体の座標系の変位を内積する.
親が計算空間の場合, 計算空間は動かないので参照剛体の座標系の変位を格納する.
結果的に, 一連のプログラムによってX0_に全剛体の計算空間から見た座標系の変位が格納される.
剛体1,2,3が直列に接続されている場合でも,
剛体1：計算空間から見た座標系
剛体2：剛体1から見た座標系&計算空間から見た剛体1の座標系
剛体3：剛体1から見た座標系&（剛体1から見た座標系&計算空間から見た剛体1の座標系）
となるのですべて計算空間から見た値になる.