贴面后不再抬回悬高;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:
@@ -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()
|
||||
|
||||
Reference in New Issue
Block a user