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