Class BodyRig

A BaseBlock that receives orientation data from a DLR BodyRig device (via an RN41 SPP Bluetooth serial dongle) or from upstream quaternion vectors, computes forward kinematics over a configurable kinematic chain, and publishes joint positions and orientations.

JSON configuration

Use these block entries inside a pipeline configuration. Input names must match other blocks in that pipeline; device addresses and file paths must match your setup.

JSON pipeline configuration:

{
  "Name": "BodyRig",
  "Type": "BodyRig",
  "Inputs": [ "Timer100Hz" ],
  "Params": [ "COM5", "7", "C:\\Calibrations" ]
}

How Type, Inputs and Params work · Block catalogue and parameter reference

Inheritance
BodyRig
Namespace: MOSAIC.Models.Devices
Assembly: MOSAIC.dll
Syntax
public sealed class BodyRig : BaseBlock, INotifyPropertyChanged, INotifyPropertyChanging, ISubscriber, IPublisher, IBaseBlockProps, IJsonExportable, IDisposable, IAsyncDisposable
Remarks

Serial mode: When the input block is a Timer, the BodyRig reads binary frames from a serial port. Each frame consists of a start byte 0x55, numSentValues × 4 data bytes, and an end byte 0xFF.

Quaternion mode: When the input block pushes a MathNet.Numerics.LinearAlgebra.Vector<T> of concatenated quaternions (w, x, y, z per sensor), the block bypasses serial I/O and directly updates the kinematic chain.

The output is a MathNet.Numerics.LinearAlgebra.Vector<T> of length numOfChannels × 7, containing [W, X, Y, Z, posX, posY, posZ] per segment. An additional UDP stream sends the pose to Unity (localhost:3339).

Constructors

BodyRig(string, double)

Initialises a new BodyRig instance.

Declaration
public BodyRig(string name = "BodyRig", double desiredRate = 100)
Parameters
Type Name Description
string name

Display name for this block.

double desiredRate

Desired processing rate in Hz.

Fields

FrameEnd

Declaration
private const char FrameEnd = 'ÿ'
Field Value
Type Description
char

FrameStart

Declaration
private const char FrameStart = 'U'
Field Value
Type Description
char

FrameTransport

Windows-1252/Latin-1 is used purely as a byte-transparent byte-to-char transport for the binary serial frames: it round-trips all 256 byte values. It must be used on BOTH sides of the buffer - decoding with 1252 and re-encoding with Encoding.Default (UTF-8 on .NET Core) corrupts every byte >= 0x80, which is most of a float payload.

Declaration
private static readonly Encoding FrameTransport
Field Value
Type Description
Encoding

MaxTrackedSensors

Declaration
private const int MaxTrackedSensors = 64
Field Value
Type Description
int

NumSentValues

Declaration
private const int NumSentValues = 5
Field Value
Type Description
int

StartEndSizeBytes

Declaration
private const int StartEndSizeBytes = 1
Field Value
Type Description
int

UnityPort

Declaration
private const int UnityPort = 3339
Field Value
Type Description
int

ValuesPerSegment

Declaration
private const int ValuesPerSegment = 7
Field Value
Type Description
int

_bufferLock

Declaration
private readonly object _bufferLock
Field Value
Type Description
object

_bufferParser

Declaration
private string _bufferParser
Field Value
Type Description
string

_bytesPerFrame

Declaration
private int _bytesPerFrame
Field Value
Type Description
int

_calibrationDirectory

Declaration
private string? _calibrationDirectory
Field Value
Type Description
string

_calibrationFile

Declaration
private string? _calibrationFile
Field Value
Type Description
string

_chain

Declaration
private BodySegment[] _chain
Field Value
Type Description
BodySegment[]

_currentProfile

Declaration
private int _currentProfile
Field Value
Type Description
int

_disposed

Declaration
private bool _disposed
Field Value
Type Description
bool

_numOfChannels

Declaration
private int _numOfChannels
Field Value
Type Description
int

_profiles

Declaration
private string[] _profiles
Field Value
Type Description
string[]

_sample

Declaration
private double[] _sample
Field Value
Type Description
double[]

_sensorPackets

Declaration
private readonly int[] _sensorPackets
Field Value
Type Description
int[]

_serialPort

Declaration
private SerialPort? _serialPort
Field Value
Type Description
SerialPort

_streamManager

Declaration
private StreamManager? _streamManager
Field Value
Type Description
StreamManager

_unityClient

