🤝 Final project: assistant robot

# Assistant robot: text (from voice) -> JSON plan (LLM) -> validation -> execution.
# --simular mode: no LLM and no hardware, for testing the logic and safety locks.
import argparse, json, subprocess, sys, time, urllib.request

OBJETOS = {"cubo": "outputs/train/act-cubo/checkpoints/last/pretrained_model",
           "bola": "outputs/train/act-bola/checkpoints/last/pretrained_model"}
SOLTAR = "outputs/train/act-soltar/checkpoints/last/pretrained_model"
LUGARES = ["mesa", "sala", "base"]
ACOES = {  # closed list: name -> allowed arguments
    "ir_para": {"lugar": LUGARES},
    "pegar": {"objeto": list(OBJETOS)},
    "entregar": {},
    "falar": {"texto": None},
    "parar": {},
}
MAX_PASSOS = 6

PROMPT = ("You control a robot with an arm and mobile base. Respond ONLY with JSON in the format "
          ' {"passos": [{"acao": "...", ...}]}. Allowed actions: ' + json.dumps(ACOES, ensure_ascii=False) +
          ". If the request doesn't fit these actions, respond with a single falar action explaining why.")

def planejar_simulado(texto):
    t = texto.lower()
    if "pare" in t or "parar" in t:
        return {"passos": [{"acao": "parar"}]}
    for obj in OBJETOS:
        if obj in t:
            return {"passos": [{"acao": "ir_para", "lugar": "mesa"}, {"acao": "pegar", "objeto": obj},
                               {"acao": "ir_para", "lugar": "sala"}, {"acao": "entregar"},
                               {"acao": "falar", "texto": f"Done, I brought you: {obj}."}]}
    if "janela" in t:  # simulates an LLM that makes up an action outside the list
        return {"passos": [{"acao": "abrir_janela"}]}
    return {"passos": [{"acao": "falar", "texto": "I don't know how to do that."}]}

def planejar_ollama(texto, modelo):
    corpo = json.dumps({"model": modelo, "stream": False, "format": "json", "options": {"temperature": 0},
                        "messages": [{"role": "system", "content": PROMPT}, {"role": "user", "content": texto}]})
    req = urllib.request.Request("http://localhost:11434/api/chat", corpo.encode(), {"Content-Type": "application/json"})
    with urllib.request.urlopen(req, timeout=60) as r:
        return json.loads(json.loads(r.read())["message"]["content"])

def validar(plano):
    passos = plano.get("passos") if isinstance(plano, dict) else None
    if not isinstance(passos, list) or not 1 <= len(passos) <= MAX_PASSOS:
        return None, "empty or too long a plan"
    for p in passos:
        regra = ACOES.get(p.get("acao"))
        if regra is None:
            return None, f"action not on the list: {p.get('acao')}"
        for arg, permitidos in regra.items():
            if arg not in p:
                return None, f"missing '{arg}' in {p['acao']}"
            if permitidos is not None and p[arg] not in permitidos:
                return None, f"value not allowed: {arg}={p[arg]}"
    return passos, "ok"

def executar(passo, simular, porta_base):
    a = passo["acao"]
    if simular:
        print("  [SIM]", json.dumps(passo, ensure_ascii=False)); time.sleep(0.2); return True
    if a in ("pegar", "entregar"):  # arm: policies trained in 5.4
        politica = OBJETOS[passo["objeto"]] if a == "pegar" else SOLTAR
        tarefa = f"Pick up the {passo['objeto']}." if a == "pegar" else "Place the object in the person's hand."
        cmd = ["lerobot-rollout", "--strategy.type=base", f"--policy.path={politica}",
               "--robot.type=so101_follower", "--robot.port=/dev/ttyACM0", "--robot.id=seguidor",
               f"--task={tarefa}", "--duration=20"]
        return subprocess.run(cmd).returncode == 0
    if a in ("ir_para", "parar"):  # mobile base: ESP32 from Track 3 over serial
        import serial
        with serial.Serial(porta_base, 115200, timeout=30) as s:
            s.write((f"{a.upper()} {passo.get('lugar', '')}".strip() + "\n").encode())
            return s.readline().decode().strip() == "OK"
    if a == "falar":
        print("  ROBÔ:", passo["texto"]); return True  # replace with Piper from 5.2
    return False

def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--texto", required=True, help="request (comes from Whisper in 5.2)")
    ap.add_argument("--simular", action="store_true")
    ap.add_argument("--modelo", default="<seu_modelo_ollama>")
    ap.add_argument("--porta-base", default="/dev/ttyUSB0")
    args = ap.parse_args()
    print("PEDIDO:", args.texto)
    plano = planejar_simulado(args.texto) if args.simular else planejar_ollama(args.texto, args.modelo)
    passos, motivo = validar(plano)
    if passos is None:
        print("RECUSADO:", motivo); sys.exit(2)
    print(f"PLAN ({len(passos)} steps):", [p["acao"] for p in passos])
    if not args.simular and input("Confirm execution? (y/n) ").strip().lower() != "s":
        print("CANCELED by operator"); sys.exit(0)
    for i, p in enumerate(passos, 1):
        print(f"step {i}/{len(passos)}: {p['acao']}")
        if not executar(p, args.simular, args.porta_base):
            print("FAILED at step", i, "- parando"); executar({"acao": "parar"}, args.simular, args.porta_base); sys.exit(1)
    print("COMPLETED")

if __name__ == "__main__":
    main()

// Assistant robot base (ESP32, esp32 package 3.x): line-based protocol with the orchestrator.
const unsigned long LIMITE_MS = 15000;   // no trip can take longer than this: stop automatically

void pararMotores() { /* call your rover's stop function here (set both wheels to speed 0) */ }
void andarRumo(const String &lugar) { /* fixed route, line follower, or Nav2 waypoint */ }
bool chegou(unsigned long t0) { return millis() - t0 > 2000; }  // simulation: 2-second trip

bool irPara(const String &lugar) {
  if (lugar != "mesa" && lugar != "sala" && lugar != "base") return false;
  unsigned long t0 = millis();
  andarRumo(lugar);
  while (!chegou(t0)) {
    if (Serial.available() && Serial.readStringUntil('\n').startsWith("PARAR")) { pararMotores(); return false; }
    if (millis() - t0 > LIMITE_MS) { pararMotores(); return false; }
    delay(10);
  }
  pararMotores();
  return true;
}

void setup() { Serial.begin(115200); pararMotores(); Serial.println("BASE READY"); }

void loop() {
  if (!Serial.available()) return;
  String linha = Serial.readStringUntil('\n');
  linha.trim();
  if (linha.startsWith("IR_PARA ")) Serial.println(irPara(linha.substring(8)) ? "OK" : "ERRO");
  else if (linha == "PARAR") { pararMotores(); Serial.println("OK"); }
  else Serial.println("ERRO comando desconhecido");
}