#!/usr/bin/env python3
"""Run one minimal example for every public AirbotClient entry."""
from __future__ import annotations
import argparse
import math
import time
from collections.abc import Callable
from dataclasses import dataclass
from types import SimpleNamespace
from typing import Any
class ExampleError(RuntimeError):
"""The example cannot continue or cannot confirm safe cleanup."""
LIFECYCLE_EXAMPLES = ("__init__", "__enter__", "__exit__", "close")
IDENTITY_EXAMPLES = (
"get_client_id",
"get_source_name",
"get_client_instance_id",
"get_client_session_id",
)
CONTROL_EXAMPLES = (
"acquire_control",
"handover_to_previous",
"release_control",
)
MODE_EXAMPLES = (
"set_arm_speed",
"set_eef_speed",
"switch_controller",
"enter_gravity_compensation_mode",
"get_eef_mode",
"switch_eef_control_mode",
)
STATE_EXAMPLES = (
"get_service_state",
"get_arm_joint_state",
"get_arm_motor_state",
"get_eef_joint_state",
"get_eef_motor_state",
"get_imu_state",
"get_end_pose",
"get_firmware_info",
)
RECOVERY_EXAMPLES = (
"set_arm_emergency_stop",
"clear_error",
"return_zero",
"clear_eef_motor_err",
"clear_arm_motor_err",
)
MOTION_EXAMPLES = (
"move_joint",
"move_eef",
"move_end_pose",
"move_end_pose_linear",
"move_end_pose_circle",
"move_joint_waypoints",
"move_end_pose_waypoints",
)
PUBLIC_EXAMPLES = (
LIFECYCLE_EXAMPLES
+ IDENTITY_EXAMPLES
+ CONTROL_EXAMPLES
+ MODE_EXAMPLES
+ STATE_EXAMPLES
+ RECOVERY_EXAMPLES
+ MOTION_EXAMPLES
)
CONFIRMATION_TOKENS = {
"acquire_control": "CONTROL",
"handover_to_previous": "CONTROL",
"release_control": "CONTROL",
"set_arm_speed": "CONFIG",
"set_eef_speed": "CONFIG",
"switch_controller": "CONTROL",
"enter_gravity_compensation_mode": "MOVE",
"switch_eef_control_mode": "CONTROL",
"set_arm_emergency_stop": "STOP",
"clear_error": "RECOVER",
"return_zero": "MOVE",
"clear_eef_motor_err": "RECOVER",
"clear_arm_motor_err": "RECOVER",
**{name: "MOVE" for name in MOTION_EXAMPLES},
}
CARTESIAN_STEP_EXAMPLES = {
"move_end_pose_linear",
"move_end_pose_circle",
"move_end_pose_waypoints",
}
@dataclass(frozen=True)
class RunInputs:
"""Explicit task inputs that cannot be safely guessed by an example."""
cartesian_step_m: float | None = None
eef_target_mm: tuple[float, ...] | None = None
eef_eff: tuple[float, ...] | None = None
eef_speed_mm_s: float = 50.0
def _parse_finite_csv(name: str, raw: str | None) -> tuple[float, ...] | None:
if raw is None:
return None
values = tuple(float(item.strip()) for item in raw.split(",") if item.strip())
if not values or not all(math.isfinite(item) for item in values):
raise ValueError(f"{name} must contain finite comma-separated numbers")
return values
def inputs_from_args(args: argparse.Namespace) -> RunInputs:
"""Read optional, task-specific motion values from command-line arguments."""
return RunInputs(
cartesian_step_m=args.cartesian_step_m,
eef_target_mm=_parse_finite_csv("--eef-target-mm", args.eef_target_mm),
eef_eff=_parse_finite_csv("--eef-eff", args.eef_eff),
eef_speed_mm_s=args.eef_speed_mm_s,
)
def client_options_from_args(args: argparse.Namespace) -> dict[str, Any]:
"""Build explicit gRPC or DDS client options from command-line arguments."""
backend = args.backend
common = {"client_name": "docs-sdk-minimal-example"}
if backend == "grpc":
if not args.host:
raise ValueError("--host is required for --backend grpc")
return {
**common,
"backend": "grpc",
"host": args.host,
"port": args.port,
}
if backend == "dds":
if args.domain_id is None:
raise ValueError("--domain-id is required for --backend dds")
return {
**common,
"backend": "dds",
"domain_id": args.domain_id,
"side": args.side,
}
raise ValueError("--backend must be 'grpc' or 'dds'")
def _require_confirmation(name: str, confirmation: str | None) -> None:
expected = CONFIRMATION_TOKENS.get(name)
if expected is not None and confirmation != expected:
raise ExampleError(f"{name} requires --confirm {expected}")
def _validate_run_inputs(name: str, inputs: RunInputs) -> None:
if name in CARTESIAN_STEP_EXAMPLES:
step = inputs.cartesian_step_m
if step is None:
raise ExampleError(
f"{name} requires --cartesian-step-m from an approved workcell path"
)
if not math.isfinite(step) or step == 0.0 or abs(step) > 0.005:
raise ExampleError("--cartesian-step-m must be finite and within ±0.005 m")
if name == "move_eef":
target = inputs.eef_target_mm
effort = inputs.eef_eff
speed = inputs.eef_speed_mm_s
if target is None or effort is None:
raise ExampleError(
"move_eef requires approved --eef-target-mm and --eef-eff values"
)
if len(target) != len(effort):
raise ExampleError(
"--eef-target-mm and --eef-eff must have equal lengths"
)
if not math.isfinite(speed) or not 10.0 <= speed <= 1000.0:
raise ExampleError("--eef-speed-mm-s must be within [10, 1000] mm/s")
def _require_true(result: Any, action: str) -> None:
if result is not True:
raise ExampleError(f"{action} returned {result!r}")
def _require_idle_snapshot(client: Any) -> tuple[float, ...]:
service = client.get_service_state()
if service is None or not service.valid or not service.service_state:
raise ExampleError("service state is unavailable or stale")
if service.fsm_state != "IDLE":
raise ExampleError(f"expected IDLE before the example, got {service.fsm_state}")
joints = client.get_arm_joint_state()
if joints is None or len(joints.angles) != 7:
raise ExampleError("arm joint state must contain seven values")
angles = tuple(float(value) for value in joints.angles)
if not all(math.isfinite(value) for value in angles):
raise ExampleError("arm joint state contains a non-finite value")
motors = client.get_arm_motor_state()
if motors is None or len(motors.error_ids) != 7:
raise ExampleError("arm motor state must contain seven error codes")
if any(code != 0 for code in motors.error_ids):
raise ExampleError(f"arm motor errors are present: {motors.error_ids}")
return angles
def _require_unknown_error(client: Any) -> None:
service = client.get_service_state()
if service is None or not service.valid or not service.service_state:
raise ExampleError("service state is unavailable or stale")
if service.fsm_state != "UNKNOWN_ERROR":
raise ExampleError(f"expected UNKNOWN_ERROR, got {service.fsm_state}")
def _require_arm_motor_errors(client: Any) -> None:
motors = client.get_arm_motor_state()
if motors is None or len(motors.error_ids) != 7:
raise ExampleError("arm motor state must contain seven error codes")
if not any(code != 0 for code in motors.error_ids):
raise ExampleError("no nonzero arm motor error code is available to clear")
def _require_pose(client: Any) -> Any:
pose = client.get_end_pose()
if pose is None or len(pose.position) != 3 or len(pose.orientation) != 4:
raise ExampleError("current end pose is unavailable or malformed")
values = tuple(pose.position) + tuple(pose.orientation)
if not all(math.isfinite(value) for value in values):
raise ExampleError("current end pose contains a non-finite value")
quaternion_norm = math.sqrt(sum(value * value for value in pose.orientation))
if not math.isclose(quaternion_norm, 1.0, rel_tol=0.0, abs_tol=1e-3):
raise ExampleError("current end pose does not contain a unit quaternion")
return pose
def _require_eef(client: Any) -> None:
mode = client.get_eef_mode()
if mode is None or mode.get("has_eef") is not True:
raise ExampleError("the runtime does not report an available EEF")
firmware = client.get_firmware_info()
if firmware is None or not str(firmware.eef_type).strip():
raise ExampleError("the EEF type is unavailable; approved parameters cannot be matched")
def _wait_for_fsm(client: Any, expected: str, timeout_s: float = 3.0) -> None:
deadline = time.monotonic() + timeout_s
while time.monotonic() < deadline:
state = client.get_service_state()
if state is not None and state.service_state and state.fsm_state == expected:
return
time.sleep(0.1)
raise ExampleError(f"FSM did not report {expected} before the timeout")
def _wait_for_eef_mode(client: Any, expected: str, timeout_s: float = 3.0) -> None:
deadline = time.monotonic() + timeout_s
while time.monotonic() < deadline:
mode = client.get_eef_mode()
if mode is not None and mode.get("current_mode_name") == expected:
return
time.sleep(0.1)
raise ExampleError(f"EEF did not report {expected} mode before the timeout")
def _joint_target_toward_zero(angles: tuple[float, ...]) -> list[float]:
target = list(angles)
step = min(abs(target[6]), 0.03)
target[6] -= math.copysign(step, target[6]) if target[6] else 0.0
return target
def _offset_pose(api: Any, pose: Any, dx: float, dy: float = 0.0) -> Any:
x, y, z = pose.position
return api.CartesianPose(
position=(x + dx, y + dy, z),
orientation=tuple(pose.orientation),
)
def _run_with_control(
client: Any,
action: Callable[[], Any],
*,
cleanup_arm: bool = False,
cleanup_eef: bool = False,
api: Any,
) -> Any:
if not client.acquire_control(lease_ms=15_000, renew_period_s=5.0):
raise ExampleError("control lease was not acquired")
result: Any = None
action_error: BaseException | None = None
release_error: BaseException | None = None
arm_idle = True
eef_idle = True
try:
result = action()
except BaseException as error: # preserve the operation error after cleanup
action_error = error
finally:
if cleanup_eef:
try:
eef_idle = bool(
client.switch_eef_control_mode(api.EEFControlMode.idle, timeout_ms=6_000)
)
if eef_idle:
_wait_for_eef_mode(client, "idle")
except Exception:
eef_idle = False
if cleanup_arm:
try:
arm_idle = bool(
client.switch_controller(api.Controller.idle, timeout_ms=5_000)
)
if arm_idle:
_wait_for_fsm(client, "IDLE")
except Exception:
arm_idle = False
try:
client.release_control()
except BaseException as error: # report release failures without hiding the action error
release_error = error
cleanup_failures = []
if not arm_idle:
cleanup_failures.append("arm idle was not confirmed")
if not eef_idle:
cleanup_failures.append("EEF idle was not confirmed")
if release_error is not None:
cleanup_failures.append(f"control release failed: {release_error}")
if cleanup_failures:
message = "; ".join(cleanup_failures)
if action_error is not None and hasattr(action_error, "add_note"):
action_error.add_note(message)
else:
raise ExampleError(message)
if action_error is not None:
raise action_error
return result
def _run_motion(client: Any, api: Any, name: str, inputs: RunInputs) -> Any:
angles = _require_idle_snapshot(client)
if name == "move_joint":
target = _joint_target_toward_zero(angles)
def action() -> bool:
_require_true(
client.switch_controller(api.Controller.servo_control, timeout_ms=5_000),
"switch_controller",
)
_wait_for_fsm(client, "SERVO_CONTROL")
_require_true(client.set_arm_speed([0.6] * 7), "set_arm_speed")
return client.move_joint(
target,
api.JointMoveOptions(blocking=True),
timeout_ms=35_000,
)
result = _run_with_control(client, action, cleanup_arm=True, api=api)
elif name == "move_joint_waypoints":
target = _joint_target_toward_zero(angles)
def action() -> bool:
_require_true(
client.switch_controller(api.Controller.planning_control, timeout_ms=5_000),
"switch_controller",
)
_wait_for_fsm(client, "PLANNING_CONTROL")
return client.move_joint_waypoints(
[list(angles), target, list(angles)],
api.JointWaypointsMoveOptions(
velocity_scaling_factor=0.05,
acceleration_scaling_factor=0.05,
enable_blend=False,
blocking=True,
),
timeout_ms=35_000,
)
result = _run_with_control(client, action, cleanup_arm=True, api=api)
elif name == "move_eef":
_require_eef(client)
target = list(inputs.eef_target_mm or ())
effort = list(inputs.eef_eff or ())
def action() -> bool:
_require_true(
client.switch_eef_control_mode(api.EEFControlMode.csp, timeout_ms=6_000),
"switch_eef_control_mode",
)
_wait_for_eef_mode(client, "csp")
_require_true(client.set_eef_speed(inputs.eef_speed_mm_s), "set_eef_speed")
return client.move_eef(
target,
api.EEFMoveOptions(eff=effort, blocking=True),
timeout_ms=35_000,
)
result = _run_with_control(client, action, cleanup_eef=True, api=api)
else:
current = _require_pose(client)
if name == "move_end_pose":
def action() -> bool:
_require_true(
client.switch_controller(api.Controller.servo_control, timeout_ms=5_000),
"switch_controller",
)
_wait_for_fsm(client, "SERVO_CONTROL")
return client.move_end_pose(
current,
api.CartesianMoveOptions(blocking=True),
timeout_ms=35_000,
)
elif name == "move_end_pose_linear":
target = _offset_pose(api, current, inputs.cartesian_step_m or 0.0)
def action() -> bool:
_require_true(
client.switch_controller(api.Controller.planning_control, timeout_ms=5_000),
"switch_controller",
)
_wait_for_fsm(client, "PLANNING_CONTROL")
return client.move_end_pose_linear(
current,
target,
api.CartesianMoveOptions(
velocity_scaling_factor=0.05,
acceleration_scaling_factor=0.05,
blocking=True,
),
timeout_ms=35_000,
)
elif name == "move_end_pose_circle":
step = inputs.cartesian_step_m or 0.0
path = _offset_pose(api, current, step / 2.0, abs(step) / 2.0)
target = _offset_pose(api, current, step)
def action() -> bool:
_require_true(
client.switch_controller(api.Controller.planning_control, timeout_ms=5_000),
"switch_controller",
)
_wait_for_fsm(client, "PLANNING_CONTROL")
return client.move_end_pose_circle(
current,
path,
target,
api.CartesianMoveOptions(
circ_is_center=False,
velocity_scaling_factor=0.05,
acceleration_scaling_factor=0.05,
blocking=True,
),
timeout_ms=35_000,
)
elif name == "move_end_pose_waypoints":
target = _offset_pose(api, current, inputs.cartesian_step_m or 0.0)
def action() -> bool:
_require_true(
client.switch_controller(api.Controller.planning_control, timeout_ms=5_000),
"switch_controller",
)
_wait_for_fsm(client, "PLANNING_CONTROL")
return client.move_end_pose_waypoints(
[current, target, current],
api.CartesianWaypointsMoveOptions(
motion_type="lin",
velocity_scaling_factor=0.05,
acceleration_scaling_factor=0.05,
enable_blend=False,
blocking=True,
),
timeout_ms=35_000,
)
else: # protected by PUBLIC_EXAMPLES
raise AssertionError(name)
result = _run_with_control(client, action, cleanup_arm=True, api=api)
_require_true(result, name)
return result
def _run_connected_example(
client: Any,
api: Any,
name: str,
inputs: RunInputs,
emit: Callable[[Any], None],
) -> None:
if name == "get_client_id":
emit(client.get_client_id)
return
if name == "get_source_name":
emit(client.get_source_name)
return
if name == "get_client_instance_id":
emit(client.get_client_instance_id)
return
if name == "get_client_session_id":
emit(client.get_client_session_id)
return
if name == "get_service_state":
result = client.get_service_state()
elif name == "get_arm_joint_state":
result = client.get_arm_joint_state()
elif name == "get_arm_motor_state":
result = client.get_arm_motor_state()
elif name == "get_eef_joint_state":
result = client.get_eef_joint_state()
elif name == "get_eef_motor_state":
result = client.get_eef_motor_state()
elif name == "get_imu_state":
result = client.get_imu_state()
elif name == "get_end_pose":
result = client.get_end_pose()
elif name == "get_firmware_info":
result = client.get_firmware_info()
elif name == "get_eef_mode":
result = client.get_eef_mode()
else:
result = None
if name in STATE_EXAMPLES or name == "get_eef_mode":
if result is None:
if name in {"get_eef_joint_state", "get_eef_motor_state"}:
emit({"available": False, "value": None})
return
raise ExampleError(f"{name} returned None")
emit(result)
return
if name == "acquire_control":
if not client.acquire_control(lease_ms=15_000, renew_period_s=5.0):
raise ExampleError("control lease was not acquired")
try:
emit("control acquired")
finally:
try:
client.release_control()
except Exception as error:
raise ExampleError(f"control release failed: {error}") from error
return
if name == "release_control":
if not client.acquire_control(lease_ms=15_000, renew_period_s=5.0):
raise ExampleError("control lease was not acquired")
try:
client.release_control()
except Exception as error:
raise ExampleError(f"control release failed: {error}") from error
emit("control released")
return
if name == "handover_to_previous":
emit({"handover_succeeded": client.handover_to_previous()})
return
if name == "set_arm_speed":
_require_true(client.set_arm_speed([0.6] * 7), name)
emit("arm speed cache updated")
return
if name == "set_eef_speed":
_require_true(client.set_eef_speed(50.0), name)
emit("EEF speed cache updated")
return
if name == "switch_controller":
def select_arm_idle() -> bool:
selected = client.switch_controller(api.Controller.idle, timeout_ms=5_000)
if selected:
_wait_for_fsm(client, "IDLE")
return selected
result = _run_with_control(
client,
select_arm_idle,
cleanup_arm=True,
api=api,
)
_require_true(result, name)
emit("arm idle confirmed")
return
if name == "enter_gravity_compensation_mode":
_require_idle_snapshot(client)
def select_gravity_compensation() -> bool:
selected = client.enter_gravity_compensation_mode(timeout_ms=5_000)
if selected:
_wait_for_fsm(client, "GRAVITY_COMPENSATION")
return selected
result = _run_with_control(
client,
select_gravity_compensation,
cleanup_arm=True,
api=api,
)
_require_true(result, name)
emit("gravity compensation exited; arm idle requested")
return
if name == "switch_eef_control_mode":
def select_eef_idle() -> bool:
selected = client.switch_eef_control_mode(
api.EEFControlMode.idle,
timeout_ms=6_000,
)
if selected:
_wait_for_eef_mode(client, "idle")
return selected
result = _run_with_control(
client,
select_eef_idle,
cleanup_eef=True,
api=api,
)
_require_true(result, name)
emit("EEF idle confirmed")
return
if name == "set_arm_emergency_stop":
_require_true(client.set_arm_emergency_stop(True), name)
emit("software emergency-stop request accepted; reset was not sent")
return
if name == "clear_error":
_require_unknown_error(client)
def clear_unknown_error() -> bool:
cleared = client.clear_error()
if cleared:
_wait_for_fsm(client, "IDLE")
return cleared
result = _run_with_control(client, clear_unknown_error, api=api)
_require_true(result, name)
emit("error-clear request completed")
return
if name == "clear_arm_motor_err":
_require_unknown_error(client)
_require_arm_motor_errors(client)
def clear_arm_errors() -> bool:
cleared = client.clear_arm_motor_err(retry=False)
if cleared:
_wait_for_fsm(client, "IDLE")
return cleared
result = _run_with_control(
client,
clear_arm_errors,
api=api,
)
_require_true(result, name)
emit("arm motor error-clear request completed")
return
if name == "clear_eef_motor_err":
try:
result = _run_with_control(client, client.clear_eef_motor_err, api=api)
except api.UnsupportedOperationError as error:
emit(f"unsupported by the current backend: {error}")
return
_require_true(result, name)
emit("EEF motor error-clear request completed")
return
if name == "return_zero":
_require_idle_snapshot(client)
result = _run_with_control(
client,
lambda: client.return_zero(timeout_ms=35_000),
cleanup_arm=True,
api=api,
)
_require_true(result, name)
emit("zero-position move completed; arm idle requested")
return
if name in MOTION_EXAMPLES:
emit({name: _run_motion(client, api, name, inputs)})
return
raise AssertionError(name)
def run_example(
name: str,
*,
client_factory: Callable[..., Any],
api: Any,
client_kwargs: dict[str, Any],
confirmation: str | None = None,
inputs: RunInputs = RunInputs(),
emit: Callable[[Any], None] = print,
) -> None:
"""Run one named public-interface example."""
if name not in PUBLIC_EXAMPLES:
raise ValueError(f"unknown example {name!r}")
_require_confirmation(name, confirmation)
_validate_run_inputs(name, inputs)
if name == "__init__":
client = client_factory(**client_kwargs)
try:
emit(client)
finally:
client.close()
return
if name == "__enter__":
client = client_factory(**client_kwargs)
with client as entered:
emit(entered)
return
if name == "__exit__":
client = client_factory(**client_kwargs)
client.__enter__()
result = client.__exit__(None, None, None)
emit({"suppressed_exception": bool(result)})
return
if name == "close":
client = client_factory(**client_kwargs)
client.close()
emit("client closed")
return
with client_factory(**client_kwargs) as client:
_run_connected_example(client, api, name, inputs, emit)
def _public_api() -> Any:
from arm_p7_sdk import (
AirbotClient,
CartesianMoveOptions,
CartesianPose,
CartesianWaypointsMoveOptions,
Controller,
EEFControlMode,
EEFMoveOptions,
JointMoveOptions,
JointWaypointsMoveOptions,
UnsupportedOperationError,
)
return SimpleNamespace(
AirbotClient=AirbotClient,
CartesianMoveOptions=CartesianMoveOptions,
CartesianPose=CartesianPose,
CartesianWaypointsMoveOptions=CartesianWaypointsMoveOptions,
Controller=Controller,
EEFControlMode=EEFControlMode,
EEFMoveOptions=EEFMoveOptions,
JointMoveOptions=JointMoveOptions,
JointWaypointsMoveOptions=JointWaypointsMoveOptions,
UnsupportedOperationError=UnsupportedOperationError,
)
def main() -> None:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("example", nargs="?", choices=PUBLIC_EXAMPLES)
parser.add_argument("--confirm", help="required token for state-changing examples")
parser.add_argument("--list", action="store_true", help="list every example and token")
parser.add_argument("--backend", choices=("grpc", "dds"), default="grpc")
parser.add_argument("--host", help="P7 gRPC host; required for --backend grpc")
parser.add_argument("--port", type=int, default=50071)
parser.add_argument("--domain-id", type=int, help="required for --backend dds")
parser.add_argument("--side", choices=("none", "left", "right"), default="none")
parser.add_argument("--cartesian-step-m", type=float)
parser.add_argument("--eef-target-mm")
parser.add_argument("--eef-eff")
parser.add_argument("--eef-speed-mm-s", type=float, default=50.0)
args = parser.parse_args()
if args.list:
for name in PUBLIC_EXAMPLES:
token = CONFIRMATION_TOKENS.get(name, "-")
print(f"{name:34} confirm={token}")
return
if args.example is None:
parser.error("choose an example or use --list")
try:
api = _public_api()
run_example(
args.example,
client_factory=api.AirbotClient,
api=api,
client_kwargs=client_options_from_args(args),
confirmation=args.confirm,
inputs=inputs_from_args(args),
)
except (ExampleError, ValueError, ConnectionError) as error:
raise SystemExit(f"example failed: {error}") from error
if __name__ == "__main__":
main()
