Motion Orchestration Practice

This document moves through a complete loco-manipulation scenario, demonstrating how to combine GR-Controller motion control with MoveTools upper-body orchestration into a single end-to-end workflow.

The motion control portion covers FSM state switching, velocity command setup, and robot posture control. The upper-body manipulation portion covers four motion interfaces — joint-space motion, Cartesian point-to-point, Cartesian linear, and multi-segment sequences — as well as switching between the base and world reference frames.

APIs covered: set_fsm_state(), set_velocity_source(), set_velocity(), move_joint(), move_point(), move_line(), move_sequence(), convert_target_poses()

States covered: JointStand → grw_policy_task → upper_body_tele_task

Prerequisites

1. GR-Controller (AuroraCore) is running on the robot side.
2. AuroraClient has successfully connected to the robot and can read/write state.
3. The robot is in a safe initial standing posture with no obstacles nearby.

Scenario Overview

The full workflow is divided into the following stages:

Stage	Operation	Description
1	Connect to robot	Create AuroraClient and MoveToolsSession
2	Switch FSM state	JointStand → grw_policy_task
3	Wheeled-base motion	Set velocity source, move forward
4	move_joint	Start MoveTools, move both arms to pre-pose and return the waist to its safe pose
5	move_point	Move both arms to grasp target (base frame)
6	move_line	Lift both arms 10 cm linearly along world frame z-axis
7	Posture control + locomotion	Lower body height, step back, restore posture
8	move_sequence	Rectangular path with both arms (move_point + multiple move_line)
9	Exit	Stop MoveTools, close connection

Constant Definitions

Python

import numpy as np

LEFT_ARM_PRE_Q  = np.array([ 0.7,  0.6,  0.1, -1.5, 0.0, 0.0, 0.0], dtype=np.float64)
RIGHT_ARM_PRE_Q = np.array([ 0.7, -0.6, -0.1, -1.5, 0.0, 0.0, 0.0], dtype=np.float64)

LEFT_ARM_START_Q  = np.array([-0.7,  0.6,  0.4, -1.2, 0.0, 0.0, 0.0], dtype=np.float64)
RIGHT_ARM_START_Q = np.array([-0.7, -0.6, -0.4, -1.2, 0.0, 0.0, 0.0], dtype=np.float64)

WAIST_SAFE_Q = np.array([-1.25, 2.15, -0.918, 0.0, 0.0], dtype=np.float64)

LEFT_GRASP_POSE  = [0.42372,  0.3, 0.186122, -0.05, -0.74, -0.04, 0.67]
RIGHT_GRASP_POSE = [0.42372, -0.3, 0.186122,  0.05, -0.74,  0.04, 0.67]

Cartesian poses use the format [x, y, z, qx, qy, qz, qw], with position in metres and quaternion in xyzw order.

Helper Functions

Python

def check_move_result(result, name):
    if not result.success:
        detail = ""
        if result.failed_segment_index >= 0:
            detail += f", segment={result.failed_segment_index}"
        if result.failed_sample_index >= 0:
            detail += f", sample={result.failed_sample_index}, t={result.failed_sample_time_s:.4f}s"
        raise RuntimeError(f"[{name}] planning failed: {result.code} {result.message}{detail}")


def wait_and_check(movetools, name):
    movetools.wait_until_idle()
    movetools.raise_if_error()
    print(f"[{name}] done.")

check_move_result() raises immediately on planning failure and includes the failed segment and sample indices to aid debugging. wait_and_check() waits for queued trajectories to finish executing and propagates any background thread exceptions.

Stage 1: Connect to Robot

Python

import os
import time

os.environ.setdefault("FASTDDS_BUILTIN_TRANSPORTS", "UDPv4")  # required on the robot

from fourier_aurora_client import AuroraClient
from movetools_session import MoveToolsSession

client = AuroraClient.get_instance(
    domain_id=123,
    robot_name="grw",
    namespace=None,
    is_ros_compatible=None,
)
time.sleep(1.0)
print("AuroraClient connected successfully.")

movetools = MoveToolsSession(client, robot_name="grw")

Wait 1 second after AuroraClient.get_instance() to allow DDS discovery to complete before issuing any state transitions. The UDP prerequisite avoids the shared-memory transport failing to deliver data against the root-run AuroraCore.

Stage 2: Switch FSM State

