#!/usr/bin/env bash
# MuJoCo O12 T3 右手仿真桥（OmniHand Pro 2025 ROS2 接口）
# URDF 来源: /home/lxqs/下载/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260909_112041.urdf
# （包内: urdf/O12/linker_hand_o12_right/linkerhand_o12_right.urdf）
# 订阅 /o12/right/joint_cmd（12 路弧度）
# 发布 /o12/right/joint_states（12 路弧度）
#
# 用法:
#   ./hands/O12/scripts/run_o12_right_mujoco_plan.sh              # 默认 --pd-drive（可见接触力，按 F）
#   ./hands/O12/scripts/run_o12_right_mujoco_plan.sh --no-pd-drive  # 纯 kinematic，最稳
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)"
cd "$ROOT"

set +u
# shellcheck disable=SC1091
source /opt/ros/jazzy/setup.bash
if [[ -f "$ROOT/install/setup.bash" ]]; then
  # shellcheck disable=SC1091
  source "$ROOT/install/setup.bash"
fi
set -u

VENV_SITE="$ROOT/venv/lib/python3.12/site-packages"
if [[ -d "$VENV_SITE" ]]; then
  export PYTHONPATH="$VENV_SITE${PYTHONPATH:+:$PYTHONPATH}"
fi

PKG_SRC="$ROOT/src/linkerhand-sim/linker_hand_mujoco_ros2"
export PYTHONPATH="$PKG_SRC${PYTHONPATH:+:$PYTHONPATH}"

XML="$PKG_SRC/linker_hand_mujoco_ros2/urdf/O12/linker_hand_o12_right/linker_hand_o12_right.xml"
URDF="$PKG_SRC/linker_hand_mujoco_ros2/urdf/O12/linker_hand_o12_right/linkerhand_o12_right.urdf"
if [[ ! -f "$XML" ]]; then
  echo "[error] missing O12 MJCF: $XML" >&2
  exit 1
fi

echo "[o12-right] urdf=$URDF"
echo "[o12-right] xml=$XML"
echo "[o12-right] cmd-topic=/o12/right/joint_cmd (12 rad)"
echo "[o12-right] state-topic=/o12/right/joint_states (12 rad)"
echo "[o12-right] default mode=pd-drive (viewer 按 F 看接触力; 默认 slew=4 rad/s 防穿模)"
echo "[o12-right] order (API_CPP_O12): thumb_roll,yaw,pitch,mcp,idx_roll,idx_pitch,idx_pip,mid_roll,mid_pitch,mid_pip,ring,pinky"

exec python3 -m linker_hand_mujoco_ros2.o12_plan_bridge \
  --xml "$XML" \
  --node-name o12_mujoco_plan_bridge_right \
  --window-title "O12 T3 right" \
  --cmd-topic /o12/right/joint_cmd \
  --sim-state-topic /o12/right/joint_states \
  --no-legacy-sdk \
  --mapping g20_urdf \
  "$@"