Cartesian Point Motion Complete Example

This example shows how to use move_point() to move both end effectors to target poses in the base frame. move_point() solves IK for the target poses and executes point-to-point motion; the end-effector path is not guaranteed to be a straight line.

Related states: PD Stand, WBC, RemoteState

Related APIs: move_joint(), move_point(), wait_until_idle(), get_cartesian_state()

Python

import time

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


left_pre_q = np.array([0.7, 0.6, 0.1, -1.5, 0.0, 0.0, 0.0])
right_pre_q = np.array([0.7, -0.6, -0.1, -1.5, 0.0, 0.0, 0.0])

left_target_pose = [0.469241, 0.305731, 0.186122, -0.05, -0.74, -0.04, 0.67]
right_target_pose = [0.44372, -0.213381, 0.186122, 0.05, -0.74, 0.04, 0.67]


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


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


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 move to the joint pre-pose...")
    result = movetools.move_joint(
        groups=["left_manipulator", "right_manipulator"],
        target_q={
            "left_manipulator": left_pre_q,
            "right_manipulator": right_pre_q,
        },
        expect_vel=300,
    )
    check_result(result, "pre-pose move_joint")
    wait_and_check(movetools, "pre-pose move_joint")

    input("Press Enter to run move_point...")
    result = movetools.move_point(
        groups=["waist", "left_manipulator", "right_manipulator"],
        target_poses={
            "left_end_effector_link": left_target_pose,
            "right_end_effector_link": right_target_pose,
        },
        expect_vel=300,
        frame="base",
    )
    check_result(result, "move_point")
    wait_and_check(movetools, "move_point")

    left_pose = client.get_cartesian_state("left_manipulator", "pose")
    right_pose = client.get_cartesian_state("right_manipulator", "pose")
    print(f"left end-effector pose:  {[round(v, 4) for v in left_pose]}")
    print(f"right end-effector pose: {[round(v, 4) for v in right_pose]}")

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