MoveTools does not manage the robot FSM directly. Before starting MoveTools, use AuroraClient to switch the robot into the state that allows upper-body takeover. A True return from set_fsm_state() only means the request was accepted — confirm by reading back:

Python

def wait_fsm(expected, timeout_s=10.0):
    deadline = time.monotonic() + timeout_s
    while client.get_fsm_state() != expected:
        if time.monotonic() >= deadline:
            raise RuntimeError(f"state {expected} not reached")
        time.sleep(0.05)


# Switch to JointStand
client.set_fsm_state(1)
wait_fsm(1)
time.sleep(2.0)

# Switch to grw_policy_task
client.set_fsm_state(3)
wait_fsm(3)
time.sleep(1.0)

State	fsm_state value	Description
JointStand	1	Move to the safe initial pose
grw_policy_task	3	Wheeled-base and policy control
upper_body_tele_task	4	Waist, head, and arm teleoperation

Stage 3: Wheeled-Base Motion

Complete wheeled-base motion in grw_policy_task before switching to upper_body_tele_task and starting MoveTools.

Python

# Set velocity source to navigation mode and confirm
if not client.set_velocity_source(2):
    raise RuntimeError("velocity source request not accepted")
deadline = time.monotonic() + 2.0
while client.get_velocity_source() != 2:
    if time.monotonic() >= deadline:
        raise RuntimeError("velocity source did not reach Navigation")
    time.sleep(0.05)

# Move forward for 2 seconds
client.set_velocity(0.2, 0.0, 0.0, duration=2.0)
time.sleep(2.5)
client.set_velocity(0.0, 0.0, 0.0, duration=1.0)

set_velocity_source(2) hands velocity control to external navigation commands; read back that the source is Navigation before sending — otherwise commands are ignored.

Stage 4: move_joint — Start MoveTools and Move to Pre-pose

After wheeled-base motion completes, switch to upper_body_tele_task, start MoveTools, then use move_joint() to move both arms and the waist to the pre-pose.

Python

# Switch to upper_body_tele_task and start MoveTools
client.set_fsm_state(4)
wait_fsm(4)
time.sleep(0.5)
movetools.start()

result = movetools.move_joint(
    groups=["left_manipulator", "right_manipulator", "waist"],
    target_q={
        "left_manipulator":  LEFT_ARM_PRE_Q,
        "right_manipulator": RIGHT_ARM_PRE_Q,
        "waist":             WAIST_SAFE_Q,
    },
    expect_vel=300,
)
check_move_result(result, "move_joint")
wait_and_check(movetools, "move_joint")

movetools.start() captures a snapshot of the current robot state to initialise the internal planner and starts the background bridge. All subsequent motion requests go through MoveTools. groups includes both arms and the waist, so all three groups move in parallel within a single planning request. expect_vel=300 means 30 % of the configured velocity limit, appropriate for an initial approach.

Stage 5: move_point — Cartesian Point-to-Point (base frame)

Use move_point() to move both end-effectors to the grasp target poses. The end-effector path is not guaranteed to be a straight line.

Python

result = movetools.move_point(
    groups=["waist", "left_manipulator", "right_manipulator"],
    target_poses={
        "left_end_effector_link":  list(LEFT_GRASP_POSE),
        "right_end_effector_link": list(RIGHT_GRASP_POSE),
    },
    expect_vel=300,
    frame="base",
)
check_move_result(result, "move_point")
wait_and_check(movetools, "move_point")

frame="base" means the target poses are interpreted relative to base_link, consistent with what AuroraClient.get_cartesian_state() returns. Adding waist to groups lets the waist participate in IK, extending the reachable workspace.

Stage 6: move_line — Cartesian Linear Motion (world frame)

Use move_line() to lift both end-effectors 10 cm linearly along the world frame z-axis. First read the current end-effector poses and convert them to the world frame.

Python

# Read current end-effector poses in base frame
left_base  = client.get_cartesian_state("left_manipulator",  "pose")
right_base = client.get_cartesian_state("right_manipulator", "pose")

# Convert to world frame
world_poses = movetools.convert_target_poses(
    {
        "left_end_effector_link":  left_base,
        "right_end_effector_link": right_base,
    },
    from_frame="base",
    to_frame="world",
)

