Teleoperation

Operator-side teleoperation stacks: the process an operator runs to stream commands to a robot process. A stack is a TeleopOperator built from a TeleopInputSource (what the operator wears or holds) and a TeleopStreamer (what it sends), configured by TeleopOptions. The stack that ships is SONIC whole-body teleoperation: PICO body tracking on the operator station, sent over ZMQ to the robot process that runs the SONIC policy. The input devices themselves (keyboard, gamepad, SpaceMouse, GELLO, Meta Quest, PICO, Wuji Glove) are on the devices page. The teleoperation guide covers every pipeline and its eden command.

from eden.extensions.teleop.base import TeleopInputSource, TeleopOperator, TeleopStreamer
from eden.options.extensions.teleop import TeleopOptions

Base classes and options

Base classes for teleoperation operator stacks.

The interfaces a teleop stack implements, so future stacks (a different robot, a different tracker, a different wire protocol) share one shape:

• :class:TeleopOperator — the operator-station lifecycle (setup -> loop -> teardown).
• :class:TeleopInputSource — a tracked-input device reader.
• :class:TeleopStreamer — a per-tick pipeline that packs and publishes one message.

:class:TeleopOperator is the configurable extension object, so it follows the Eden convention shared by every other extension base — ConfigurableMixin bound to a declarative :class:~eden.options.extensions.teleop.TeleopOptions (the same shape as DeploymentBase): its config is mirrored onto the instance from the options, and a concrete stack adds its device-specific parameters as class attributes.

:class:TeleopInputSource and :class:TeleopStreamer are plain :class:abc.ABC contracts instead, because they are dependency-injected internal components — constructed with live callables and sockets (send, a socket reader), not from a serializable options schema — so ConfigurableMixin (which exists to bind an Options instance) would not fit them. This mirrors sysid's split between SystemIdentificationBase (ConfigurableMixin) and its plain-ABC Excitation interface. The SONIC stack under teleop/sonic is the reference implementation of all three.

Classes

TeleopInputSource

Bases: ABC

A tracked-input device reader (headset / controllers / trackers).

A concrete source owns a background thread or socket and hands the operator loop the latest sample without blocking. get_latest returns None until the first frame arrives; disconnected reports whether the stream has since gone stale.

Attributes

disconnected abstractmethod property

disconnected: bool

True once the input has gone stale (no fresh frames within the timeout).

Methods:

start abstractmethod

start() -> None

Begin reading (spawn the thread / open the socket).

Source code in eden/extensions/teleop/base.py

@abstractmethod
def start(self) -> None:
    """Begin reading (spawn the thread / open the socket)."""

stop abstractmethod

stop() -> None

Stop reading and release the device / socket.

Source code in eden/extensions/teleop/base.py

@abstractmethod
def stop(self) -> None:
    """Stop reading and release the device / socket."""

get_latest abstractmethod

get_latest()

The most recent tracked-body sample, or None before the first.

Source code in eden/extensions/teleop/base.py

@abstractmethod
def get_latest(self):
    """The most recent tracked-body sample, or ``None`` before the first."""

get_timestamp_ns abstractmethod

get_timestamp_ns() -> int

The device's latest timestamp in nanoseconds (a liveness / pacing signal).

Source code in eden/extensions/teleop/base.py

@abstractmethod
def get_timestamp_ns(self) -> int:
    """The device's latest timestamp in nanoseconds (a liveness / pacing signal)."""

controllers abstractmethod

controllers()

The current controller state (buttons / sticks / triggers).

Source code in eden/extensions/teleop/base.py

@abstractmethod
def controllers(self):
    """The current controller state (buttons / sticks / triggers)."""

TeleopStreamer

Bases: ABC

A per-tick operator pipeline that packs and publishes one wire message.

A streamer owns one control mode. run_once consumes the latest input and calls the injected send at most once per tick, self-pacing to the mode's rate; its exact arguments are mode-specific, hence the loose signature. reset_yaw re-zeros the accumulated heading on (re)entry and on_mode_exit runs when the mode is left.

Methods:

run_once abstractmethod

run_once(*args, **kwargs) -> None

Advance one tick: read the latest input and maybe publish one message.

Source code in eden/extensions/teleop/base.py

@abstractmethod
def run_once(self, *args, **kwargs) -> None:
    """Advance one tick: read the latest input and maybe publish one message."""

reset_yaw abstractmethod

reset_yaw() -> None

Reset the accumulated heading (called on mode entry).

Source code in eden/extensions/teleop/base.py

@abstractmethod
def reset_yaw(self) -> None:
    """Reset the accumulated heading (called on mode entry)."""

on_mode_exit

on_mode_exit() -> None

Hook run when this mode is left. Default: no-op.

Source code in eden/extensions/teleop/base.py

def on_mode_exit(self) -> None:
    """Hook run when this mode is left. Default: no-op."""

TeleopOperator

TeleopOperator(options: T)

Bases: ConfigurableMixin[TeleopOptions], ABC

The operator-station lifecycle: build the stack, run the loop, tear down.

Configured by a :class:~eden.options.extensions.teleop.TeleopOptions (the fields below are mirrored onto the instance by ConfigurableMixin); a concrete stack adds its device-specific parameters as further class attributes. Construct it from an options instance built with configure().

run is the template method — setup once, then step until should_stop is true, then teardown in a finally (so it runs even on KeyboardInterrupt, which routes through on_interrupt). setup runs inside the same try — it can block for a long time (headset wait) — so teardown must cope with a partially built stack. The loop cadence is set by the streamers' own pacing inside step, not by this loop.

Source code in eden/utils/common.py

def __init__(self, options: T) -> None:
    self._uid = gs.UID()
    self._options: T = options

    mirror_options_onto(self, self._options)

    # Deferred, not module-scope — see the NOTE above this module's imports.
    from eden.options._metadata import OPTION_METADATA_KEYS

    # check if there are unused parameters in the options. ``_extra_option_params`` are legitimate
    # options-only fields (not mirrored as attrs), so they are expected to be "unused" here.
    unused_params = set(self._options.keys()) - set(self.get_parameter_names())
    tolerated = {"name", "class_name"} | set(OPTION_METADATA_KEYS) | self._extra_option_params
    # ``params`` is a declared framework field on function-capable term options. An *empty* one is
    # legitimately unused when the same options class configures a class-based term; a non-empty
    # ``params`` on a class-based term would be silently ignored, so it stays invalid.
    if not getattr(self._options, "params", None):
        tolerated.add("params")
    invalid_params = unused_params - tolerated
    if invalid_params:
        # Name the term, not only the class: for a function term the class is the shared
        # ``*FuncWrapper``, which cannot tell the author which of a dozen terms is wrong.
        # Same wording as the construction-time check (``ManagerTermOptions._validate_param_names``).
        term_name = getattr(self._options, "name", None)
        subject = f"term '{term_name}' ({type(self).__name__})" if term_name else type(self).__name__
        raise ValueError(
            invalid_parameters_message(subject, invalid_params, self.get_configure_specs(), self._retired_params)
        )

Attributes

robot_host class-attribute instance-attribute

robot_host: str = 'localhost'

port class-attribute instance-attribute

port: int = 5556

feedback_port class-attribute instance-attribute

feedback_port: int = 5557

Methods:

run

run() -> None

Set up the stack, drive the control loop, and tear down on exit.

Source code in eden/extensions/teleop/base.py

def run(self) -> None:
    """Set up the stack, drive the control loop, and tear down on exit."""
    try:
        self.setup()
        while not self.should_stop():
            self.step()
    except KeyboardInterrupt:
        self.on_interrupt()
    finally:
        self.teardown()

setup abstractmethod

setup() -> None

Construct readers / streamers / sockets and block until inputs are live.

Source code in eden/extensions/teleop/base.py

@abstractmethod
def setup(self) -> None:
    """Construct readers / streamers / sockets and block until inputs are live."""

step abstractmethod

step() -> None

One control tick: poll input, advance state, stream, and publish.

Source code in eden/extensions/teleop/base.py

@abstractmethod
def step(self) -> None:
    """One control tick: poll input, advance state, stream, and publish."""

teardown abstractmethod

teardown() -> None

Release every resource (publish socket first, then slow scenes / windows).

Also runs after a setup that raised or was interrupted part-way, so it releases only what was actually built.

Source code in eden/extensions/teleop/base.py

@abstractmethod
def teardown(self) -> None:
    """Release every resource (publish socket first, then slow scenes / windows).

    Also runs after a ``setup`` that raised or was interrupted part-way, so it
    releases only what was actually built.
    """

should_stop

should_stop() -> bool

Whether run should leave the loop. Default: never (Ctrl-C exits).

Source code in eden/extensions/teleop/base.py

def should_stop(self) -> bool:
    """Whether ``run`` should leave the loop. Default: never (Ctrl-C exits)."""
    return False

on_interrupt

on_interrupt() -> None

Hook for a KeyboardInterrupt raised inside the loop. Default: no-op.

Source code in eden/extensions/teleop/base.py

def on_interrupt(self) -> None:
    """Hook for a ``KeyboardInterrupt`` raised inside the loop. Default: no-op."""

Configuration options for teleoperation operator stacks.

Classes

TeleopOptions

TeleopOptions(**data)

Bases: DynamicConfigurableOptions

Options for teleoperation operator stacks.

Carries the fields common to any operator process; a concrete stack declares its device-/protocol-specific parameters as class attributes on its :class:~eden.extensions.teleop.base.TeleopOperator subclass, which are mirrored through as extras (this options class is extra="allow").

PARAMETER	DESCRIPTION
robot_host	Host running the robot process this operator drives (its feedback channel). TYPE: str
port	Port the operator binds to publish its commands / pose / planner stream. TYPE: int
feedback_port	Port the robot process publishes its debug / feedback state on. TYPE: int

Source code in eden/options/options.py

def __init__(self, **data):
    # Seed type metadata as construction data so BaseModel validates it into
    # __pydantic_extra__ (no-op for extra="forbid" classes, which use
    # the serialization envelope instead).
    data.update(option_metadata(self))
    # NOTE: all of the option modifications/validations should be done in the __init__ method
    try:
        BaseModel.__init__(self, **data)
    except ValidationError as exc:
        # A misspelled field on an extra="forbid" class should teach the field set, not just
        # name the typo. Only rewritten when every error is extra_forbidden.
        enriched = _enrich_extra_forbidden(type(self), exc)
        if enriched is None:
            raise
        raise enriched from None

Attributes

robot_host class-attribute instance-attribute

robot_host: str = 'localhost'

port class-attribute instance-attribute

port: int = 5556

feedback_port class-attribute instance-attribute

feedback_port: int = 5557

SONIC operator station

SONIC operator station: PICO body tracking -> ZMQ commands for the robot process.

The Eden counterpart of the reference stack's T3. One manager loop polls the controllers, advances the operator state machine, runs the active mode's streamer (POSE at ~50 Hz, planner modes at ~20 Hz), and publishes the command / manager_state messages. Run it with eden teleop sonic operator.

PICO input arrives through eden pico bridge, which this process spawns (pose-only, port --bridge-port) unless --bridge-host points at an external one or --no-bridge-start is given; --sdk reads the XRoboToolkit SDK in-process instead (the reference-parity path).

Controls (PICO): A+B+X+Y engage + calibrate (stand in the neutral pose) / stop + exit A+X toggle POSE <-> PLANNER B+Y toggle POSE <-> PLANNER_FROZEN_UPPER_BODY L-stick click: toggle VR_3PT from either planner mode A+B/X+Y next / previous locomotion mode (planner modes) triggers close/open that hand menu hold pause the pose stream (re-align before releasing)

Attributes

Classes

SonicTeleopOperator

SonicTeleopOperator(options: TeleopOptions)

Bases: TeleopOperator

The SONIC operator station as a :class:TeleopOperator.

setup builds the reader, publish socket, calibration FK, streamers and state machine (blocking until body data arrives); step runs one control tick (poll -> state machine -> effects -> stream -> publish); teardown releases everything, publish socket first. The loop cadence is set by the active streamer's own pacing sleep inside its run_once. Config is mirrored from a :class:~eden.options.extensions.teleop.TeleopOptions (build one with SonicTeleopOperator.configure(**vars(args))); the PICO/SONIC-specific parameters below ride along as options extras.

Source code in eden/extensions/teleop/sonic/manager.py

def __init__(self, options: TeleopOptions) -> None:
    super().__init__(options)
    self._stop = False
    self._final_command_queued = False  # a planner hand-off is queued: teardown must flush it
    # The robot was handed to the planner and kept its session: it is still engaged, with the
    # heading it captured at the first engage, so the next engage must not reset the facing.
    self._robot_kept_heading = False
    # Built by setup(); None until then, so teardown can release a partial stack.
    self._context = self._socket = self._reader = self._bridge_proc = None
    self._feedback = self._reset_watcher = self._fk = self._viz = self._sm = None

Attributes

target_fps class-attribute instance-attribute

target_fps: int = 50

num_frames class-attribute instance-attribute

num_frames: int = 5

record_dir class-attribute instance-attribute

record_dir: str = ''

no_service_start class-attribute instance-attribute

no_service_start: bool = False

bridge_host class-attribute instance-attribute

