工位操作改成桌面窗口：夹爪位置、力度、速度和旋转角可以调，阶段能分步或整体跑，并能软拖拽记点后回到该点。相机预览按窗口一次铺满，不再跟着每帧慢慢放大。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -33,6 +33,7 @@ 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``。
|
||||
@@ -169,6 +170,69 @@ 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()
|
||||
@@ -423,6 +487,65 @@ 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()
|
||||
|
||||
Executable
+17
@@ -0,0 +1,17 @@
|
||||
#!/usr/bin/env bash
|
||||
# Orisys 通止规工位桌面程序（窗口，不是网页）。
|
||||
#
|
||||
# bash ~/ros2/scripts/run_console.sh
|
||||
|
||||
set -eo pipefail
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/env.sh"
|
||||
export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
|
||||
if [[ -z "${DISPLAY:-}" && -S /tmp/.X11-unix/X1 ]]; then
|
||||
export DISPLAY=:1
|
||||
fi
|
||||
if [[ -z "${XAUTHORITY:-}" && -r "/run/user/$(id -u)/gdm/Xauthority" ]]; then
|
||||
export XAUTHORITY="/run/user/$(id -u)/gdm/Xauthority"
|
||||
fi
|
||||
exec python3 -m arm_application.operator_app
|
||||
+13
-1
@@ -12,7 +12,9 @@
|
||||
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open
|
||||
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli close
|
||||
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli rotate 90
|
||||
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --force 40 --speed 20 --cli rotate_by -15
|
||||
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli revolutions 2
|
||||
# --force / --speed 写在 --cli 前面，覆盖 site.yaml 里的力度和速度。
|
||||
# bash ~/ros2/scripts/run_gripper.sh --arm 89 # ROS 节点 PGEA
|
||||
# bash ~/ros2/scripts/run_gripper.sh --arm 91 # ROS 节点 RGI
|
||||
# bash ~/ros2/scripts/run_dual_gripper.sh # 两个 ROS 节点都起
|
||||
@@ -21,6 +23,8 @@ set -eo pipefail
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
CLI=()
|
||||
DUAL=0
|
||||
GRIP_FORCE=""
|
||||
GRIP_SPEED=""
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
||||
@@ -29,6 +33,8 @@ set -- "${PARSE_ARM_REST[@]}"
|
||||
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--force) GRIP_FORCE="${2:?}"; shift 2 ;;
|
||||
--speed) GRIP_SPEED="${2:?}"; shift 2 ;;
|
||||
--cli) shift; CLI=("$@"); break ;;
|
||||
--dual) DUAL=1; shift ;;
|
||||
-h|--help) sed -n '2,24p' "$0"; exit 0 ;;
|
||||
@@ -70,11 +76,17 @@ if [[ ${#CLI[@]} -gt 0 ]]; then
|
||||
ARGS=(--arm right --test rotate)
|
||||
if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--angle "${REST[0]}"); fi
|
||||
;;
|
||||
rotate_by)
|
||||
ARGS=(--arm right --test rotate_by)
|
||||
if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--angle "${REST[0]}"); fi
|
||||
;;
|
||||
revolutions)
|
||||
ARGS=(--arm right --test rotate)
|
||||
if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--revolutions "${REST[0]}"); fi
|
||||
;;
|
||||
esac
|
||||
if [[ -n "${GRIP_FORCE}" ]]; then ARGS+=(--force "${GRIP_FORCE}"); fi
|
||||
if [[ -n "${GRIP_SPEED}" ]]; then ARGS+=(--speed "${GRIP_SPEED}"); fi
|
||||
echo "=== DH RGI SN=${SITE_GRIP_SN:-?} ${SITE_GRIP_PORT} arm=${SITE_ROBOT_SHORT_ID:-?} ==="
|
||||
exec python3 -m arm_application.dual_gripper_cli "${ARGS[@]}"
|
||||
fi
|
||||
@@ -84,7 +96,7 @@ if [[ ${#CLI[@]} -gt 0 ]]; then
|
||||
echo "=== DH ${MODEL} SN=${SITE_GRIP_SN:-?} ${PORT} @${BAUD} slave=${SLAVE} ==="
|
||||
exec python3 "${ROOT}/src/arm_application/arm_application/dh_pgea.py" \
|
||||
--port "${PORT}" --baud "${BAUD}" --slave "${SLAVE}" \
|
||||
--force "${SITE_GRIP_FORCE:-50}" --speed "${SITE_GRIP_SPEED:-50}" \
|
||||
--force "${GRIP_FORCE:-${SITE_GRIP_FORCE:-50}}" --speed "${GRIP_SPEED:-${SITE_GRIP_SPEED:-50}}" \
|
||||
--rts "${SITE_GRIP_RS485_RTS:-}" \
|
||||
"${CLI[@]}"
|
||||
fi
|
||||
|
||||
Reference in New Issue
Block a user