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
Assembly: MOSAIC.dll
Syntax
public sealed class BodyRig : BaseBlock, INotifyPropertyChanged, INotifyPropertyChanging, ISubscriber, IPublisher, IBaseBlockProps, IJsonExportable, IDisposable, IAsyncDisposable
Remarks
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
FrameStart
Declaration
private const char FrameStart = 'U'
Field Value
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
MaxTrackedSensors
Declaration
private const int MaxTrackedSensors = 64
Field Value
NumSentValues
Declaration
private const int NumSentValues = 5
Field Value
StartEndSizeBytes
Declaration
private const int StartEndSizeBytes = 1
Field Value
UnityPort
Declaration
private const int UnityPort = 3339
Field Value
ValuesPerSegment
Declaration
private const int ValuesPerSegment = 7
Field Value
_bufferLock
Declaration
private readonly object _bufferLock
Field Value
_bufferParser
Declaration
private string _bufferParser
Field Value
_bytesPerFrame
Declaration
private int _bytesPerFrame
Field Value
_calibrationDirectory
Declaration
private string? _calibrationDirectory
Field Value
_calibrationFile
Declaration
private string? _calibrationFile
Field Value
_chain
Declaration
private BodySegment[] _chain
Field Value
_currentProfile
Declaration
private int _currentProfile
Field Value
_disposed
Declaration
Field Value
_numOfChannels
Declaration
private int _numOfChannels
Field Value
_profiles
Declaration
private string[] _profiles
Field Value
_sample
Declaration
Field Value
_sensorPackets
Declaration
private readonly int[] _sensorPackets
Field Value
_serialPort
Declaration
private SerialPort? _serialPort
Field Value
_streamManager
Declaration
private StreamManager? _streamManager
Field Value
_unityClient
Declaration
private readonly UdpClient _unityClient
Field Value
Properties
CalibrationDirectory
The calibration directory scanned for profiles. Assigning re-scans it.
Declaration
public string? CalibrationDirectory { get; set; }
Property Value
CalibrationName
Gets the current calibration file path.
Declaration
public string? CalibrationName { get; }
Property Value
Info
Observable status information for UI binding.
Declaration
public BodyRigInfo Info { get; }
Property Value
IsPortOpen
Whether the serial port is actually open, read from the port itself.
Declaration
public bool IsPortOpen { get; }
Property Value
Remarks
JsonTypeName
Block type identifier for JSON serialization. Override for a custom name.
Declaration
protected override string JsonTypeName { get; }
Property Value
Overrides
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
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
NumOfDevices
Number of detected devices (updated dynamically in quaternion mode).
Declaration
public int NumOfDevices { get; private set; }
Property Value
PortNumber
Serial port name (e.g. "COM5").
Declaration
public string PortNumber { get; set; }
Property Value
Profiles
Gets the calibration profile paths discovered in the calibration directory.
Declaration
public IReadOnlyList<string> Profiles { get; }
Property Value
SegmentCount
Number of segments currently allocated in the kinematic chain.
Declaration
public int SegmentCount { get; }
Property Value
SensorSources
Declaration
public IReadOnlyList<BodyRigSensorSource> SensorSources { get; private set; }
Property Value
Remarks
Visualization
This block's visualization, or null when it has none.
Declaration
protected override BlockVisualization? Visualization { get; }
Property Value
Overrides
Remarks
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
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
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
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
DetachFromParent(BodySegment)
Removes the two-way link between segment and its parent.
Declaration
private static void DetachFromParent(BodySegment segment)
Parameters
Disconnect()
Stops streaming and closes the serial port.
Declaration
Dispose()
Disconnects serial, closes the Unity UDP client, and releases all resources.
Declaration
public override void Dispose()
Overrides
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
GetJsonParams()
Declaration
protected override IReadOnlyList<object>? GetJsonParams()
Returns
Overrides
Remarks
GetLinkLengths(int)
Gets the link lengths for a segment.
Declaration
public Vector3F GetLinkLengths(int index)
Parameters
| Type |
Name |
Description |
| int |
index |
|
Returns
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
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
GetSensorIndex(int)
Gets the sensor index assigned to a segment.
Declaration
public int GetSensorIndex(int index)
Parameters
| Type |
Name |
Description |
| int |
index |
|
Returns
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
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
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
Overrides
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
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
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
SelectProfile(string)
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
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
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
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
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
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