Declaration
private readonly UdpClient _unityClient
Field Value
Type Description
UdpClient

Properties

CalibrationDirectory

The calibration directory scanned for profiles. Assigning re-scans it.

Declaration
public string? CalibrationDirectory { get; set; }
Property Value
Type Description
string

CalibrationName

Gets the current calibration file path.

Declaration
public string? CalibrationName { get; }
Property Value
Type Description
string

Info

Observable status information for UI binding.

Declaration
public BodyRigInfo Info { get; }
Property Value
Type Description
BodyRigInfo

IsPortOpen

Whether the serial port is actually open, read from the port itself.

Declaration
public bool IsPortOpen { get; }
Property Value
Type Description
bool
Remarks

Connect() records a failure in Info and returns normally rather than throwing, so a caller that merely watches for an exception can believe it is connected over a port that never opened. This is the truth.

JsonTypeName

Block type identifier for JSON serialization. Override for a custom name.

Declaration
protected override string JsonTypeName { get; }
Property Value
Type Description
string
Overrides
BaseBlock.JsonTypeName

Mode

Which source last drove this block. OnReceive(object, object) discriminates on the sender type, and that is the only honest source for it — the block's declared inputs are names, not types, so the mode cannot be inferred from configuration.

Declaration
public BodyRigMode Mode { get; private set; }
Property Value
Type Description
BodyRigMode

NumOfChannels

Number of kinematic chain segments (IMU channels). Assigning a new value rebuilds the kinematic chain and the output buffer, so the chain never drifts out of sync with the configured channel count.

Declaration
public int NumOfChannels { get; set; }
Property Value
Type Description
int

NumOfDevices

Number of detected devices (updated dynamically in quaternion mode).

Declaration
public int NumOfDevices { get; private set; }
Property Value
Type Description
int

PortNumber

Serial port name (e.g. "COM5").

Declaration
public string PortNumber { get; set; }
Property Value
Type Description
string

Profiles

Gets the calibration profile paths discovered in the calibration directory.

Declaration
public IReadOnlyList<string> Profiles { get; }
Property Value
Type Description
IReadOnlyList<string>

SegmentCount

Number of segments currently allocated in the kinematic chain.

Declaration
public int SegmentCount { get; }
Property Value
Type Description
int

SensorSources

What each positional sensor slot physically is, when the pipeline makes it knowable. Empty until DescribeSensorSources(IReadOnlyList<BodyRigSensorSource>?) is called.

Declaration
public IReadOnlyList<BodyRigSensorSource> SensorSources { get; private set; }
Property Value
Type Description
IReadOnlyList<BodyRigSensorSource>
Remarks

Display only; the kinematics never consult it. The block cannot work this out for itself: in upstream mode it receives a concatenated vector, and which ESP node produced a given group is a fact about the graph rather than about the data.

Visualization

This block's visualization, or null when it has none.

Declaration
protected override BlockVisualization? Visualization { get; }
Property Value
Type Description
BlockVisualization
Overrides
BaseBlock.Visualization
Remarks

Blocks own their scope as a public Viz property rather than inheriting one, so the base class has no way to reach it. Overriding this is what lets the rate below be wired up automatically. With correct publication metadata, the bundle derives the time axis for one vector per publication or fixed-size contiguous sample packets from the feed shape.

When a plot needs a rate that publication frequency and feed shape cannot express (for example, variable-sized packets or overlapping windows), declare it with Viz.UpdateSignalRate. An explicit declaration wins over the derived rate. A block with several bundles must configure each one; this hook exposes only one bundle.

Viz

Live plot of the published pose, shown on the device card. Every other block that emits a stream owns one of these; without it the block computes correctly but nothing it produces is ever drawn, which reads to the user as "no data".

Declaration
public BlockVisualization? Viz { get; }
Property Value
Type Description
BlockVisualization

Methods

ApplySensorQuaternion(int, float, float, float, float)

Applies one sensor reading to every chain segment bound to sensorId.

Declaration
private void ApplySensorQuaternion(int sensorId, float x, float y, float z, float w)
Parameters
Type Name Description
int sensorId
float x
float y
float z
float w

ConfigureInput(IServiceProvider, JsonModel)

Factory method that creates a BodyRig from a JSON pipeline definition.

Declaration
public static BodyRig ConfigureInput(IServiceProvider sp, JsonModel m)
Parameters
Type Name Description
IServiceProvider sp

The application's dependency injection service provider.

JsonModel m

