Joint-Space Motion Complete Example

This example shows how to run dual-arm joint-space point-to-point motion with MoveTools: connect to GR-Controller, switch the upper body to a remotely controllable state, start MoveTools, and call move_joint() with target joint positions for both arms.

Related states: PD Stand, WBC, RemoteState

Related APIs: AuroraClient.get_instance(), set_fsm_state(), set_upper_fsm_state(), MoveToolsSession.start(), move_joint(), wait_until_idle()

Python

import time

import numpy as np
from fourier_aurora_client import AuroraClient
from movetools_session import MoveToolsSession


left_target_q = np.array([0.7, 0.6, 0.1, -1.5, 0.0, 0.0, 0.0])
right_target_q = np.array([0.7, -0.6, -0.1, -1.5, 0.0, 0.0, 0.0])


def check_result(result, name):
    if not result.success:
        raise RuntimeError(f"{name} failed: {result.code} {result.message}")


client = AuroraClient.get_instance(domain_id=123, robot_name="gr3")
movetools = MoveToolsSession(client, robot_name="gr3")

try:
    input("Press Enter to switch to PD Stand...")
    client.set_fsm_state(2)
    time.sleep(1.0)

    input("Press Enter to switch to WBC and RemoteState...")
    client.set_fsm_state(3)
    time.sleep(0.5)
    client.set_upper_fsm_state(2)
    time.sleep(0.5)

    movetools.start()

    input("Press Enter to run move_joint...")
    result = movetools.move_joint(
        groups=["left_manipulator", "right_manipulator"],
        target_q={
            "left_manipulator": left_target_q,
            "right_manipulator": right_target_q,
        },
        expect_vel=300,
    )
    check_result(result, "move_joint")

    movetools.wait_until_idle()
    movetools.raise_if_error()
    print("move_joint complete")

finally:
    movetools.stop()
    client.close()