bridge_host: str = ''

bridge_port class-attribute instance-attribute

bridge_port: int = SONIC_BRIDGE_POSE_PORT

bridge_fps class-attribute instance-attribute

bridge_fps: float = 120.0

no_bridge_start class-attribute instance-attribute

no_bridge_start: bool = False

sdk class-attribute instance-attribute

sdk: bool = False

vis_vr3pt class-attribute instance-attribute

vis_vr3pt: bool = False

vis_smpl class-attribute instance-attribute

vis_smpl: bool = False

waist_tracking class-attribute instance-attribute

waist_tracking: bool = False

no_g1 class-attribute instance-attribute

no_g1: bool = False

Methods:

setup

setup() -> None

Source code in eden/extensions/teleop/sonic/manager.py

def setup(self) -> None:
    import genesis as gs
    import zmq

    import eden as en
    from eden.extensions.teleop.sonic.feedback import FeedbackReader, RobotResetWatcher
    from eden.extensions.teleop.sonic.g1_fk import G1KeyFrameFk
    from eden.extensions.teleop.sonic.planner_streamer import PlannerStreamer
    from eden.extensions.teleop.sonic.pose_streamer import PoseStreamer
    from eden.extensions.teleop.sonic.three_point import ThreePointPose

    # CPU backend: the calibration FK is a single-env kinematics scene, and
    # the operator pipelines are numpy — claiming a GPU (and torch's default
    # device) here would only fight the robot process.
    en.init(backend=gs.cpu)

    # A --bridge-host bridge runs on another machine, next to its own service.
    self._bridge_proc = None
    if not self.no_service_start and not self.bridge_host and os.path.exists(_SERVICE_SCRIPT):
        subprocess.Popen(["bash", _SERVICE_SCRIPT])
        print(f"[sonic operator] started the XRoboToolkit PC service ({_SERVICE_SCRIPT})")

    if self.sdk:
        from eden.extensions.teleop.sonic.inputs import PicoReader

        self._reader = PicoReader()
    else:
        from eden.extensions.teleop.sonic.inputs import BridgeReader

        if not self.bridge_host and not self.no_bridge_start:
            # PR_SET_PDEATHSIG is Linux-only; elsewhere the bridge is reaped by teardown alone.
            preexec_fn = _die_with_parent if sys.platform.startswith("linux") else None
            self._bridge_proc = subprocess.Popen(_bridge_argv(self), preexec_fn=preexec_fn)  # noqa: PLW1509
            print(
                f"[sonic operator] started 'eden pico bridge' (pose port {self.bridge_port}, {self.bridge_fps:g} fps)"
            )
        self._reader = BridgeReader(host=self.bridge_host or "127.0.0.1", port=self.bridge_port)
    self._reader.start()

    # Bound before the slow startup (FK compile, headset wait) so a port
    # collision fails fast instead of minutes in. The command socket gets its
    # own context so teardown can term() it — the only call that waits for a
    # queued stop command to be written — without waiting on the feedback /
    # reader sockets that live on the shared instance.
    self._context = zmq.Context()
    self._socket = self._context.socket(zmq.PUB)
    try:
        self._socket.bind(f"tcp://*:{self.port}")
    except zmq.ZMQError as e:
        self._socket.close(linger=0)
        self._context.term()
        self._context = self._socket = None
        raise SystemExit(
            f"[sonic operator] cannot bind tcp://*:{self.port} ({e}) — a previous operator (or the "
            "reference streamer) is still running or still shutting down; stop it, or pass --port"
        ) from e
    print(f"[sonic operator] publishing on tcp://*:{self.port}")

    # Built eagerly, while the headset connects: the first calibration (the
    # engage press) must not stall on the kernel compile.
    print("[sonic operator] building the G1 kinematics scene (first run compiles, up to ~30 s)...")
    self._fk = G1KeyFrameFk()

    print("[sonic operator] waiting for body tracking data (headset in Full-body mode, service WORKING)...")
    while self._reader.get_latest() is None:
        if self._bridge_proc is not None and self._bridge_proc.poll() is not None:
            raise SystemExit(
                f"[sonic operator] the pico bridge exited with code {self._bridge_proc.returncode} — "
                f"check the xrobotoolkit_sdk build and that port {self.bridge_port} is free"
            )
        print("waiting for body data...")
        time.sleep(1)

    on_frame = None
    self._viz = None
    if self.vis_vr3pt:
        from eden.extensions.teleop.sonic.viz import Vr3PtPoseVisualizer

        self._viz = Vr3PtPoseVisualizer(
            with_g1_robot=not self.no_g1,
            enable_waist_tracking=self.waist_tracking,
            enable_smpl_vis=self.vis_smpl,
        )
        on_frame = self._viz.on_frame

    self._three_point = ThreePointPose(self._fk)
    self._feedback = FeedbackReader(host=self.robot_host, port=self.feedback_port)
    self._reset_watcher = RobotResetWatcher(host=self.robot_host, port=self.feedback_port)
    self._pose_streamer = PoseStreamer(
        send=self._socket.send,
        three_point=self._three_point,
        num_frames_to_send=self.num_frames,
        target_fps=self.target_fps,
        record_dir=self.record_dir,
        on_frame=on_frame,
    )
    self._planner_streamer = PlannerStreamer(
        send=self._socket.send, three_point=self._three_point, feedback=self._feedback, on_frame=on_frame
    )
    self._sm = OperatorStateMachine()

    print("[sonic operator] A+B+X+Y engages (stand in the neutral pose: forearms bent 90° forward)")
    print(
        "[sonic operator] A+B+X+Y again disengages: the robot balances in place on the planner; "
        "Ctrl-C exits the operator (the robot keeps idling); end the robot session from its own terminal"
    )
    self._prev_toggle_dc = False
    self._prev_toggle_da = False
    self._bridge_died = False

step

step() -> None

Source code in eden/extensions/teleop/sonic/manager.py

def step(self) -> None:
    reader = self._reader
    socket = self._socket
    sm = self._sm

    if self._bridge_proc is not None and not self._bridge_died and self._bridge_proc.poll() is not None:
        self._bridge_died = True
        print(
            f"[sonic operator] WARNING: the pico bridge exited with code {self._bridge_proc.returncode}; "
            "the input stream is frozen — Ctrl-C to exit"
        )
    if input_lost(sm.mode, bridge_died=self._bridge_died, body_stale=reader.disconnected):
        # Frozen input would otherwise leave the robot on the last streamed pose
        # (POSE never times out robot-side) or the last VR 3-point target.
        forced = self._force_off("input lost", "A+B+X+Y re-engages once tracking is back")
        self._release_robot_to_planner(forced.previous)
        return
    controllers = reader.controllers()

    if self._reset_watcher.poll() and sm.mode != StreamMode.OFF:
        self._force_off("robot fell and auto-reset", "stand in the neutral pose and press A+B+X+Y to re-engage")
        self._robot_kept_heading = False  # the reset disengaged the robot: the next engage re-captures it
        return

    transition = sm.step(
        ButtonSnapshot(
            a=controllers.a,
            b=controllers.b,
            x=controllers.x,
            y=controllers.y,
            left_axis_click=controllers.left_axis_click,
            left_menu=controllers.left_menu,
        )
    )

    for effect in transition.effects:
        if effect == Effect.POSE_EXIT:
            self._pose_streamer.on_mode_exit()
        elif effect == Effect.CALIBRATE_FULL:
            sample = reader.get_latest()
            if sample is not None:
                self._three_point.calibrate_now(sample.body_poses)
            else:
                print("[sonic operator] WARNING: no body data available for calibration")
        elif effect == Effect.RESET_POSE_YAW:
            self._pose_streamer.reset_yaw()
        elif effect == Effect.RESET_PLANNER_YAW:
            if transition.previous == StreamMode.OFF and self._robot_kept_heading:
                # The robot stayed engaged on the planner, so it re-captures no heading on
                # this engage: the facing it is idling with must carry over, not snap to forward.
                self._robot_kept_heading = False
            else:
                self._planner_streamer.reset_yaw()
        elif effect == Effect.RELEASE_TO_PLANNER:
            self._release_robot_to_planner(transition.previous)
            print(
                "[sonic operator] disengaged — A+B+X+Y re-engages (full calibration: neutral pose, "
                "looking ahead); Ctrl-C exits the operator"
            )
        elif effect == Effect.SAVE_FROZEN_TARGETS:
            self._planner_streamer.save_upper_body_position_target()
        elif effect == Effect.RECALIBRATE_WRISTS:
            self._planner_streamer.recalibrate_for_vr3pt()

    # Run one tick of the new mode (its pacing sleep sets the loop rate).
    if transition.mode == StreamMode.POSE:
        self._pose_streamer.run_once(reader.get_latest(), controllers)
    elif transition.mode in PLANNER_MODES:
        self._planner_streamer.run_once(
            transition.mode, reader.get_latest(), controllers, reader.get_timestamp_ns()
        )
    else:
        time.sleep(0.005)  # OFF / POSE_PAUSE: idle poll (no hot spin)

    # Command messages go out after the first tick of the new mode, so its
    # data is already in flight when the robot switches.
    if transition.changed:
        print(f"[sonic operator] {transition.previous.name} -> {transition.mode.name}")
        if transition.command is not None:
            kind, planner_flag = transition.command
            socket.send(build_command_message(start=kind == "start", stop=False, planner=planner_flag))

    # manager_state every tick (consumed by data recorders, not the robot).
    toggle_dc_now = controllers.a and controllers.left_grip > 0.5
    toggle_da_now = controllers.b and controllers.left_grip > 0.5
    socket.send(
        build_manager_state_message(
            stream_mode=transition.mode.value,
            toggle_data_collection=toggle_dc_now and not self._prev_toggle_dc,
            toggle_data_abort=toggle_da_now and not self._prev_toggle_da,
        )
    )
    self._prev_toggle_dc = toggle_dc_now
    self._prev_toggle_da = toggle_da_now

should_stop

should_stop() -> bool

Source code in eden/extensions/teleop/sonic/manager.py

def should_stop(self) -> bool:
    return self._stop

on_interrupt

on_interrupt() -> None

Source code in eden/extensions/teleop/sonic/manager.py

def on_interrupt(self) -> None:
    print("\n[sonic operator] interrupted")

teardown

teardown() -> None

Source code in eden/extensions/teleop/sonic/manager.py

def teardown(self) -> None:
    if self._socket is not None:
        # Leaving while engaged (Ctrl-C, an exception in step) without the stop
        # combo (which leaves the machine OFF): don't leave the robot holding the
        # last streamed pose.
        if self._sm is not None and self._sm.mode != StreamMode.OFF:
            self._release_robot_to_planner(self._sm.mode)
        # Release the command port first: the viz window and the FK scene can
        # take a while to tear down, and an immediate operator relaunch must not
        # find :5556 still bound to this dying process. linger=0 discards
        # whatever ZMQ has not yet written, though, and the last command is sent
        # microseconds before this — so after one, linger and term() (which
        # blocks until the queue is written or the linger expires) so the robot
        # actually receives it.
        self._socket.close(linger=_FINAL_COMMAND_LINGER_MS if self._final_command_queued else 0)
        self._context.term()
    if self._reader is not None:
        self._reader.stop()
    if self._feedback is not None:
        self._feedback.close()
    if self._reset_watcher is not None:
        self._reset_watcher.close()
    if self._bridge_proc is not None and self._bridge_proc.poll() is None:
        self._bridge_proc.terminate()
        try:
            self._bridge_proc.wait(timeout=3.0)
        except subprocess.TimeoutExpired:
            self._bridge_proc.kill()
    if self._viz is not None:
        self._viz.close()
    if self._fk is not None:
        self._fk.close()
    print("[sonic operator] shutdown complete")

Functions:

input_lost

input_lost(mode: StreamMode, *, bridge_died: bool, body_stale: bool) -> bool

Whether an engaged operator has lost the input its current mode streams from.