# Lift 10 cm in world frame
left_world  = list(world_poses["left_end_effector_link"])
right_world = list(world_poses["right_end_effector_link"])
left_world[2]  += 0.1
right_world[2] += 0.1

result = movetools.move_line(
    groups=["waist", "left_manipulator", "right_manipulator"],
    target_poses={
        "left_end_effector_link":  left_world,
        "right_end_effector_link": right_world,
    },
    expect_vel=100,
    frame="world",
)
check_move_result(result, "move_line")
wait_and_check(movetools, "move_line")

frame="world" keeps the end-effector orientation constrained and moves it in a straight line in world space — suitable for tasks that need absolute spatial precision rather than robot-relative motion. expect_vel=100 uses a lower speed to maintain linear accuracy.

Frame selection: base frame targets move with the robot; world frame targets are fixed in space. Use world frame for tasks that require absolute height constraints such as a straight vertical lift.

Stage 7: Move the Wheeled Base Backward

upper_body_tele_task can receive MoveTools joint commands and wheeled-base velocity commands at the same time. Keep the speed low and ensure the surrounding area is clear.

Python

# Move backward for 2 seconds
client.set_velocity(vx=-0.2, vy=0.0, yaw=0.0, duration=2.0)
time.sleep(2.5)
client.set_velocity(vx=0.0, vy=0.0, yaw=0.0, duration=1.0)

Stage 8: move_sequence — Rectangular Path

Use move_sequence() to submit multiple motion segments in one call: a move_point to reach the start of the path, followed by four move_line segments tracing a rectangle.

Python

left_pose_a  = [0.38372,  0.3, 0.186122, -0.05, -0.74, -0.04, 0.67]
right_pose_a = [0.38372, -0.3, 0.186122,  0.05, -0.74,  0.04, 0.67]
left_pose_b  = [0.28372,  0.3, 0.186122, -0.05, -0.74, -0.04, 0.67]
right_pose_b = [0.28372, -0.3, 0.186122,  0.05, -0.74,  0.04, 0.67]
left_pose_c  = [0.28372,  0.3, 0.386122, -0.05, -0.74, -0.04, 0.67]
right_pose_c = [0.28372, -0.3, 0.386122,  0.05, -0.74,  0.04, 0.67]
left_pose_d  = [0.38372,  0.3, 0.386122, -0.05, -0.74, -0.04, 0.67]
right_pose_d = [0.38372, -0.3, 0.386122,  0.05, -0.74,  0.04, 0.67]

seq = movetools.create_move_sequence_builder(
    groups=["waist", "left_manipulator", "right_manipulator"],
)

# Segment 1: move_point to reach the rectangle start
seq.add_move_point(
    target_poses={
        "left_end_effector_link":  left_pose_a,
        "right_end_effector_link": right_pose_a,
    },
    expect_vel=100,
    frame="base",
)

# Segments 2–5: move_line along the rectangular path
for left_pose, right_pose in [
    (left_pose_b, right_pose_b),
    (left_pose_c, right_pose_c),
    (left_pose_d, right_pose_d),
    (left_pose_a, right_pose_a),
]:
    seq.add_move_line(
        {
            "left_end_effector_link":  left_pose,
            "right_end_effector_link": right_pose,
        },
        expect_vel=200,
        frame="base",
    )

result = movetools.move_sequence(seq)
check_move_result(result, "move_sequence")
wait_and_check(movetools, "move_sequence")

move_sequence() plans all segments in one shot and transitions between them continuously with no pauses. The first segment uses move_point at expect_vel=100 for a slow, controlled approach to the start; subsequent linear segments use expect_vel=200.

groups must include waist

The rectangle start (z≈0.19) is lower than the end-effector height after the stage-6 lift, and the path includes a 20 cm vertical span. With arms only, the approach segment fails with IK_FAILED (failed_segment_index=0); putting waist in groups lets the lift columns join the solution and the path plans and executes normally (consistent with stages 5 and 6).

Segment	Type	From → To	Description
1	move_point	current → pose_a	Joint-space approach to rectangle start
2	move_line	pose_a → pose_b	Linear move inward along x-axis
3	move_line	pose_b → pose_c	Linear move upward along z-axis
4	move_line	pose_c → pose_d	Linear move outward along x-axis
5	move_line	pose_d → pose_a	Linear move downward along z-axis, back to start

Stage 9: Exit

Python

