6D Body

6DOF rigid body with gravity

Collaboration diagram for 6D Body:

Simulation of a rigid body in six degrees of freedom with gravity. Implemented in class C6DBody.

\begin{eqnarray*} \mathbf{\dot{x}}_{1} &=&\mathbf{R}_{b}^{n}\mathbf{x}_{3}, \\ \mathbf{\dot{x}}_{2} &=&\mathbf{T}_{\mathbf{\Theta }}\left( \mathbf{x}_{2}\right) \mathbf{x}_{4}, \\ \left[ \begin{array}{c} \mathbf{\dot{x}}_{3} \\ \mathbf{\dot{x}}_{4} \end{array} \right] &=&\left( \mathbf{M}_{b}^{RB}\right) ^{-1}\left( \mathbf{\sum \tau }^{b} \right), \\ \end{eqnarray*}

where the states are designated

\begin{eqnarray*} \mathbf{x} &=&\left[ \begin{array}{cccc} \mathbf{x}_{1}^{T} & \mathbf{x}_{2}^{T} & \mathbf{x}_{3}^{T} & \mathbf{x}_{4}^{T}\end{array}\right] ^{T}, \\ \mathbf{x}_{1} &\mathbf{=}&\left[ \begin{array}{ccc} x_{bn}^{n} & y_{bn}^{n} & z_{bn}^{n}\end{array}\right] ^{T}, \\ \mathbf{x}_{2} &\mathbf{=}&\left[ \begin{array}{ccc} \varphi & \theta & \psi\end{array}\right] ^{T}, \\ \mathbf{x}_{3} &\mathbf{=}&\left[ \begin{array}{ccc} u_{bn}^{b} & v_{bn}^{b} & w_{bn}^{b}\end{array}\right] ^{T}, \\ \mathbf{x}_{4} &\mathbf{=}&\left[ \begin{array}{ccc} p_{bn}^{b} & q_{bn}^{b} & r_{bn}^{b}\end{array} \right] ^{T}, \end{eqnarray*}

and \( \mathbf{M}_{b}^{RB}\in \mathbf{R}^{6\times 6} \) is the rigid body mass matrix of the body. In this SimObject, the mass matrix is taken to be diagonal, and it is calculated based on the mass and the specified inertia radi. \(\mathbf{\tau}^{b}\), are the sum of the forces and moments acting on the body. The Coriolis force terms are neglected due to an assumption of low speeds and small mass of the system.

Bug:

Orientation is wrongly defined above. Define using quaternion instead.

Todo:

Fix documentation equations.

Author

Karl-Johan Reite

Karl Gunnar Aarsaether

Date

07.12.2010 KGA: Added documentation.

25.02.2011 KJR: Ported to FhSim v2.0, added model documentation.

Example configuration excerpt

<Lib
  LibName      = "fhsim_base"
  SimObject    = "Ball/6DBody"
  Name         = "A"
  Mass         = "1"
  InertiaRadi  = "1,1,1"
/>
...
<Connection
  A.Force="0,0,-9.81"
  A.Moment="0,0,0"
/>
...
<InitialCondition
  A.Pos="0,0,0"
  A.Quater="1,0,0,0"
  A.LocalVel="0,5,0"
  A.AngVel="5,0,0"
/>

Input Ports

Name	Width	Description
Force	3	Force acting on the body at the centre of gravity [N].
Moment	3	Moment around the center of gravity [Nm].

Output Ports

Name	Width	Description
Pos	3	Position of the body center of gravity.
Quater	4	Quaternion orientation of the body.
LocalVel	3	Linear velocity of the body.
AngVel	3	Angular velocity of the body.

Configuration parameters

Name	Width	Description
Mass	1	Mass of the object in kg.
Scale	3	Scale (Default: [1,1,1]).
InertiaRadi	3	Inertia ?.
Material	1	The visualization material (Default: Simple/Black).
Mesh	1	The visualization mesh (geometry) (Default: fhSphere8).
MeshScale	3	Mesh scale in visualization (Default: [1,1,1].
MeshOrigin	3	Mesh origin relative to body center of gravity in visualization (Default: [0,0,0].
Size	3	Body size, used with MeshScale to set visualization size (Default: [1,1,1]).

Initial conditions

Name	Width	Description
Pos	3	3D position [m].
Qauter	4	Quaternion orientation.
LocalVel	3	Linear velocity [m/s].
AngVel	3	Angular velocity [rad/s].

Full example file

<Contents>
  <OBJECTS>
    <Lib
        LibName="base"
        SimObject="Body/6DBody"
        Name="A"
        Mass ="1"
        InertiaRadi="1,1,1"
       />
 
  </OBJECTS>
 
  <INTERCONNECTIONS>
    <Connection
    A.Force="0,0,-9.81"
    A.Moment="0,0,0"
    />
  </INTERCONNECTIONS>
 
  <INITIALIZATION>
    <InitialCondition
    A.Pos="0,0,0"
    A.Quater="1,0,0,0"
    A.LocalVel="0,5,0"
    A.AngVel="5,0,0"
    />
  </INITIALIZATION>
 
  <INTEGRATION>
    <Engine
    IntegratorMethod="2"
    NumCores="1"
    TOutput="0, 0:1:20, 21"
    LogStates ="1"
    stepsize ="0"
    HMax="0.002"
    HMin="0.00000001"
    AbsTol="1e-3" RelTol="1e-3"
    UseRSSNormInsteadOfInfNorm="0"
    FileOutput="objects:all"
    />
  </INTEGRATION>
</Contents>

This SimObject is referred to as Body/6DBody