JSON model. Params format: [portName, numChannels, calibrationDirectory, profileFileName].

Returns
Type Description
BodyRig

A configured BodyRig block, with its calibration applied.

Connect()

Opens the serial port and starts streaming binary frames from the BodyRig dongle.

Declaration
public void Connect()

ConsumeBuffer(int)

Drops the first count characters from the parse buffer.

Declaration
private void ConsumeBuffer(int count)
Parameters
Type Name Description
int count

DescribeSensorSources(IReadOnlyList<BodyRigSensorSource>?)

Records what the pipeline resolved each sensor slot to, for the card to show.

Declaration
public void DescribeSensorSources(IReadOnlyList<BodyRigSensorSource>? sources)
Parameters
Type Name Description
IReadOnlyList<BodyRigSensorSource> sources

DetachFromParent(BodySegment)

Removes the two-way link between segment and its parent.

Declaration
private static void DetachFromParent(BodySegment segment)
Parameters
Type Name Description
BodySegment segment

Disconnect()

Stops streaming and closes the serial port.

Declaration
public void Disconnect()

Dispose()

Disconnects serial, closes the Unity UDP client, and releases all resources.

Declaration
public override void Dispose()
Overrides
BaseBlock.Dispose()

GetDhAngles(int)

Gets the DH Euler angles for a segment as (roll, pitch, yaw) in radians.

Declaration
public Vector3F GetDhAngles(int index)
Parameters
Type Name Description
int index
Returns
Type Description
Vector3F

GetJsonParams()

Emits the positional params [port, channels, calibrationDirectory, profileFileName] that ConfigureInput(IServiceProvider, JsonModel) reads back.

Declaration
protected override IReadOnlyList<object>? GetJsonParams()
Returns
Type Description
IReadOnlyList<object>
Overrides
BaseBlock.GetJsonParams()
Remarks

Without this the base implementation returns null and a saved pipeline records Params: null, silently discarding the port, the channel count and the calibration directory. The profile name is the fourth field because the directory alone only says where the profiles live, not which one the rig was configured with.

GetLinkLengths(int)

Gets the link lengths for a segment.

Declaration
public Vector3F GetLinkLengths(int index)
Parameters
Type Name Description
int index
Returns
Type Description
Vector3F

GetParentIndex(int)

Gets the parent index for a segment (-1 if root).

Declaration
public int GetParentIndex(int index)
Parameters
Type Name Description
int index
Returns
Type Description
int

GetSegment(int)

Gets the chain segment at index, or null when the index is outside the chain.

Declaration
public BodySegment? GetSegment(int index)
Parameters
Type Name Description
int index
Returns
Type Description
BodySegment

GetSensorIndex(int)

Gets the sensor index assigned to a segment.

Declaration
public int GetSensorIndex(int index)
Parameters
Type Name Description
int index
Returns
Type Description
int

GetSensorPacketCount(int)

Total packets seen for sensorId since the block was created, counted whether or not any segment is bound to it. Poll it and watch for an increase to determine liveness; the absolute value is only meaningful as a difference.

Declaration
public int GetSensorPacketCount(int sensorId)
Parameters
Type Name Description
int sensorId
Returns
Type Description
int

InitialiseChain()

(Re)creates the kinematic chain array and output sample buffer, and refreshes the calibration profile list. Safe to call at any time; any existing chain topology and calibration are discarded.

Declaration
public void InitialiseChain()

LoadCalibration()

Loads calibration data from the current calibration file.

Declaration
public void LoadCalibration()

LoadCalibration(string)

Loads calibration data from path, making it the current profile.

Declaration
public void LoadCalibration(string path)
Parameters
Type Name Description
string path
Remarks

Sets the current file before delegating to the parameterless overload.

NextProfile()

Advances to the next calibration profile and loads it.

Declaration
public void NextProfile()

OnReceive(object, object)

Processes incoming pipeline data. Handles both serial timer ticks and upstream quaternion vectors.

Declaration
protected override void OnReceive(object sender, object value)
Parameters
Type Name Description
object sender
object value
Overrides
BaseBlock.OnReceive(object, object)

OnSerialDataReceived(byte[], int)

Declaration
private void OnSerialDataReceived(byte[] buffer, int count)
Parameters
Type Name Description
byte[] buffer
int count

OrientationRelativeTo(int, int)

Computes the relative orientation of segment index1 with respect to segment index2.