movetools.stop()
client.close()

movetools.stop() shuts down the background bridge thread. Place this in a finally block to ensure clean shutdown even when exceptions occur.

Python

try:
    # ... motion logic
finally:
    movetools.stop()
    client.close()

Full Example

Python

import argparse
import os
import time

import numpy as np

os.environ.setdefault("FASTDDS_BUILTIN_TRANSPORTS", "UDPv4")  # required on the robot

from fourier_aurora_client import AuroraClient
from movetools_session import MoveToolsSession


LEFT_ARM_PRE_Q   = np.array([ 0.7,  0.6,  0.1, -1.5, 0.0, 0.0, 0.0], dtype=np.float64)
RIGHT_ARM_PRE_Q  = np.array([ 0.7, -0.6, -0.1, -1.5, 0.0, 0.0, 0.0], dtype=np.float64)
WAIST_SAFE_Q     = np.array([-1.25, 2.15, -0.918, 0.0, 0.0], dtype=np.float64)
LEFT_GRASP_POSE  = [0.42372,  0.3, 0.186122, -0.05, -0.74, -0.04, 0.67]
RIGHT_GRASP_POSE = [0.42372, -0.3, 0.186122,  0.05, -0.74,  0.04, 0.67]


def check_move_result(result, name):
    if not result.success:
        detail = ""
        if result.failed_segment_index >= 0:
            detail += f", segment={result.failed_segment_index}"
        if result.failed_sample_index >= 0:
            detail += f", sample={result.failed_sample_index}, t={result.failed_sample_time_s:.4f}s"
        raise RuntimeError(f"[{name}] planning failed: {result.code} {result.message}{detail}")


def wait_and_check(movetools, name):
    movetools.wait_until_idle()
    movetools.raise_if_error()
    print(f"[{name}] done.")


def wait_fsm(client, expected, timeout_s=10.0):
    deadline = time.monotonic() + timeout_s
    while client.get_fsm_state() != expected:
        if time.monotonic() >= deadline:
            raise RuntimeError(f"state {expected} not reached")
        time.sleep(0.05)


