|
Coordinate System: The coordinate system is
right hand rule. X cross Y gives Z and Roll is right
hand rule about X, Pitch is right hand rule about Y, and
Yaw is right hand rule about Z.
Vector Storage:
All linear three vectors (LinPos, LinVel) store their
values as [X, Y, Z].
All angular three vectors (AngVel)
store their values as [Roll, Pitch, Yaw].
The
transformation matrix TM is stored:
Pre-multiplying a local vector by the TM will generate
the vector in global coordinates.
Multibody Dynamics, Link Numbers and 60 and 20 Length
Vectors:
Please refer to
Multibody Dynamics and 60 Length Vectors for
information on 60
length vectors.
Methodology:
The forces merge takes the input position, velocity,
and acceleration from Side 3 and directly outputs it to
Side 1, Side 2, and Side 4. It also propagates the Link
Numbers from Side 3 to Side 1, 2, and 4.
The joint also takes the internal forces and moments
from Side 1, Side 2, and Side 3 and sums them and
outputs them on Side 4.
It only sums the internal forces up to the number of
links in LinkNumber3, since the mass matrix is
symmetric.
Specifically:
for(i=0; i<=LinkNumber3[0]*3; i++){
Force3[i]=Force1[i]+Force2[i]+Force4[i];
Moment3[i]=Moment1[i]+Moment2[i]+Moment4[i];
}
|
