Locomotion Full Example

This example demonstrates the GR-W wheeled-base control flow: enter grw_policy_task (state 3), switch the velocity source to Navigation with read-back confirmation, and send low-speed base commands.

States used: GRW Policy Task State

APIs used: set_velocity, set_velocity_source

Read before running

1. Set export FASTDDS_BUILTIN_TRANSPORTS=UDPv4 before creating any DDS participant (see the connection guide).
2. Confirm the e-stop is within reach and the surroundings are clear before running. The forward leg travels about 0.4 m; the rotation leg turns about 20° in place counter-clockwise (nominal 0.4 rad, about 0.3 rad after policy smoothing).
3. The base vel_B/pos_W readings carry a resting bias and drift; they cannot be used to verify that the robot has stopped — rely on on-site observation.

Python

import os
import time

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


from fourier_aurora_client import AuroraClient


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 wait_velocity_source(client, expected, timeout=2.0):
    deadline = time.monotonic() + timeout
    while client.get_velocity_source() != expected:
        if time.monotonic() >= deadline:
            raise RuntimeError(f"velocity source did not switch to {expected} within {timeout}s")
        time.sleep(0.05)


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

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

    # Step 2: switch the velocity source to Navigation (2) and confirm
    input("Press Enter to set velocity source to Navigation...")
    if not client.set_velocity_source(2):
        raise RuntimeError("velocity source request not accepted")
    wait_velocity_source(client, 2)

    # Step 3: move forward at 0.2 m/s for 2 s (auto-stop when duration expires)
    input("Press Enter to move forward at 0.2 m/s for 2 s...")
    print("Moving forward at 0.2 m/s")
    client.set_velocity(0.2, 0.0, 0.0, 2.0)
    time.sleep(2.0)
    client.set_velocity(0.0, 0.0, 0.0, 1.0)   # explicit stop

    # Step 4: rotate anti-clockwise at 0.2 rad/s for 2 s (positive yaw = counter-clockwise)
    input("Press Enter to rotate anti-clockwise at 0.2 rad/s for 2 s...")
    print("Rotating anti-clockwise at 0.2 rad/s")
    client.set_velocity(0.0, 0.0, 0.2, 2.0)
    time.sleep(2.0)
    client.set_velocity(0.0, 0.0, 0.0, 1.0)   # explicit stop

    print("Motion command demonstration complete")
finally:
    client.set_velocity(0.0, 0.0, 0.0, 1.0)   # safety-net zero velocity
    client.set_velocity_source(0)             # hand control back to the joystick
    client.close()

Behavior notes:

• Both state 3 and velocity source 2 require a "request + read-back confirm" pair; a True service return only means the request was accepted.
• When the velocity source is not 2, AuroraCore ignores velocity commands and logs Ignore velocity command because current velocity source is not 'Navigation'.
• Commands expire automatically when duration elapses; e.g. a 0.05 m/s command with duration=0.5 decelerates within about 0.3 s after expiry — still send an explicit zero velocity afterwards.