Declaration
public QuaternionF OrientationRelativeTo(int index1, int index2)
Parameters
Type Name Description
int index1
int index2
Returns
Type Description
QuaternionF

PeripheralForSlot(int)

The peripheral number feeding slot, or null when the pipeline does not say.

Declaration
public int? PeripheralForSlot(int slot)
Parameters
Type Name Description
int slot
Returns
Type Description
int?

PreviousProfile()

Returns to the previous calibration profile and loads it.

Declaration
public void PreviousProfile()

ProcessQuaternionVector(Vector<double>?)

Processes a concatenated quaternion vector from an upstream block.

Declaration
private void ProcessQuaternionVector(Vector<double>? quaternions)
Parameters
Type Name Description
Vector<double> quaternions

ProcessSerialFrame()

Parses a single binary frame from the serial buffer and updates the kinematic chain.

Declaration
private void ProcessSerialFrame()

PublishOutput()

Publishes the current sample as a MathNet vector and sends the body pose via UDP to Unity.

Declaration
private void PublishOutput()

RescanProfiles()

Re-reads the calibration directory and refreshes Profiles.

Declaration
public void RescanProfiles()

SanitiseName(string?)

Removes the characters that would break the one-record-per-line format.

Declaration
private static string SanitiseName(string? name)
Parameters
Type Name Description
string name
Returns
Type Description
string

SelectProfile(string)

Makes path the current profile, and points NextProfile() and PreviousProfile() at its position rather than the start of the list.

Declaration
private void SelectProfile(string path)
Parameters
Type Name Description
string path

SelectProfileByName(string)

Makes the profile whose file name matches fileName current, if the calibration directory holds one; leaves the selection alone when it does not.

Declaration
public void SelectProfileByName(string fileName)
Parameters
Type Name Description
string fileName
Remarks

Matching on the bare name rather than a full path is what lets a saved pipeline record which profile it was using without hard-coding one machine's folder layout a second time.

SendBodyPoseToUnity()

Sends per-segment pose data (ID + quaternion + position) as float arrays over UDP.

Declaration
private void SendBodyPoseToUnity()

SetDhAngles(int, float, float, float)

Sets the DH-to-sensor mapping from Euler angles (degrees) on a segment, in the (roll, pitch, yaw) order returned by GetDhAngles(int).

Declaration
public void SetDhAngles(int index, float roll, float pitch, float yaw)
Parameters
Type Name Description
int index
float roll
float pitch
float yaw

SetDhQuaternion(int, float, float, float, float)

Sets the DH-to-sensor mapping from quaternion components on a segment.

Declaration
public void SetDhQuaternion(int index, float w, float x, float y, float z)
Parameters
Type Name Description
int index
float w
float x
float y
float z

SetLinkLength(int, float, float, float)

Sets the link length vector on a kinematic chain segment.

Declaration
public void SetLinkLength(int index, float lx, float ly, float lz)
Parameters
Type Name Description
int index
float lx
float ly
float lz

SetParent(int, int)

Sets the parent of a segment in the kinematic chain, or detaches it when parentIndex is -1. Cycles and self-parenting are rejected.

Declaration
public void SetParent(int childIndex, int parentIndex)
Parameters
Type Name Description
int childIndex
int parentIndex

SetSensorIndex(int, int)

Sets the sensor index on a kinematic chain segment.

Declaration
public void SetSensorIndex(int segmentIndex, int sensorId)
Parameters
Type Name Description
int segmentIndex
int sensorId

StartStream()

Declaration
private void StartStream()

StopStream()

Declaration
private void StopStream()

StoreCalibration(string)

Stores the current chain to filename and makes it the current profile.

Declaration
public bool StoreCalibration(string filename)
Parameters
Type Name Description
string filename
Returns
Type Description
bool

true when the file was actually written.

Remarks

Failures are reported and returned as false rather than interrupting a running pipeline. Callers must check the result before reporting success.

UpdateSampleBuffer()

Copies current chain state (orientations + positions) into the flat output array. Skips segments with NaN or Infinity values.

Declaration
private void UpdateSampleBuffer()

WriteAsciiId(byte[], int)

Writes value as four zero-padded ASCII digits into id.

Declaration
private static void WriteAsciiId(byte[] id, int value)
Parameters
Type Name Description
byte[] id
int value

Implements

INotifyPropertyChanged
INotifyPropertyChanging
ISubscriber
IPublisher
IBaseBlockProps
IJsonExportable
IDisposable
IAsyncDisposable

Search documentation

Start typing to search the documentation.