Skip to content

armgit / moss

Public Imported from GitHub
0

MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.

Python
7.8 KB
Raw Blame
#!/usr/bin/env python3
"""Play the pick-and-drop cycle on the arm through the running `moss-arm`
coordinator (simulation or robot), watching the arm follow.

    dimos --simulation mujoco run moss-dimos.moss-arm        # one terminal: the simulator and the coordinator
    tools/moss_arm_play.py --sim                             # another: the cycle, from wherever the arm is
    tools/moss_arm_play.py --sim --stop-after release        # up to the release: the arm waits over the bin
    tools/moss_arm_play.py --sim --dry-run                   # the trajectory's numbers, nothing sent

On the robot the same, without --sim, after `dimos run moss-dimos.moss-arm`
(the Feetech adapter on the arm's port). Nothing here checks the scene:
the cycle is the one tools/moss_arm_drop.py plan --mujoco verified in the CAD
scene, run as delivered. What this tool adds is the watch: every 50 ms it
reads the joints back and compares them with the trajectory; a joint more
than --max-lag ticks behind for longer than --lag-seconds cancels the
trajectory (the coordinator holds the last commanded position) and the run
ends with ARM LAG and the joint's numbers. That is the case of the shoulder
short of its target under load; the number is printed on every run, lag or
not, so the gap between the simulation and the arm is a measurement.

Options:
  --sim                 the gripper in the simulator's unit (see moss_dimos.arm)
  --poses PATH          the cycle (config/arm_drop_poses.json)
  --calib PATH          the calibration (the drive pose); MOSS_ARM_CALIB, else config/arm_calibration.json
  --speed TICKS_PER_S   the ceiling (400)
  --stop-after PHASE    end the cycle after this phase (e.g. release)
  --from PHASE          start the cycle at this phase (e.g. back_to_clear: the return from the bin)
  --pick-pose NAME      the pose played as "pick": pick (the video's, an object standing) or pick_low (lying on
                        the floor); MOSS_ARM_PICK_POSE
  --max-lag TICKS       the following error that counts as a lag (150 = 13 deg)
  --lag-seconds S       how long a lag must last before the stop (1.0)
  --no-goto             refuse to start unless the arm is at the cycle's first pose
  --dry-run             print the steps and the trajectory, send nothing
"""

from __future__ import annotations

import argparse
import math
import sys
import time
from pathlib import Path

ROOT = Path(__file__).resolve().parent.parent
sys.path.insert(0, str(ROOT))

from moss_dimos import arm as A  # noqa: E402

POLL_S = 0.05
DEG = 180.0 / math.pi


def say(*a):
    print(*a, flush=True)


