Class BodySegment

Represents a single rigid segment in a kinematic chain, storing its orientation (global and local), position, and relationship to parent/child segments.

Inheritance
BodySegment
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: { W, X, Y, Z }.

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: [roll, pitch, yaw] of the local orientation.

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: [flexion/extension, pronation/supination].

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: { W, X, Y, Z }.

Search documentation

Start typing to search the documentation.