#!/usr/bin/env python3 """RDK Free-Drive bridge (must run under rdk_py310 / Python 3.10). Why soft-impedance drag ----------------------- Under AUTO (Remote) the Studio Free Drive button is unavailable (remote owns the robot). RDK FloatingJoint/FloatingCartesian often need an extra license. So we default to soft NRT_JOINT_IMPEDANCE: low stiffness + target tracks measured joints → arm can be pushed by hand while Remote stays on. IMPORTANT: flexivrdk.Robot is NOT thread-safe. All robot API calls (including the drag loop) share one lock. Joint reads during drag use a cache so Enter does not race Stop()/SendJointPosition. Line protocol (JSON on stdout only; RDK logs → stderr): {"cmd":"ping"} {"cmd":"connect","sn":"Rizon4-063989"} {"cmd":"drag_on","mode":"soft"} # soft | floating {"cmd":"drag_off"} {"cmd":"joints"} {"cmd":"quit"} """ from __future__ import annotations import json import sys import threading import time from typing import List, Optional _JSON_OUT = sys.stdout sys.stdout = sys.stderr def reply(obj: dict) -> None: _JSON_OUT.write(json.dumps(obj, ensure_ascii=False) + '\n') _JSON_OUT.flush() def log(msg: str) -> None: print(f'[rdk_bridge] {msg}', file=sys.stderr, flush=True) class SoftDrag: def __init__(self, robot, flexivrdk, lock: threading.Lock) -> None: self.robot = robot self.flexivrdk = flexivrdk self._lock = lock self._stop = threading.Event() self._thread: Optional[threading.Thread] = None self.latest_q: List[float] = [] self._groups = {} def start(self) -> None: mode = self.flexivrdk.Mode robot = self.robot with self._lock: robot.SwitchMode(mode.NRT_JOINT_IMPEDANCE) st = robot.states() self.latest_q = [float(v) for v in list(st.q)[:7]] try: info = robot.info() self._groups = dict(getattr(info, 'single_arm_groups', None) or {}) except Exception as exc: log(f'info.single_arm_groups: {exc}') self._groups = {} if self._groups: for g in self._groups: try: nom = list(info.K_q_nom[g]) soft = [max(8.0, 0.08 * float(v)) for v in nom] robot.SetJointImpedance(g, soft) log(f'SetJointImpedance {g} K={soft}') except Exception as exc: log(f'SetJointImpedance group {g}: {exc}') else: n = max(7, len(self.latest_q)) try: robot.SetJointImpedance([35.0] * n, [0.7] * n) except Exception as exc: log(f'SetJointImpedance flat: {exc}') self._stop.clear() self._thread = threading.Thread(target=self._loop, name='soft-drag', daemon=True) self._thread.start() def _loop(self) -> None: robot = self.robot flexivrdk = self.flexivrdk while not self._stop.is_set(): try: with self._lock: if self._stop.is_set(): break st = robot.states() q = [float(v) for v in list(st.q)[:7]] self.latest_q = q if self._groups: cmds = {} for g in self._groups: try: gq = list(st[g].q) except Exception: gq = q n = len(gq) cmds[g] = flexivrdk.NrtJointPositionCmd( gq, [0.0] * n, [1.5] * n, [2.0] * n ) robot.SendJointPosition(cmds) else: n = len(q) robot.SendJointPosition(q, [0.0] * n, [1.5] * n, [2.0] * n) except Exception as exc: log(f'soft loop: {exc}') time.sleep(0.15) # ~20 Hz — keep lock released most of the time for _ in range(10): if self._stop.is_set(): return time.sleep(0.005) def stop(self) -> None: self._stop.set() th = self._thread if th is not None: th.join(timeout=4.0) if th.is_alive(): log('soft loop still alive after join (will idle anyway)') self._thread = None def _ensure_operational(robot) -> Optional[str]: try: if robot.fault(): robot.ClearFault() time.sleep(0.5) if not robot.operational(): for fn in ('ServoOn', 'Enable'): if hasattr(robot, fn): try: getattr(robot, fn)() break except Exception: pass t0 = time.monotonic() while not robot.operational() and time.monotonic() - t0 < 10: time.sleep(0.2) if not robot.operational(): return ( 'Robot not operational (fault/E-Stop?). ' 'Clear fault on pendant, release E-Stop, AUTO Remote.' ) except Exception as exc: return str(exc) return None def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> bool: """Switch to IDLE without blocking the stdin loop forever.""" done = threading.Event() def run() -> None: try: acquired = lock.acquire(timeout=2.0) try: if acquired: try: robot.Stop() except Exception as exc: log(f'Stop: {exc}') try: robot.SwitchMode(flexivrdk.Mode.IDLE) except Exception as exc: log(f'SwitchMode IDLE: {exc}') finally: if acquired: lock.release() finally: done.set() threading.Thread(target=run, name='idle', daemon=True).start() ok = done.wait(timeout) if not ok: log('IDLE/Stop timed out — stdin loop continues') return ok def main() -> int: import flexivrdk robot = None drag_on = False soft: Optional[SoftDrag] = None rlock = threading.Lock() cached_q: List[float] = [] for line in sys.stdin: line = line.strip() if not line: continue try: msg = json.loads(line) except Exception as exc: # noqa: BLE001 reply({'ok': False, 'error': f'bad json: {exc}'}) continue cmd = str(msg.get('cmd', '')).strip() try: if cmd == 'ping': reply({'ok': True, 'py': sys.version.split()[0]}) elif cmd == 'connect': sn = str(msg.get('sn', 'Rizon4-063989')) iface = str(msg.get('iface') or '') robot = None last_err = None attempts = [] if iface: attempts.extend([(sn, [iface]), (sn, [iface], True)]) attempts.append((sn,)) for args in attempts: try: robot = flexivrdk.Robot(*args) log(f'connected {sn} args={args!r}') last_err = None break except TypeError: continue except Exception as exc: last_err = exc log(f'connect {args!r}: {exc}') robot = None if robot is None: reply({'ok': False, 'error': str(last_err or 'connect failed')}) continue err = _ensure_operational(robot) if err: reply({'ok': False, 'error': err}) continue with rlock: q = [float(v) for v in list(robot.states().q)[:7]] cached_q = q reply({'ok': True, 'sn': sn, 'q': q}) elif cmd == 'drag_on': if robot is None: reply({'ok': False, 'error': 'not connected'}) continue err = _ensure_operational(robot) if err: reply({'ok': False, 'error': err}) continue prefer = str(msg.get('mode', 'soft')).strip().lower() if prefer in ('floating', 'primitive', 'floatingjoint'): name = str(msg.get('primitive', 'FloatingJoint')).replace('()', '') try: with rlock: robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION) robot.ExecutePrimitive(name, {}) drag_on = True reply({'ok': True, 'drag': True, 'mode': 'floating', 'primitive': name}) continue except Exception as exc: log(f'Floating failed, fallback soft: {exc}') try: if soft is not None: soft.stop() soft = SoftDrag(robot, flexivrdk, rlock) soft.start() if soft.latest_q: cached_q = list(soft.latest_q) drag_on = True log('soft drag ON') reply({ 'ok': True, 'drag': True, 'mode': 'soft', 'hint': 'Push the arm gently; low joint impedance. Enter=photo (s=hold).', }) except Exception as exc: reply({'ok': False, 'error': f'soft drag failed: {exc}'}) elif cmd == 'drag_off': log('drag_off requested') if soft is not None: if soft.latest_q: cached_q = list(soft.latest_q) soft.stop() soft = None if robot is not None: _idle_async(robot, rlock, flexivrdk, timeout=4.0) drag_on = False log('drag_off done') reply({'ok': True, 'drag': False, 'q': cached_q}) elif cmd == 'joints': q: Optional[List[float]] = None if soft is not None and len(soft.latest_q) >= 7: q = list(soft.latest_q) elif len(cached_q) >= 7: q = list(cached_q) if q is None or len(q) < 7: reply({'ok': False, 'error': 'no joint cache'}) continue cached_q = q reply({'ok': True, 'q': q, 'drag': drag_on}) elif cmd == 'quit': if soft is not None: soft.stop() soft = None if robot is not None and drag_on: _idle_async(robot, rlock, flexivrdk, timeout=3.0) reply({'ok': True}) return 0 else: reply({'ok': False, 'error': f'unknown cmd {cmd}'}) except Exception as exc: # noqa: BLE001 log(f'cmd={cmd} error: {exc}') reply({'ok': False, 'error': str(exc)}) return 0 if __name__ == '__main__': raise SystemExit(main())