def main() -> int:
    ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
    ap.add_argument("--sim", action="store_true")
    ap.add_argument("--poses", default=str(A.POSES_PATH))
    ap.add_argument("--calib", default=None)
    ap.add_argument("--speed", type=float, default=A.SPEED_TICKS_PER_S)
    ap.add_argument("--stop-after", default=None)
    ap.add_argument("--from", dest="start", default=None)
    ap.add_argument("--pick-pose", default=None)
    ap.add_argument("--max-lag", type=float, default=150.0)
    ap.add_argument("--lag-seconds", type=float, default=1.0)
    ap.add_argument("--no-goto", action="store_true")
    ap.add_argument("--dry-run", action="store_true")
    a = ap.parse_args()

    cycle = A.load_cycle(a.poses, calibration=a.calib, speed=a.speed, stop_after=a.stop_after, start=a.start,
                         pick_pose=a.pick_pose)
    say(f"cycle: {len(cycle.steps)} moves, {cycle.timed_s:.1f} s as timed, {cycle.duration_s:.1f} s at {a.speed:g} ticks/s, "
        f"pick pose {a.pick_pose or A.pick_pose_name()}")
    for s in cycle.steps:
        say(f"  {s.phase:14s} {s.start:>7s} -> {s.end:<7s} {s.duration_s:5.2f} s  gap {s.gap_from_m * 1000:.0f} -> {s.gap_to_m * 1000:.0f} mm"
            f"{'  (holds the can)' if s.holds_can else ''}")
    if a.dry_run:
        traj = A.cycle_trajectory(cycle.steps, sim=a.sim)
        say(f"trajectory: {len(traj.points)} points, {traj.duration:.2f} s, joints {traj.joint_names}")
        for p in traj.points[:: max(1, len(traj.points) // 12)]:
            say(f"  t {p.time_from_start:6.2f}  " + " ".join(f"{A.rad_to_ticks(v):5d}" for v in p.positions[:5]) + f"  gripper {p.positions[5]:.4f}")
        return 0

    from dimos.control.coordinator import ControlCoordinator
    from dimos.control.tasks.trajectory_task.trajectory_task import TrajectoryExecutionStatus
    from dimos.core.rpc_client import RPCClient

    client = RPCClient(None, ControlCoordinator, remote_name=A.ARM_COORDINATOR_NAME)
    try:
        now = client.get_joint_positions() or {}
        missing = [j for j in A.JOINT_NAMES if j not in now]
        if missing:
            say(f"refused: the coordinator does not report {missing} (is moss-arm running? joints: {sorted(now)})")
            return 2
        q_now = [now[f"{A.HW_ID}/{j}"] for j in A.ARM_JOINTS]
        g_raw = now[f"{A.HW_ID}/{A.GRIPPER}"]
        gap_now = A.sim_gripper_gap(g_raw) if a.sim else float(g_raw)
        say("arm now: " + " ".join(f"{j} {A.rad_to_ticks(v)}" for j, v in zip(A.ARM_JOINTS, q_now)) + f"  gap {gap_now * 1000:.0f} mm")
        steps = list(cycle.steps)
        off = max(abs(x - y) for x, y in zip(q_now, cycle.first_pose)) * A.TICKS_PER_RAD
        if off > A.START_TOLERANCE_RAD * A.TICKS_PER_RAD:
            if a.no_goto:
                say(f"refused: the arm is {off:.0f} ticks from the cycle's first pose ({steps[0].start}) and --no-goto is set")
                return 2
            goto = A.goto_step(q_now, gap_now, cycle.first_pose, cycle.first_gap_m, speed=a.speed)
            say(f"goto: {off:.0f} ticks from {steps[0].start}, {goto.duration_s:.1f} s first")
            steps.insert(0, goto)
        traj = A.cycle_trajectory(steps, sim=a.sim)
        res = client.execute_trajectory(traj)
        if res is None or res.status is not TrajectoryExecutionStatus.ACCEPTED:
            say(f"refused by the coordinator: {getattr(res, 'status', None)} {getattr(res, 'message', '')}")
            return 3
        say(f"running: {len(traj.points)} points, {traj.duration:.1f} s")
        t0 = time.time()
        worst: dict[str, float] = {}
        lag_since: dict[str, float] = {}
        last_line = 0.0
        while True:
            t = time.time() - t0
            cur = client.get_joint_positions() or {}
            err = A.following_error_ticks(traj, t, cur)
            for j, e in err.items():
                worst[j] = max(worst.get(j, 0.0), e)
                if e > a.max_lag:
                    lag_since.setdefault(j, t)
                    if t - lag_since[j] >= a.lag_seconds:
                        client.cancel_trajectory()
                        say(f"ARM LAG: {j} {e:.0f} ticks behind for {t - lag_since[j]:.1f} s at t = {t:.1f} s; "
                            f"trajectory cancelled, the coordinator holds the last commanded position")
                        _report(worst)
                        return 4
                else:
                    lag_since.pop(j, None)
            if t - last_line >= 2.0:
                last_line = t
                say(f"  t {t:5.1f} s  " + " ".join(f"{j.split('/')[-1]} {e:4.0f}" for j, e in err.items()))
            if t >= traj.duration + 0.3:
                break
            time.sleep(POLL_S)
        fin = client.get_joint_positions() or {}
        say("done: " + " ".join(f"{j} {A.rad_to_ticks(fin.get(f'{A.HW_ID}/{j}', float('nan')))}" for j in A.ARM_JOINTS))
        _report(worst)
        return 0
    finally:
        try:
            client.stop_rpc_client()
        except Exception:  # noqa: BLE001
            pass


def _report(worst: dict[str, float]) -> None:
    say("worst following error: " + ", ".join(f"{j.split('/')[-1]} {e:.0f} ticks ({e / A.TICKS_PER_RAD * DEG:.1f} deg)" for j, e in worst.items()))


if __name__ == "__main__":
    sys.exit(main())