Cartesian Line Motion Complete Example
This example shows how to use move_line() to lift both end effectors in the world frame and then make a small tool-frame displacement, returning to the start. Targets are generated from the current end-effector poses via convert_target_poses() plus bounded offsets β no hardcoded absolute poses.
Related states: JointStand, upper_body_tele_task
Related APIs: move_line(), convert_target_poses(), wait_until_idle(), get_cartesian_state()
Python
import math
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
ARMS = ("left_manipulator", "right_manipulator")
WORLD_Z_OFFSET = 0.03 # world-frame lift (m)
TOOL_DELTA = [0.02, 0.0, 0.02] # tool-frame small displacement (m)
def wait_fsm(client, expected, timeout_s=8.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 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")
def normalized_pose(pose):
pose = list(pose)
norm = math.sqrt(sum(q * q for q in pose[3:]))
return pose[:3] + [q / norm for q in pose[3:]]
client = AuroraClient.get_instance(
domain_id=123, # Must match the DDS domain used by the running AuroraCore
robot_name="grw",
)
movetools = MoveToolsSession(client, robot_name="grw")
started = False
try:
input("Press Enter to switch to JointStand...")
client.set_fsm_state(1)
wait_fsm(client, 1)
time.sleep(2.0)
input("Press Enter to switch to upper_body_tele_task...")
client.set_fsm_state(4)
wait_fsm(client, 4)
time.sleep(1.0)
movetools.start()
started = True
start_poses = {
arm: normalized_pose(client.get_cartesian_state(arm, "pose")) for arm in ARMS
}
print(f"start poses: {start_poses}")
# ---- world frame: dual-arm straight-line lift of 3 cm, then back ----
world = movetools.convert_target_poses(
{
"left_end_effector_link": start_poses["left_manipulator"],
"right_end_effector_link": start_poses["right_manipulator"],
},
from_frame="base",
to_frame="world",
)
up_world = {k: list(v) for k, v in world.items()}
for v in up_world.values():
v[2] += WORLD_Z_OFFSET
input("Press Enter to run move_line (world z+0.03)...")
result = movetools.move_line(
groups=list(ARMS),
target_poses=up_world,
expect_vel=150,
frame="world",
)
check_result(result, "move_line world")
wait_and_check(movetools, "move_line world")
moved = {arm: normalized_pose(client.get_cartesian_state(arm, "pose")) for arm in ARMS}
print(f"z delta: {[round(moved[a][2] - start_poses[a][2], 4) for a in ARMS]}")
input("Press Enter to return in world frame...")
result = movetools.move_line(
groups=list(ARMS),
target_poses=world,
expect_vel=150,
frame="world",
)
check_result(result, "move_line world back")
wait_and_check(movetools, "move_line world back")
# ---- tool frame: small left-arm displacement, then back ----
input("Press Enter to run move_line (tool delta)...")
result = movetools.move_line(
groups=["left_manipulator"],
target_poses={
"left_end_effector_link": TOOL_DELTA + [0.0, 0.0, 0.0, 1.0],
},
expect_vel=120,
frame="tool",
)
check_result(result, "move_line tool")
wait_and_check(movetools, "move_line tool")
input("Press Enter to return in tool frame...")
result = movetools.move_line(
groups=["left_manipulator"],
target_poses={
"left_end_effector_link": [-d for d in TOOL_DELTA] + [0.0, 0.0, 0.0, 1.0],
},
expect_vel=120,
frame="tool",
)
check_result(result, "move_line tool back")
wait_and_check(movetools, "move_line tool back")
finally:
if started:
movetools.stop()
if client.get_fsm_state() != 3:
client.set_fsm_state(3) # back to the standing policy state
wait_fsm(client, 3)
client.close()