forwardDynamics.C
Go to the documentation of this file.
1/*---------------------------------------------------------------------------*\
2 ========= |
3 \\ / F ield | OpenFOAM: The Open Source CFD Toolbox
4 \\ / O peration |
5 \\ / A nd | www.openfoam.com
6 \\/ M anipulation |
7-------------------------------------------------------------------------------
8 Copyright (C) 2016 OpenFOAM Foundation
9-------------------------------------------------------------------------------
10License
11 This file is part of OpenFOAM.
12
13 OpenFOAM is free software: you can redistribute it and/or modify it
14 under the terms of the GNU General Public License as published by
15 the Free Software Foundation, either version 3 of the License, or
16 (at your option) any later version.
17
18 OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
19 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
20 FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
21 for more details.
22
23 You should have received a copy of the GNU General Public License
24 along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>.
25
26\*---------------------------------------------------------------------------*/
27
28#include "rigidBodyModel.H"
29#include "rigidBodyModelState.H"
30#include "rigidBodyRestraint.H"
31
32// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
33
35(
36 scalarField& tau,
38 const rigidBodyModelState& state
39) const
40{
41 if (restraints_.empty())
42 {
43 return;
44 }
45
47 {
48 DebugInfo << "Restraint " << restraints_[ri].name();
49
50 // Accumulate the restraint forces
51 restraints_[ri].restrain(tau, fx, state);
52 }
53}
54
55
57(
59 const scalarField& tau,
60 const Field<spatialVector>& fx
61) const
62{
63 const scalarField& q = state.q();
64 const scalarField& qDot = state.qDot();
65 scalarField& qDdot = state.qDdot();
66
68 << "q = " << q << nl
69 << "qDot = " << qDot << nl
70 << "tau = " << tau << endl;
71
72 // Joint state returned by jcalc
74
75 v_[0] = Zero;
76
77 for (label i=1; i<nBodies(); i++)
78 {
79 const joint& jnt = joints()[i];
80 jnt.jcalc(J, q, qDot);
81
82 S_[i] = J.S;
83 S1_[i] = J.S1;
84
85 Xlambda_[i] = J.X & XT_[i];
86
87 const label lambdai = lambda_[i];
88
89 if (lambdai != 0)
90 {
91 X0_[i] = Xlambda_[i] & X0_[lambdai];
92 }
93 else
94 {
95 X0_[i] = Xlambda_[i];
96 }
97
98 v_[i] = (Xlambda_[i] & v_[lambdai]) + J.v;
99 c_[i] = J.c + (v_[i] ^ J.v);
100 IA_[i] = I(i);
101 pA_[i] = v_[i] ^* (I(i) & v_[i]);
102
103 if (fx.size())
104 {
105 pA_[i] -= *X0_[i] & fx[i];
106 }
107 }
108
109 for (label i=nBodies()-1; i>0; i--)
110 {
111 const joint& jnt = joints()[i];
112 const label qi = jnt.qIndex();
113
114 if (jnt.nDoF() == 1)
115 {
116 U1_[i] = IA_[i] & S1_[i];
117 Dinv_[i].xx() = 1/(S1_[i] && U1_[i]);
118 u_[i].x() = tau[qi] - (S1_[i] && pA_[i]);
119
120 const label lambdai = lambda_[i];
121
122 if (lambdai != 0)
123 {
124 const spatialTensor Ia
125 (
126 IA_[i] - (U1_[i]*(Dinv_[i].xx()*U1_[i]))
127 );
128
129 const spatialVector pa
130 (
131 pA_[i] + (Ia & c_[i]) + U1_[i]*(Dinv_[i].xx()*u_[i].x())
132 );
133
134 IA_[lambdai] +=
135 spatialTensor(Xlambda_[i].T())
136 & Ia
137 & spatialTensor(Xlambda_[i]);
138
139 pA_[lambdai] += Xlambda_[i].T() & pa;
140 }
141 }
142 else
143 {
144 U_[i] = IA_[i] & S_[i];
145 Dinv_[i] = (S_[i].T() & U_[i]).inv();
146
147 u_[i] = tau.block<vector>(qi) - (S_[i].T() & pA_[i]);
148
149 const label lambdai = lambda_[i];
150
151 if (lambdai != 0)
152 {
154 (
155 IA_[i]
156 - (U_[i] & Dinv_[i] & U_[i].T())
157 );
158
160 (
161 pA_[i]
162 + (Ia & c_[i])
163 + (U_[i] & Dinv_[i] & u_[i])
164 );
165
166 IA_[lambdai] +=
167 spatialTensor(Xlambda_[i].T())
168 & Ia
169 & spatialTensor(Xlambda_[i]);
170
171 pA_[lambdai] += Xlambda_[i].T() & pa;
172 }
173 }
174 }
175
176 a_[0] = spatialVector(Zero, -g_);
177
178 for (label i=1; i<nBodies(); i++)
179 {
180 const joint& jnt = joints()[i];
181 const label qi = jnt.qIndex();
182
183 a_[i] = (Xlambda_[i] & a_[lambda_[i]]) + c_[i];
184
185 if (jnt.nDoF() == 1)
186 {
187 qDdot[qi] = Dinv_[i].xx()*(u_[i].x() - (U1_[i] && a_[i]));
188 a_[i] += S1_[i]*qDdot[qi];
189 }
190 else
191 {
192 vector qDdoti(Dinv_[i] & (u_[i] - (U_[i].T() & a_[i])));
193
194 // Need to add mutable "block<vector>" to Field
195 qDdot[qi] = qDdoti.x();
196 qDdot[qi+1] = qDdoti.y();
197 qDdot[qi+2] = qDdoti.z();
198
199 a_[i] += (S_[i] & qDdoti);
200 }
201 }
202
204 << "qDdot = " << qDdot << nl
205 << "a = " << a_ << endl;
206}
207
208
210(
211 const rigidBodyModelState& state
212) const
213{
215
216 const scalarField& q = state.q();
217 const scalarField& qDot = state.qDot();
218 const scalarField& qDdot = state.qDdot();
219
220 // Joint state returned by jcalc
221 joint::XSvc J;
222
223 v_[0] = Zero;
224 a_[0] = spatialVector(Zero, -g_);
225
226 for (label i=1; i<nBodies(); i++)
227 {
228 const joint& jnt = joints()[i];
229 const label qi = jnt.qIndex();
230
231 jnt.jcalc(J, q, qDot);
232
233 S_[i] = J.S;
234 S1_[i] = J.S1;
235
236 Xlambda_[i] = J.X & XT_[i];
237
238 const label lambdai = lambda_[i];
239
240 if (lambdai != 0)
241 {
242 X0_[i] = Xlambda_[i] & X0_[lambdai];
243 }
244 else
245 {
246 X0_[i] = Xlambda_[i];
247 }
248
249 v_[i] = (Xlambda_[i] & v_[lambdai]) + J.v;
250 c_[i] = J.c + (v_[i] ^ J.v);
251 a_[i] = (Xlambda_[i] & a_[lambdai]) + c_[i];
252
253 if (jnt.nDoF() == 1)
254 {
255 a_[i] += S1_[i]*qDdot[qi];
256 }
257 else
258 {
259 a_[i] += S_[i] & qDdot.block<vector>(qi);
260 }
261 }
262
263 DebugInfo<< "a = " << a_ << endl;
264}
265
266
267// ************************************************************************* //
VSForm block(const label start) const
Definition: Field.C:593
Joint state returned by jcalc.
Definition: joint.H:125
spatialVector c
The constrained joint acceleration correction.
Definition: joint.H:142
compactSpatialTensor S
The joint motion sub-space (3-DoF)
Definition: joint.H:132
spatialVector v
The constrained joint velocity.
Definition: joint.H:138
spatialVector S1
The joint motion sub-space (1-DoF)
Definition: joint.H:135
spatialTransform X
The joint transformation.
Definition: joint.H:129
Abstract base-class for all rigid-body joints.
Definition: joint.H:85
label nDoF() const
Return the number of degrees of freedom in this joint.
Definition: jointI.H:40
virtual void jcalc(XSvc &J, const scalarField &q, const scalarField &qDot) const =0
Update the rigidBodyModel state for the joint given.
Holds the motion state of rigid-body model.
const scalarField & qDdot() const
Return access to the joint acceleration.
const scalarField & qDot() const
Return access to the joint velocity.
const scalarField & q() const
Return access to the joint position and orientation.
void forwardDynamicsCorrection(const rigidBodyModelState &state) const
Correct the velocity and acceleration of the bodies in the model.
void applyRestraints(scalarField &tau, Field< spatialVector > &fx, const rigidBodyModelState &state) const
Apply the restraints and accumulate the internal joint forces.
PtrList< restraint > restraints_
Motion restraints.
void forwardDynamics(rigidBodyModelState &state, const scalarField &tau, const Field< spatialVector > &fx) const
Calculate the joint acceleration qDdot from the joint state q,.
Templated 3D spatial tensor derived from MatrixSpace used to represent transformations of spatial vec...
Definition: SpatialTensor.H:70
void size(const label n)
Older name for setAddressableSize.
Definition: UList.H:114
const Cmpt & z() const
Access to the vector z component.
Definition: VectorI.H:85
const Cmpt & y() const
Access to the vector y component.
Definition: VectorI.H:79
const Cmpt & x() const
Access to the vector x component.
Definition: VectorI.H:73
const volScalarField & T
#define DebugInfo
Report an information message using Foam::Info.
#define DebugInFunction
Report an information message using Foam::Info.
SpatialTensor< scalar > spatialTensor
SpatialTensor of scalars.
Definition: spatialTensor.H:50
static const Identity< scalar > I
Definition: Identity.H:94
Ostream & endl(Ostream &os)
Add newline and flush stream.
Definition: Ostream.H:372
SpatialVector< scalar > spatialVector
SpatialVector of scalars.
Definition: spatialVector.H:50
static constexpr const zero Zero
Global zero (0)
Definition: zero.H:131
dimensionedSphericalTensor inv(const dimensionedSphericalTensor &dt)
constexpr char nl
The newline '\n' character (0x0a)
Definition: Ostream.H:53
#define forAll(list, i)
Loop across all elements in list.
Definition: stdFoam.H:333