贴面后不再抬回悬高;MoveL 切回关节后不再把整段位移当前进速度。

搜孔开局会抬到悬高再落回 hole_hover_z_m,螺纹还没咬合臂就离开孔口。没有接触传感器,到位只认 final_gap 算出的法兰高度,贴面后停在这个 Z。09:25 厂家 MoveL 切回 NRT_JOINT_POSITION 的第一拍把整段关节差算成 1.686 rad/s,前瞻再推 0.07 rad,光心从 5 mm 冲到 276 mm 后急停。切回后钉住结束瞬间的关节角并清零速度,JTC 还指着出发前的关节时不放行。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-10-19 11:30:49 +08:00
parent 8c4dccccb1
commit 7af9e3d4b6
103 changed files with 5594 additions and 2498 deletions
-123
View File
@@ -33,7 +33,6 @@ RDK 的 FloatingJoint/FloatingCartesian 通常需要额外授权。
{"cmd":"drag_on","mode":"soft"} # soft | floating
{"cmd":"drag_off"}
{"cmd":"joints"}
{"cmd":"move_joints","q":[0,0,0,0,0,0,0],"speed":0.15}
{"cmd":"quit"}
每个命令的应答都是单行 JSON,含 ``ok`` 字段;失败时另有 ``error``。
@@ -170,69 +169,6 @@ def _ensure_operational(robot) -> Optional[str]:
return ensure_operational(robot)
def _restore_tracking_impedance(robot, flexivrdk) -> None:
"""把关节阻抗从软拖拽拉回名义值,否则回放点位时臂跟不住指令。"""
try:
info = robot.info()
groups = dict(getattr(info, 'single_arm_groups', None) or {})
except Exception as exc:
log(f'读名义阻抗失败: {exc}')
groups = {}
if groups:
for group in groups:
try:
nominal = [float(v) for v in list(info.K_q_nom[group])]
robot.SetJointImpedance(group, nominal)
log(f'恢复阻抗 {group} K={nominal}')
except Exception as exc:
log(f'恢复阻抗 {group}: {exc}')
return
try:
robot.SetJointImpedance([350.0] * 7, [7.0] * 7)
except Exception as exc:
log(f'恢复阻抗: {exc}')
def _send_joint_target(robot, flexivrdk, target: List[float], grouped: Optional[bool]) -> bool:
"""下发 7 关节目标。返回是否走了分组接口。"""
if grouped is True:
_send_grouped_joint_target(robot, flexivrdk, target)
return True
try:
n = len(target)
robot.SendJointPosition(target, [0.0] * n, [1.5] * n, [2.0] * n)
return False
except Exception as exc:
if grouped is False:
raise
log(f'扁平关节指令失败,改分组: {exc}')
_send_grouped_joint_target(robot, flexivrdk, target)
return True
def _send_grouped_joint_target(robot, flexivrdk, target: List[float]) -> None:
info = robot.info()
groups = dict(getattr(info, 'single_arm_groups', None) or {})
if not groups:
raise RuntimeError('没有分组,扁平关节指令也失败了')
states = robot.states()
cmds = {}
placed = False
for group in groups:
try:
current = [float(v) for v in list(states[group].q)]
except Exception:
current = list(target)
use = target if (not placed and len(current) == len(target)) else current
if use is target:
placed = True
n = len(use)
cmds[group] = flexivrdk.NrtJointPositionCmd(use, [0.0] * n, [1.5] * n, [2.0] * n)
if not placed:
raise RuntimeError('没有一组关节数和示教点一致,拒绝下发')
robot.SendJointPosition(cmds)
def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> bool:
"""切到 IDLE,同时避免把 stdin 循环永久阻塞住。"""
done = threading.Event()
@@ -487,65 +423,6 @@ def main() -> int:
info['tool'] = ''
reply(info)
elif cmd == 'move_joints':
if robot is None:
reply({'ok': False, 'error': '尚未连接'})
continue
raw = msg.get('q')
if not isinstance(raw, list) or len(raw) < 7:
reply({'ok': False, 'error': '需要 7 个关节角(弧度)'})
continue
try:
target = [float(v) for v in raw[:7]]
except (TypeError, ValueError):
reply({'ok': False, 'error': '关节角不是数字'})
continue
if any(v != v or abs(v) > 12.6 for v in target):
reply({'ok': False, 'error': '关节角不是有限值,或超出约两圈'})
continue
try:
speed = float(msg.get('speed', 0.15))
except (TypeError, ValueError):
speed = 0.15
speed = max(0.05, min(0.35, speed))
if soft is not None:
soft.stop()
soft = None
drag_on = False
err = _ensure_operational(robot)
if err:
reply({'ok': False, 'error': err})
continue
try:
with rlock:
robot.SwitchMode(flexivrdk.Mode.NRT_JOINT_IMPEDANCE)
current = [float(v) for v in list(robot.states().q)[:7]]
_restore_tracking_impedance(robot, flexivrdk)
max_delta = max(abs(a - b) for a, b in zip(current, target))
step_s = 0.05
steps = max(1, int(max_delta / (speed * step_s)))
if steps * step_s > 150.0:
reply({
'ok': False,
'error': f'路程太长({max_delta:.2f} rad),请先拖近一点再回放',
})
continue
grouped: Optional[bool] = None
for index in range(1, steps + 1):
alpha = index / steps
sample = [a + (b - a) * alpha for a, b in zip(current, target)]
with rlock:
grouped = _send_joint_target(robot, flexivrdk, sample, grouped)
time.sleep(step_s)
for _hold in range(8):
with rlock:
grouped = _send_joint_target(robot, flexivrdk, target, grouped)
time.sleep(step_s)
cached_q = list(target)
reply({'ok': True, 'q': target, 'from': current, 'steps': steps, 'speed': speed})
except Exception as exc:
reply({'ok': False, 'error': f'回点失败: {exc}'})
elif cmd == 'quit':
if soft is not None:
soft.stop()