Move a Plate

import asyncio
from dataclasses import dataclass

from unitelabs.sdk import AsyncApiClient

DEVICE = "PF400"  # the arm's name on the Devices page

# Motion profiles: fixed slots built into the connector (0.4.0 and later)
SETTLE = 0  # 10% of rated speed, stops exactly on each point
BLEND = -4  # 25% of rated speed, flows through points without stopping

# The plate, as the jaw holds it
PLATE_WIDTH = 127.76  # mm between the jaws: an SBS plate, landscape
GRIP_SPEED = 20.0  # % of the jaw's top speed
GRIP_FORCE = 5.0  # N, the most the jaw squeezes


async def main():
    client = AsyncApiClient()

    # 1. Find the arm, and load what was taught in the Teach view
    device_id, arm = await find_arm(client, DEVICE)
    teaching = await load_teaching(client, device_id)

    # 2. Plan both moves first. A wrong station name fails here,
    #    while the arm is still at rest
    pick = plan(teaching, "A1")
    place = plan(teaching, "B1", coming_from="A1")

    # 3. The arm only moves once it is homed
    state = await read_state(arm)
    if not state["Homed"]:
        await arm.movement_controller.home()

    # 4. Pick the plate up at A1 and put it down at B1
    pick_up, put_down = await transport_actions(arm)
    grip = dict(width=PLATE_WIDTH, grip_speed=GRIP_SPEED)
    await pick_up(**pick, **grip, force=GRIP_FORCE)
    await put_down(**place, **grip)
    print("Plate moved from A1 to B1")


@dataclass
class Teaching:
    points: dict  # point name -> joint values
    stations: dict  # station -> point names, nest first, safe point last
    spine: list  # safe point names, in the order the arm passes them


def plan(teaching, station, coming_from=None):
    """The path in to a station's nest and back out again."""
    path = teaching.stations[station]
    nest = path[0]
    approach = path[1]
    way_out = path[2:]  # any transitions, then the safe point
    way_in = list(reversed(way_out))

    # Coming from another station, travel along the safe spine first
    if coming_from:
        start = teaching.stations[coming_from][-1]  # its safe point
        way_in = spine_between(teaching.spine, start, way_out[-1]) + way_in

    joints = teaching.points
    return dict(
        approach_path=[joints[name] for name in way_in],
        approach_path_profile=BLEND,
        approach=joints[approach],
        approach_profile=SETTLE,
        target=joints[nest],
        target_profile=SETTLE,
        retract=joints[approach],
        retract_profile=SETTLE,
        retract_path=[joints[name] for name in way_out],
        retract_path_profile=BLEND,
    )


def spine_between(spine, start, end):
    """The safe points between two safe points, in travel order."""
    i, j = spine.index(start), spine.index(end)
    if i < j:
        return spine[i + 1 : j]
    return list(reversed(spine[j + 1 : i]))


async def load_teaching(client, device_id):
    """Every teachpoint of the arm, grouped into stations and the spine."""
    points, names = {}, {}
    url = f"/devices/{device_id}/teach-points"
    for point in await get_all(client, url):
        # The platform says "column": 0, the connector wants "Column": 0.0
        joints = {}
        for axis, value in point["joints"].items():
            joints[axis.capitalize()] = float(value)
        points[point["name"]] = joints
        names[point["id"]] = point["name"]

    # A group lists its points in path order. A station is a group
    # named after its nest; the spine is the group of kind SAFE
    stations, spine = {}, []
    url = f"/devices/{device_id}/teach-groups"
    for group in await get_all(client, url):
        members = sorted(group["members"], key=lambda m: m["position"])
        path = [names[m["teachPointId"]] for m in members]
        if group["kind"] == "NEST":
            stations[group["name"].removeprefix("NEST_")] = path
        elif group["kind"] == "SAFE":
            spine = path

    # On an arm with a rail, the spine runs along the rail
    if spine and all("Rail" in points[name] for name in spine):
        spine.sort(key=lambda name: points[name]["Rail"])
    return Teaching(points, stations, spine)


async def find_arm(client, name):
    """The device with this name, and the connector that drives it."""
    found = await client.get("/devices", params={"name": name})
    if not found["data"]:
        raise RuntimeError(f"No device named {name!r}")
    device_id = found["data"][0]["id"]
    services = await client.get(f"/devices/{device_id}/services")
    if not services:
        raise RuntimeError(f"{name!r} has no connector")
    return device_id, await client.get_service(services[0]["id"])


async def transport_actions(arm):
    """Pick-up and put-down. On a rail arm they end in "_with_rail"."""
    rail = await arm.information_provider.get_rail_installed()
    suffix = "_with_rail" if rail else ""
    actions = arm.transport_controller.actions
    pick_up = actions["pick_up_joints" + suffix]
    put_down = actions["put_down_joints" + suffix]
    return pick_up, put_down


async def read_state(arm):
    """Powered, homed and idle? The first event of the stream is empty."""
    states = await arm.information_provider.subscribe_state()
    async with states:
        async for state in states:
            if "Homed" in state:
                return state
    raise RuntimeError("The state stream closed without a state")


async def get_all(client, url):
    """One page of up to 100 entries; refuse to drop any silently."""
    page = await client.get(url, params={"_take": 100})
    if page["pagination"]["hasMore"]:
        raise RuntimeError(f"More than 100 entries at {url}")
    return page["data"]


asyncio.run(main())