Joint Control Full Example

This example demonstrates the complete GR-W joint-group control workflow: enter upper_body_tele_task (state 4), read current joint positions, move both arms through a small offset relative to the current pose using linear interpolation, and return exactly to the starting pose.

States used: GRW Policy Task State, Upper Body Teleoperation Task State

APIs used: set_group_cmd, get_group_state

Read before running

1. Set export FASTDDS_BUILTIN_TRANSPORTS=UDPv4 before creating any DDS participant (see the connection guide).
2. This example does not use hardcoded absolute target poses: joint zero positions may differ between power cycles, so public examples always generate targets from "current reading + bounded offset". Keep single-joint offsets within ±0.03 rad.
3. upper_body_tele_task expects a continuous joint command stream (this example sends at 100 Hz). Once you stop sending, uncommanded joints slowly droop under gravity (up to 0.09 rad within 1 s); either keep streaming all controlled groups or switch back to state 3 promptly.
4. The waist (waist, 5 DOF) only gets its current position echoed back in this example — no offset interpolation. The waist safe default is [-1.25, 2.15, -0.918, 0, 0]; never command all waist joints to zero.

Python

import os
import time

os.environ.setdefault("FASTDDS_BUILTIN_TRANSPORTS", "UDPv4")

from fourier_aurora_client import AuroraClient

ARM_OFFSET = 0.03      # single-joint offset limit (rad)
STEPS = 200            # interpolation steps
DT = 0.01              # 10 ms command period (100 Hz)


def wait_fsm(client, expected, timeout=4.0):
    deadline = time.monotonic() + timeout
    while client.get_fsm_state() != expected:
        if time.monotonic() >= deadline:
            raise RuntimeError(f"state did not switch to {expected} within {timeout}s")
        time.sleep(0.05)


def interpolate(client, current, target, steps=STEPS, dt=DT):
    for index in range(steps + 1):
        ratio = index / steps
        cmd = {group: [a + (b - a) * ratio for a, b in zip(current[group], target[group])]
               for group in target}
        client.set_group_cmd(cmd)
        time.sleep(dt)


client = AuroraClient.get_instance(domain_id=123, robot_name="grw")
original = None
try:
    time.sleep(1.0)

    # Step 1: from state 3, enter upper_body_tele_task (state 4) and confirm
    input("Press Enter to set FSM to upper_body_tele_task (state 4)...")
    if not client.set_fsm_state(4):
        raise RuntimeError("state change request not accepted")
    wait_fsm(client, 4)

    # Step 2: read current positions of all control groups
    original = {
        "left_manipulator":  client.get_group_state("left_manipulator", key="position"),
        "right_manipulator": client.get_group_state("right_manipulator", key="position"),
        "waist":             client.get_group_state("waist", key="position"),
    }
    assert len(original["left_manipulator"]) == 7
    assert len(original["right_manipulator"]) == 7
    assert len(original["waist"]) == 5
    print(f"original positions: {original}")

    # Step 3: target = current + bounded offset (both arm joint 0 only); waist keeps current
    input("Press Enter to move both arm joint0 by +0.03 rad...")
    target = {group: list(values) for group, values in original.items()}
    target["left_manipulator"][0] += ARM_OFFSET
    target["right_manipulator"][0] += ARM_OFFSET

    interpolate(client, original, target)     # outbound
    time.sleep(0.3)

    # Step 4: return to the starting pose
    input("Press Enter to return to the original position...")
    start = {
        group: client.get_group_state(group, key="position") for group in original
    }
    interpolate(client, start, original)      # return
    print("Joint control demonstration complete")
finally:
    if original is not None:
        try:
            now = {group: client.get_group_state(group, key="position") for group in original}
            interpolate(client, now, original, steps=100)
        except Exception as error:
            print(f"recovery interpolation failed: {error}")
    if client.get_fsm_state() != 3:
        client.set_fsm_state(3)               # back to the standing policy state
        wait_fsm(client, 3)
    client.close()

Behavior notes:

• The state 3 → 4 switch reports back within about 50 ms after acceptance; in state 4 get_upper_fsm_state() stays 0 (Default), and set_upper_fsm_state() returns False in both states 3 and 4 — upper-body teleoperation is always entered via whole-body state 4.
• Commanding +0.03 rad on both arm joint 0 reaches about +0.024 ~ +0.026 rad after the command stream ends (small tracking lag).
• get_group_motion_state() stays 0 (idle) during 100 Hz streamed interpolation; do not use it to detect when teleoperation motion completes.