chore: 框架科学优化前的完整基线
在开始'设备契约层 + 目录重构 + 通止规装配'改造之前，先钉住当前可运行状态， 使后续任何删除/重构都可回退。 本基线包含： - 现有 ROS 2 工作区全部源码（arm_application / arm_control / arm_bringup 等） - 官方 flexiv_ros2 栈（含 flexiv_hardware / flexiv_controllers 等符号链接） - 设备契约层 src/arm_interfaces（新增，46 项单测通过） - devices/ 设备选型与品牌清单（新增） - scripts/gen_site_example.py 与由它生成的 config/site.example.yaml 按 .gitignore 排除：build/ / install/ / log/ / config/site.yaml / 模型权重二进制。
This commit is contained in:
arm_dev
Executable
+329
@@ -0,0 +1,329 @@
|
||||
#!/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())
|
||||
Reference in New Issue
Block a user