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];
#define forAll(list, i)
Loop across all elements in list.
VSForm block(const label start) const
void size(const label)
Override size to be inconsistent with allocated storage.
Joint state returned by jcalc.
spatialVector c
The constrained joint acceleration correction.
compactSpatialTensor S
The joint motion sub-space (3-DoF)
spatialVector v
The constrained joint velocity.
spatialVector S1
The joint motion sub-space (1-DoF)
spatialTransform X
The joint transformation.
Abstract base-class for all rigid-body joints.
label nDoF() const
Return the number of degrees of freedom in this joint.
virtual void jcalc(XSvc &J, const rigidBodyModelState &state) 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...
#define DebugInfo
Report an information message using Foam::Info.
#define DebugInFunction
Report an information message using Foam::Info.
intWM_LABEL_SIZE_t label
A label is an int32_t or int64_t as specified by the pre-processor macro WM_LABEL_SIZE.
Ostream & endl(Ostream &os)
Add newline and flush stream.
SpatialTensor< scalar > spatialTensor
SpatialTensor of scalars.
static const Identity< scalar > I
SpatialVector< scalar > spatialVector
SpatialVector of scalars.
dimensionedSphericalTensor inv(const dimensionedSphericalTensor &dt)
void T(FieldField< Field, Type > &f1, const FieldField< Field, Type > &f2)