optitrack_localization

OptiTrack Localization Module.

This module provides functionality for localizing rigid bodies using the OptiTrack NatNet system. It includes methods to fetch real-time frame data, manage rigid body and marker sets, and perform transformations between task spaces (e.g., world and optical frames).

The module integrates with the NatNet client to retrieve and process motion capture data.

class robotblockset.optitrack.optitrack_localization.optitrack_localization(host_ip: str = '127.0.0.1', client_ip: str = '127.0.0.1', multicast_ip: str = '239.255.42.99', frame_callback: Callable[[...], None] | None = None, body_callback: Callable[[...], None] | None = None)[source]

Bases: rbs_object

A class for managing the localization of objects (rigid bodies) using OptiTrack NatNet.

Attributes:

host_ip (str) – IP address of the OptiTrack host.
client_ip (str) – IP address of the client.
multicast_ip (str) – Multicast IP address for OptiTrack communication.
natnet_model (MoCapModel, optional) – Model description from the OptiTrack system.
frame (MoCapFrame, optional) – Latest frame captured from OptiTrack.
bodies (list) – List of rigid body names.
n_bodies (int) – Number of rigid bodies.
markersets (list) – List of marker set names.
n_markersets (int) – Number of marker sets.
n_markers (int) – Total number of markers.
n_markers_in_set (list) – List with the number of markers in each marker set.
BodyID (int) – ID for the platform’s rigid body.
last_valid_body_data (RigidBody, optional) – Last valid rigid body data for the platform.
last_valid_body_frame_id (int, optional) – Last frame ID with valid rigid body data.
last_valid_body_frame_time (float, optional) – Last frame time with valid rigid body data.
TWorld (np.ndarray) – Transformation matrix between optical and world frames.
client (NatNetClient) – The NatNet client used to interface with OptiTrack.

Initializes the optitrack_localization class by connecting to the OptiTrack system.

Parameters:

host_ip (str, optional) – IP address of the OptiTrack host. Default is “127.0.0.1”.
client_ip (str, optional) – IP address of the client. Default is “127.0.0.1”.
multicast_ip (str, optional) – Multicast IP address for OptiTrack communication. Default is “239.255.42.99”.
frame_callback (Callable[..., None], optional) – Callback function to process new frames.
body_callback (Callable[..., None], optional) – Callback function to process rigid body data.

__init__(host_ip: str = '127.0.0.1', client_ip: str = '127.0.0.1', multicast_ip: str = '239.255.42.99', frame_callback: Callable[[...], None] | None = None, body_callback: Callable[[...], None] | None = None) → None[source]

Initializes the optitrack_localization class by connecting to the OptiTrack system.

Parameters:

host_ip (str, optional) – IP address of the OptiTrack host. Default is “127.0.0.1”.
client_ip (str, optional) – IP address of the client. Default is “127.0.0.1”.
multicast_ip (str, optional) – Multicast IP address for OptiTrack communication. Default is “239.255.42.99”.
frame_callback (Callable[..., None], optional) – Callback function to process new frames.
body_callback (Callable[..., None], optional) – Callback function to process rigid body data.

isReady() → bool[source]

Checks if the OptiTrack system is ready (connected).

Returns:: True if the system is connected and ready, False otherwise.
Return type:: bool

InitObject() → None[source]

Initializes the object for various attributes.

Returns:: This method does not return any value. It modifies the internal state of the object.
Return type:: None

SetBodyID(id: int | str = 0) → None[source]

Sets the ID of the rigid body to be used for localization.

Parameters:: id (Union[int, str], optional) – The rigid body ID or name to set. Default is 0.
Raises:: ValueError – If the rigid body ID is invalid.

property x: ndarray

Returns the current pose (position and orientation) of the rigid body.

Returns:: The pose as a 7-element array [position, orientation].
Return type:: np.ndarray

property p: ndarray

Returns the current position of the rigid body.

Returns:: The position as a 3-element array.
Return type:: np.ndarray

property Q: ndarray

Returns the current orientation of the rigid body.

Returns:: The orientation as a 4-element array (quaternion).
Return type:: np.ndarray

property R: ndarray

Returns the current rotation matrix of the rigid body.

Returns:: The rotation matrix as a 3x3 matrix.
Return type:: np.ndarray

property T: ndarray

Returns the current transformation matrix of the rigid body.

Returns:: The transformation matrix as a 4x4 matrix.
Return type:: np.ndarray

