Pose3D

Represents a single SE(3) pose: a 3D position plus an orientation quaternion.

Parameters

Field	Type	Description
data	np.ndarray | list | tuple	Array-like input of shape (7,): [x, y, z, qw, qx, qy, qz], converted to a contiguous float32 array.

Raises

Exception	Condition
TypeError	data can't be converted to float32 (e.g. non-numeric elements)
ValueError	data is not rank-1, or its shape isn't (7,)
ValueError	data contains a non-finite value (NaN/Inf)
ValueError	The quaternion sub-range data[3:7] is the zero quaternion (norm 0.0)
ValueError	The quaternion sub-range data[3:7]'s norm deviates from 1.0 by more than quat_norm_atol (1e-3)

Attributes

Attribute	Type	Description
data	np.ndarray	Defensive copy of the underlying (7,) float32 array. Assigning a new value re-validates it (shape, finiteness, unit-norm quaternion) the same way as construction.
shape	tuple[int, ...]	Always (7,).
ndim	int	Always 1.
dtype	np.dtype	Always float32.
size	int	Always 7.
quat_slice	slice	Class-level, slice(3, 7) — the last-axis range checked for unit norm.
quat_norm_atol	float	Class-level, 1e-3 — max allowed abs(norm - 1).
quat_order	str	Class-level, "wxyz" — this class's native scalar-first quaternion order.

Methods

Method	Description
to_transform_matrix()	Returns a plain (4, 4) np.ndarray (not a Transform3D) — the homogeneous transformation matrix equivalent to this pose. Wrap the result in Transform3D(...) if you need the validated datatype.
Pose3D.from_transform_matrix(transformation_matrix)	Classmethod; the inverse of to_transform_matrix. Builds a Pose3D from a (4, 4) homogeneous matrix (np.ndarray or list). Raises TypeError if not an np.ndarray/list; ValueError if its shape isn't (4, 4).
convert_pose_format(rot_type)	Returns a new np.ndarray with the rotation re-encoded per RotationType (or its string value): the quaternion unchanged (shape (7,)) for QUATERNION, or Euler angles/a rotation vector (shape (6,)) otherwise. Raises TypeError/ValueError for an unrecognized rot_type.
Pose3D.from_pose_format(pose, rot_type)	Classmethod; the inverse of convert_pose_format. Builds a Pose3D from a pose array/list already expressed in rot_type's rotation format. Raises TypeError/ValueError for an unrecognized rot_type.
Pose3D.to_xyzw(q)	Inherited from QuaternionValidationMixin. Converts a quaternion array from quat_order ("wxyz") to scalar-last [x, y, z, w], e.g. before handing it to scipy.
Pose3D.from_xyzw(q)	Inherited from QuaternionValidationMixin. Converts a quaternion array from scalar-last [x, y, z, w] to quat_order ("wxyz"), e.g. after reading scipy's Rotation.as_quat().
to_numpy(copy=True)	Returns the pose as np.ndarray. Pass copy=False for a reference to the internal array instead — faster, but mutating it mutates the Pose3D too.
copy()	Returns a new Pose3D with an independent copy of the data.
Pose3D.coerce(value)	Returns value unchanged if it's already a Pose3D; otherwise wraps an array-like into one (running full validation). Raises TypeError for any other input.

Operators

Operation	Behavior
p == other	True only if other is also a Pose3D with element-equal data. False for anything else.
len(p)	Always 7.
np.asarray(p) / np.reshape(p, ...)	Works directly via __array__. Always returns a copy; use to_numpy(copy=False) for a zero-copy view.
hash(p)	Not supported — mutable via the data setter.

Visualization

datatypes.visualize(pose, entity_path=...) logs the pose's posed frame (rotated basis axes from the quaternion, translated by [x, y, z]) alongside a labeled world-origin frame. Per-axis labels are omitted; only the world-origin frame is labeled "origin". Passing label="..." attaches a floating text label at the posed frame's translation.

Example

"""Demonstrates the Telekinesis Pose3D datatype."""

import time

import numpy as np
from loguru import logger
import rerun as rr

from telekinesis import datatypes