A dead bridge freezes everything. Body staleness only matters to the modes that stream body poses: PLANNER / FROZEN_UPPER run on the controllers (whose stalls the robot's planner timeout already handles), so a tracker occlusion must not end joystick walking.

Source code in eden/extensions/teleop/sonic/manager.py

def input_lost(mode: StreamMode, *, bridge_died: bool, body_stale: bool) -> bool:
    """Whether an engaged operator has lost the input its current mode streams from.

    A dead bridge freezes everything. Body staleness only matters to the modes that
    stream body poses: PLANNER / FROZEN_UPPER run on the controllers (whose stalls the
    robot's planner timeout already handles), so a tracker occlusion must not end
    joystick walking.
    """
    if mode == StreamMode.OFF:
        return False
    return bridge_died or (body_stale and mode in _BODY_STREAM_MODES)

build_arg_parser

build_arg_parser() -> ArgumentParser

Source code in eden/extensions/teleop/sonic/manager.py

def build_arg_parser() -> argparse.ArgumentParser:
    parser = argparse.ArgumentParser(
        prog="eden teleop sonic operator", description="SONIC operator station (PICO body tracking -> robot commands)."
    )
    parser.add_argument(
        "--robot-host", default="localhost", help="Host running 'eden teleop sonic robot' (g1_debug SUB)"
    )
    parser.add_argument("--port", type=int, default=5556, help="Command/planner/pose publish port (bound here)")
    parser.add_argument("--feedback-port", type=int, default=5557, help="Robot g1_debug port")
    parser.add_argument("--target-fps", type=int, default=50, help="Pose stream rate")
    parser.add_argument("--num-frames", type=int, default=5, help="Pose window size (frames per message)")
    parser.add_argument("--record-dir", default="", help="Record each sent pose message as .npz here")
    parser.add_argument("--no-service-start", action="store_true", help="Do not spawn the XRoboToolkit PC service")
    parser.add_argument(
        "--bridge-host",
        default="",
        help="Connect to an 'eden pico bridge' on this host (skips spawning the bridge and the PC service)",
    )
    parser.add_argument(
        "--bridge-port",
        type=int,
        default=SONIC_BRIDGE_POSE_PORT,
        help="Bridge pose PUB port (the default avoids the SONIC wire's 5556/5557)",
    )
    parser.add_argument(
        "--bridge-fps",
        type=float,
        default=120.0,
        help="Publish rate for the spawned bridge; keep it above the device body rate (~90 Hz)",
    )
    parser.add_argument(
        "--no-bridge-start", action="store_true", help="Do not spawn the bridge; connect to one already running locally"
    )
    parser.add_argument(
        "--sdk", action="store_true", help="Read the XRoboToolkit SDK in-process instead of via the bridge"
    )
    parser.add_argument("--vis-vr3pt", action="store_true", help="Visualize the calibrated VR 3-point pose")
    parser.add_argument("--vis-smpl", action="store_true", help="Also render the SMPL body (needs --vis-vr3pt)")
    parser.add_argument("--waist-tracking", action="store_true", help="Viz only: robot waist follows the neck")
    parser.add_argument("--no-g1", action="store_true", help="Viz only: hide the G1 mesh")
    return parser

main

main(argv: list[str] | None = None) -> None

Source code in eden/extensions/teleop/sonic/manager.py

def main(argv: list[str] | None = None) -> None:
    args = build_arg_parser().parse_args(argv)
    if args.vis_smpl and not args.vis_vr3pt:
        raise SystemExit("--vis-smpl requires --vis-vr3pt (they share one window)")
    # Every argparse dest is a configurable parameter of the operator, so the
    # namespace maps straight onto TeleopOptions via configure().
    options = SonicTeleopOperator.configure(**vars(args))
    SonicTeleopOperator(options).run()

Operator state machine for SONIC teleoperation (pure logic, no I/O).

Port of the reference manager's mode table. Two chains share the VR_3PT sub-mode, which remembers its parent:

chain 1:  POSE <--(B+Y)--> PLANNER_FROZEN_UPPER_BODY <--(L-stick click)--> PLANNER_VR_3PT
chain 2:  POSE <--(A+X)--> PLANNER                   <--(L-stick click)--> PLANNER_VR_3PT
VR_3PT --(A+X or B+Y)--> POSE;  A+B+X+Y engages from OFF / disengages from anywhere
POSE --(menu held)--> POSE_PAUSE --(menu released)--> POSE  (level-triggered)

Button combos are rising-edge except the menu pause. Each transition carries the side effects the manager must run before streaming the new mode's first tick, plus the command message to send after it.

Disengaging is an Eden deviation: the reference's A+B+X+Y from a live mode sends stop, which ends the robot process — on hardware a damping-only command, and an unsupported robot falls. Here it hands the robot to the planner's IDLE instead, so it keeps balancing in place; the robot session ends only from the robot side.

Attributes

PLANNER_MODES module-attribute

PLANNER_MODES = (StreamMode.PLANNER, StreamMode.PLANNER_FROZEN_UPPER_BODY, StreamMode.PLANNER_VR_3PT)

Classes

StreamMode

Bases: Enum

Attributes

OFF class-attribute instance-attribute

OFF = 0

POSE class-attribute instance-attribute

POSE = 1

PLANNER class-attribute instance-attribute

PLANNER = 2

PLANNER_FROZEN_UPPER_BODY class-attribute instance-attribute

PLANNER_FROZEN_UPPER_BODY = 3

POSE_PAUSE class-attribute instance-attribute

POSE_PAUSE = 4

PLANNER_VR_3PT class-attribute instance-attribute

PLANNER_VR_3PT = 5

Effect

Bases: Enum

Side effects to run before the new mode's first streamed tick.

Attributes

CALIBRATE_FULL class-attribute instance-attribute

CALIBRATE_FULL = 'calibrate_full'

RECALIBRATE_WRISTS class-attribute instance-attribute

RECALIBRATE_WRISTS = 'recalibrate_wrists'

RESET_POSE_YAW class-attribute instance-attribute

RESET_POSE_YAW = 'reset_pose_yaw'

RESET_PLANNER_YAW class-attribute instance-attribute

RESET_PLANNER_YAW = 'reset_planner_yaw'

SAVE_FROZEN_TARGETS class-attribute instance-attribute

SAVE_FROZEN_TARGETS = 'save_frozen_targets'

POSE_EXIT class-attribute instance-attribute

POSE_EXIT = 'pose_exit'

RELEASE_TO_PLANNER class-attribute instance-attribute

RELEASE_TO_PLANNER = 'release_to_planner'

ButtonSnapshot dataclass

ButtonSnapshot(a: bool = False, b: bool = False, x: bool = False, y: bool = False, left_axis_click: bool = False, left_menu: bool = False)

Controller levels for one manager tick (A/B = right, X/Y = left).

Attributes

a class-attribute instance-attribute

a: bool = False

b class-attribute instance-attribute

b: bool = False

x class-attribute instance-attribute

x: bool = False

y class-attribute instance-attribute

y: bool = False

left_axis_click class-attribute instance-attribute

left_axis_click: bool = False

left_menu class-attribute instance-attribute

left_menu: bool = False

Methods:

Transition dataclass

Transition(previous: StreamMode, mode: StreamMode, changed: bool, effects: list[Effect] = list(), command: tuple[str, bool] | None = None)

Outcome of one state-machine step.

Attributes

previous instance-attribute

previous: StreamMode

mode instance-attribute

mode: StreamMode

changed instance-attribute

changed: bool

effects class-attribute instance-attribute

effects: list[Effect] = field(default_factory=list)

command class-attribute instance-attribute

command: tuple[str, bool] | None = None

Methods:

OperatorStateMachine

OperatorStateMachine()

Rising-edge mode table with VR_3PT parent tracking.

Source code in eden/extensions/teleop/sonic/state_machine.py

def __init__(self) -> None:
    self.mode = StreamMode.OFF
    self.vr3pt_parent = StreamMode.PLANNER
    self._prev_ax = False
    self._prev_by = False
    self._prev_start = False
    self._prev_stick = False

Attributes

mode instance-attribute

mode = StreamMode.OFF

vr3pt_parent instance-attribute

vr3pt_parent = StreamMode.PLANNER

Methods:

step

step(buttons: ButtonSnapshot) -> Transition

Advance one tick; returns the (possibly unchanged) mode and its effects.

Source code in eden/extensions/teleop/sonic/state_machine.py

def step(self, buttons: ButtonSnapshot) -> Transition:
    """Advance one tick; returns the (possibly unchanged) mode and its effects."""
    ax = buttons.a and buttons.x
    by = buttons.b and buttons.y
    start_combo = buttons.a and buttons.b and buttons.x and buttons.y
    stick = buttons.left_axis_click

    ax_edge = ax and not self._prev_ax
    by_edge = by and not self._prev_by
    start_edge = start_combo and not self._prev_start
    stick_edge = stick and not self._prev_stick

    current = self.mode
    new_mode = current
    if current == StreamMode.OFF:
        if start_edge:
            new_mode = StreamMode.PLANNER
    elif current == StreamMode.PLANNER:
        if start_edge:
            new_mode = StreamMode.OFF
        elif ax_edge:
            new_mode = StreamMode.POSE
        elif stick_edge:
            new_mode = StreamMode.PLANNER_VR_3PT
    elif current == StreamMode.POSE:
        if start_edge:
            new_mode = StreamMode.OFF
        elif ax_edge:
            new_mode = StreamMode.PLANNER
        elif by_edge:
            new_mode = StreamMode.PLANNER_FROZEN_UPPER_BODY
        elif buttons.left_menu:  # level, not edge
            new_mode = StreamMode.POSE_PAUSE
    elif current == StreamMode.PLANNER_FROZEN_UPPER_BODY:
        if start_edge:
            new_mode = StreamMode.OFF
        elif by_edge:
            new_mode = StreamMode.POSE
        elif stick_edge:
            new_mode = StreamMode.PLANNER_VR_3PT
    elif current == StreamMode.POSE_PAUSE:
        if start_edge:
            new_mode = StreamMode.OFF
        elif not buttons.left_menu:
            new_mode = StreamMode.POSE
    elif current == StreamMode.PLANNER_VR_3PT:
        if start_edge:
            new_mode = StreamMode.OFF
        elif stick_edge:
            new_mode = self.vr3pt_parent
        elif ax_edge or by_edge:
            new_mode = StreamMode.POSE

    self._prev_ax = ax
    self._prev_by = by
    self._prev_start = start_combo
    self._prev_stick = stick

    transition = Transition(previous=current, mode=new_mode, changed=new_mode != current)
    if transition.changed:
        self._collect_effects(transition)
    self.mode = new_mode
    return transition

force_off

force_off() -> Transition

External force-off (robot fall auto-reset, input loss): back to OFF, no effects but the pose exit.

The caller decides what the robot needs: nothing after a fall auto-reset (it already reset itself), a planner hand-off after an input loss. The next A+B+X+Y re-engages through the normal OFF -> PLANNER edge with full calibration.

Source code in eden/extensions/teleop/sonic/state_machine.py

def force_off(self) -> Transition:
    """External force-off (robot fall auto-reset, input loss): back to OFF, no effects but the pose exit.

    The caller decides what the robot needs: nothing after a fall auto-reset
    (it already reset itself), a planner hand-off after an input loss. The
    next A+B+X+Y re-engages through the normal OFF -> PLANNER edge with full
    calibration.
    """
    previous = self.mode
    self.mode = StreamMode.OFF
    transition = Transition(previous=previous, mode=StreamMode.OFF, changed=previous != StreamMode.OFF)
    if previous in (StreamMode.POSE, StreamMode.POSE_PAUSE):
        transition.effects.append(Effect.POSE_EXIT)
    return transition

PICO body-tracking readers for the SONIC operator station.

Both readers hand the pipelines the raw XRoboToolkit service data — the (24, 7) full-body joint poses in the service's own frame (positions + scalar-last quaternions) plus controller buttons/sticks/triggers. The SONIC pipelines apply their own frame conversions (see smpl.py / three_point.py), so neither reader goes through :class:eden.utils.devices.pico.xr_client.PicoXrClient's robot-frame poses, whose conversion differs from the reference teleop math.

:class:BridgeReader (the default) subscribes to eden pico bridge's pose channel and consumes its lossless body_raw passthrough, so the SDK and its service-library linkage live in the bridge process. :class:PicoReader is the in-process port of the reference input_readers.PicoReader: a daemon thread spins on is_body_data_available, dedupes frames by device timestamp, keeps exactly one latest sample, and flags a disconnect after 5 s without fresh data. The bridge reader keeps the same sample semantics (dedupe on the same timestamp, identical dt/fps bookkeeping).

Attributes

SONIC_BRIDGE_POSE_PORT module-attribute

SONIC_BRIDGE_POSE_PORT = 5559

Classes

BodySample dataclass

BodySample(body_poses: ndarray, timestamp_realtime: float, timestamp_monotonic: float, timestamp_ns: int, dt: float, fps: float)

One deduplicated full-body frame from the PICO service.

Attributes

body_poses instance-attribute

body_poses: ndarray

timestamp_realtime instance-attribute

timestamp_realtime: float

timestamp_monotonic instance-attribute

timestamp_monotonic: float

timestamp_ns instance-attribute

timestamp_ns: int

dt instance-attribute

dt: float

fps instance-attribute

fps: float

Methods:

ControllerSample dataclass

ControllerSample(a: bool, b: bool, x: bool, y: bool, left_axis_click: bool, right_axis_click: bool, left_menu: bool, right_menu: bool, left_trigger: float, right_trigger: float, left_grip: float, right_grip: float, left_axis: tuple[float, float], right_axis: tuple[float, float])

Controller buttons/sticks/triggers, read at the manager rate.

Attributes

a instance-attribute

a: bool

b instance-attribute

b: bool

x instance-attribute

x: bool

y instance-attribute

y: bool

left_axis_click instance-attribute

left_axis_click: bool

right_axis_click instance-attribute

right_axis_click: bool

left_menu instance-attribute

left_menu: bool

right_menu instance-attribute

right_menu: bool

left_trigger instance-attribute

left_trigger: float

right_trigger instance-attribute

right_trigger: float

left_grip instance-attribute

left_grip: float

right_grip instance-attribute

right_grip: float

left_axis instance-attribute

left_axis: tuple[float, float]

right_axis instance-attribute

right_axis: tuple[float, float]

Methods:

PicoReader

PicoReader()

Bases: _BodyStreamReader

In-process reader for the raw PICO body stream + on-demand controller reads.

Source code in eden/extensions/teleop/sonic/inputs.py

def __init__(self) -> None:
    super().__init__()
    self._sdk = load_sdk()
    self._sdk.init()
    self._thread = threading.Thread(target=self._run, name="sonic-pico-reader", daemon=True)

Methods:

start

start() -> None

Source code in eden/extensions/teleop/sonic/inputs.py

def start(self) -> None:
    if not self._thread.is_alive():
        self._thread.start()

stop

stop() -> None

Source code in eden/extensions/teleop/sonic/inputs.py

def stop(self) -> None:
    self._stop.set()
    if self._thread.is_alive():
        self._thread.join(timeout=1.0)

get_timestamp_ns

get_timestamp_ns() -> int

The service's latest device timestamp (used as a liveness check).

Source code in eden/extensions/teleop/sonic/inputs.py

def get_timestamp_ns(self) -> int:
    """The service's latest device timestamp (used as a liveness check)."""
    return int(self._sdk.get_time_stamp_ns())

controllers

controllers() -> ControllerSample

Read the current controller state (level values; edges are the caller's job).

Source code in eden/extensions/teleop/sonic/inputs.py

def controllers(self) -> ControllerSample:
    """Read the current controller state (level values; edges are the caller's job)."""
    sdk = self._sdk
    lx, ly = sdk.get_left_axis()
    rx, ry = sdk.get_right_axis()
    return ControllerSample(
        a=bool(sdk.get_A_button()),
        b=bool(sdk.get_B_button()),
        x=bool(sdk.get_X_button()),
        y=bool(sdk.get_Y_button()),
        left_axis_click=bool(sdk.get_left_axis_click()),
        right_axis_click=bool(sdk.get_right_axis_click()),
        left_menu=bool(sdk.get_left_menu_button()),
        right_menu=bool(sdk.get_right_menu_button()),
        left_trigger=float(sdk.get_left_trigger()),
        right_trigger=float(sdk.get_right_trigger()),
        left_grip=float(sdk.get_left_grip()),
        right_grip=float(sdk.get_right_grip()),
        left_axis=(float(lx), float(ly)),
        right_axis=(float(rx), float(ry)),
    )

BridgeReader

BridgeReader(host: str = '127.0.0.1', port: int = SONIC_BRIDGE_POSE_PORT)

Bases: _BodyStreamReader

PicoReader-compatible reader fed by eden pico bridge over ZMQ.

Same interface and sample semantics; the SDK (and its service-library linkage) lives in the bridge process instead. Consumes the bridge state's body_raw / body_raw_timestamp_ns passthrough, so the pipelines see exactly the bytes the SDK reported; buttons map from the bridge's metaquest-compatible keys (left primary = X, right primary = A).

Source code in eden/extensions/teleop/sonic/inputs.py

def __init__(self, host: str = "127.0.0.1", port: int = SONIC_BRIDGE_POSE_PORT) -> None:
    super().__init__()
    # Lazy import: the sonic tests import this module before gating on zmq.
    import zmq

    self._zmq = zmq
    self._sock = zmq.Context.instance().socket(zmq.SUB)
    self._sock.setsockopt(zmq.SUBSCRIBE, b"")
    self._sock.setsockopt(zmq.RCVTIMEO, 100)
    self._sock.setsockopt(zmq.LINGER, 0)
    self._sock.connect(f"tcp://{host}:{port}")
    self._state: dict = {}
    self._thread = threading.Thread(target=self._run, name="sonic-bridge-reader", daemon=True)

Methods:

start

start() -> None

Source code in eden/extensions/teleop/sonic/inputs.py

def start(self) -> None:
    if not self._thread.is_alive():
        self._thread.start()

stop

stop() -> None

Source code in eden/extensions/teleop/sonic/inputs.py

def stop(self) -> None:
    self._stop.set()
    if self._thread.is_alive():
        self._thread.join(timeout=1.0)
    self._sock.close()

get_timestamp_ns

get_timestamp_ns() -> int

The bridge's latest service timestamp (advances at the publish rate).

Source code in eden/extensions/teleop/sonic/inputs.py

def get_timestamp_ns(self) -> int:
    """The bridge's latest service timestamp (advances at the publish rate)."""
    with self._lock:
        return int(self._state.get("timestamp_ns", 0))

controllers

controllers() -> ControllerSample

The latest published controller state (neutral before the first message).

Source code in eden/extensions/teleop/sonic/inputs.py

def controllers(self) -> ControllerSample:
    """The latest published controller state (neutral before the first message)."""
    with self._lock:
        state = self._state
    if not state:
        return _NEUTRAL_CONTROLLERS
    lx, ly = state.get("left_thumbstick_value", (0.0, 0.0))
    rx, ry = state.get("right_thumbstick_value", (0.0, 0.0))
    return ControllerSample(
        a=bool(state.get("right_a_button", False)),
        b=bool(state.get("right_b_button", False)),
        x=bool(state.get("left_a_button", False)),
        y=bool(state.get("left_b_button", False)),
        left_axis_click=bool(state.get("left_thumbstick", False)),
        right_axis_click=bool(state.get("right_thumbstick", False)),
        left_menu=bool(state.get("left_menu_button", False)),
        right_menu=bool(state.get("right_menu_button", False)),
        left_trigger=float(state.get("left_trigger_value", 0.0)),
        right_trigger=float(state.get("right_trigger_value", 0.0)),
        left_grip=float(state.get("left_squeeze_value", 0.0)),
        right_grip=float(state.get("right_squeeze_value", 0.0)),
        left_axis=(float(lx), float(ly)),
        right_axis=(float(rx), float(ry)),
    )

Functions:

POSE-mode streamer: PICO body frames -> 50 Hz windowed SMPL pose messages.

Port of the reference PoseStreamer. Device frames (~90 Hz) are resampled onto a fixed 50 Hz virtual clock by interpolating between the two frames that bracket each virtual tick (nlerp for rotations), then the last five resampled frames are re-sent as an overlapping window on every tick — the robot's motion merger aligns them by frame index and rides out packet loss. Publishing is gated until the window is full after a mode entry.

Each message also carries: the wrist joint targets derived from the elbow twist/swing mapping, the calibrated VR 3-point pose of the current frame, Dex3 hand targets from the triggers, data-collection toggle edges, and the right-stick heading increment.

Attributes

JOYSTICK_DEADZONE module-attribute

JOYSTICK_DEADZONE = 0.15

IDLE_POLL_S module-attribute

IDLE_POLL_S = 0.002

Classes

YawAccumulator

YawAccumulator(yaw_gain: float = 1.5, deadzone: float = JOYSTICK_DEADZONE)

Accumulates a heading angle from the right stick (yaw gain 1.5 rad/s).

Source code in eden/extensions/teleop/sonic/pose_streamer.py

def __init__(self, yaw_gain: float = 1.5, deadzone: float = JOYSTICK_DEADZONE) -> None:
    self.yaw_gain = yaw_gain
    self.deadzone = deadzone
    self.reset()

Attributes

yaw_gain instance-attribute

yaw_gain = yaw_gain

deadzone instance-attribute

deadzone = deadzone

Methods:

reset

reset() -> None

Source code in eden/extensions/teleop/sonic/pose_streamer.py

def reset(self) -> None:
    self.heading = [1.0, 0.0, 0.0]
    self.yaw_angle_rad = 0.0
    self.dyaw = 0.0

update

update(rx: float, dt: float) -> list[float]

Integrate the stick's x-axis; returns the facing unit vector [x, y, 0].

Source code in eden/extensions/teleop/sonic/pose_streamer.py

def update(self, rx: float, dt: float) -> list[float]:
    """Integrate the stick's x-axis; returns the facing unit vector [x, y, 0]."""
    self.dyaw = self.yaw_gain * (-rx) * dt
    if abs(rx) >= self.deadzone:
        self.yaw_angle_rad += self.dyaw
        self.heading = [np.cos(self.yaw_angle_rad), np.sin(self.yaw_angle_rad), 0.0]
    return self.heading

PoseStreamer

PoseStreamer(send: Callable[[bytes], None], three_point: ThreePointPose, fk: SmplFk | None = None, num_frames_to_send: int = 5, target_fps: int = 50, record_dir: str = '', on_frame: Callable[[ndarray, ndarray], None] | None = None)

Bases: TeleopStreamer

One POSE-mode streaming iteration per manager tick.

send publishes one packed message; on_frame(vr_3pt_pose, smpl_joints_local) is the visualization hook, called once per processed device frame.

Source code in eden/extensions/teleop/sonic/pose_streamer.py

def __init__(
    self,
    send: Callable[[bytes], None],
    three_point: ThreePointPose,
    fk: SmplFk | None = None,
    num_frames_to_send: int = 5,
    target_fps: int = 50,
    record_dir: str = "",
    on_frame: Callable[[np.ndarray, np.ndarray], None] | None = None,
) -> None:
    """``send`` publishes one packed message; ``on_frame(vr_3pt_pose,
    smpl_joints_local)`` is the visualization hook, called once per
    processed device frame.
    """
    self._send = send
    self._three_point = three_point
    self._fk = fk or SmplFk()
    self._on_frame = on_frame
    self.num_frames_to_send = num_frames_to_send
    self.target_fps = target_fps
    self.record_dir = record_dir
    self._record_idx = 0
    if record_dir:
        import os

        os.makedirs(record_dir, exist_ok=True)

    self.yaw_accumulator = YawAccumulator()
    # 95% of the frame period: the remaining 5% absorbs processing overhead.
    self.frame_time = 0.95 / max(1, target_fps)
    self._sleep = time.sleep

    self.step = 0
    self.frame_buffer: dict[str, deque] = defaultdict(lambda: deque(maxlen=num_frames_to_send))
    self.prev_stamp_ns: int | None = None
    self._last_seen_stamp_ns: int | None = None  # newest device frame run_once has processed
    self.prev_smpl_pose: np.ndarray | None = None
    self.prev_smpl_joints: np.ndarray | None = None
    self.prev_body_quat: np.ndarray | None = None
    self.next_target_ns: int | None = None
    self.frame_start = time.time()
    self.buffer_cleared = True  # publish only once the window refills
    self._toggle_dc_last = False
    self._toggle_da_last = False
    self._emit_count = 0
    self._rate_log_time = time.time()

Attributes

num_frames_to_send instance-attribute

num_frames_to_send = num_frames_to_send

target_fps instance-attribute

target_fps = target_fps

record_dir instance-attribute

record_dir = record_dir

yaw_accumulator instance-attribute

yaw_accumulator = YawAccumulator()

frame_time instance-attribute

frame_time = 0.95 / max(1, target_fps)

step instance-attribute

step = 0

frame_buffer instance-attribute

frame_buffer: dict[str, deque] = defaultdict(lambda: deque(maxlen=num_frames_to_send))

prev_stamp_ns instance-attribute

prev_stamp_ns: int | None = None

prev_smpl_pose instance-attribute

prev_smpl_pose: ndarray | None = None

prev_smpl_joints instance-attribute

prev_smpl_joints: ndarray | None = None

prev_body_quat instance-attribute

prev_body_quat: ndarray | None = None

next_target_ns instance-attribute

next_target_ns: int | None = None

frame_start instance-attribute

frame_start = time.time()

buffer_cleared instance-attribute

buffer_cleared = True

Methods:

reset_yaw

reset_yaw() -> None

Source code in eden/extensions/teleop/sonic/pose_streamer.py

def reset_yaw(self) -> None:
    self.yaw_accumulator.reset()

on_mode_exit

on_mode_exit() -> None

Clear the stream window; re-entry publishes only after it refills.

Source code in eden/extensions/teleop/sonic/pose_streamer.py

def on_mode_exit(self) -> None:
    """Clear the stream window; re-entry publishes only after it refills."""
    self.frame_buffer.clear()
    self.prev_stamp_ns = None
    self._last_seen_stamp_ns = None
    self.prev_smpl_pose = None
    self.prev_smpl_joints = None
    self.prev_body_quat = None
    self.next_target_ns = None
    self.buffer_cleared = True
    self.step = 0

run_once

run_once(sample: BodySample | None, controllers: ControllerSample) -> None

Process the latest device frame; publish when a virtual tick completes.

Source code in eden/extensions/teleop/sonic/pose_streamer.py

def run_once(self, sample: BodySample | None, controllers: ControllerSample) -> None:
    """Process the latest device frame; publish when a virtual tick completes."""
    if sample is None:
        self._sleep(0.005)
        return
    if sample.timestamp_ns == self._last_seen_stamp_ns:
        # Same device frame as last tick: every path below would reach the same
        # result again, after a full SMPL-X FK. Without this wait the manager
        # loop would spin between device frames, re-running the FK and publishing
        # manager_state at kHz rates while competing with the reader thread.
        self._sleep(IDLE_POLL_S)
        return
    self._last_seen_stamp_ns = sample.timestamp_ns

    frame = process_body_poses(sample.body_poses, self._fk)
    smpl_pose = frame.body_pose[:63].reshape(21, 3).astype(np.float32)
    smpl_joints = frame.smpl_joints_local.astype(np.float32)
    body_quat = frame.global_orient_quat.astype(np.float32)

    toggle_dc_now = controllers.a and controllers.left_grip > 0.5
    toggle_da_now = controllers.b and controllers.left_grip > 0.5
    toggle_dc = toggle_dc_now and not self._toggle_dc_last
    toggle_da = toggle_da_now and not self._toggle_da_last
    self._toggle_dc_last = toggle_dc_now
    self._toggle_da_last = toggle_da_now

    left_hand, right_hand = hand_targets_from_triggers(controllers.left_trigger, controllers.right_trigger)

    # --- resample onto the 50 Hz virtual clock ---
    curr_stamp_ns = sample.timestamp_ns
    step_ns = int(1e9 / max(1, self.target_fps))
    if self.prev_stamp_ns is None:
        self.prev_stamp_ns = curr_stamp_ns
        self.prev_smpl_pose = smpl_pose
        self.prev_smpl_joints = smpl_joints
        self.prev_body_quat = body_quat
        self.next_target_ns = curr_stamp_ns
        return
    if curr_stamp_ns <= self.prev_stamp_ns:
        return  # duplicate device frame
    if self.next_target_ns is None:
        self.next_target_ns = self.prev_stamp_ns + step_ns
    self.next_target_ns = max(self.next_target_ns, self.prev_stamp_ns)
    if self.next_target_ns > curr_stamp_ns:
        return  # virtual tick not yet bracketed
    denom = float(curr_stamp_ns - self.prev_stamp_ns)
    alpha = float(np.clip((self.next_target_ns - self.prev_stamp_ns) / denom if denom > 0 else 1.0, 0.0, 1.0))

    use_joints = (1.0 - alpha) * self.prev_smpl_joints + alpha * smpl_joints
    # Per-joint NLERP of the (21, 3) axis-angle pose, via quaternions.
    prev_q = sRot.from_rotvec(self.prev_smpl_pose.reshape(-1, 3)).as_quat()
    curr_q = sRot.from_rotvec(smpl_pose.reshape(-1, 3)).as_quat()
    use_pose = (
        sRot.from_quat(quat_nlerp(prev_q, curr_q, alpha)).as_rotvec().reshape(smpl_pose.shape).astype(np.float32)
    )
    use_body_quat = quat_nlerp(self.prev_body_quat, body_quat, alpha).astype(np.float32)

    joint_pos = wrist_joints_from_body_pose(use_pose)

    # Calibrated VR 3-point pose of the current (raw) frame.
    vr_3pt_pose = self._three_point.process(sample.body_poses)
    if self._on_frame is not None:
        self._on_frame(vr_3pt_pose, frame.smpl_joints_local)

    self.frame_buffer["smpl_pose"].append(use_pose)
    self.frame_buffer["smpl_joints"].append(use_joints)
    self.frame_buffer["body_quat_w"].append(use_body_quat)
    self.frame_buffer["frame_index"].append(int(self.step))
    self.frame_buffer["joint_pos"].append(joint_pos)

    buffer_is_full = len(self.frame_buffer["frame_index"]) >= self.num_frames_to_send
    if buffer_is_full and self.buffer_cleared:
        self.buffer_cleared = False

    self.yaw_accumulator.update(controllers.right_axis[0], self.frame_time)

    if buffer_is_full and not self.buffer_cleared:
        n = len(self.frame_buffer["frame_index"])
        message_data = {
            "smpl_pose": np.stack(self.frame_buffer["smpl_pose"]),
            "smpl_joints": np.stack(self.frame_buffer["smpl_joints"]),
            "body_quat_w": np.stack(self.frame_buffer["body_quat_w"]),
            "joint_pos": np.stack(self.frame_buffer["joint_pos"]),
            "joint_vel": np.zeros((n, 29)),
            "vr_position": vr_3pt_pose[:, :3].flatten().astype(np.float32),
            "vr_orientation": vr_3pt_pose[:, 3:].flatten().astype(np.float32),
            "frame_index": np.array(self.frame_buffer["frame_index"], dtype=np.int64),
            "left_trigger": np.array([controllers.left_trigger], dtype=np.float32),
            "right_trigger": np.array([controllers.right_trigger], dtype=np.float32),
            "left_grip": np.array([controllers.left_grip], dtype=np.float32),
            "right_grip": np.array([controllers.right_grip], dtype=np.float32),
            "pico_dt": np.array([sample.dt], dtype=np.float32),
            "pico_fps": np.array([sample.fps], dtype=np.float32),
            "timestamp_realtime": np.array([sample.timestamp_realtime], dtype=np.float64),
            "timestamp_monotonic": np.array([sample.timestamp_monotonic], dtype=np.float64),
            "left_hand_joints": left_hand.astype(np.float32),
            "right_hand_joints": right_hand.astype(np.float32),
            "toggle_data_collection": np.array([toggle_dc], dtype=bool),
            "toggle_data_abort": np.array([toggle_da], dtype=bool),
            "heading_increment": np.array([self.yaw_accumulator.dyaw], dtype=np.float32),
        }
        self._send(pack_pose_message(message_data, topic="pose"))
        if self.record_dir:
            import os

            np.savez_compressed(os.path.join(self.record_dir, f"pose_{self._record_idx:06d}.npz"), **message_data)
            self._record_idx += 1

    self.step += 1
    self.next_target_ns += step_ns
    self.prev_stamp_ns = curr_stamp_ns
    self.prev_smpl_pose = smpl_pose
    self.prev_smpl_joints = smpl_joints
    self.prev_body_quat = body_quat

    # Reference parity ([PoseLoop] FPS/Step every 5 s). The resampler emits
    # at most one virtual frame per distinct device sample, so a sustained
    # rate below target means the input path is starving the robot's stream.
    self._emit_count += 1
    now = time.time()
    window = now - self._rate_log_time
    if window >= 5.0:
        rate = self._emit_count / window
        print(f"[PoseLoop] FPS: {rate:.1f} Step: {self.step}")
        if rate < 0.9 * self.target_fps:
            print(
                f"[PoseLoop] WARNING: emitting {rate:.1f} Hz < target {self.target_fps} — "
                "device/bridge rate too low, the robot-side stream will stall"
            )
        self._emit_count = 0
        self._rate_log_time = now

    elapsed = time.time() - self.frame_start
    if elapsed < self.frame_time:
        self._sleep(self.frame_time - elapsed)
    self.frame_start = time.time()

Functions:

Planner-mode streamer: joystick -> locomotion commands (~20 Hz).

Port of the reference PlannerStreamer. The left stick sets the movement direction/speed (deadzone-rescaled, rotated into the world by the accumulated facing), the right stick turns, and A+B / X+Y cycle the locomotion mode. PLANNER_FROZEN_UPPER_BODY attaches the frozen upper-body targets grabbed from the robot's feedback; PLANNER_VR_3PT attaches the calibrated 3-point pose and trigger-driven hand targets.

Attributes

Classes

PlannerStreamer

PlannerStreamer(send: Callable[[bytes], None], three_point: ThreePointPose, feedback: FeedbackReader, poll_hz: int = 20, on_frame: Callable[[ndarray, ndarray | None], None] | None = None)

Bases: TeleopStreamer

One planner-mode streaming iteration per manager tick.

Source code in eden/extensions/teleop/sonic/planner_streamer.py

def __init__(
    self,
    send: Callable[[bytes], None],
    three_point: ThreePointPose,
    feedback: FeedbackReader,
    poll_hz: int = 20,
    on_frame: Callable[[np.ndarray, np.ndarray | None], None] | None = None,
) -> None:
    self._send = send
    self._three_point = three_point
    self.feedback = feedback
    self._on_frame = on_frame
    self.dt = 1.0 / max(1, poll_hz)
    self.mode = LocomotionMode.IDLE
    self.yaw_accumulator = YawAccumulator()
    self._prev_ab = False
    self._prev_xy = False
    self._last_send = time.time()
    self._last_timestamp_ns: int | None = None
    self._sleep = time.sleep
    # The mode whose entry needed robot feedback it did not get; retried every tick.
    self._feedback_pending: StreamMode | None = None

Attributes

feedback instance-attribute

feedback = feedback

dt instance-attribute

dt = 1.0 / max(1, poll_hz)

mode instance-attribute

mode = LocomotionMode.IDLE

yaw_accumulator instance-attribute

yaw_accumulator = YawAccumulator()

Methods:

reset_yaw

reset_yaw() -> None

Source code in eden/extensions/teleop/sonic/planner_streamer.py

def reset_yaw(self) -> None:
    self.yaw_accumulator.reset()

send_idle

send_idle() -> None

Send one IDLE planner message, holding the current facing, with no optional fields.

No upper-body, hand or VR 3-point fields: the robot reads their absence as "not operator-driven", so this drops VR 3-point control (back to the g1 encoder) and the frozen upper body. Used when the operator hands the robot back to the planner.

Source code in eden/extensions/teleop/sonic/planner_streamer.py

def send_idle(self) -> None:
    """Send one IDLE planner message, holding the current facing, with no optional fields.

    No upper-body, hand or VR 3-point fields: the robot reads their absence as "not
    operator-driven", so this drops VR 3-point control (back to the g1 encoder) and the
    frozen upper body. Used when the operator hands the robot back to the planner.
    """
    self._send(
        build_planner_message(
            mode=int(LocomotionMode.IDLE),
            movement=[0.0, 0.0, 0.0],
            facing=np.asarray(self.yaw_accumulator.heading),
            speed=-1.0,
            height=-1.0,
        )
    )

save_upper_body_position_target

save_upper_body_position_target() -> None

Grab the robot's measured upper body as the frozen targets (retried until it arrives).

Source code in eden/extensions/teleop/sonic/planner_streamer.py

def save_upper_body_position_target(self) -> None:
    """Grab the robot's measured upper body as the frozen targets (retried until it arrives)."""
    self._feedback_pending = None
    if not self.feedback.poll_feedback():
        self._feedback_pending = StreamMode.PLANNER_FROZEN_UPPER_BODY
        print(
            f"[PlannerStreamer] WARNING: no {_FEEDBACK} to freeze the upper body at — it stays on "
            f"the planner until that arrives; {_FEEDBACK_HINT}"
        )

recalibrate_for_vr3pt

recalibrate_for_vr3pt() -> None

Schedule wrist recalibration against the robot's measured joints (retried until they arrive).

Until then no 3-point targets are sent, so the robot keeps the g1 encoder: calibrating against an all-zero configuration instead (the arms are not at zero) would produce silently wrong wrist offsets and a wrist jump.

Source code in eden/extensions/teleop/sonic/planner_streamer.py

def recalibrate_for_vr3pt(self) -> None:
    """Schedule wrist recalibration against the robot's measured joints (retried until they arrive).

    Until then no 3-point targets are sent, so the robot keeps the g1 encoder: calibrating
    against an all-zero configuration instead (the arms are not at zero) would produce silently
    wrong wrist offsets and a wrist jump.
    """
    self._feedback_pending = None
    if not self._try_recalibrate_for_vr3pt(quiet=False):
        self._feedback_pending = StreamMode.PLANNER_VR_3PT
        print(
            f"[PlannerStreamer] WARNING: no {_FEEDBACK} for the VR 3PT wrist calibration — 3-point "
            f"targets are held back until it arrives; {_FEEDBACK_HINT}"
        )

run_once

run_once(stream_mode: StreamMode, sample: BodySample | None, controllers: ControllerSample, timestamp_ns: int) -> None

Send one planner command (skipped while the headset stream is stalled).

Source code in eden/extensions/teleop/sonic/planner_streamer.py

def run_once(
    self, stream_mode: StreamMode, sample: BodySample | None, controllers: ControllerSample, timestamp_ns: int
) -> None:
    """Send one planner command (skipped while the headset stream is stalled)."""
    if timestamp_ns == self._last_timestamp_ns:
        self._sleep(IDLE_POLL_S)  # stalled headset stream: poll, don't spin the manager loop
        return
    self._last_timestamp_ns = timestamp_ns
    self._retry_pending_feedback(stream_mode)

    # A+B -> next mode, X+Y -> previous (rising edges), clamped to the
    # modes the reference exposes (IDLE .. INJURED_WALK).
    ab_now = controllers.a and controllers.b
    xy_now = controllers.x and controllers.y
    if ab_now and not self._prev_ab:
        self.mode = LocomotionMode(min(int(LocomotionMode.INJURED_WALK), int(self.mode) + 1))
        print(f"[PlannerStreamer] mode -> {int(self.mode)}: {self.mode.name}")
    if xy_now and not self._prev_xy:
        self.mode = LocomotionMode(max(int(LocomotionMode.IDLE), int(self.mode) - 1))
        print(f"[PlannerStreamer] mode -> {int(self.mode)}: {self.mode.name}")
    self._prev_ab = ab_now
    self._prev_xy = xy_now

    lx, ly = controllers.left_axis
    rx, _ = controllers.right_axis
    facing = self.yaw_accumulator.update(rx, self.dt)

    raw_mag = float(np.clip(np.hypot(lx, ly), 0.0, 1.0))
    if raw_mag < JOYSTICK_DEADZONE:
        mag, speed = 0.0, -1.0
        mode_to_send = LocomotionMode.IDLE
    else:
        mag = min((raw_mag - JOYSTICK_DEADZONE) / (1.0 - JOYSTICK_DEADZONE), 1.0)
        mode_to_send = self.mode
        if self.mode == LocomotionMode.SLOW_WALK:
            speed = 0.1 + 0.5 * mag  # 0.1 .. 0.6 m/s
        elif self.mode == LocomotionMode.WALK:
            speed = -1.0  # mode default
        elif self.mode == LocomotionMode.RUN:
            speed = 1.5 + 3.0 * mag  # 1.5 .. 4.5 m/s
        else:
            speed = mag

    scale = mag / (raw_mag if raw_mag > 0.0 else 1.0)
    movement_local = np.array([-lx, ly]) * scale
    rotation_facing = np.array([[-facing[1], facing[0]], [facing[0], facing[1]]])
    movement_global = rotation_facing @ movement_local
    movement = [float(movement_global[0]), float(movement_global[1]), 0.0]

    upper_body_position = None
    left_hand = None
    right_hand = None
    if stream_mode == StreamMode.PLANNER_FROZEN_UPPER_BODY:
        upper_body_position = self.feedback.upper_body_position_target
        left_hand = self.feedback.left_hand_position_target
        right_hand = self.feedback.right_hand_position_target

    vr_position = None
    vr_orientation = None
    if stream_mode == StreamMode.PLANNER_VR_3PT:
        if sample is not None and self._feedback_pending != StreamMode.PLANNER_VR_3PT:
            vr_pose = self._three_point.process(sample.body_poses)
            vr_position = vr_pose[:, :3].flatten()
            vr_orientation = vr_pose[:, 3:].flatten()
            if self._on_frame is not None:
                self._on_frame(vr_pose, None)
        left_hand, right_hand = hand_targets_from_triggers(controllers.left_trigger, controllers.right_trigger)

    self._send(
        build_planner_message(
            mode=int(mode_to_send),
            movement=movement,
            facing=np.asarray(facing),
            speed=float(speed),
            height=-1.0,
            upper_body_position=upper_body_position,
            left_hand_position=left_hand,
            right_hand_position=right_hand,
            vr_3pt_position=vr_position,
            vr_3pt_orientation=vr_orientation,
        )
    )

    # ~20 Hz pacing.
    now = time.time()
    sleep_t = self.dt - (now - self._last_send)
    if sleep_t > 0:
        self._sleep(sleep_t)
    self._last_send = time.time()

Functions:

VR 3-point pose extraction and calibration (VR_3PT mode).

Port of the reference streamer's 3-point path. From the raw (24, 7) service body frame it takes root (0), left hand (22), right hand (23) and neck (12 — deliberately not the head, which moves too freely), converts them to the robot frame, applies per-joint alignment offsets, and expresses the three keypoints relative to the root. :class:ThreePointPose then calibrates them against G1 forward kinematics so the streamed targets live in the same pelvis-local frame the policy's teleop encoder was trained on:

• full calibration (every engage): operator stands in the neutral pose == the G1's all-zero joint configuration; the neck orientation and both wrist offsets are captured against FK of q = 0.
• wrist recalibration (every VR_3PT entry): only the wrist offsets are re-captured, against FK of the robot's measured joints; the neck calibration is preserved to avoid jumps from tracking noise.

The neck position is never measured: it is reconstructed from the calibrated neck orientation through a fixed two-link chain.

Attributes

Q_ROBOT_SERVICE module-attribute

Q_ROBOT_SERVICE = np.array([[-1.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, 1.0, 0.0]])

TORSO_LINK_OFFSET_Z module-attribute

TORSO_LINK_OFFSET_Z = 0.05

NECK_LINK_LENGTH module-attribute

NECK_LINK_LENGTH = 0.35

NECK_TILT_WARN_DEG module-attribute

NECK_TILT_WARN_DEG = 20.0

Classes

ThreePointPose

ThreePointPose(fk)

Calibrated 3-point targets from raw PICO body frames (in-memory state only).

fk provides key_frame_poses(q_mj29) -> {left_wrist, right_wrist, torso} (see :class:~eden.extensions.teleop.sonic.g1_fk.G1KeyFrameFk).

Source code in eden/extensions/teleop/sonic/three_point.py

def __init__(self, fk) -> None:
    """``fk`` provides ``key_frame_poses(q_mj29) -> {left_wrist, right_wrist, torso}``
    (see :class:`~eden.extensions.teleop.sonic.g1_fk.G1KeyFrameFk`).
    """
    self._fk = fk
    self._calibration_pending = False
    self._neck_quat_inv: np.ndarray | None = None  # wxyz
    self._lwrist_offset: np.ndarray | None = None
    self._rwrist_offset: np.ndarray | None = None
    self._lwrist_rot_offset: sRot | None = None
    self._rwrist_rot_offset: sRot | None = None
    self._override_robot_q: np.ndarray | None = None
    #: (left, right) degrees between the operator's and the robot's wrist at the last calibration.
    self.wrist_rot_offset_deg: tuple[float, float] | None = None

Attributes

wrist_rot_offset_deg instance-attribute

wrist_rot_offset_deg: tuple[float, float] | None = None

is_calibrated property

is_calibrated: bool

Methods:

process

process(body_poses: ndarray) -> ndarray

Extract + calibrate the 3-point pose for one raw body frame.

Source code in eden/extensions/teleop/sonic/three_point.py

def process(self, body_poses: np.ndarray) -> np.ndarray:
    """Extract + calibrate the 3-point pose for one raw body frame."""
    raw = process_3pt_pose(body_poses)
    if self._calibration_pending:
        self._capture_calibration(raw)
    return self._apply_calibration(raw)

calibrate_now

calibrate_now(body_poses: ndarray) -> None

Full calibration against FK of the all-zero joint configuration.

The operator's neutral pose (upright, feet together, forearms bent 90° forward) is the G1's zero configuration.

Source code in eden/extensions/teleop/sonic/three_point.py

def calibrate_now(self, body_poses: np.ndarray) -> None:
    """Full calibration against FK of the all-zero joint configuration.

    The operator's neutral pose (upright, feet together, forearms bent 90°
    forward) *is* the G1's zero configuration.
    """
    raw = process_3pt_pose(body_poses)
    # Full means the neck too: it is otherwise captured once and kept across
    # wrist recalibrations, so a re-engage after a fall auto-reset (operator
    # re-donned the headset, or re-ran body calibration) would reuse the first one.
    self._neck_quat_inv = None
    self._override_robot_q = np.zeros(29)
    self._capture_calibration(raw)
    print("[ThreePointPose] calibration completed (zero-pose reference)")

reset_with_measured_q

reset_with_measured_q(body_q_measured: ndarray) -> None

Queue a wrist-only recalibration against FK of the measured joints.

Source code in eden/extensions/teleop/sonic/three_point.py

def reset_with_measured_q(self, body_q_measured: np.ndarray) -> None:
    """Queue a wrist-only recalibration against FK of the measured joints."""
    self._lwrist_offset = self._rwrist_offset = None
    self._lwrist_rot_offset = self._rwrist_rot_offset = None
    self._override_robot_q = np.asarray(body_q_measured, dtype=np.float64).copy()
    self._calibration_pending = True
    print("[ThreePointPose] wrist recalibration pending (neck preserved, measured q)")

Functions:

process_3pt_pose

process_3pt_pose(body_poses: ndarray) -> ndarray

Raw (24, 7) service frame -> (3, 7) root-relative keypoints.

Rows are [left wrist, right wrist, neck], each [x, y, z, qw, qx, qy, qz].

Source code in eden/extensions/teleop/sonic/three_point.py

def process_3pt_pose(body_poses: np.ndarray) -> np.ndarray:
    """Raw ``(24, 7)`` service frame -> ``(3, 7)`` root-relative keypoints.

    Rows are [left wrist, right wrist, neck], each ``[x, y, z, qw, qx, qy, qz]``.
    """
    body_poses = np.asarray(body_poses, dtype=np.float64)
    kp = np.zeros((4, 7))
    for rel_i, joint in enumerate(_KEYPOINT_JOINTS):
        pos, quat = _service_pose_to_robot(body_poses[joint])
        rot = sRot.from_quat(quat, scalar_first=True) * _OFFSETS[rel_i]
        kp[rel_i, :3] = pos
        kp[rel_i, 3:] = rot.as_quat(scalar_first=True)

    root_pos = kp[0, :3].copy()
    root_rot = sRot.from_quat(kp[0, 3:], scalar_first=True)
    for i in range(1, 4):
        kp[i, :3] = root_rot.inv().apply(kp[i, :3] - root_pos)
        kp[i, 3:] = (root_rot.inv() * sRot.from_quat(kp[i, 3:], scalar_first=True)).as_quat(scalar_first=True)
    return kp[1:]

PICO full-body tracking -> SMPL parameters and root-local joints (POSE mode).

Port of the reference streamer's SMPL path:

1. The service's 24 global joint rotations (scalar-last, service frame) become parent-local axis-angle after a per-joint Ry(180°) frame fix.
2. The root orientation is re-based: pre-multiply Rx(+90°) (service Y-up -> robot Z-up), then post-multiply away SMPL's rest orientation (the 120° rotation about (1,1,1)).
3. Joint positions come from an SMPL-X forward kinematics over the rest skeleton in sonic/smplx_joints_info.npz (fetched from the Kashu7100/eden_assets HF dataset; 55 joints; output = the 22 body joints
4. the two thumb tips, SMPL-X indices 39 and 54 — NOT the SMPL left_hand/right_hand joints), expressed in the root-local frame.

No body-shape (betas) or operator-height scaling exists anywhere in this pipeline — the reference relies on calibration offsets and the policy itself.

Attributes

SMPL_PARENTS module-attribute

SMPL_PARENTS: tuple[int, ...] = (-1, 0, 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 9, 9, 12, 13, 14, 16, 17, 18, 19, 20, 21)

Classes

SmplFk

SmplFk(info_path: str | None = None)

SMPL-X forward kinematics over the HF-hosted rest skeleton (positions only).

Source code in eden/extensions/teleop/sonic/smpl.py

def __init__(self, info_path: str | None = None) -> None:
    info = np.load(info_path or get_asset_path(_SMPLX_INFO_ASSET))
    self._rest_joints = np.asarray(info["J"], dtype=np.float64)  # (55, 3)
    self._parents = np.asarray(info["parents"], dtype=np.int64)  # (55,)
    if self._rest_joints.shape != (55, 3) or self._parents.shape != (55,):
        raise ValueError(f"unexpected SMPL-X skeleton shapes in {info_path}")

Methods:

joints

joints(global_orient: ndarray, body_pose: ndarray) -> ndarray

Posed joint positions (24, 3) for a root axis-angle + 63-dim body pose.

The full 55-joint pose is [global_orient, body_pose, zeros(99)]; the output gathers the 22 body joints plus the two thumb tips.

Source code in eden/extensions/teleop/sonic/smpl.py

def joints(self, global_orient: np.ndarray, body_pose: np.ndarray) -> np.ndarray:
    """Posed joint positions ``(24, 3)`` for a root axis-angle + 63-dim body pose.

    The full 55-joint pose is ``[global_orient, body_pose, zeros(99)]``;
    the output gathers the 22 body joints plus the two thumb tips.
    """
    body_pose = np.asarray(body_pose, dtype=np.float64).reshape(-1)[:63]
    full_pose = np.concatenate([np.asarray(global_orient, dtype=np.float64).reshape(3), body_pose, np.zeros(99)])
    rot_mats = sRot.from_rotvec(full_pose.reshape(55, 3)).as_matrix()  # (55, 3, 3)

    rel_joints = self._rest_joints.copy()
    rel_joints[1:] -= self._rest_joints[self._parents[1:]]

    transforms = np.zeros((55, 4, 4))
    transforms[:, :3, :3] = rot_mats
    transforms[:, :3, 3] = rel_joints
    transforms[:, 3, 3] = 1.0

    chain = [transforms[0]]
    for i in range(1, 55):
        chain.append(chain[self._parents[i]] @ transforms[i])
    positions = np.stack(chain)[:, :3, 3]
    return positions[_OUTPUT_JOINTS]

SmplFrame dataclass

SmplFrame(body_pose: ndarray, global_orient_quat: ndarray, smpl_joints_local: ndarray)

One processed SMPL frame for the pose stream.

Attributes

body_pose instance-attribute

body_pose: ndarray

global_orient_quat instance-attribute

global_orient_quat: ndarray

smpl_joints_local instance-attribute

smpl_joints_local: ndarray

Methods:

Functions:

smpl_root_ytoz_up

smpl_root_ytoz_up(quat_wxyz: ndarray) -> ndarray

Rebase the SMPL root quaternion from Y-up to Z-up (pre-multiply Rx(+90°)).

Source code in eden/extensions/teleop/sonic/smpl.py

def smpl_root_ytoz_up(quat_wxyz: np.ndarray) -> np.ndarray:
    """Rebase the SMPL root quaternion from Y-up to Z-up (pre-multiply Rx(+90°))."""
    return quat_mul(_RX_90, np.asarray(quat_wxyz, dtype=np.float64))

remove_smpl_base_rot

remove_smpl_base_rot(quat_wxyz: ndarray) -> ndarray

Remove SMPL's rest orientation (post-multiply by conj([.5, .5, .5, .5])).

Source code in eden/extensions/teleop/sonic/smpl.py

def remove_smpl_base_rot(quat_wxyz: np.ndarray) -> np.ndarray:
    """Remove SMPL's rest orientation (post-multiply by conj([.5, .5, .5, .5]))."""
    return quat_mul(np.asarray(quat_wxyz, dtype=np.float64), _SMPL_BASE_ROT_CONJ)

global_quats_to_local_aa

global_quats_to_local_aa(body_poses: ndarray) -> ndarray

Service-frame global joint rotations -> parent-local axis-angle (24, 3).

body_poses is the raw (24, 7) service array (scalar-last quats). Every joint is post-multiplied by Ry(180°) first (the service's joint frames are flipped relative to SMPL's).

Source code in eden/extensions/teleop/sonic/smpl.py

def global_quats_to_local_aa(body_poses: np.ndarray) -> np.ndarray:
    """Service-frame global joint rotations -> parent-local axis-angle ``(24, 3)``.

    ``body_poses`` is the raw ``(24, 7)`` service array (scalar-last quats).
    Every joint is post-multiplied by ``Ry(180°)`` first (the service's joint
    frames are flipped relative to SMPL's).
    """
    global_quats = np.asarray(body_poses, dtype=np.float64)[:, [6, 3, 4, 5]]
    global_rots = sRot.from_quat(global_quats, scalar_first=True) * sRot.from_euler("y", 180, degrees=True)
    local = []
    for i in range(24):
        parent = SMPL_PARENTS[i]
        rot = global_rots[i] if parent == -1 else global_rots[parent].inv() * global_rots[i]
        local.append(rot.as_rotvec())
    return np.asarray(local)

process_body_poses

process_body_poses(body_poses: ndarray, fk: SmplFk) -> SmplFrame

Full POSE-mode conversion of one raw (24, 7) service frame.

Source code in eden/extensions/teleop/sonic/smpl.py

def process_body_poses(body_poses: np.ndarray, fk: SmplFk) -> SmplFrame:
    """Full POSE-mode conversion of one raw ``(24, 7)`` service frame."""
    pose_aa = global_quats_to_local_aa(body_poses)
    body_pose = pose_aa[1:].reshape(-1)  # (69,)

    root_quat = sRot.from_rotvec(pose_aa[0]).as_quat(scalar_first=True)
    root_quat = smpl_root_ytoz_up(root_quat)
    global_orient_new = sRot.from_quat(root_quat, scalar_first=True).as_rotvec()

    joints = fk.joints(global_orient_new, body_pose[:63])

    root_quat = remove_smpl_base_rot(root_quat)
    joints_local = sRot.from_quat(root_quat, scalar_first=True).inv().apply(joints)
    return SmplFrame(body_pose=body_pose, global_orient_quat=root_quat, smpl_joints_local=joints_local)

SMPL elbow/wrist rotations -> G1 wrist joint targets (POSE mode).

The G1 elbow has a single pitch DOF, so the SMPL elbow's roll/yaw content is decomposed off (twist about the elbow axis vs. swing) and folded into the wrist joints, preserving the SMPL wrist's own rotation. Fills the six wrist entries of the 29-dim IsaacLab joint vector; everything else stays zero on the pose stream.

Attributes

SMPL_L_ELBOW module-attribute

SMPL_L_ELBOW = 17

SMPL_R_ELBOW module-attribute

SMPL_R_ELBOW = 18

SMPL_L_WRIST module-attribute

SMPL_L_WRIST = 19

SMPL_R_WRIST module-attribute

SMPL_R_WRIST = 20

Functions:

decompose_rotation_aa

decompose_rotation_aa(rotation_aa: ndarray, axis: ndarray) -> tuple[ndarray, ndarray]

Split axis-angle rotations into twist about axis and the residual swing.

Returns wxyz quaternions (q_twist, q_swing) with q = q_twist ⊗ q_swing (:func:eden.utils.geom.quat_swing_twist on the rotations' quaternions). A zero rotation decomposes into identities (the reference divides by the zero angle there).

Source code in eden/extensions/teleop/sonic/wrist.py

def decompose_rotation_aa(rotation_aa: np.ndarray, axis: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
    """Split axis-angle rotations into twist about ``axis`` and the residual swing.

    Returns wxyz quaternions ``(q_twist, q_swing)`` with ``q = q_twist ⊗ q_swing``
    (:func:`eden.utils.geom.quat_swing_twist` on the rotations' quaternions). A zero
    rotation decomposes into identities (the reference divides by the zero angle there).
    """
    return quat_swing_twist(gu.rotvec_to_quat(np.atleast_2d(np.asarray(rotation_aa, dtype=np.float64))), axis)

wrist_joints_from_body_pose

wrist_joints_from_body_pose(body_pose: ndarray) -> ndarray

The 29-dim IsaacLab joint vector for a pose-stream frame (wrists only).

body_pose is the frame's (21, 3) (or flat 63-dim) local axis-angle pose. All Euler readings are intrinsic XYZ; the per-side signs follow the reference mapping verbatim.

Source code in eden/extensions/teleop/sonic/wrist.py

def wrist_joints_from_body_pose(body_pose: np.ndarray) -> np.ndarray:
    """The 29-dim IsaacLab joint vector for a pose-stream frame (wrists only).

    ``body_pose`` is the frame's ``(21, 3)`` (or flat 63-dim) local axis-angle
    pose. All Euler readings are intrinsic XYZ; the per-side signs follow the
    reference mapping verbatim.
    """
    pose = np.asarray(body_pose, dtype=np.float64).reshape(21, 3)
    joint_pos = np.zeros(29)

    _, l_swing = decompose_rotation_aa(pose[SMPL_L_ELBOW][None], _ELBOW_AXIS)
    _, r_swing = decompose_rotation_aa(pose[SMPL_R_ELBOW][None], _ELBOW_AXIS)
    l_swing_euler = sRot.from_quat(l_swing[:, [1, 2, 3, 0]]).as_euler("XYZ")[0]
    r_swing_euler = sRot.from_quat(r_swing[:, [1, 2, 3, 0]]).as_euler("XYZ")[0]
    l_wrist_euler = sRot.from_rotvec(pose[SMPL_L_WRIST]).as_euler("XYZ")
    r_wrist_euler = sRot.from_rotvec(pose[SMPL_R_WRIST]).as_euler("XYZ")

    l_roll = l_swing_euler[0] + l_wrist_euler[0]
    l_pitch = -l_wrist_euler[1]
    l_yaw = l_swing_euler[2] + l_wrist_euler[2]
    r_roll = -(r_swing_euler[0] + r_wrist_euler[0])
    r_pitch = -r_wrist_euler[1]
    r_yaw = r_swing_euler[2] + r_wrist_euler[2]

    joint_pos[G1_L_WRIST_ROLL] = l_roll
    joint_pos[G1_L_WRIST_PITCH] = -l_pitch  # double negation kept verbatim from the reference
    joint_pos[G1_L_WRIST_YAW] = l_yaw
    joint_pos[G1_R_WRIST_ROLL] = r_roll
    joint_pos[G1_R_WRIST_PITCH] = r_pitch
    joint_pos[G1_R_WRIST_YAW] = r_yaw
    return joint_pos

Trigger -> Dex3 hand joint targets (the reference's gripper gesture solver).

There is no Cartesian hand IK: the controller trigger fabricates synthetic fingertip positions (thumb tip fixed, the middle tip snapped onto it when the trigger passes 0.5), and the solver turns thumb-to-finger distances into a gesture choice plus a linear interpolation from the open pose to a preset closed pose. With trigger input only, the result is a binary open/close of the middle-finger power grasp.

Classes

G1GripperGestureSolver

G1GripperGestureSolver(side: str)

Distance-based gesture selection + lerp to a preset Dex3 closed pose.

Source code in eden/extensions/teleop/sonic/hands.py

def __init__(self, side: str) -> None:
    self.side = "L" if side.lower() == "left" else "R"

Attributes

side instance-attribute

side = 'L' if side.lower() == 'left' else 'R'

Methods:

Functions:

generate_finger_data

generate_finger_data(trigger: float, grip: float) -> ndarray

Synthesize (25, 4, 4) fingertip transforms from controller inputs.

The thumb tip sits at x=1; the middle tip joins it when the trigger passes 0.5 (closing the middle-gesture distance to zero). grip is accepted for interface parity but unused, exactly like the reference.

Source code in eden/extensions/teleop/sonic/hands.py

def generate_finger_data(trigger: float, grip: float) -> np.ndarray:
    """Synthesize ``(25, 4, 4)`` fingertip transforms from controller inputs.

    The thumb tip sits at x=1; the middle tip joins it when the trigger passes
    0.5 (closing the middle-gesture distance to zero). ``grip`` is accepted
    for interface parity but unused, exactly like the reference.
    """
    del grip
    fingertips = np.zeros((25, 4, 4))
    fingertips[_TIP["thumb"], 0, 3] = 1.0
    if trigger > 0.5:
        fingertips[_TIP["middle"], 0, 3] = 1.0
    return fingertips

hand_targets_from_triggers

hand_targets_from_triggers(left_trigger: float, right_trigger: float) -> tuple[ndarray, ndarray]

Convenience: both hands' Dex3 targets straight from the trigger values.

Source code in eden/extensions/teleop/sonic/hands.py

def hand_targets_from_triggers(left_trigger: float, right_trigger: float) -> tuple[np.ndarray, np.ndarray]:
    """Convenience: both hands' Dex3 targets straight from the trigger values."""
    left = G1GripperGestureSolver("left")(generate_finger_data(left_trigger, 0.0))
    right = G1GripperGestureSolver("right")(generate_finger_data(right_trigger, 0.0))
    return left, right

Robot feedback for the operator station (g1_debug channel, polled on demand).

The operator polls the robot's state only at mode transitions: entering PLANNER_FROZEN_UPPER_BODY grabs the measured upper-body joints as the frozen targets, and entering PLANNER_VR_3PT grabs the full measured configuration for wrist recalibration. :class:RobotResetWatcher is a separate per-tick subscriber for the robot's fall auto-reset counter — separate because :meth:FeedbackReader.poll_feedback drains the conflated socket, and the transition effects rely on finding a message there.

Attributes

UPPER_BODY_MJ_GATHER module-attribute

UPPER_BODY_MJ_GATHER = (12, 13, 14, 15, 22, 16, 23, 17, 24, 18, 25, 19, 26, 20, 27, 21, 28)

Classes

FeedbackReader

FeedbackReader(host: str = 'localhost', port: int = DEFAULT_DEBUG_PORT)

Conflate-latest reader of the robot's g1_debug state.

Source code in eden/extensions/teleop/sonic/feedback.py

def __init__(self, host: str = "localhost", port: int = DEFAULT_DEBUG_PORT) -> None:
    self._poller = G1DebugPoller(host=host, port=port)
    self.upper_body_position_target: np.ndarray | None = None
    self.left_hand_position_target: np.ndarray | None = None
    self.right_hand_position_target: np.ndarray | None = None
    self.full_body_q_measured: np.ndarray | None = None

Attributes

upper_body_position_target instance-attribute

upper_body_position_target: ndarray | None = None

left_hand_position_target instance-attribute

left_hand_position_target: ndarray | None = None

right_hand_position_target instance-attribute

right_hand_position_target: ndarray | None = None

full_body_q_measured instance-attribute

full_body_q_measured: ndarray | None = None

Methods:

poll_feedback

poll_feedback(quiet: bool = False) -> bool

Read the newest robot state (if any) into the target fields.

Returns whether the measured body configuration arrived. quiet skips the miss message, for callers that retry every tick and report once themselves.

Source code in eden/extensions/teleop/sonic/feedback.py

def poll_feedback(self, quiet: bool = False) -> bool:
    """Read the newest robot state (if any) into the target fields.

    Returns whether the measured body configuration arrived. ``quiet`` skips the
    miss message, for callers that retry every tick and report once themselves.
    """
    data = self._poller.get_latest()
    if data is None:
        if not quiet:
            print("[FeedbackReader] no feedback data received")
        self.upper_body_position_target = None
        self.left_hand_position_target = None
        self.right_hand_position_target = None
        self.full_body_q_measured = None
        return False

    body_q = data.get("body_q_measured")
    if body_q is not None:
        full = np.asarray(body_q, dtype=np.float64)
        self.full_body_q_measured = full
        self.upper_body_position_target = full[list(UPPER_BODY_MJ_GATHER)]
    else:
        if not quiet:
            print("[FeedbackReader] body_q_measured not in feedback data")
        self.full_body_q_measured = None
        self.upper_body_position_target = None

    left = data.get("left_hand_q_measured")
    right = data.get("right_hand_q_measured")
    self.left_hand_position_target = None if left is None else np.asarray(left, dtype=np.float64)
    self.right_hand_position_target = None if right is None else np.asarray(right, dtype=np.float64)
    return self.full_body_q_measured is not None

close

close() -> None

Source code in eden/extensions/teleop/sonic/feedback.py

def close(self) -> None:
    self._poller.close()

RobotResetWatcher

RobotResetWatcher(host: str = 'localhost', port: int = DEFAULT_DEBUG_PORT)

Watches g1_debug's reset_count for robot-side fall auto-resets.

Owns its own conflate subscriber (PUB fans out to every subscriber). The counter is monotonic, so conflation can never hide an increment; the first observed value is the baseline, which also covers an operator (re)started after the robot already reset.

Source code in eden/extensions/teleop/sonic/feedback.py

def __init__(self, host: str = "localhost", port: int = DEFAULT_DEBUG_PORT) -> None:
    self._poller = G1DebugPoller(host=host, port=port)
    self._last_count: int | None = None

Methods:

poll

poll() -> bool

True exactly when a previously-seen reset_count has increased.

Source code in eden/extensions/teleop/sonic/feedback.py

def poll(self) -> bool:
    """True exactly when a previously-seen ``reset_count`` has increased."""
    data = self._poller.get_latest()
    count = None if data is None else data.get("reset_count")
    if count is None:
        return False
    count = int(count)
    increased = self._last_count is not None and count > self._last_count
    self._last_count = count
    return increased

close

close() -> None

Source code in eden/extensions/teleop/sonic/feedback.py

def close(self) -> None:
    self._poller.close()

G1 key-frame forward kinematics for 3-point calibration (Genesis-backed).

Computes the poses of the two wrist frames and the torso frame — with the training-time local offsets applied — for a given 29-DOF joint configuration, root fixed at the origin with identity orientation. This is the reference frame the teleop encoder's vr_3point_local_* observations were trained against, so the offsets must match gear_sonic/config/manager_env/commands/terms/motion.yaml exactly.

The kinematics run on the same Genesis model the robot process simulates (:class:~eden.options.robots.unitree_g1.G1Dofs29Dex3), via a single-env kinematics-only scene (transient_robot_env) held open for the process lifetime. One subtlety: Genesis re-origins the base link onto its inertial frame, so with the FK root at identity the world origin is the solver base frame, not the MJCF pelvis body frame the reference (Pinocchio) uses. The key frames are therefore expressed relative to the pelvis body frame recovered via get_pos/get_quat(relative=True), which cancels the re-origin exactly — pinned against a MuJoCo reference in tests/extensions/teleop/sonic/test_g1_fk.py.

Attributes

G1_KEY_FRAMES module-attribute

G1_KEY_FRAMES = {'left_wrist': ('left_wrist_yaw_link', np.array([0.18, -0.025, 0.0])), 'right_wrist': ('right_wrist_yaw_link', np.array([0.18, 0.025, 0.0])), 'torso': ('torso_link', np.array([0.0, 0.0, 0.35]))}

Classes

G1KeyFrameFk

G1KeyFrameFk(robot_options: G1Dofs29Dex3 | None = None)

FK for the calibration key frames, root pinned at the origin.

Builds the kinematics scene once (the first construction per process pays the kernel compile, up to ~30 s) and reuses it for every query; requires eden.init beforehand and a close() when done.

Source code in eden/extensions/teleop/sonic/g1_fk.py

def __init__(self, robot_options: G1Dofs29Dex3 | None = None) -> None:
    import torch

    from eden.utils.genesis_runtime import transient_robot_env

    self._torch = torch
    self._stack = ExitStack()
    self._env, self._robot = self._stack.enter_context(transient_robot_env(robot_options or G1Dofs29Dex3()))
    robot = self._robot
    if list(robot.dofs_name[:29]) != list(G1_DOFS29_NAME):
        self._stack.close()
        raise ValueError("the FK robot's dofs_name must start with the 29 body joints in MJCF order")
    # Raises (naming the available links) on a miss — same fail-fast as a
    # bad body name in the reference FK.
    _, self._links_idx = self._robot.find_named_links_idx_local([body for body, _ in G1_KEY_FRAMES.values()])
    self._offsets = torch.tensor(
        np.stack([offset for _, offset in G1_KEY_FRAMES.values()]), dtype=torch.float32, device=self._env.device
    )
    self._n_qs = 7 + len(robot.dofs_name)

Methods:

key_frame_poses

key_frame_poses(q_mj29: ndarray) -> dict[str, dict[str, ndarray]]

Key-frame poses for a 29-joint configuration (MJCF order, radians).

Returns {key: {"position" (3,), "orientation_wxyz" (4,)}} with the local offsets folded into the positions.

Source code in eden/extensions/teleop/sonic/g1_fk.py

def key_frame_poses(self, q_mj29: np.ndarray) -> dict[str, dict[str, np.ndarray]]:
    """Key-frame poses for a 29-joint configuration (MJCF order, radians).

    Returns ``{key: {"position" (3,), "orientation_wxyz" (4,)}}`` with the
    local offsets folded into the positions.
    """
    from genesis.utils.geom import transform_by_quat

    from eden.utils.geom import subtract_frame_transforms

    torch = self._torch
    q_mj29 = np.asarray(q_mj29, dtype=np.float64).reshape(29)
    qpos = torch.zeros((1, self._n_qs), dtype=torch.float32, device=self._env.device)
    qpos[0, 3] = 1.0  # free-base identity quaternion (wxyz), root at the origin
    qpos[0, 7:36] = torch.as_tensor(q_mj29, dtype=torch.float32, device=self._env.device)
    self._robot.set_qpos(qpos)  # hands stay at zero; refreshes link poses

    n = len(self._links_idx)
    # relative=True: the MJCF pelvis body frame (the reference's FK root),
    # not the re-origined solver frame the qpos root addresses.
    root_pos = self._robot.get_pos(relative=True)
    root_quat = self._robot.get_quat(relative=True)
    link_pos = self._robot.get_links_pos(ls_idx_local=self._links_idx)[0]
    link_quat = self._robot.get_links_quat(ls_idx_local=self._links_idx)[0]
    rel_pos, rel_quat = subtract_frame_transforms(
        root_pos.expand(n, -1), root_quat.expand(n, -1), link_pos, link_quat
    )
    rel_pos = rel_pos + transform_by_quat(self._offsets, rel_quat)

    return {
        key: {
            "position": rel_pos[i].detach().cpu().numpy().astype(np.float64),
            "orientation_wxyz": rel_quat[i].detach().cpu().numpy().astype(np.float64),
        }
        for i, key in enumerate(G1_KEY_FRAMES)
    }

close

close() -> None

Destroy the kinematics scene.

Source code in eden/extensions/teleop/sonic/g1_fk.py

def close(self) -> None:
    """Destroy the kinematics scene."""
    self._stack.close()

Operator visualization: calibrated VR 3-point pose, G1 mesh, SMPL body (PyVista).

Port of the reference VR3Pt visualizer's rendered content, in one window:

• the three calibrated keypoints (left wrist light green, right wrist light blue, head orange) as balls with RGB frame triads,
• the synthesized head chain (origin -> torso offset -> neck along its local Z),
• a semi-transparent G1 mesh at the origin for spatial reference (geometry from the same MJCF asset the calibration FK loads); with waist tracking the three waist joints follow the calibrated neck orientation,
• optionally the 24 SMPL joints (lower body blue, upper red) with bones, anchored beside the robot — a pre-engage check that the avatar mirrors the operator.

Requires pyvista (the sonic-viz extra). The window renders inline on the manager thread, exactly like the reference (visualization costs stream latency; run without --vis-vr3pt for the tightest loop).

Attributes

VR_BALL_COLORS module-attribute

VR_BALL_COLORS = ('lightgreen', 'lightblue', 'orange')

SMPL_LOWER_BODY module-attribute

SMPL_LOWER_BODY = frozenset({0, 1, 2, 4, 5, 7, 8, 10, 11})

SMPL_PARENTS_VIS module-attribute

SMPL_PARENTS_VIS = (-1, 0, 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 9, 9, 12, 13, 14, 16, 17, 18, 19, 20, 21)

Classes

Vr3PtPoseVisualizer

Vr3PtPoseVisualizer(with_g1_robot: bool = True, enable_waist_tracking: bool = False, enable_smpl_vis: bool = False, axis_length: float = 0.08, ball_radius: float = 0.015, robot_opacity: float = 0.4, off_screen: bool = False)

One interactive window updated once per processed device frame.

Source code in eden/extensions/teleop/sonic/viz.py

def __init__(
    self,
    with_g1_robot: bool = True,
    enable_waist_tracking: bool = False,
    enable_smpl_vis: bool = False,
    axis_length: float = 0.08,
    ball_radius: float = 0.015,
    robot_opacity: float = 0.4,
    off_screen: bool = False,
) -> None:
    try:
        import pyvista as pv
    except ImportError as exc:
        raise ImportError(f"--vis-vr3pt needs pyvista: {install_hint('sonic-viz')}") from exc
    self._pv = pv
    self._waist_tracking = enable_waist_tracking
    self._smpl_vis = enable_smpl_vis

    self.plotter = pv.Plotter(window_size=(1100, 850), off_screen=off_screen)
    self.plotter.add_axes()
    self.plotter.add_mesh(pv.Plane(i_size=2.0, j_size=2.0), color="lightgray", opacity=0.3)
    for axis, color in zip(np.eye(3), ("red", "green", "blue")):
        self.plotter.add_mesh(pv.Line((0, 0, 0), tuple(0.2 * axis)), color=color, line_width=4)

    # VR keypoints: ball + RGB triad each, positioned via the actor matrix.
    self._kp_actors = []
    for color in VR_BALL_COLORS:
        ball = self.plotter.add_mesh(pv.Sphere(radius=ball_radius), color=color)
        triad = []
        for axis, axis_color in zip(np.eye(3), ("red", "green", "blue")):
            actor = self.plotter.add_mesh(
                pv.Line((0, 0, 0), tuple(axis_length * axis)), color=axis_color, line_width=3
            )
            triad.append(actor)
        self._kp_actors.append((ball, triad))

    # Synthesized head chain: origin -> torso offset -> neck.
    self._chain = pv.PolyData(np.zeros((3, 3)), lines=np.array([2, 0, 1, 2, 1, 2]), force_float=False)
    self.plotter.add_mesh(self._chain, color="orange", line_width=2)

    self._robot = _G1MeshView(self.plotter, robot_opacity) if with_g1_robot else None

    self._smpl_points = None
    if enable_smpl_vis:
        self._smpl_spheres = []
        for i in range(24):
            color = "steelblue" if i in SMPL_LOWER_BODY else "indianred"
            self._smpl_spheres.append(self.plotter.add_mesh(pv.Sphere(radius=0.02), color=color))
        lines = []
        for i in range(1, 24):
            lines.extend([2, SMPL_PARENTS_VIS[i], i])
        self._smpl_bones = pv.PolyData(np.zeros((24, 3)), lines=np.array(lines), force_float=False)
        self.plotter.add_mesh(self._smpl_bones, color="gray", line_width=2)

    self.plotter.show(interactive_update=True, auto_close=False)

Attributes

plotter instance-attribute

plotter = pv.Plotter(window_size=(1100, 850), off_screen=off_screen)

Methods:

on_frame

on_frame(vr_3pt_pose: ndarray, smpl_joints_local: ndarray | None) -> None

Update the scene for one calibrated (3, 7) pose (+ optional SMPL joints).

Source code in eden/extensions/teleop/sonic/viz.py

def on_frame(self, vr_3pt_pose: np.ndarray, smpl_joints_local: np.ndarray | None) -> None:
    """Update the scene for one calibrated ``(3, 7)`` pose (+ optional SMPL joints)."""
    for row, (ball, triad) in enumerate(self._kp_actors):
        mat = _pose_matrix(vr_3pt_pose[row, :3], vr_3pt_pose[row, 3:])
        ball.user_matrix = mat
        for actor in triad:
            actor.user_matrix = mat

    neck_z = sRot.from_quat(vr_3pt_pose[2, 3:], scalar_first=True).apply([0.0, 0.0, 1.0])
    torso = np.array([0.0, 0.0, TORSO_LINK_OFFSET_Z])
    self._chain.points = np.stack([np.zeros(3), torso, torso + NECK_LINK_LENGTH * neck_z])

    if self._robot is not None and self._waist_tracking:
        # Waist follows the calibrated neck orientation. as_euler("ZYX")
        # returns [z, y, x] = [yaw, pitch, roll]; the waist joints are
        # yaw (q12), roll (q13), pitch (q14).
        yaw, pitch, roll = sRot.from_quat(vr_3pt_pose[2, 3:], scalar_first=True).as_euler("ZYX")
        q = np.zeros(29)
        q[12], q[13], q[14] = yaw, roll, pitch
        self._robot.set_pose(q)

    if self._smpl_vis and smpl_joints_local is not None:
        points = np.asarray(smpl_joints_local) + _SMPL_ANCHOR
        for i, sphere in enumerate(self._smpl_spheres):
            mat = np.eye(4)
            mat[:3, 3] = points[i]
            sphere.user_matrix = mat
        self._smpl_bones.points = points

    self.plotter.update()

close

close() -> None

Source code in eden/extensions/teleop/sonic/viz.py

def close(self) -> None:
    self.plotter.close()

Functions:

The robot-side half (the SONIC policy runtime and its ZMQ server) is in eden.extensions.policies.sonic; its policy class is on the policies page.

See also

• Teleoperation guide
• Teleoperation devices