39 if (restraints_.empty())
46 DebugInfo <<
"Restraint " << restraints_[ri].name();
49 restraints_[ri].restrain(tau, fx, state);
64 <<
"q = " << state.
q() <<
nl 65 <<
"qDot = " << state.
qDot() <<
nl 66 <<
"tau = " << tau <<
endl;
73 for (
label i=1; i<nBodies(); i++)
75 const joint& jnt = joints()[i];
81 Xlambda_[i] = J.
X & XT_[i];
83 const label lambdai = lambda_[i];
87 X0_[i] = Xlambda_[i] & X0_[lambdai];
94 v_[i] = (Xlambda_[i] & v_[lambdai]) + J.
v;
95 c_[i] = J.
c + (v_[i] ^ J.
v);
97 pA_[i] = v_[i] ^* (
I(i) & v_[i]);
101 pA_[i] -= *X0_[i] & fx[i];
105 for (
label i=nBodies()-1; i>0; i--)
107 const joint& jnt = joints()[i];
108 const label qi = jnt.qIndex();
115 const label lambdai = lambda_[i];
128 pA_[lambdai] += Xlambda_[i].T() & pa;
131 else if (jnt.
nDoF() == 1)
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]);
137 const label lambdai = lambda_[i];
143 IA_[i] - (U1_[i]*(Dinv_[i].xx()*U1_[i]))
148 pA_[i] + (Ia & c_[i]) + U1_[i]*(Dinv_[i].xx()*u_[i].
x())
156 pA_[lambdai] += Xlambda_[i].T() & pa;
161 U_[i] = IA_[i] & S_[i];
162 Dinv_[i] = (S_[i].T() & U_[i]).
inv();
165 const label lambdai = lambda_[i];
171 IA_[i] - (U_[i] & Dinv_[i] & U_[i].
T())
176 pA_[i] + (Ia & c_[i]) + (U_[i] & Dinv_[i] & u_[i])
184 pA_[lambdai] += Xlambda_[i].T() & pa;
191 for (
label i=1; i<nBodies(); i++)
193 const joint& jnt = joints()[i];
194 const label qi = jnt.qIndex();
196 a_[i] = (Xlambda_[i] & a_[lambda_[i]]) + c_[i];
202 else if (jnt.
nDoF() == 1)
204 qDdot[qi] = Dinv_[i].xx()*(u_[i].x() - (U1_[i] && a_[i]));
205 a_[i] += S1_[i]*qDdot[qi];
209 vector qDdoti(Dinv_[i] & (u_[i] - (U_[i].
T() & a_[i])));
212 qDdot[qi] = qDdoti.
x();
213 qDdot[qi+1] = qDdoti.
y();
214 qDdot[qi+2] = qDdoti.
z();
216 a_[i] += (S_[i] & qDdoti);
221 <<
"qDdot = " << qDdot <<
nl 222 <<
"a = " << a_ <<
endl;
241 for (
label i=1; i<nBodies(); i++)
243 const joint& jnt = joints()[i];
244 const label qi = jnt.qIndex();
251 Xlambda_[i] = J.
X & XT_[i];
253 const label lambdai = lambda_[i];
257 X0_[i] = Xlambda_[i] & X0_[lambdai];
261 X0_[i] = Xlambda_[i];
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];
272 else if (jnt.
nDoF() == 1)
274 a_[i] += S1_[i]*qDdot[qi];
Abstract base-class for all rigid-body joints.
Templated 3D spatial tensor derived from MatrixSpace used to represent transformations of spatial vec...
#define forAll(list, i)
Loop across all elements in list.
intWM_LABEL_SIZE_t label
A label is an int32_t or int64_t as specified by the pre-processor macro WM_LABEL_SIZE.
spatialVector v
The constrained joint velocity.
spatialVector S1
The joint motion sub-space (1-DoF)
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)
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.
Ostream & endl(Ostream &os)
Add newline and flush stream.
spatialVector c
The constrained joint acceleration correction.
SpatialTensor< scalar > spatialTensor
SpatialTensor of scalars.
Holds the motion state of rigid-body model.
Joint state returned by jcalc.
static const Identity< scalar > I
const scalarField & q() const
Return access to the joint position and orientation.
SpatialVector< scalar > spatialVector
SpatialVector of scalars.
#define DebugInFunction
Report an information message using Foam::Info.
const scalarField & qDot() const
Return access to the joint velocity.
VSForm block(const label start) const
void applyRestraints(scalarField &tau, Field< spatialVector > &fx, const rigidBodyModelState &state) const
Apply the restraints and accumulate the internal joint forces.
#define DebugInfo
Report an information message using Foam::Info.
label nDoF() const
Return the number of degrees of freedom in this joint.
spatialTransform X
The joint transformation.
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,.