def pose3d_example():
    """Demonstrate creation, access, visualization, update, transform-matrix conversion, pose-format conversion, NumPy interop, and serialization."""

    # ======================= Create ============================================
    pose_data = [0.5, 0.2, 0.5, 0.4619398, 0.1913417, 0.4619398, 0.7325378]
    pose3d = datatypes.Pose3D(pose_data)

    logger.info(f"Original Pose3D: {pose3d}")

    # ======================= Inspect ===========================================
    data = pose3d.data
    shape = pose3d.shape
    size = pose3d.size
    dtype = pose3d.dtype
    ndim = pose3d.ndim
    numpy_pose3d = pose3d.to_numpy()
    pose3d_copy = pose3d.copy()

    logger.info(f"shape={shape}, size={size}, ndim={ndim}, dtype={dtype}")
    logger.info(f"Underlying data: {data}")
    logger.info(f"NumPy array: {numpy_pose3d}")
    logger.info(f"Copy: {pose3d_copy}")

    # ======================= Visualize =========================================
    rr.init("pose3d_example", spawn=True)
    datatypes.visualize(pose3d, entity_path="/Pose3D", label="my_pose3d")

    # ======================= Update ============================================
    pose3d.data = [0.1, 0.2, 0.3, 0.0, 0.0, 0.0, 1.0]
    logger.info(f"Updated Pose3D: {pose3d}")
    datatypes.visualize(pose3d, entity_path="/Pose3D/updated", label="updated_pose3d")

    # ======================= Transform Matrix ==================================
    matrix = pose3d.to_transform_matrix()
    logger.info(f"Pose3D as transformation matrix:\n{matrix}")
    transform3d = datatypes.Transform3D(matrix)
    datatypes.visualize(transform3d, entity_path="/Transform3D", label="pose3d_transform")

    pose3d_from_transform = datatypes.Pose3D.from_transform_matrix(transform3d.data)
    logger.info(f"Pose3D from transformation matrix: {pose3d_from_transform}")
    logger.info(f"Converted back to Pose3D is equal to original: {pose3d_from_transform == pose3d}")

    # ======================= Pose Formats ======================================
    pose3d_deg = datatypes.Pose3D.from_pose_format(
        [30, 45, 60, 0.4619398, 0.1913417, 0.4619398, 0.7325378], rot_type="deg"
    )
    logger.info(f"Pose3D from pose with rotation in degrees: {pose3d_deg}")

    pose3d_rad = datatypes.Pose3D.from_pose_format(
        [
            np.radians(30),
            np.radians(45),
            np.radians(60),
            0.4619398,
            0.1913417,
            0.4619398,
            0.7325378,
        ],
        rot_type="rad",
    )
    logger.info(f"Pose3D from pose with rotation in radians: {pose3d_rad}")

    pose3d_rotvec = datatypes.Pose3D.from_pose_format(
        [0.5235988, 0.7853982, 1.0471976, 0.4619398, 0.1913417, 0.4619398, 0.7325378],
        rot_type="rotvec",
    )
    logger.info(f"Pose3D from pose with rotation as rotation vector: {pose3d_rotvec}")

    # ======================= Convert ===========================================
    pose_as_deg = pose3d.convert_pose_format(rot_type="deg")
    logger.info(f"Pose3D as rotation in degrees: {pose_as_deg}")

    pose_as_rad = pose3d.convert_pose_format(rot_type="rad")
    logger.info(f"Pose3D as rotation in radians: {pose_as_rad}")

    pose_as_rotvec = pose3d.convert_pose_format(rot_type="rotvec")
    logger.info(f"Pose3D as rotation vector: {pose_as_rotvec}")

    # ======================= NumPy Interop =====================================
    reshaped = np.reshape(pose3d, (7,))
    logger.info(f"Underlying Pose3D with np.reshape: {reshaped}")

    # ======================= Serialize / Deserialize ===========================
    start = time.perf_counter()
    serialized = datatypes.serialize(pose3d)
    serialization_ms = (time.perf_counter() - start) * 1000

    start = time.perf_counter()
    deserialized = datatypes.deserialize(serialized)["param_0"]
    deserialization_ms = (time.perf_counter() - start) * 1000

    logger.info(f"Deserialized Pose3D: {deserialized}")
    logger.info(f"Round-trip successful: {deserialized == pose3d}")
    logger.info(f"Serialization time: {serialization_ms:.3f} ms")
    logger.info(f"Deserialization time: {deserialization_ms:.3f} ms")


if __name__ == "__main__":
    pose3d_example()