Motion Orchestration Practice

This document walks 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_upper_fsm_state(), set_velocity_source(), set_velocity(), set_stand_pose(), move_joint(), move_point(), move_line(), move_sequence(), convert_target_poses()

States covered: PD Stand → WBC Policy → RemoteState

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	PD Stand → WBC policy, upper body to act state
3	Lower-body locomotion	Set velocity source, walk forward
4	move_joint	Start MoveTools, move both arms to pre-pose and zero the waist
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_ZERO_Q = np.array([0.0, 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 time
from fourier_aurora_client import AuroraClient
from movetools_session import MoveToolsSession

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

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

Wait 1 second after AuroraClient.get_instance() to allow DDS discovery to complete before issuing any state transitions.

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.

Python

# Switch to PD Stand
client.set_fsm_state(2)
time.sleep(1.0)

# Switch to WBC
client.set_fsm_state(3)
time.sleep(0.2)

State	fsm_state value	Description
PD Stand	2	Upright and stable, ready to enter WBC
WBC	3	Whole-body control, upper-body takeover allowed

Stage 3: Lower-Body Locomotion

Complete lower-body walking before handing upper-body control to MoveTools.

Python

# Set velocity source to navigation mode
client.set_velocity_source(2)
time.sleep(0.2)

# Switch upper body to act state for natural arm swing
client.set_upper_fsm_state(1)
time.sleep(2.0)

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

set_velocity_source(2) hands velocity control to external navigation commands. set_upper_fsm_state(1) puts the upper body in follow mode so the arms swing naturally during walking.

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

After lower-body walking completes, switch the upper body to RemoteState, start MoveTools, then immediately use move_joint() to move both arms and the waist to the pre-pose.

Python

# Switch upper body to RemoteState and start MoveTools
client.set_upper_fsm_state(2)
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_ZERO_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.

Note: set_upper_fsm_state(2) and movetools.start() must be called after the lower-body velocity command has finished. Switching the upper body to RemoteState while the lower body is still moving can cause unexpected posture changes.

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: Posture Control and Step Back

While MoveTools is running, you can still use AuroraClient to adjust body height and lower-body velocity. The upper body remains under RemoteState control and is unaffected.

Python

# Lower body height by 20 cm
client.set_stand_pose(delta_z=-0.2, delta_pitch=0.0, delta_yaw=0.0)
time.sleep(1.0)

# Step back for 2 seconds
client.set_velocity(vx=-1.0, vy=0.0, yaw=0.0, duration=2.0)
time.sleep(0.5)

# Restore normal body height
client.set_stand_pose(delta_z=0.0, delta_pitch=0.0, delta_yaw=0.0)
time.sleep(2.0)

set_stand_pose() adjusts the overall standing height; delta_z is in metres. set_velocity() issues velocity commands while the lower body remains in WBC, independently of the upper-body MoveTools control.

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=["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.

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 time

import numpy as np

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_ZERO_Q     = np.array([0.0, 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 main():
    parser = argparse.ArgumentParser()
    parser.add_argument("-d", dest="domain_id", type=int, default=123)
    parser.add_argument("--robot-name", type=str, default="gr3")
    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
        client.set_fsm_state(2)
        time.sleep(1.0)
        client.set_fsm_state(3)
        time.sleep(0.2)

        # Lower-body walking
        client.set_velocity_source(2)
        time.sleep(0.2)
        client.set_upper_fsm_state(1)
        time.sleep(2.0)
        client.set_velocity(1.0, 0.0, 0.0, duration=2.0)
        time.sleep(2.5)

        # Start MoveTools
        client.set_upper_fsm_state(2)
        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_ZERO_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")

        # Posture control + step back
        client.set_stand_pose(delta_z=-0.2, delta_pitch=0.0, delta_yaw=0.0)
        time.sleep(1.0)
        client.set_velocity(vx=-1.0, vy=0.0, yaw=0.0, duration=2.0)
        time.sleep(0.5)
        client.set_stand_pose(delta_z=0.0, delta_pitch=0.0, delta_yaw=0.0)
        time.sleep(2.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=["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.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 lower-body 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

• modules/gr-controller/index.md
• modules/gr-manipulation/index.md