spatial(x: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Validates the shape of the input x and returns it in an appropriate format.

Parameters:: x (ArrayLike) – The input array representing a spatial quantity, which can be one of the following shapes: - (7,) : pose (position and quaternion) - (4, 4) : transformation matrix - (3,) : position vector - (4,) : quaternion - (3, 3) : rotation matrix - (6,) : twist (linear and angular velocity) - (3, 4) : homogeneous matrix without the last row (assumed to be 3x4)
Returns:: The input x in the validated shape, possibly modified if the shape was (3, 4).
Return type:: np.ndarray
Raises:: TypeError – If the input x does not have a valid shape.

SetWorld(x: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → None[source]

Sets the transformation between the world frame and the optical frame.

Parameters:: x (ArrayLike) – The transformation matrix or pose to set.

GetFrameTime() → float[source]

Retrieves the time of the current frame if available.

This method checks if the frame attribute is not None and returns the time of the current frame. If the frame is None, it returns a default value of 0.

Returns:: The time of the current frame. Returns 0 if the frame is not set.
Return type:: float

GetPose(out: str | None = None, task_space: str | None = None) → ndarray[source]

Gets the pose of the rigid body in the specified task space.

Parameters:

out (str, optional) – The output format (“Pose”, “Position”, “Quaternion”, “RotationMatrix”, etc.).
task_space (str, optional) – The task space frame (“World”, “Optical”).

Returns:

The pose in the requested format.

Return type:

np.ndarray

GetPos(out: str = 'p', task_space: str | None = None) → ndarray[source]

Get the platform’s position in the specified task space.

Parameters:

out (str, optional) – The output form of the position. By default, it returns the position (“p”). Other possible values: “Position”.
task_space (str, optional) – The task space frame in which to return the position. By default, it is “World”. Other options may include “Robot” or “Object”.

Returns:

The platform’s position, represented as a 3-element array (3,).

Return type:

np.ndarray

Raises:

ValueError – If the out parameter is not one of the supported values.

GetOri(out: str = 'Q', task_space: str | None = None) → ndarray[source]

Get the platform’s orientation in the specified task space.

Parameters:

out (str, optional) – The output form of the orientation. By default, it returns the orientation as a quaternion (“Q”). Other possible values: “Quaternion”, “RotationMatrix”, “R”.
task_space (str, optional) – The task space frame in which to return the orientation. By default, it is “World”. Other options may include “Robot” or “Object”.

Returns:

The platform’s orientation, either as a quaternion (4,) or rotation matrix (3, 3).

Return type:

np.ndarray

Raises:

ValueError – If the out parameter is not one of the supported values.

GetRigidBodyPose(id: int | str = 0, task_space: str | None = None, out: str | None = None) → ndarray[source]

Get the pose of a specific rigid body in the specified task space.

Parameters:

id (Union[int, str], optional) – The ID of the rigid body. By default, it is 0. If a string is provided and it corresponds to a body name, it will be converted to the corresponding ID.
task_space (str, optional) – The task space frame in which to return the pose. By default, it is the task space defined in the default settings.
out (str, optional) – The output form of the pose. By default, it is set to the task pose form defined in the default settings.

Returns:

The pose of the rigid body, represented as a 7-element array (position + orientation).

Return type:

np.ndarray

Raises:

ValueError – If the id is incorrect, or if the rigid body does not exist.

GetModel() → None[source]

Fetch the model from OptiTrack.

This method retrieves the current model from the OptiTrack system via the client. It waits briefly after fetching the model and then retrieves the model description.

Return type:: None

GetFrame() → None[source]

Fetch the latest frame from OptiTrack.

This method retrieves the latest frame of data from the OptiTrack system. It checks if the frame contains valid data for the rigid body specified by the BodyID and updates relevant fields accordingly.

Return type:: None

GetRigidBodies(task_space: str | None = None, out: str = 'x') → ndarray[source]

Retrieves the poses of all rigid bodies.

Parameters:

task_space (str, optional) – The task space frame (“World”, “Optical”).
out (str, optional) – The output format (“Pose”, “Position”, “Quaternion”, etc.).

Returns:

The poses of all rigid bodies.

Return type:

np.ndarray

GetMarkers(markerset_id: int | str | None = None, task_space: str | None = None, out: str = 'p') → ndarray | None[source]

Retrieves the marker positions for a specified marker set.

Parameters:

markerset_id (Union[int, str], optional) – The marker set ID or name.
task_space (str, optional) – The task space frame (“World”, “Optical”).
out (str, optional) – The output format (“Position”, “p”).

Returns:

The marker positions in the requested format, or None if unavailable.

Return type:

np.ndarray or None

name2id(typ: str = 'body', name: str | None = None) → int | None[source]

Converts the name of an object (rigid body or marker set) to its corresponding ID.

Parameters:

typ (str, optional) – The type of object (“body” or “markerset”).
name (str, optional) – The name of the object.

Returns:

The ID of the object if it exists, otherwise None.

Return type:

Optional[int]

Raises:

ValueError – If the object type is not supported.

robotblockset.optitrack.optitrack_localization.receiveNewFrame(frameNumber: int, markerSetCount: int, unlabeledMarkersCount: int, rigidBodyCount: int, skeletonCount: int, labeledMarkerCount: int, timecode: int, timecodeSub: int, timestamp: float, isRecording: bool, trackedModelsChanged: bool) → None[source]

robotblockset.optitrack.optitrack_localization.receiveRigidBodyFrame(id: int, position: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], rotation: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → None[source]

Functions

`receiveNewFrame`(frameNumber, markerSetCount, ...)
`receiveRigidBodyFrame`(id, position, rotation)

Classes

optitrack_localization([host_ip, client_ip, ...])

A class for managing the localization of objects (rigid bodies) using OptiTrack NatNet.