Class BodySegment
Represents a single rigid segment in a kinematic chain, storing its orientation (global and local), position, and relationship to parent/child segments.
Inherited Members
Namespace: MOSAIC.Components.BodyRig
Assembly: MOSAIC.dll
Syntax
public class BodySegment
Remarks
Each segment holds a DhRelativeToGyro quaternion that maps from the sensor (gyroscope/IMU) frame to the segment's Denavit–Hartenberg frame. When UpdateOrientation(QuaternionF) is called with a new sensor quaternion, the segment computes its global orientation, and — if a parent exists — derives local orientation and position via forward kinematics.
The kinematic chain is a tree: each segment has one Parent and any number of Children, so a torso can carry several limbs. Calling UpdateOrientation(QuaternionF) propagates recursively down every branch.
Constructors
BodySegment()
Initialises a body segment with a default DH offset of −90° yaw (about Z) and a unit link along the X axis.
Declaration
public BodySegment()
BodySegment(float, float, float, float, float, float)
Initialises a body segment with explicit DH Euler angles (degrees) and link length.
Declaration
public BodySegment(float dhRoll, float dhPitch, float dhYaw, float linkX, float linkY, float linkZ)
Parameters
| Type | Name | Description |
|---|---|---|
| float | dhRoll | DH roll angle (about X) in degrees. |
| float | dhPitch | DH pitch angle (about Y) in degrees. |
| float | dhYaw | DH yaw angle (about Z) in degrees. |
| float | linkX | Link length X component. |
| float | linkY | Link length Y component. |
| float | linkZ | Link length Z component. |
Fields
_children
Declaration
private readonly List<BodySegment> _children
Field Value
| Type | Description |
|---|---|
| List<BodySegment> |
_poseForCalibration
Declaration
private QuaternionF _poseForCalibration
Field Value
| Type | Description |
|---|---|
| QuaternionF |
Properties
Children
Child segments hanging off this one. Empty for a terminal segment.
Declaration
public IReadOnlyList<BodySegment> Children { get; }
Property Value
| Type | Description |
|---|---|
| IReadOnlyList<BodySegment> |
Remarks
A list rather than a single reference because real rigs branch. The lab's
double_hand model hangs two arms off one torso and bimanual hangs three
branches off it; while this was a single Child, attaching the second one severed
the first, so those models loaded as one intact chain plus limbs floating at the origin —
and nothing reported it.
DhRelativeToGyro
Quaternion that maps from the sensor (IMU/gyroscope) frame to the segment's Denavit–Hartenberg reference frame. Set during construction or calibration.
Declaration
public QuaternionF DhRelativeToGyro { get; private set; }
Property Value
| Type | Description |
|---|---|
| QuaternionF |
LinkLength
Link length vector that defines the offset from this segment's proximal joint to its distal end, expressed in the segment's own frame.
Declaration
public Vector3F LinkLength { get; set; }
Property Value
| Type | Description |
|---|---|
| Vector3F |
LocalOrientation
Orientation relative to the parent segment. For the root segment this equals Orientation.
Declaration
public QuaternionF LocalOrientation { get; set; }
Property Value
| Type | Description |
|---|---|
| QuaternionF |
LocalPosition
Position relative to the parent segment's frame. For the root segment this equals Position.
Declaration
public Vector3F LocalPosition { get; set; }
Property Value
| Type | Description |
|---|---|
| Vector3F |
Name
Human-readable name for this segment, e.g. "rightForeArm". Optional, but a chain of segments called "segment 0".."segment 9" tells a reader nothing about the body it models.
Declaration
public string? Name { get; set; }
Property Value
| Type | Description |
|---|---|
| string |
OrdinalIndex
Ordinal position in the kinematic chain (0 = root).
Declaration
public int OrdinalIndex { get; set; }
Property Value
| Type | Description |
|---|---|
| int |
Orientation
Global (world-frame) orientation of this segment.
Declaration
public QuaternionF Orientation { get; set; }
Property Value
| Type | Description |
|---|---|
| QuaternionF |
Parent
Parent segment in the kinematic chain, or null for the root.
Declaration
public BodySegment? Parent { get; set; }
Property Value
| Type | Description |
|---|---|
| BodySegment |
Position
Global (world-frame) position of this segment's distal end.
Declaration
public Vector3F Position { get; set; }
Property Value
| Type | Description |
|---|---|
| Vector3F |
RotationOffset
Optional additional rotation offset applied before the DH transform.
Declaration
public QuaternionF RotationOffset { get; set; }
Property Value
| Type | Description |
|---|---|
| QuaternionF |
SensorIndex
Sensor or hardware index associated with this segment (e.g. IMU slot number).
Declaration
public int SensorIndex { get; set; }
Property Value
| Type | Description |
|---|---|
| int |
Methods
AddChild(BodySegment)
Adds child to this segment's children, if not already one.
Declaration
public void AddChild(BodySegment child)
Parameters
| Type | Name | Description |
|---|---|---|
| BodySegment | child |
Remarks
Deliberately one-directional: it does not touch Parent. Maintaining both
links is BodyRig.SetParent's job and should stay its alone — keeping the two apart
is also what lets IsDescendantOf(BodySegment)'s loop guard be tested against a
deliberately corrupted graph.
Calibrate(QuaternionF)
Calibrates the DH-to-sensor mapping by recording the current sensor orientation. After calibration, DhRelativeToGyro is set so that the calibration pose corresponds to the recorded sensor reading.
Declaration
public void Calibrate(QuaternionF currentSensorOrientation)
Parameters
| Type | Name | Description |
|---|---|---|
| QuaternionF | currentSensorOrientation | The raw quaternion from the IMU at the moment of calibration. |
Calibrate(float, float, float, float)
Calibrates from individual components in (X, Y, Z, W) order.
Declaration
public void Calibrate(float x, float y, float z, float w)
Parameters
| Type | Name | Description |
|---|---|---|
| float | x | |
| float | y | |
| float | z | |
| float | w |
Calibrate(float[])
Calibrates from a float array in [W, X, Y, Z] order (Muovi/Delsys convention).
Declaration
public void Calibrate(float[] wxyz)
Parameters
| Type | Name | Description |
|---|---|---|
| float[] | wxyz | A 4-element array: |
InverseKinematicsArm()
Computes 3-DOF joint angles for a shoulder-like joint from LocalOrientation.
Declaration
public float[] InverseKinematicsArm()
Returns
| Type | Description |
|---|---|
| float[] | Array of three angles in degrees: |
InverseKinematicsElbow()
Computes 2-DOF joint angles for an elbow-like joint from LocalPosition and LocalOrientation.
Declaration
public float[] InverseKinematicsElbow()
Returns
| Type | Description |
|---|---|
| float[] | Array of two angles in degrees: |
IsChildOf(BodySegment)
Tests whether this segment is the immediate child of potentialParent.
Declaration
public bool IsChildOf(BodySegment potentialParent)
Parameters
| Type | Name | Description |
|---|---|---|
| BodySegment | potentialParent |
Returns
| Type | Description |
|---|---|
| bool |
IsDescendantOf(BodySegment)
Tests whether this segment is a descendant of potentialAncestor
anywhere in the kinematic chain.
Declaration
public bool IsDescendantOf(BodySegment potentialAncestor)
Parameters
| Type | Name | Description |
|---|---|---|
| BodySegment | potentialAncestor |
Returns
| Type | Description |
|---|---|
| bool |
RemoveChild(BodySegment)
Removes child from this segment's children.
Declaration
public void RemoveChild(BodySegment child)
Parameters
| Type | Name | Description |
|---|---|---|
| BodySegment | child |
SetCalibrationPose(QuaternionF)
Sets the reference pose quaternion used during Calibrate(QuaternionF).
Declaration
public void SetCalibrationPose(QuaternionF pose)
Parameters
| Type | Name | Description |
|---|---|---|
| QuaternionF | pose | The desired orientation the segment should represent at calibration. |
SetDh(float, float, float)
Sets the DH-to-sensor mapping from Euler angles in degrees, in the
(roll, pitch, yaw) order produced by ToEulerAngles().
Declaration
public void SetDh(float roll, float pitch, float yaw)
Parameters
| Type | Name | Description |
|---|---|---|
| float | roll | |
| float | pitch | |
| float | yaw |
SetDh(float, float, float, float)
Sets the DH-to-sensor mapping from explicit quaternion components (W, X, Y, Z).
Declaration
public void SetDh(float w, float x, float y, float z)
Parameters
| Type | Name | Description |
|---|---|---|
| float | w | |
| float | x | |
| float | y | |
| float | z |
Remarks
Normalised on the way in, and this is not cosmetic. The chain propagation in
UpdateOrientation(QuaternionF) hands a child O · D̄ and the child
immediately recomputes (O · D̄) · D. Because D̄ · D = |D|² rather than 1,
a non-unit D multiplies the child's quaternion by |D|² on every
parent update: at the rates these rigs run, |D| = 0.975 drives the limb to the origin
within a second, and |D| > 1 overflows to infinity — where the finiteness guard in
BodyRig.UpdateSampleBuffer then silently republishes stale values while the frame
counter keeps rising, so the rig looks healthy and is frozen.
Values reach here unvalidated from LoadCalibration, which parses four raw floats
out of a text file.
UpdateOrientation(QuaternionF)
Updates this segment's orientation from a new sensor reading and propagates forward kinematics down the chain.
Declaration
public void UpdateOrientation(QuaternionF sensorOrientation)
Parameters
| Type | Name | Description |
|---|---|---|
| QuaternionF | sensorOrientation | The raw quaternion from the IMU sensor. |
Remarks
The global orientation is computed as sensorOrientation × DhRelativeToGyro.
If a Parent exists, LocalOrientation and
Position are derived relative to the parent.
Every segment in Children is then recursively updated, so each branch of the chain stays geometrically consistent with this segment's new tip.
UpdateOrientation(float, float, float, float)
Updates orientation from individual components in (X, Y, Z, W) order.
Declaration
public void UpdateOrientation(float x, float y, float z, float w)
Parameters
| Type | Name | Description |
|---|---|---|
| float | x | |
| float | y | |
| float | z | |
| float | w |
UpdateOrientation(float[])
Updates orientation from a float array in [W, X, Y, Z] order.
Declaration
public void UpdateOrientation(float[] wxyz)
Parameters
| Type | Name | Description |
|---|---|---|
| float[] | wxyz | A 4-element array: |