def wait_source(client, expected, timeout_s=2.0):
    deadline = time.monotonic() + timeout_s
    while client.get_velocity_source() != expected:
        if time.monotonic() >= deadline:
            raise RuntimeError(f"velocity source did not reach {expected}")
        time.sleep(0.05)


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("-d", dest="domain_id", type=int, default=123)
    parser.add_argument("--robot-name", type=str, default="grw")
    args = parser.parse_args()

    client = AuroraClient.get_instance(
        domain_id=args.domain_id,
        robot_name=args.robot_name,
        namespace=None,
        is_ros_compatible=None,
    )
    time.sleep(1.0)

    movetools = MoveToolsSession(client, robot_name=args.robot_name)

    try:
        # FSM state transitions (with read-back confirmation)
        client.set_fsm_state(1)
        wait_fsm(client, 1)
        time.sleep(2.0)
        client.set_fsm_state(3)
        wait_fsm(client, 3)
        time.sleep(1.0)

        # Lower-body moving
        if not client.set_velocity_source(2):
            raise RuntimeError("velocity source request not accepted")
        wait_source(client, 2)
        client.set_velocity(0.2, 0.0, 0.0, duration=2.0)
        time.sleep(2.5)
        client.set_velocity(0.0, 0.0, 0.0, duration=1.0)

        # Start MoveTools
        client.set_fsm_state(4)
        wait_fsm(client, 4)
        time.sleep(0.5)
        movetools.start()

        # move_joint
        result = movetools.move_joint(
            groups=["left_manipulator", "right_manipulator", "waist"],
            target_q={
                "left_manipulator":  LEFT_ARM_PRE_Q,
                "right_manipulator": RIGHT_ARM_PRE_Q,
                "waist":             WAIST_SAFE_Q,
            },
            expect_vel=300,
        )
        check_move_result(result, "move_joint")
        wait_and_check(movetools, "move_joint")

        # move_point
        result = movetools.move_point(
            groups=["waist", "left_manipulator", "right_manipulator"],
            target_poses={
                "left_end_effector_link":  list(LEFT_GRASP_POSE),
                "right_end_effector_link": list(RIGHT_GRASP_POSE),
            },
            expect_vel=300,
            frame="base",
        )
        check_move_result(result, "move_point")
        wait_and_check(movetools, "move_point")

        # move_line (world frame)
        left_base  = client.get_cartesian_state("left_manipulator",  "pose")
        right_base = client.get_cartesian_state("right_manipulator", "pose")
        world_poses = movetools.convert_target_poses(
            {"left_end_effector_link": left_base, "right_end_effector_link": right_base},
            from_frame="base", to_frame="world",
        )
        left_world  = list(world_poses["left_end_effector_link"])
        right_world = list(world_poses["right_end_effector_link"])
        left_world[2]  += 0.1
        right_world[2] += 0.1
        result = movetools.move_line(
            groups=["waist", "left_manipulator", "right_manipulator"],
            target_poses={
                "left_end_effector_link":  left_world,
                "right_end_effector_link": right_world,
            },
            expect_vel=100,
            frame="world",
        )
        check_move_result(result, "move_line")
        wait_and_check(movetools, "move_line")

        # Move the wheeled base backward at low speed
        client.set_velocity(vx=-0.2, vy=0.0, yaw=0.0, duration=2.0)
        time.sleep(2.5)
        client.set_velocity(vx=0.0, vy=0.0, yaw=0.0, duration=1.0)

        # move_sequence
        left_pose_a  = [0.38372,  0.3, 0.186122, -0.05, -0.74, -0.04, 0.67]
        right_pose_a = [0.38372, -0.3, 0.186122,  0.05, -0.74,  0.04, 0.67]
        left_pose_b  = [0.28372,  0.3, 0.186122, -0.05, -0.74, -0.04, 0.67]
        right_pose_b = [0.28372, -0.3, 0.186122,  0.05, -0.74,  0.04, 0.67]
        left_pose_c  = [0.28372,  0.3, 0.386122, -0.05, -0.74, -0.04, 0.67]
        right_pose_c = [0.28372, -0.3, 0.386122,  0.05, -0.74,  0.04, 0.67]
        left_pose_d  = [0.38372,  0.3, 0.386122, -0.05, -0.74, -0.04, 0.67]
        right_pose_d = [0.38372, -0.3, 0.386122,  0.05, -0.74,  0.04, 0.67]

        seq = movetools.create_move_sequence_builder(
            groups=["waist", "left_manipulator", "right_manipulator"],
        )
        seq.add_move_point(
            target_poses={
                "left_end_effector_link":  left_pose_a,
                "right_end_effector_link": right_pose_a,
            },
            expect_vel=100,
            frame="base",
        )
        for left_pose, right_pose in [
            (left_pose_b, right_pose_b),
            (left_pose_c, right_pose_c),
            (left_pose_d, right_pose_d),
            (left_pose_a, right_pose_a),
        ]:
            seq.add_move_line(
                {"left_end_effector_link": left_pose, "right_end_effector_link": right_pose},
                expect_vel=200,
                frame="base",
            )
        result = movetools.move_sequence(seq)
        check_move_result(result, "move_sequence")
        wait_and_check(movetools, "move_sequence")

        print("Demo finished successfully.")

    finally:
        movetools.stop()
        client.set_velocity(0.0, 0.0, 0.0, duration=1.0)
        client.set_velocity_source(0)
        if client.get_fsm_state() != 3:
            client.set_fsm_state(3)
            wait_fsm(client, 3)
        client.close()


if __name__ == "__main__":
    main()

Troubleshooting

Diagnosing a planning failure

check_move_result() reports failed_segment_index and failed_sample_index, which pinpoint the exact segment and sample point where IK failed. Common causes are a target pose outside the reachable workspace or a target that is too far from the current configuration.

Upper-body target drifting after base movement

base frame targets move with the robot. If the lower body is moving while the upper body is executing a Cartesian motion, the actual spatial position of the target shifts accordingly. Solutions:

• Wait for the lower body to come to a full stop (add sufficient time.sleep after the velocity command) before issuing upper-body motion.
• For tasks with strict spatial constraints, use world frame and re-read and convert end-effector poses immediately before planning.

A segment fails inside move_sequence

Check result.failed_segment_index in check_move_result() to identify the failing segment. A common cause is a discontinuity in velocity or joint state at the boundary between adjacent move_line segments. Try reducing expect_vel or adjusting the intermediate waypoints.

See Also

• GR-Controller module docs
• GR-Manipulation module docs