EOM 6DOF Quaternion — Robotics/Equations Of Motion
Robotics/Equations_Of_Motion/EOM_6DOF_Quaternion · 2 input / 7 output port(s) at insert · exports to Python, MATLAB, Java, Rust, C, C++, VHDL, Verilog, SystemVerilog, PLC Structured Text
Description#
The block's own DESCRIPTION_HTML, rendered verbatim — the same text the config dialog's info panel and the library navigator show. Fix a wrong sentence in the block's .cpp (R-D9), never here.
EOM 6DOF Quaternion
Robotics / Equations Of Motion
The six-degree-of-freedom equations of motion of a rigid body of fixed mass, carried in body axes with the attitude as a quaternion q = [q0; q1; q2; q3], so there is no gimbal lock. With the body velocity Vb = [u; v; w], the body rates ω = [p; q; r], the inertia tensor I and the flat-earth position Xe:
- Vb' = F/m − ω × Vb
- ω' = I−1(M − ω × (Iω))
- q' = ½Ω(ω)q + K(1 − |q|2)q, with K = 1: the second term pulls the quaternion back to unit length should rounding move it
- Xe' = DCMbeT Vb
Ports
- F – the applied force in body axes, [3,1].
- M – the applied moment about the centre of gravity in body axes, [3,1].
- Ve – the velocity in the flat-earth frame, DCMbeT Vb, [3,1].
- Xe – the position in the flat-earth frame, [3,1].
- Euler – the Euler angles [φ; θ; ψ] in radians, read back from the DCM: φ = atan2(D23, D33), θ = asin(−D13), ψ = atan2(D12, D11), [3,1].
- q – the quaternion [q0; q1; q2; q3], [4,1].
- DCMbe – the direction cosine matrix from the flat-earth frame to body axes, [3,3].
- Vb – the velocity in body axes [u; v; w], [3,1].
- wb – the body rates [p; q; r] in rad/s, [3,1].
Parameters
- Units – Metric (MKS) (the default) or English (velocity in ft/s). The two are the same arithmetic, so the choice only says which units the numbers are in.
- Initial Position [Xe Ye Ze] – three values. Defaults to [0 0 0].
- Initial Velocity [U V W] – the initial body velocity, three values. Defaults to [0 0 0].
- Initial Euler Angles [phi theta psi] – in radians, three values; the initial quaternion is their 3-2-1 half-angle product. Defaults to [0 0 0].
- Initial Body Rates [p q r] – in rad/s, three values. Defaults to [0 0 0].
- Mass – m, a scalar > 0. Defaults to 1.
- Inertia – the inertia tensor I, a [3,3] with a nonzero determinant. Defaults to eye(3).
- Integration Substeps – M, how many fourth-order Runge-Kutta steps a sample is integrated with on the discrete solver and in exported code, a whole number of 1 or more. Defaults to 100. No Simulink counterpart.
- Sampling Time (s) – zero or less inherits the solver's rate; a positive value runs the block at that period.
Code export
All ten targets: Python, MATLAB, Java, Rust, C, C++, VHDL, Verilog, SystemVerilog and PLC Structured Text (which has no atan2, so the angle read-back rebuilds it from ATAN). A core holds the thirteen states, publishes the seven outputs from them, and integrates one sample with the inputs held and M Runge-Kutta substeps – with M = 100, the same computation as Simulink's fixed-step ode4 at one hundredth of the sample time. The mass and the inertia's inverse are folded to constants.
The three HDL targets are simulation-only real
arithmetic, quantized at the port: an arctangent of a state has no Q16.16 form. The cores
simulate correctly and are not offered as synthesizable.
Simulink bridge
Import and export, mapped to Aerospace Blockset's
aerolib6dof2/6DOF (Quaternion): Units →
units (the two offered values 1:1), Initial Position [Xe Ye Ze]
→ xme_0, Initial Velocity [U V W] →
Vm_0, Initial Euler Angles [phi theta psi] →
eul_0, Initial Body Rates [p q r] → pm_0,
Mass → mass_0 and Inertia →
inertia. Always emitted with mtype = Fixed
and vre_flag and mass_flag off: each of those moves that
block's port list, and this block has one. The knots unit system is not offered
– it converts velocities inside the integration – and an imported one
is reported. "Sampling Time (s)" does not cross: the Simulink block is
continuous and defines no SampleTime.
Notes
- Stateful, continuous and nonlinear: thirteen continuous states. The outputs are states or functions of them, so the block has no direct feedthrough and a loop through it is not an algebraic loop.
- No gimbal lock: the quaternion kinematics are regular at every attitude. The Euler-angle OUTPUT is still a read-back, so φ and ψ wrap at ±π and θ is clamped to the arcsine's domain at ±90°.
- The inertia's inverse is formed once, at configuration load, from the adjugate; the Simulink block solves the same system each step. The two agree to rounding.
Code facts#
| Fact | Value |
|---|---|
| registered type | Robotics/Equations_Of_Motion/EOM_6DOF_Quaternion |
| family | Robotics/Equations_Of_Motion |
| solver environment class | ICoreBlock_0_Robotics_1_Equations_Of_Motion_2_EOM_6DOF_Quaternion |
| source | src/ICoreBlocks/ICoreBlockLibrary/Blocks/Robotics/Equations_Of_Motion/EOM_6DOF_Quaternion/ICoreBlock_0_Robotics_1_Equations_Of_Motion_2_EOM_6DOF_Quaternion.cpp |
| header | src/ICoreBlocks/ICoreBlockLibrary/Blocks/Robotics/Equations_Of_Motion/EOM_6DOF_Quaternion/ICoreBlock_0_Robotics_1_Equations_Of_Motion_2_EOM_6DOF_Quaternion.h |
| default size on canvas | 150 × 150 px |
| ports at insert | 2 in, 7 out |
| code generators implemented | Python, MATLAB, Java, Rust, C, C++, VHDL, Verilog, SystemVerilog, PLC Structured Text |
Ports#
| # | Direction | Signal type | Description label |
|---|---|---|---|
| 1 | in | ICoreDouble | F |
| 2 | in | ICoreDouble | M |
| 3 | out | ICoreDouble | Ve |
| 4 | out | ICoreDouble | Xe |
| 5 | out | ICoreDouble | Euler |
| 6 | out | ICoreDouble | q |
| 7 | out | ICoreDouble | DCMbe |
| 8 | out | ICoreDouble | Vb |
| 9 | out | ICoreDouble | wb |
Ports the constructor creates. A block whose port list changes with its configuration adds or removes ports at load time; the count above is the one a freshly inserted block has.
Configuration variables#
| Config variable | Default | Simulink parameter |
|---|---|---|
Units | Metric (MKS)%~%English (velocity in ft/s)~~Metric (MKS) | units |
Initial Position [Xe Ye Ze] | [0 0 0] | xme_0 |
Initial Velocity [U V W] | [0 0 0] | Vm_0 |
Initial Euler Angles [phi theta psi] | [0 0 0] | eul_0 |
Initial Body Rates [p q r] | [0 0 0] | pm_0 |
Mass | 1.0 | mass_0 |
Inertia | [1 0 0; 0 1 0; 0 0 1] | inertia |
Integration Substeps | 100 | not crossed |
Every block also carries Sampling Time (s) from ICoreBlockSolverEnvironment: zero or less inherits the solver's rate, a positive value runs the block at that period.
Simulink bridge#
| support | Support::Both |
| Simulink path | aerolib6dof2/6DOF (Quaternion) |
| port-count rule | PortsParam::None |
SampleTime parameter | no — the counterpart defines none; the rate stays on the ICore side |
| deliberately not crossed | Integration Substeps |
| always set | mtype = Fixed, vre_flag = off, mass_flag = off |
| ICore config | Simulink parameter | Value translation |
|---|---|---|
Units | units | Metric (MKS) → Metric (MKS), English (velocity in ft/s) → English (velocity in ft/s) |
Initial Position [Xe Ye Ze] | xme_0 | passes through |
Initial Velocity [U V W] | Vm_0 | passes through |
Initial Euler Angles [phi theta psi] | eul_0 | passes through |
Initial Body Rates [p q r] | pm_0 | passes through |
Mass | mass_0 | passes through |
Inertia | inertia | passes through |
Caveat (shown to the user): aerolib6dof2/6DOF (Quaternion) is continuous and has NO SampleTime parameter (verified against the R2026a block dialog). mtype, vre_flag and mass_flag are pinned because each moves that block's port list; the kts unit system is not offered because it converts velocities inside the integration; "Integration Substeps" is how this block integrates a sample and has no counterpart
Catalog contract: src/ICoreBlocks/ICoreCoder/ICoreCommandSystem/SimulinkBridge/ICoreSimulinkBlockCatalog.h
Description vs code#
The lists agree. check_block_descriptions.py finds no disagreement between the description's Ports, Parameters, Code export and Simulink bridge lists and the code's.
The verdict above is
tools/docs/check_block_descriptions.py(P7.1), which compares LISTS. It cannot read a sentence: "stateless" on a block with a state, an initial-value semantic the recursion does not implement, a "not synthesizable" caveat the HDL banner contradicts. That is the agent audit (P7.3) on BLOCK_DESCRIPTION_AUDIT.md, and this tool's green is not a substitute for one.
File banner (developer view)#
The top comment of the block's .cpp — the maths, the realization and the export strategy, addressed to whoever changes it. It must not contradict the description above (P7.5).
6DOF (Quaternion) -- rigid-body equations of motion, fixed mass, quaternion attitude State [Vb (u v w), omega (p q r), quaternion (q0 q1 q2 q3), Xe (xe ye ze)], inputs F and M ([3,1] each, body axes):
Vb' = F/m - omega x Vb omega' = I^-1 (M - omega x (I omega)) q' = 1/2 [0 -p -q -r; p 0 r -q; q -r 0 p; r q -p 0] q + K*(1 - |q|^2)*q, K = 1 Xe' = DCMbe' Vb, DCMbe the quaternion's direction cosines [phi theta psi] = [atan2(D23, D33), asin(-D13), atan2(D12, D11)] of that DCM
⚠ MEASURED AGAINST R2026a: aerolib6dof2/6DOF (Quaternion) is a compiled EOM6DOFBodyQuat block. Integrated with ode4 at 1e-4 for 0.5 s from the Euler block's probe state, the equations above reproduce omega to every printed digit and the quaternion, DCM, angles and position to ~1e-15. What else the measurement settled:
- SEVEN outputs, in the order Ve, Xe, [phi theta psi], quaternion [4,1], DCMbe [3,3], Vb,
omega -- the Euler block's six with the quaternion inserted fourth.
- The initial quaternion is the 3-2-1 half-angle product of the dialog's Euler angles, and
the angles read back through the DCM (atan2, asin, atan2).
- The normalization term K*(1 - |q|^2)*q is INVISIBLE at this scale (K = 0, 1 and 2 all land
within an ULP of the block over 5000 steps), and the R2026a dialog exposes no gain. K = 1 is the documented form and is what this block uses.
omega' uses the inertia's inverse, formed once at config load (the block solves); the asin is clamped to [-1, 1], where rounding at theta = +/-90 deg could otherwise leave its domain. Units, pins and the ode4 equivalence are the Euler block's.
Sample results#
Plotted: vector — Sine Wave, [3,1]: amplitudes 1/2/3 at 2 rad/s (tried only because every scalar stimulus was refused)
Category dynamic · sample time 0.1 · 60 steps · commit 8f276a0f4 · produced by docsSample --out <folder> --blocks EOM_6DOF_Quaternion --steps 60 · data docs/generated/samples/Robotics__Equations_Of_Motion__EOM_6DOF_Quaternion.json · the SVG is generated from those numbers by tools/docs/plot_svg.py, so it is a run and not a drawing (R-D10).