69 <<
"qDot = " << qDot <<
nl
70 <<
"tau = " << tau <<
endl;
77 for (label i=1; i<nBodies(); i++)
79 const joint& jnt = joints()[i];
80 jnt.
jcalc(J, q, qDot);
85 Xlambda_[i] = J.
X & XT_[i];
87 const label lambdai = lambda_[i];
91 X0_[i] = Xlambda_[i] & X0_[lambdai];
98 v_[i] = (Xlambda_[i] & v_[lambdai]) + J.
v;
99 c_[i] = J.
c + (v_[i] ^ J.
v);
101 pA_[i] = v_[i] ^* (
I(i) & v_[i]);
105 pA_[i] -= *X0_[i] & fx[i];
109 for (label i=nBodies()-1; i>0; i--)
111 const joint& jnt = joints()[i];
112 const label qi = jnt.qIndex();
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]);
120 const label lambdai = lambda_[i];
126 IA_[i] - (U1_[i]*(Dinv_[i].xx()*U1_[i]))
131 pA_[i] + (Ia & c_[i]) + U1_[i]*(Dinv_[i].xx()*u_[i].
x())
139 pA_[lambdai] += Xlambda_[i].T() & pa;
144 U_[i] = IA_[i] & S_[i];
145 Dinv_[i] = (S_[i].T() & U_[i]).
inv();
149 const label lambdai = lambda_[i];
156 - (U_[i] & Dinv_[i] & U_[i].
T())
163 + (U_[i] & Dinv_[i] & u_[i])
171 pA_[lambdai] += Xlambda_[i].T() & pa;
178 for (label i=1; i<nBodies(); i++)
180 const joint& jnt = joints()[i];
181 const label qi = jnt.qIndex();
183 a_[i] = (Xlambda_[i] & a_[lambda_[i]]) + c_[i];
187 qDdot[qi] = Dinv_[i].xx()*(u_[i].x() - (U1_[i] && a_[i]));
188 a_[i] += S1_[i]*qDdot[qi];
192 vector qDdoti(Dinv_[i] & (u_[i] - (U_[i].
T() & a_[i])));
195 qDdot[qi] = qDdoti.
x();
196 qDdot[qi+1] = qDdoti.
y();
197 qDdot[qi+2] = qDdoti.
z();
199 a_[i] += (S_[i] & qDdoti);
204 <<
"qDdot = " << qDdot <<
nl
205 <<
"a = " << a_ <<
endl;
226 for (label i=1; i<nBodies(); i++)
228 const joint& jnt = joints()[i];
229 const label qi = jnt.qIndex();
231 jnt.
jcalc(J, q, qDot);
236 Xlambda_[i] = J.
X & XT_[i];
238 const label lambdai = lambda_[i];
242 X0_[i] = Xlambda_[i] & X0_[lambdai];
246 X0_[i] = Xlambda_[i];
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];
255 a_[i] += S1_[i]*qDdot[qi];
VSForm block(const label start) const
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 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...
void size(const label n)
Older name for setAddressableSize.
const Cmpt & z() const
Access to the vector z component.
const Cmpt & y() const
Access to the vector y component.
const Cmpt & x() const
Access to the vector x component.
#define DebugInfo
Report an information message using Foam::Info.
#define DebugInFunction
Report an information message using Foam::Info.
SpatialTensor< scalar > spatialTensor
SpatialTensor of scalars.
static const Identity< scalar > I
Ostream & endl(Ostream &os)
Add newline and flush stream.
SpatialVector< scalar > spatialVector
SpatialVector of scalars.
static constexpr const zero Zero
Global zero (0)
dimensionedSphericalTensor inv(const dimensionedSphericalTensor &dt)
constexpr char nl
The newline '\n' character (0x0a)
#define forAll(list, i)
Loop across all elements in list.