From 11978b53806db431e10c4e94b7ee25a40708b6de Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Tue, 7 Jul 2026 14:33:42 -0600 Subject: [PATCH 01/19] :sparkles: Add headless Apple-Silicon (arm64/CPU) sim port MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Standalone Colima-based bringup of ROS + MuJoCo/RoboCasa on Apple Silicon, headless and CPU-only (Isaac omitted). Verified end-to-end: both containers boot and exchange data (/clock ~86 Hz, /joint_states ~18 Hz, sim receives /lowcmd), with the Pink IK frame_task_server and safety_node running. New: - docker/docker-compose.mac.yml: standalone compose (no NVIDIA/X11/dri/input); robocasa + ros on network_mode:host, RMW=rmw_fastrtps_cpp. - docker/{Base,Robocasa}Dockerfile.arm64, docker/RosDockerfile.slim.arm64: arm64 images; slim ROS image drops the heavy x86 ML stack. - docker/scripts/launch_{robocasa,ros}_mac.sh: osmesa+headless sim launcher; slim 5-package colcon build + minimal bringup. - core_ws/.../h1_sim_bringup_mac.launch.py: core nodes only (state pubs, Pink IK, safety, camera TF) — no vision/Nav2/rviz. Two cross-container fixes needed on arm64/Colima: - ipc:host on both services. FastDDS prefers shared-memory transport for same-host peers; with private IPC namespaces (separate /dev/shm) discovery works over UDP but data is silently dropped (sim logs "Command timeout! Releasing motors"). x86 uses CycloneDDS/UDP and avoids this. - colcon --build-base/--install-base on named volumes (native ext4), not the virtiofs bind-mount, where --symlink-install stalls for minutes at 0% CPU. h12_ros2_model derives CL_Assets from its build dir, so CL_Assets is also mounted at /opt/CL_Assets for the ros build. Also: gitignore process core dumps; drop a stray blank line in h12_mujoco.py. Co-Authored-By: Claude Opus 4.8 --- .gitignore | 5 + .../launch/h1_sim_bringup_mac.launch.py | 74 +++++++++++ docker/BaseDockerfile.arm64 | 111 ++++++++++++++++ docker/RobocasaDockerfile.arm64 | 121 ++++++++++++++++++ docker/RosDockerfile.slim.arm64 | 100 +++++++++++++++ docker/docker-compose.mac.yml | 87 +++++++++++++ docker/scripts/launch_robocasa_mac.sh | 40 ++++++ docker/scripts/launch_ros_mac.sh | 45 +++++++ h1_robocasa/h12_mujoco.py | 1 - 9 files changed, 583 insertions(+), 1 deletion(-) create mode 100644 core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py create mode 100644 docker/BaseDockerfile.arm64 create mode 100644 docker/RobocasaDockerfile.arm64 create mode 100644 docker/RosDockerfile.slim.arm64 create mode 100644 docker/docker-compose.mac.yml create mode 100755 docker/scripts/launch_robocasa_mac.sh create mode 100755 docker/scripts/launch_ros_mac.sh diff --git a/.gitignore b/.gitignore index 2e7a3b0..ade1fe6 100644 --- a/.gitignore +++ b/.gitignore @@ -62,3 +62,8 @@ wandb/ # host-side caches for docker container build artefacts /container_cache/ + +# process crash dumps (e.g. h1_robocasa/core from unitree_sdk2py segfaults) +core +core.* +*.core diff --git a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py new file mode 100644 index 0000000..faf34e9 --- /dev/null +++ b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py @@ -0,0 +1,74 @@ +"""Minimal, CPU-only (Apple Silicon) bringup for the H1 MuJoCo sim. + +A trimmed variant of h1_sim_bringup.launch.py that runs ONLY the core robot +nodes — no vision foundation models (gemini/sam/graspgen), no h12_skills, no +Nav2/SLAM, no rviz/sliders. Those need the heavy x86-tuned ML stack that the +slim arm64 ROS image intentionally omits. + +Nodes started: + * static camera_link -> camera_color_optical_frame TF + * joint_state_publisher (h12_ros2_controller) + * robot_state_publisher + * frame_task_server (h12_ros2_controller, Pink IK) + * safety_node (h12_safety_layer) + +Launch by path from the slim ROS container: + ros2 launch /home/code/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py +""" +import os + +from launch import LaunchDescription +from launch_ros.actions import Node + +ASSETS_DIR = '/home/code/CL_Assets' + + +def generate_launch_description(): + with open(os.path.join(ASSETS_DIR, 'ros_assets', 'h1_2_magpie_ros.urdf'), 'r') as urdf_file: + robot_description = urdf_file.read() + + # MuJoCo publishes /clock with sim time; keep all nodes on it. + sim_time_param = {'use_sim_time': True} + + return LaunchDescription([ + Node( + package='tf2_ros', + executable='static_transform_publisher', + name='camera_optical_frame_broadcaster', + arguments=['0', '0', '0', + '-1.5707963267948966', '0', '-1.5707963267948966', + 'camera_link', 'camera_color_optical_frame'], + parameters=[sim_time_param], + output='screen', + ), + Node( + package='h12_ros2_controller', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[sim_time_param], + output='screen', + ), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[{'robot_description': robot_description}, sim_time_param], + output='screen', + ), + Node( + package='h12_ros2_controller', + executable='frame_task_server', + name='frame_task_server', + arguments=['--config', 'sim_safety_split.yaml'], + parameters=[sim_time_param], + output='screen', + ), + Node( + package='h12_safety_layer', + executable='safety_node', + name='safety_node', + arguments=['--config', 'sim_safety_split.yaml'], + parameters=[sim_time_param], + output='screen', + ), + ]) diff --git a/docker/BaseDockerfile.arm64 b/docker/BaseDockerfile.arm64 new file mode 100644 index 0000000..21ab02b --- /dev/null +++ b/docker/BaseDockerfile.arm64 @@ -0,0 +1,111 @@ +# ============================== +# Apple-Silicon (arm64) / CPU-only fork of BaseDockerfile. +# +# Shared base for the MuJoCo (RoboCasa) and ROS simulation environments, +# built to run natively under Colima on Apple Silicon — no NVIDIA GPU. +# +# Differences from the stock BaseDockerfile: +# * FROM ubuntu:22.04 (was nvidia/cuda:12.2.0-devel-ubuntu22.04) +# * PyTorch CPU wheels from PyPI (was cu130 from download.pytorch.org) +# Everything else is identical: Python 3.10 + ROS 2 Humble via apt, +# CycloneDDS 0.10.x from source, uv, unitree_sdk2_python, pin/pink/mink. +# +# Build: docker build -t hams_base:latest -f docker/BaseDockerfile.arm64 . +# ============================== +FROM ubuntu:22.04 + +ENV DEBIAN_FRONTEND=noninteractive +ENV TZ=America/Denver +ENV LANG=C.UTF-8 +ENV ROS_DISTRO=humble +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + +ARG http_proxy +ARG https_proxy +ENV http_proxy=${http_proxy} +ENV https_proxy=${https_proxy} + +# Superset of packages needed by both simulators: compilers, graphics, Python 3.10, +# locale setup, ROS repo prerequisites. (No CUDA base, so pull mesa software GL / +# OSMesa here for CPU rendering.) +RUN apt-get update && apt-get install -y --no-install-recommends \ + ca-certificates curl gnupg locales software-properties-common \ + gcc-12 g++-12 cmake build-essential git git-lfs wget unzip \ + python3 python3-dev python3-pip python-is-python3 \ + libglu1-mesa-dev vulkan-tools libvulkan1 \ + libx11-6 libxext6 libxrender1 libxi6 libxrandr2 libxcursor1 libxinerama1 \ + libgl1-mesa-glx libglib2.0-0 libsm6 libxt6 libxkbcommon-x11-0 \ + libegl1-mesa-dev libosmesa6-dev \ + && locale-gen en_US en_US.UTF-8 && update-locale LC_ALL=C.UTF-8 LANG=C.UTF-8 \ + && add-apt-repository universe \ + && update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-12 100 \ + && update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-12 100 \ + && rm -rf /var/lib/apt/lists/* + +# ROS 2 Humble apt repository. `dpkg --print-architecture` resolves to arm64 here, +# and packages.ros.org publishes Humble for arm64 (Ubuntu Jammy). +RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg \ + && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu jammy main" \ + > /etc/apt/sources.list.d/ros2.list + +# ROS 2 Humble packages needed by the sim processes (rclpy + sensor/TF publishers). +# ros-humble-ros-base brings rclpy, rclcpp, rmw, launch, ament. +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-ros-base \ + ros-humble-rmw-cyclonedds-cpp \ + ros-humble-sensor-msgs-py \ + ros-humble-tf2-ros \ + ros-humble-tf2-ros-py \ + ros-humble-geometry-msgs \ + ros-humble-rosgraph-msgs \ + ros-humble-cv-bridge \ + && rm -rf /var/lib/apt/lists/* + +# Install uv for fast, deterministic pip-compatible installs. +# UV_INSTALL_DIR overrides the default ~/.local/bin location. +RUN curl -LsSf https://astral.sh/uv/install.sh | env UV_INSTALL_DIR=/usr/local/bin sh \ + && uv --version + +# Build CycloneDDS 0.10.x. Pinned because the PyPI `cyclonedds` wheel (transitive +# dep of unitree_sdk2_python) references `dds/ddsi/q_radmin.h`, removed post-0.10. +# CYCLONEDDS_HOME points the PyPI wheel at this install at pip-build time. +RUN git clone --depth 1 --branch releases/0.10.x https://github.com/eclipse-cyclonedds/cyclonedds /cyclonedds && \ + cd /cyclonedds && mkdir build install && cd build && \ + cmake .. -DCMAKE_INSTALL_PREFIX=../install && \ + cmake --build . --target install && \ + rm -rf /cyclonedds/build + +ENV CYCLONEDDS_HOME=/cyclonedds/install + +# Install unitree_sdk2_python from the local checkout and keep the source in +# the image for development/debugging. +COPY unitree_sdk2_python /home/code/unitree_sdk2_python +RUN cd /home/code/unitree_sdk2_python && \ + uv pip install --system . && \ + SP=$(python3 -c "import site; print(site.getsitepackages()[0])") && \ + cp -r /home/code/unitree_sdk2_python/unitree_sdk2py/* "$SP/unitree_sdk2py/" + +# PyTorch CPU build (arm64). The stock base pulls cu130 wheels for NVIDIA GPUs. +# NOTE: PyPI's DEFAULT `torch` wheel is CUDA-enabled even on aarch64 (it bundles +# ~3 GB of dormant nvidia-*-cu13 libs), so pin the explicit CPU index to get a +# lean CPU-only build. RoboCasa downgrades torch to a CPU 2.7.1 build anyway. +# UV_HTTP_TIMEOUT raised because the torch wheels are large. +RUN UV_HTTP_TIMEOUT=600 uv pip install --system --no-cache \ + torch torchvision \ + --index-url https://download.pytorch.org/whl/cpu + +# Shared kinematics / IK libs used by sim code across Isaac and MuJoCo. +# PyPI `pin` provides the `pinocchio` module. `pink` / `mink` are Pythonic IK +# wrappers built on pinocchio. Kept in base so per-sim Dockerfiles don't +# duplicate them; ROS container is standalone and installs its own pinocchio. +RUN uv pip install --system --no-cache pin pink mink + +# Source ROS 2 on every interactive shell. +RUN echo 'source /opt/ros/humble/setup.bash' >> /root/.bashrc + +# Clear build-time proxy vars so they don't leak into the image or child images. +ENV http_proxy= +ENV https_proxy= + +WORKDIR /home/code diff --git a/docker/RobocasaDockerfile.arm64 b/docker/RobocasaDockerfile.arm64 new file mode 100644 index 0000000..928609c --- /dev/null +++ b/docker/RobocasaDockerfile.arm64 @@ -0,0 +1,121 @@ +# ============================== +# Apple-Silicon (arm64) / CPU-only fork of RobocasaDockerfile. +# MuJoCo 3.3.1 sim on hams_base (arm64). Identical to the stock Dockerfile +# except the default render backend is OSMesa (pure-software, CPU) instead of +# EGL, since there is no NVIDIA GPU under Colima. Source tree bind-mounted at +# runtime (docker-compose.mac.yml -> /home/code/h1_robocasa). +# +# Build: docker build -t hams_sim_robocasa:latest -f docker/RobocasaDockerfile.arm64 . +# ============================== +FROM hams_base:latest + +ARG http_proxy +ARG https_proxy +ENV http_proxy=${http_proxy} +ENV https_proxy=${https_proxy} + +# Default backend baked into the image. On Apple Silicon there is no EGL/GPU +# path, so default to OSMesa (software). launch_robocasa_mac.sh also exports +# MUJOCO_GL=osmesa explicitly. +ENV MUJOCO_GL=osmesa + +# MuJoCo runtime graphics libs (OSMesa for headless software rendering; EGL +# libs kept for parity though unused on CPU). +RUN apt-get update && apt-get install -y --no-install-recommends \ + libegl1-mesa libosmesa6 \ + && rm -rf /var/lib/apt/lists/* + +# MuJoCo Python binding + Pillow for image encoding. Pinned to 3.3.1 to match +# Robocasa's hard pin (see below). +RUN uv pip install --system --no-cache \ + "mujoco==3.3.1" \ + "numpy>=2.2.6" \ + "Pillow>=10.0" + +# Robocasa kitchen scenes. Both packages are git-only. robosuite is pinned to +# master per robocasa.ai. Side effect: lerobot (a Robocasa transitive) pins +# torch==2.7.1 (CPU) — fine here. +RUN git clone --depth 1 https://github.com/ARISE-Initiative/robosuite /opt/robosuite && \ + uv pip install --system --no-cache -e /opt/robosuite + +RUN git clone --depth 1 https://github.com/robocasa/robocasa /opt/robocasa && \ + uv pip install --system --no-cache -e /opt/robocasa + +# Robocasa & robosuite setup scripts (create the macros file, silence warnings). +RUN python /opt/robosuite/robosuite/scripts/setup_macros.py && \ + python /opt/robocasa/robocasa/scripts/setup_macros.py + +# Optional robosuite/robocasa deps that print "WARNING: not installed" otherwise. +RUN git clone --depth 1 https://github.com/ARISE-Initiative/robosuite_models /opt/robosuite_models && \ + uv pip install --system --no-cache -e /opt/robosuite_models && \ + git clone --depth 1 https://github.com/NVlabs/mimicgen /opt/mimicgen && \ + uv pip install --system --no-cache -e /opt/mimicgen && \ + uv pip install --system --no-cache --no-deps "mink==0.0.5" + +# Robocasa's textures, fixtures, and objects (~10 GB) — required for any +# `--scene kitchen` run. `echo y` answers the interactive confirmation. +RUN echo y | python -m robocasa.scripts.download_kitchen_assets --type all + +WORKDIR /home/code/h1_robocasa + +CMD ["/bin/bash"] + +# ----------------------------------------------------------------------------- +# msgs_ws toolchain — minimal colcon + ament + rosidl_default_generators so +# launch_robocasa*.sh can build the IDL-only workspace at every container start. +# ----------------------------------------------------------------------------- +RUN apt-get update && apt-get install -y --no-install-recommends \ + python3-colcon-common-extensions \ + ros-humble-ament-cmake \ + ros-humble-ament-cmake-auto \ + ros-humble-ament-cmake-python \ + ros-humble-rosidl-default-generators \ + && rm -rf /var/lib/apt/lists/* + +RUN mkdir -p /home/code/msgs_ws/src + +# ----------------------------------------------------------------------------- +# Livox-SDK2 — required by livox_ros_driver2's C++ library. +# ----------------------------------------------------------------------------- +RUN git clone --depth 1 https://github.com/Livox-SDK/Livox-SDK2.git /tmp/Livox-SDK2 && \ + cd /tmp/Livox-SDK2 && mkdir build && cd build && \ + cmake .. -DCMAKE_POLICY_VERSION_MINIMUM=3.5 && make -j"$(nproc)" && make install && \ + ldconfig && \ + rm -rf /tmp/Livox-SDK2 + +# ----------------------------------------------------------------------------- +# apt deps required by livox_ros_driver2's C++ build. +# ----------------------------------------------------------------------------- +RUN apt-get update && apt-get install -y --no-install-recommends \ + libpcl-dev libapr1-dev \ + ros-humble-pcl-conversions ros-humble-pcl-ros \ + ros-humble-rclcpp-components ros-humble-rcl-interfaces \ + ros-humble-rosbag2 \ + ros-humble-sensor-msgs ros-humble-std-msgs \ + && rm -rf /var/lib/apt/lists/* + +# ----------------------------------------------------------------------------- +# livox_ros_driver2 baked into the image so the mujoco bridge can publish +# livox_ros_driver2/msg/CustomMsg on /livox/lidar. +# ----------------------------------------------------------------------------- +COPY core_ws/src/livox_ros_driver2 /opt/livox_ws/src/livox_ros_driver2 +RUN bash -c "source /opt/ros/humble/setup.bash && \ + cd /opt/livox_ws/src/livox_ros_driver2 && \ + ./build.sh humble" + +# ----------------------------------------------------------------------------- +# FastDDS RMW for the ROS (rclpy) layer — arm64 coexistence fix. +# +# h12_mujoco.py runs unitree_sdk2py DDS (rt/lowstate|lowcmd via the `cyclonedds` +# PyPI binding, linked to the source-built libddsc) AND rclpy (ROS topics) in the +# SAME process. If rclpy also uses rmw_cyclonedds_cpp, two separate CycloneDDS C +# libraries load into one process and, on arm64, the second stack to create DDS +# entities crashes / returns an invalid handle. Running rclpy on FastDDS instead +# keeps the two stacks fully independent (cyclonedds for rt/low*, FastDDS for ROS +# topics); RMW_IMPLEMENTATION is set to rmw_fastrtps_cpp in docker-compose.mac.yml. +# Both sim + ROS containers use FastDDS for ROS topics, so they still interop. +# Appended last so it does not bust the heavy mujoco/robocasa/asset layers above. +# ----------------------------------------------------------------------------- +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-rmw-fastrtps-cpp \ + && rm -rf /var/lib/apt/lists/* diff --git a/docker/RosDockerfile.slim.arm64 b/docker/RosDockerfile.slim.arm64 new file mode 100644 index 0000000..965df73 --- /dev/null +++ b/docker/RosDockerfile.slim.arm64 @@ -0,0 +1,100 @@ +# ============================== +# Apple-Silicon (arm64) / CPU-only SLIM ROS 2 workspace image on hams_base. +# +# Deliberately NOT a port of the full RosDockerfile: it omits the x86-tuned, +# exact-pinned vision / grasp ML stack (transformers, SAM3, CLIP, GraspGenX, +# diffusers, ultralytics, open3d, viser) and the Nav2 / SLAM / FAST-LIO / rqt +# layers. It carries only what the CORE robot bringup needs — robot_state_ +# publisher, joint_state_publisher, the Pink IK frame_task_server, and the +# safety_node — plus the message packages and IK/QP backends they depend on. +# +# Build: docker build -t hams_ros:latest -f docker/RosDockerfile.slim.arm64 . +# ============================== +FROM hams_base:latest + +ARG http_proxy +ARG https_proxy +ENV http_proxy=${http_proxy} +ENV https_proxy=${https_proxy} + +# ----------------------------------------------------------------------------- +# apt: colcon + ament/rosidl build system, message packages, TF, description + +# launch tooling, and the C libs the ament_cmake message packages link against. +# ----------------------------------------------------------------------------- +RUN apt-get update && apt-get install -y --no-install-recommends \ + pkg-config \ + python3-colcon-common-extensions \ + python3-rosdep \ + python3-serial \ + python3-numpy python3-scipy python3-yaml python3-transforms3d \ + libeigen3-dev libyaml-cpp-dev \ + ros-humble-ament-cmake \ + ros-humble-ament-cmake-auto \ + ros-humble-ament-cmake-python \ + ros-humble-rosidl-default-generators \ + ros-humble-action-msgs \ + ros-humble-builtin-interfaces \ + ros-humble-geometry-msgs \ + ros-humble-sensor-msgs \ + ros-humble-std-msgs \ + ros-humble-std-srvs \ + ros-humble-xacro \ + ros-humble-robot-state-publisher \ + ros-humble-joint-state-publisher \ + ros-humble-launch-ros \ + ros-humble-launch-xml \ + ros-humble-tf2-ros \ + ros-humble-tf2-eigen \ + ros-humble-tf2-tools \ + && rm -rf /var/lib/apt/lists/* + +# ----------------------------------------------------------------------------- +# Python: the Pink differential-IK stack + QP backends used by +# h12_ros2_controller's frame_task_server. numpy<2 first so transitive +# C-extension installs bind to the numpy 1.x ABI (matches apt scipy). +# ----------------------------------------------------------------------------- +RUN pip install --no-cache-dir --ignore-installed blinker \ + "numpy<2" \ + "scikit-learn>=1.4" \ + pin-pink \ + qpsolvers \ + quadprog \ + proxsuite \ + pyquaternion \ + tqdm \ + meshcat \ + meshcat-shapes + +# ----------------------------------------------------------------------------- +# setuptools / wheel / numpy clamp — REQUIRED for colcon ament_python builds. +# colcon's python_setup_py literal_eval chokes on setuptools>=70; wheel>=0.44 +# references a stub bdist_wheel module; numpy>=2 breaks the scipy/pinocchio ABI. +# Mirrors RosDockerfile's step-7 clamp. The rm -rf clears stale .dist-info +# ghosts left by layered uv/pip upgrades so importlib.metadata resolves the +# pinned versions cleanly. +# ----------------------------------------------------------------------------- +RUN rm -rf /usr/local/lib/python3.10/dist-packages/setuptools \ + /usr/local/lib/python3.10/dist-packages/setuptools-*.dist-info \ + /usr/local/lib/python3.10/dist-packages/pkg_resources \ + /usr/local/lib/python3.10/dist-packages/pkg_resources-*.dist-info \ + /usr/local/lib/python3.10/dist-packages/_distutils_hack \ + /usr/local/lib/python3.10/dist-packages/wheel \ + /usr/local/lib/python3.10/dist-packages/wheel-*.dist-info \ + && pip install --no-cache-dir --no-deps "numpy<2" "setuptools==59.6.0" "wheel<0.44" \ + && rm -f /usr/local/lib/python3.10/dist-packages/distutils-precedence.pth + +# FastDDS RMW for the ROS (rclpy) layer — arm64 coexistence fix (see the +# matching block in RobocasaDockerfile.arm64). Nodes that also touch unitree +# DDS (e.g. the safety_node's ChannelSubscriber) would otherwise load a second +# CycloneDDS libddsc alongside rmw_cyclonedds_cpp and crash on arm64. rclpy runs +# on FastDDS via RMW_IMPLEMENTATION=rmw_fastrtps_cpp (docker-compose.mac.yml). +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-rmw-fastrtps-cpp \ + && rm -rf /var/lib/apt/lists/* + +# Overlay core_ws/install if present (created by launch_ros_mac.sh's colcon build). +RUN echo '[ -f /home/code/core_ws/install/setup.bash ] && source /home/code/core_ws/install/setup.bash' >> /root/.bashrc + +WORKDIR /home/code/core_ws + +CMD ["/home/code/h12_sim_scripts/launch_ros_mac.sh"] diff --git a/docker/docker-compose.mac.yml b/docker/docker-compose.mac.yml new file mode 100644 index 0000000..a42887a --- /dev/null +++ b/docker/docker-compose.mac.yml @@ -0,0 +1,87 @@ +# ============================== +# Apple-Silicon (arm64) / CPU-only compose for HAMS. +# Standalone file (NOT an override) so the NVIDIA runtime, GPU env vars, and +# /dev/dri | /dev/input | X11 device mounts from docker-compose.yml are simply +# absent. Runs headless under Colima. Isaac is intentionally omitted. +# +# docker compose -f docker/docker-compose.mac.yml build robocasa +# docker compose -f docker/docker-compose.mac.yml run --rm robocasa +# ============================== +services: + robocasa: + container_name: hams_sim_robocasa + build: + context: .. + dockerfile: docker/RobocasaDockerfile.arm64 + image: hams_sim_robocasa:latest + shm_size: 8g + # Share one network namespace so robocasa and ros see each other's DDS + # discovery on the same ROS_DOMAIN_ID. + network_mode: host + # Share the host IPC namespace (=> shared /dev/shm) so FastDDS's shared-memory + # data transport works ACROSS the two containers. Without this, discovery + # (UDP) matches but data is silently dropped — the sim never receives /lowcmd + # and rclpy subscribers get nothing. (x86 uses CycloneDDS/UDP and dodges this; + # the arm64 port is on FastDDS, which prefers SHM for same-host peers.) + ipc: host + stdin_open: true + tty: true + environment: + - MUJOCO_GL=osmesa + - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1} + # rclpy runs on FastDDS so it does not load a 2nd CycloneDDS libddsc + # alongside unitree_sdk2py's (arm64 in-process coexistence fix). + - RMW_IMPLEMENTATION=rmw_fastrtps_cpp + volumes: + - ../h1_robocasa:/home/code/h1_robocasa + - ../CL_Assets:/home/code/CL_Assets:ro + - ./scripts:/home/code/h12_sim_scripts + - ../container_cache/msgs_ws:/home/code/msgs_ws:rw + - ../core_ws/src/magpie_msgs:/home/code/msgs_ws/src/magpie_msgs:ro + - ../core_ws/src/custom_ros_messages:/home/code/msgs_ws/src/custom_ros_messages:ro + # colcon build/install on the VM's native ext4, NOT the virtiofs mount + # (symlink-install onto virtiofs stalls for minutes). Persistent = cached. + - msgs_build:/opt/msgs_ws/build + - msgs_install:/opt/msgs_ws/install + command: /home/code/h12_sim_scripts/launch_robocasa_mac.sh + + ros: + container_name: hams_ros + build: + context: .. + dockerfile: docker/RosDockerfile.slim.arm64 + image: hams_ros:latest + shm_size: 4g + network_mode: host + # Shared IPC namespace: required for FastDDS shared-memory transport to the + # robocasa container (see the robocasa service comment above). + ipc: host + stdin_open: true + tty: true + environment: + - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1} + # Match the robocasa container: rclpy on FastDDS (arm64 coexistence fix). + - RMW_IMPLEMENTATION=rmw_fastrtps_cpp + volumes: + - ../core_ws:/home/code/core_ws + - ../CL_Assets:/home/code/CL_Assets:ro + # h12_ros2_model/setup.py locates CL_Assets as ../../../CL_Assets relative + # to __file__, and colcon runs setup.py from the build-base — so with + # build-base at /opt/core_ws/build that resolves to /opt/CL_Assets. Mount + # the submodule there too so the build finds meshes/URDFs. (Installed + # artifacts are copied into core_ws_install, so runtime is unaffected.) + - ../CL_Assets:/opt/CL_Assets:ro + - ./scripts:/home/code/h12_sim_scripts + # colcon build/install on the VM's native ext4, NOT the virtiofs mount + # (symlink-install onto virtiofs stalls for minutes). Persistent = cached. + - core_ws_build:/opt/core_ws/build + - core_ws_install:/opt/core_ws/install + command: /home/code/h12_sim_scripts/launch_ros_mac.sh + +# Named volumes live on the Colima VM's native ext4 — fast symlink-install and +# persistent colcon caches across `up`/`run` cycles (avoids the virtiofs stall). +volumes: + msgs_build: + msgs_install: + core_ws_build: + core_ws_install: diff --git a/docker/scripts/launch_robocasa_mac.sh b/docker/scripts/launch_robocasa_mac.sh new file mode 100755 index 0000000..3769ac3 --- /dev/null +++ b/docker/scripts/launch_robocasa_mac.sh @@ -0,0 +1,40 @@ +#!/bin/bash +# Apple-Silicon / CPU launcher for the MuJoCo (RoboCasa) sim. +# Same as launch_robocasa.sh but forces OSMesa software rendering and headless +# operation (no NVIDIA EGL, no X display under Colima). Extra args pass through +# to h12_mujoco.py. +set -e + +source /opt/ros/humble/setup.bash +# livox_ros_driver2 (CustomMsg/CustomPoint) is baked into the robocasa image at +# /opt/livox_ws so mujoco_ros_bridge.py can import it. +source /opt/livox_ws/install/setup.bash + +# Build the IDL-only msgs workspace (fast no-op when unchanged). +# build/install go to /opt paths backed by named docker volumes on the VM's +# native ext4: writing colcon's --symlink-install tree onto the virtiofs +# bind-mount stalls for minutes (hundreds of symlinks at ~0% CPU). Named volumes +# are both fast and persistent, so this is a real no-op on unchanged rebuilds. +MSGS_WS=/home/code/msgs_ws +MSGS_BUILD=/opt/msgs_ws/build +MSGS_INSTALL=/opt/msgs_ws/install +echo "[launch_robocasa_mac] building $MSGS_WS -> $MSGS_INSTALL" +(cd "$MSGS_WS" && colcon build --symlink-install \ + --build-base "$MSGS_BUILD" --install-base "$MSGS_INSTALL" \ + --packages-select magpie_msgs custom_ros_messages) +source "$MSGS_INSTALL/setup.bash" + +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-1}" + +# Pure-software offscreen rendering — the only backend available without a GPU. +export MUJOCO_GL=osmesa + +# Force headless: there is no reachable X display inside the Colima VM. +case " $* " in + *" --headless "*) ;; + *) set -- "$@" --headless ;; +esac + +cd /home/code/h1_robocasa +echo "[launch_robocasa_mac] MUJOCO_GL=$MUJOCO_GL ROS_DOMAIN_ID=$ROS_DOMAIN_ID args: $*" +python h12_mujoco.py "$@" diff --git a/docker/scripts/launch_ros_mac.sh b/docker/scripts/launch_ros_mac.sh new file mode 100755 index 0000000..2653b8e --- /dev/null +++ b/docker/scripts/launch_ros_mac.sh @@ -0,0 +1,45 @@ +#!/bin/bash +# Apple-Silicon / CPU launcher for the SLIM ROS workspace. +# Builds only the core bringup package subset (skips the vision / grasp / nav2 +# / FAST-LIO packages that need the heavy ML stack), then launches the minimal +# Mac bringup (robot_state_publisher, joint_state_publisher, Pink IK +# frame_task_server, safety_node) against the running MuJoCo sim. +# +# Pass `bash` as the first arg to drop to a shell instead of launching. +set -e + +source /opt/ros/humble/setup.bash + +WS=/home/code/core_ws +# build/install go to /opt paths backed by named docker volumes on the VM's +# native ext4: writing colcon's --symlink-install tree onto the virtiofs +# bind-mount stalls for minutes. Named volumes are fast AND persistent, so the +# 9-min C++ package (h12_ros2_model) incrementally caches across runs. +BUILD_BASE=/opt/core_ws/build +INSTALL_BASE=/opt/core_ws/install +cd "$WS" + +# Only the packages the minimal Mac bringup needs. h1_bringup itself is NOT +# built: it declares exec-deps on the heavy fast_lio / model_server / +# livox_ros_driver2 packages (which need the ML stack), and the minimal launch +# file is run by absolute path so the package need not be installed. Everything +# else in core_ws (model_server, h12_skills, FAST_LIO, cl_realsense, +# magpie_control, nav2 ...) is intentionally skipped on this CPU-only image. +PKGS="custom_ros_messages magpie_msgs h12_ros2_model h12_ros2_controller h12_safety_layer" + +echo "[launch_ros_mac] colcon build --packages-select $PKGS" +colcon build --symlink-install \ + --build-base "$BUILD_BASE" --install-base "$INSTALL_BASE" \ + --packages-select $PKGS + +source "$INSTALL_BASE/setup.bash" + +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-1}" + +if [ "${1:-}" = "bash" ]; then + echo "[launch_ros_mac] workspace built; dropping to shell (ROS_DOMAIN_ID=$ROS_DOMAIN_ID)" + exec bash +fi + +echo "[launch_ros_mac] launching minimal bringup (ROS_DOMAIN_ID=$ROS_DOMAIN_ID)" +exec ros2 launch "$WS/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py" diff --git a/h1_robocasa/h12_mujoco.py b/h1_robocasa/h12_mujoco.py index 09ab08f..1027375 100644 --- a/h1_robocasa/h12_mujoco.py +++ b/h1_robocasa/h12_mujoco.py @@ -99,7 +99,6 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None): drives it; RoboCasa's _check_success/reward/lang are read off the shared env. """ - create_kwargs = {} if layout is not None: create_kwargs["layout_ids"] = layout From f8a83cc7f806805536c128430f2876a4f96f0fe9 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Tue, 7 Jul 2026 17:36:22 -0600 Subject: [PATCH 02/19] :sparkles: Add VNC/noVNC GUI (MuJoCo viewer + RViz) and a robot-command helper to the Mac arm64 port MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit MuJoCo's and RViz's OpenGL windows cannot forward to macOS XQuartz (broken indirect GLX on Apple Silicon), so render them in-container with software GL (llvmpipe) into an Xvfb and stream pixels out over VNC/noVNC. - RobocasaDockerfile.arm64 / RosDockerfile.slim.arm64: add xvfb, x11vnc, novnc, websockify, fluxbox, libgl1-mesa-dri (+ ros-humble-rviz2 for the ros image), appended last so they don't bust the heavy asset/colcon layers. - launch_robocasa_mac.sh: HAMS_DISPLAY=vnc renders the MuJoCo viewer to Xvfb :99, served on noVNC :6080. Default headless (OSMesa) is unchanged. - launch_ros_mac.sh: HAMS_RVIZ=vnc renders RViz to Xvfb :100, served on :6081. Distinct display number is required — both containers share one network namespace (network_mode: host), so two Xvfb on :99 collide on the abstract X socket. - docker-compose.mac.yml: pass HAMS_DISPLAY (robocasa) and HAMS_RVIZ (ros). - mac_vnc_tunnel.sh: SSH-tunnel the noVNC/VNC ports to the Mac (Colima does not forward host-network container ports); forwards both viewers. - h1_sim.rviz: minimal RViz config (RobotModel + TF, fixed frame pelvis). - robot_cli.sh: rob_pose (named-config postures) + rob_grip helpers to drive the H1. rob_reach/FrameTask is stubbed out — it currently crashes frame_task_server. - README.md: add a self-contained macOS (Apple Silicon) section. Co-Authored-By: Claude Opus 4.8 --- README.md | 101 ++++++++++++++++++++++++++ docker/RobocasaDockerfile.arm64 | 27 +++++++ docker/RosDockerfile.slim.arm64 | 19 +++++ docker/docker-compose.mac.yml | 12 +++ docker/scripts/h1_sim.rviz | 70 ++++++++++++++++++ docker/scripts/launch_robocasa_mac.sh | 93 +++++++++++++++++++++--- docker/scripts/launch_ros_mac.sh | 60 +++++++++++++++ docker/scripts/mac_vnc_tunnel.sh | 76 +++++++++++++++++++ docker/scripts/robot_cli.sh | 78 ++++++++++++++++++++ 9 files changed, 524 insertions(+), 12 deletions(-) create mode 100644 docker/scripts/h1_sim.rviz create mode 100755 docker/scripts/mac_vnc_tunnel.sh create mode 100644 docker/scripts/robot_cli.sh diff --git a/README.md b/README.md index 1b8ac97..fc41cdf 100644 --- a/README.md +++ b/README.md @@ -160,3 +160,104 @@ ros2 action send_goal /skill/grasp custom_ros_messages/action/SkillGrasp \ "{target_object: 'vertical fridge handle', arm: 'right', timeout: {sec: 60, nanosec: 0}}" \ --feedback ``` + +## macOS (Apple Silicon) — headless CPU port + +On Apple-Silicon Macs there is a self-contained, CPU-only port that runs **just +ROS 2 + MuJoCo/RoboCasa** — Isaac is dropped and there is no NVIDIA/EGL path, so +MuJoCo renders in software (OSMesa/llvmpipe). It uses its own standalone compose +file, `docker/docker-compose.mac.yml`, and runs the ROS layer on **FastDDS** +instead of CycloneDDS (an arm64 in-process coexistence fix). None of the x86 +instructions above apply — use this section instead. + +### Prerequisites + +- [Colima](https://github.com/abiosoft/colima) + the Docker CLI + (`brew install colima docker docker-compose`). No Docker Desktop, no NVIDIA + toolkit, and no XQuartz (see the GUI note below). +- Git LFS and submodules, exactly as in [Prerequisites](#prerequisites) above. +- No `docker/.env` is needed; the Mac compose only reads `ROS_DOMAIN_ID` + (optional, defaults to `1`), `HAMS_DISPLAY`, and `HAMS_RVIZ`. +- Start the VM with enough resources — one software-GL viewer alone uses ~5 + cores, and running both the MuJoCo viewer and RViz wants headroom: + + ```bash + colima start --cpu 12 --memory 24 # sized for a 14-core / 48 GB Mac; scale to yours + ``` + +### Build and run + +```bash +# build both arm64 images (robocasa + ros); the base image builds automatically +docker compose -f docker/docker-compose.mac.yml build + +# headless (no viewer) — the default +docker compose -f docker/docker-compose.mac.yml up +``` + +The first run is slow (colcon builds the message + controller workspaces into +named volumes); later runs are cached and fast. Both containers use +`network_mode: host` + `ipc: host` so FastDDS's shared-memory transport works +across them. + +> **Always bring both containers up together.** If RoboCasa runs alone for more +> than a few seconds it releases the robot's motors ("Command timeout"), the H1 +> collapses under gravity, and when the ROS controller then connects it reads the +> fallen pose and trips its e-stop. `docker compose … up` (both services) engages +> the controller before the robot can fall. + +### Viewing the GUIs (MuJoCo viewer + RViz) + +MuJoCo's and RViz's OpenGL windows **cannot** be forwarded to XQuartz — +Apple-Silicon XQuartz has broken indirect GLX. Instead each is rendered +in-container with software GL into a virtual display and streamed to your browser +over noVNC. Enable them per-viewer: + +```bash +# MuJoCo viewer on :6080, RViz on :6081 (set either or both) +HAMS_DISPLAY=vnc HAMS_RVIZ=vnc docker compose -f docker/docker-compose.mac.yml up -d + +# open the SSH tunnel to both noVNC ports (Colima does not forward container ports) +./docker/scripts/mac_vnc_tunnel.sh # --open also opens the browser; --stop closes the tunnel +``` + +Then open: + +- **MuJoCo viewer** → +- **RViz** (RobotModel + TF) → + +`HAMS_DISPLAY` (RoboCasa/MuJoCo) and `HAMS_RVIZ` (ROS/RViz) are independent — set +either or both; the defaults are headless. + +### Driving the robot + +Bringup starts automatically in the `ros` container (`joint_state_publisher`, +`robot_state_publisher`, the `frame_task_server` IK solver, `safety_node`). To +command the H1, source the helper and send joint-space postures or gripper +commands: + +```bash +docker exec -it hams_ros bash # if the host docker CLI is flaky: colima ssh, then docker exec … +source /home/code/h12_sim_scripts/robot_cli.sh + +rob_poses # list postures +rob_pose t_pose # MOVE: home t_pose arms_front arms_overhead elbow_only … (rob_poses lists all) +rob_grip right close # open/close a gripper +rob_home +``` + +Watch the motion in either viewer. (`rob_pose` takes a name and moves the robot; +`rob_poses` only prints the list.) + +### macOS gotchas + +- **The host `docker` CLI socket is intermittent** under Colima — `docker …` may + fail with "Cannot connect to the Docker daemon" while the VM and containers are + perfectly healthy. Use `colima ssh -- docker …` as the reliable fallback, or + `colima stop && colima start` to relink the socket (this restarts the containers). +- **Cartesian reaching is disabled.** The `/frame_task` action currently crashes + the controller node, so `rob_reach` is a no-op stub — drive the robot in joint + space with `rob_pose` for now. +- **Two viewers use two displays.** RoboCasa renders on X display `:99` and RViz + on `:100`; they must differ because the containers share one network namespace + (`network_mode: host`). The launchers already handle this. diff --git a/docker/RobocasaDockerfile.arm64 b/docker/RobocasaDockerfile.arm64 index 928609c..d5a3285 100644 --- a/docker/RobocasaDockerfile.arm64 +++ b/docker/RobocasaDockerfile.arm64 @@ -119,3 +119,30 @@ RUN bash -c "source /opt/ros/humble/setup.bash && \ RUN apt-get update && apt-get install -y --no-install-recommends \ ros-humble-rmw-fastrtps-cpp \ && rm -rf /var/lib/apt/lists/* + +# ----------------------------------------------------------------------------- +# Optional VNC/noVNC display stack (Apple-Silicon only). +# +# MuJoCo's interactive GLFW window CANNOT be forwarded to macOS XQuartz: on +# Apple Silicon XQuartz's indirect GLX advertises no modern fbConfigs, so the +# container's Mesa fails with "No matching fbConfigs / GLXBadContext". Instead we +# render *in-container* with software GL (llvmpipe, from libgl1-mesa-dri) into a +# virtual X display (Xvfb) and stream the pixels out over VNC/noVNC — the GL +# never leaves the container, so XQuartz's GLX limits are irrelevant. Proven to +# give MuJoCo a full OpenGL 4.5 context. +# +# Enabled at runtime via HAMS_DISPLAY=vnc (see launch_robocasa_mac.sh); with the +# default HAMS_DISPLAY=headless none of these run and the image behaves exactly +# as before (OSMesa offscreen). Appended last so it never busts the heavy +# mujoco/robocasa/asset layers above. +# xvfb virtual framebuffer X server +# x11vnc exports the Xvfb display as a VNC server +# novnc/websockify browser (HTML5) VNC client + WebSocket bridge +# fluxbox minimal WM so the MuJoCo window is framed/manageable +# libgl1-mesa-dri llvmpipe software OpenGL (swrast_dri.so) +# x11-utils xdpyinfo, for the Xvfb readiness check +# ----------------------------------------------------------------------------- +RUN apt-get update && apt-get install -y --no-install-recommends \ + xvfb x11vnc novnc websockify fluxbox \ + libgl1-mesa-dri x11-utils \ + && rm -rf /var/lib/apt/lists/* diff --git a/docker/RosDockerfile.slim.arm64 b/docker/RosDockerfile.slim.arm64 index 965df73..389c3aa 100644 --- a/docker/RosDockerfile.slim.arm64 +++ b/docker/RosDockerfile.slim.arm64 @@ -92,6 +92,25 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ ros-humble-rmw-fastrtps-cpp \ && rm -rf /var/lib/apt/lists/* +# ----------------------------------------------------------------------------- +# Optional RViz + VNC/noVNC display stack (Apple-Silicon only). +# +# RViz2 is a Qt/OGRE OpenGL app; like the MuJoCo viewer it cannot forward its GL +# window to macOS XQuartz (broken indirect GLX on Apple Silicon — see +# RobocasaDockerfile.arm64). So we render RViz in-container with software GL +# (llvmpipe, from libgl1-mesa-dri) into a virtual X display (Xvfb) and stream the +# pixels out over VNC/noVNC on its OWN port (5901 / 6081), separate from the +# robocasa MuJoCo viewer (5900 / 6080), so both can run at once. +# +# Enabled at runtime via HAMS_RVIZ=vnc (see launch_ros_mac.sh); with the default +# HAMS_RVIZ=0 none of this runs and the image behaves exactly as before. +# Appended late so it does not bust the colcon/message layers above. +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-rviz2 \ + xvfb x11vnc novnc websockify fluxbox \ + libgl1-mesa-dri x11-utils \ + && rm -rf /var/lib/apt/lists/* + # Overlay core_ws/install if present (created by launch_ros_mac.sh's colcon build). RUN echo '[ -f /home/code/core_ws/install/setup.bash ] && source /home/code/core_ws/install/setup.bash' >> /root/.bashrc diff --git a/docker/docker-compose.mac.yml b/docker/docker-compose.mac.yml index a42887a..0a78c3b 100644 --- a/docker/docker-compose.mac.yml +++ b/docker/docker-compose.mac.yml @@ -29,6 +29,13 @@ services: environment: - MUJOCO_GL=osmesa - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1} + # Display mode for the MuJoCo sim. Default headless (OSMesa, no window). + # Set HAMS_DISPLAY=vnc to render the interactive viewer with software GL + # (llvmpipe) into an in-container Xvfb and stream it over VNC/noVNC — the + # only way to see the MuJoCo GUI on Apple Silicon, since its GL window + # cannot forward to XQuartz. View it with docker/scripts/mac_vnc_tunnel.sh: + # HAMS_DISPLAY=vnc docker compose -f docker/docker-compose.mac.yml up robocasa + - HAMS_DISPLAY=${HAMS_DISPLAY:-headless} # rclpy runs on FastDDS so it does not load a 2nd CycloneDDS libddsc # alongside unitree_sdk2py's (arm64 in-process coexistence fix). - RMW_IMPLEMENTATION=rmw_fastrtps_cpp @@ -62,6 +69,11 @@ services: - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1} # Match the robocasa container: rclpy on FastDDS (arm64 coexistence fix). - RMW_IMPLEMENTATION=rmw_fastrtps_cpp + # Set HAMS_RVIZ=vnc to also run RViz2 (software GL -> Xvfb -> noVNC on 6081). + # Independent of the robocasa MuJoCo viewer (HAMS_DISPLAY=vnc, 6080), so you + # can run either or both. View via docker/scripts/mac_vnc_tunnel.sh: + # HAMS_RVIZ=vnc docker compose -f docker/docker-compose.mac.yml up + - HAMS_RVIZ=${HAMS_RVIZ:-0} volumes: - ../core_ws:/home/code/core_ws - ../CL_Assets:/home/code/CL_Assets:ro diff --git a/docker/scripts/h1_sim.rviz b/docker/scripts/h1_sim.rviz new file mode 100644 index 0000000..b4ca87e --- /dev/null +++ b/docker/scripts/h1_sim.rviz @@ -0,0 +1,70 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1 + - /RobotModel1 + Splitter Ratio: 0.5 + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Class: "" + Name: root + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: pelvis + Frame Rate: 20 + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Enabled: true + Cell Size: 0.5 + Plane Cell Count: 20 + Color: 160; 160; 164 + Reference Frame: + - Class: rviz_default_plugins/TF + Name: TF + Enabled: true + Show Names: true + Show Axes: true + Show Arrows: false + Marker Scale: 0.3 + Update Interval: 0 + - Class: rviz_default_plugins/RobotModel + Name: RobotModel + Enabled: true + Visual Enabled: true + Collision Enabled: false + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Views: + Current: + Class: rviz_default_plugins/Orbit + Name: Current View + Distance: 3.5 + Focal Point: + X: 0 + Y: 0 + Z: 0.6 + Pitch: 0.35 + Yaw: 0.8 + Target Frame: +Window Geometry: + Displays: + collapsed: false + Views: + collapsed: false + Height: 900 + Width: 1600 diff --git a/docker/scripts/launch_robocasa_mac.sh b/docker/scripts/launch_robocasa_mac.sh index 3769ac3..c651ceb 100755 --- a/docker/scripts/launch_robocasa_mac.sh +++ b/docker/scripts/launch_robocasa_mac.sh @@ -1,10 +1,71 @@ #!/bin/bash # Apple-Silicon / CPU launcher for the MuJoCo (RoboCasa) sim. -# Same as launch_robocasa.sh but forces OSMesa software rendering and headless -# operation (no NVIDIA EGL, no X display under Colima). Extra args pass through -# to h12_mujoco.py. +# Same as launch_robocasa.sh but CPU/software rendering, no NVIDIA EGL. +# +# Two display modes, selected by the HAMS_DISPLAY env var (default: headless): +# HAMS_DISPLAY=headless OSMesa offscreen, no viewer window (original behaviour) +# HAMS_DISPLAY=vnc render the interactive MuJoCo viewer with software GL +# (llvmpipe) into an in-container Xvfb and stream it out +# over VNC/noVNC. MuJoCo's GL window cannot be forwarded +# to macOS XQuartz on Apple Silicon (broken indirect +# GLX), so we ship pixels instead. View from the Mac by +# running docker/scripts/mac_vnc_tunnel.sh, then opening +# http://localhost:6080/vnc.html. +# +# Extra args pass through to h12_mujoco.py. set -e +HAMS_DISPLAY="${HAMS_DISPLAY:-headless}" + +# VNC/noVNC ports (only used when HAMS_DISPLAY=vnc). Bound to localhost inside the +# Colima VM; reachable from the Mac only through the SSH tunnel (Colima does not +# forward host-network container ports, so a tunnel is required — see +# mac_vnc_tunnel.sh). +VNC_DISPLAY=:99 +VNC_PORT=5900 +NOVNC_PORT=6080 +VNC_GEOMETRY="${VNC_GEOMETRY:-1280x800x24}" + +# Bring up Xvfb + fluxbox + x11vnc + noVNC and point DISPLAY at the virtual +# framebuffer. All children are killed on exit via the trap below. +start_vnc_stack() { + echo "[launch_robocasa_mac] starting VNC stack on $VNC_DISPLAY ($VNC_GEOMETRY)" + Xvfb "$VNC_DISPLAY" -screen 0 "$VNC_GEOMETRY" +extension GLX +render -noreset \ + >/tmp/xvfb.log 2>&1 & + export DISPLAY="$VNC_DISPLAY" + # wait for the X server to accept connections + for _ in $(seq 1 30); do + xdpyinfo -display "$VNC_DISPLAY" >/dev/null 2>&1 && break + sleep 0.5 + done + xdpyinfo -display "$VNC_DISPLAY" >/dev/null 2>&1 \ + || { echo "[launch_robocasa_mac] Xvfb failed to start"; cat /tmp/xvfb.log; exit 1; } + + fluxbox >/tmp/fluxbox.log 2>&1 & + sleep 1 + + # -localhost: only accept VNC connections from within the VM (the SSH tunnel + # terminates on the VM's loopback). -nopw is safe because the port is not + # otherwise reachable from the Mac. + x11vnc -display "$VNC_DISPLAY" -rfbport "$VNC_PORT" -localhost \ + -forever -shared -nopw -quiet -bg >/tmp/x11vnc.log 2>&1 + sleep 1 + + websockify --web /usr/share/novnc "127.0.0.1:$NOVNC_PORT" "localhost:$VNC_PORT" \ + >/tmp/websockify.log 2>&1 & + sleep 1 + + echo "[launch_robocasa_mac] noVNC ready. From the Mac run mac_vnc_tunnel.sh, then open:" + echo "[launch_robocasa_mac] http://localhost:${NOVNC_PORT}/vnc.html?autoconnect=1&resize=scale" +} + +cleanup_vnc_stack() { + pkill -f "websockify .*${NOVNC_PORT}" 2>/dev/null || true + pkill -x x11vnc 2>/dev/null || true + pkill -x fluxbox 2>/dev/null || true + pkill -f "Xvfb ${VNC_DISPLAY}" 2>/dev/null || true +} + source /opt/ros/humble/setup.bash # livox_ros_driver2 (CustomMsg/CustomPoint) is baked into the robocasa image at # /opt/livox_ws so mujoco_ros_bridge.py can import it. @@ -26,15 +87,23 @@ source "$MSGS_INSTALL/setup.bash" export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-1}" -# Pure-software offscreen rendering — the only backend available without a GPU. -export MUJOCO_GL=osmesa - -# Force headless: there is no reachable X display inside the Colima VM. -case " $* " in - *" --headless "*) ;; - *) set -- "$@" --headless ;; -esac +if [ "$HAMS_DISPLAY" = "vnc" ]; then + # Interactive viewer via software GL into Xvfb, streamed over VNC/noVNC. + trap cleanup_vnc_stack EXIT + start_vnc_stack + export LIBGL_ALWAYS_SOFTWARE=1 # force llvmpipe; there is no GPU under Colima + export MUJOCO_GL=glfw # windowed viewer (renders onto Xvfb :99) + # Do NOT inject --headless here: the whole point of this mode is the window. +else + # Pure-software offscreen rendering — the only backend available without a GPU. + export MUJOCO_GL=osmesa + # Force headless: there is no reachable X display inside the Colima VM. + case " $* " in + *" --headless "*) ;; + *) set -- "$@" --headless ;; + esac +fi cd /home/code/h1_robocasa -echo "[launch_robocasa_mac] MUJOCO_GL=$MUJOCO_GL ROS_DOMAIN_ID=$ROS_DOMAIN_ID args: $*" +echo "[launch_robocasa_mac] HAMS_DISPLAY=$HAMS_DISPLAY MUJOCO_GL=$MUJOCO_GL ROS_DOMAIN_ID=$ROS_DOMAIN_ID args: $*" python h12_mujoco.py "$@" diff --git a/docker/scripts/launch_ros_mac.sh b/docker/scripts/launch_ros_mac.sh index 2653b8e..b10134e 100755 --- a/docker/scripts/launch_ros_mac.sh +++ b/docker/scripts/launch_ros_mac.sh @@ -6,8 +6,61 @@ # frame_task_server, safety_node) against the running MuJoCo sim. # # Pass `bash` as the first arg to drop to a shell instead of launching. +# +# Set HAMS_RVIZ=vnc to also run RViz2 rendered with software GL (llvmpipe) into +# an in-container Xvfb and streamed over VNC/noVNC on port 6081 (separate from +# the MuJoCo viewer's 6080). RViz's GL window can't forward to XQuartz on Apple +# Silicon, so we ship pixels. View from the Mac: docker/scripts/mac_vnc_tunnel.sh +# then open http://localhost:6081/vnc.html. set -e +HAMS_RVIZ="${HAMS_RVIZ:-0}" + +# VNC/noVNC for RViz (only used when HAMS_RVIZ=vnc). Localhost-bound in the VM; +# reachable from the Mac only via the SSH tunnel (mac_vnc_tunnel.sh). Ports are +# offset from the robocasa MuJoCo viewer's (5900/6080) so both run together. +# +# DISPLAY MUST differ from robocasa's :99. Both containers run network_mode:host, +# so they share ONE network namespace — and X11's abstract socket +# (@/tmp/.X11-unix/X) plus TCP 60 are namespace-scoped. Two Xvfb on :99 +# would collide: one wins the abstract socket and BOTH viewers land on that single +# display (both noVNC ports then show the same mixed screen). :100 keeps RViz on +# its own X server. +RVIZ_DISPLAY=:100 +RVIZ_VNC_PORT=5901 +RVIZ_NOVNC_PORT=6081 +RVIZ_GEOMETRY="${RVIZ_GEOMETRY:-1600x900x24}" +RVIZ_CONFIG="${RVIZ_CONFIG:-/home/code/h12_sim_scripts/h1_sim.rviz}" + +start_rviz_stack() { + echo "[launch_ros_mac] starting RViz VNC stack on $RVIZ_DISPLAY ($RVIZ_GEOMETRY)" + Xvfb "$RVIZ_DISPLAY" -screen 0 "$RVIZ_GEOMETRY" +extension GLX +render -noreset \ + >/tmp/xvfb_rviz.log 2>&1 & + export DISPLAY="$RVIZ_DISPLAY" + for _ in $(seq 1 30); do + xdpyinfo -display "$RVIZ_DISPLAY" >/dev/null 2>&1 && break + sleep 0.5 + done + xdpyinfo -display "$RVIZ_DISPLAY" >/dev/null 2>&1 \ + || { echo "[launch_ros_mac] Xvfb failed to start"; cat /tmp/xvfb_rviz.log; return 1; } + + fluxbox >/tmp/fluxbox_rviz.log 2>&1 & + sleep 1 + x11vnc -display "$RVIZ_DISPLAY" -rfbport "$RVIZ_VNC_PORT" -localhost \ + -forever -shared -nopw -quiet -bg >/tmp/x11vnc_rviz.log 2>&1 + sleep 1 + websockify --web /usr/share/novnc "127.0.0.1:$RVIZ_NOVNC_PORT" "localhost:$RVIZ_VNC_PORT" \ + >/tmp/websockify_rviz.log 2>&1 & + sleep 1 + + export LIBGL_ALWAYS_SOFTWARE=1 # force llvmpipe; no GPU under Colima + local cfg_arg=() + [ -f "$RVIZ_CONFIG" ] && cfg_arg=(-d "$RVIZ_CONFIG") + rviz2 "${cfg_arg[@]}" >/tmp/rviz2.log 2>&1 & + echo "[launch_ros_mac] RViz launched. From the Mac run mac_vnc_tunnel.sh, then open:" + echo "[launch_ros_mac] http://localhost:${RVIZ_NOVNC_PORT}/vnc.html?autoconnect=1&resize=scale" +} + source /opt/ros/humble/setup.bash WS=/home/code/core_ws @@ -36,6 +89,13 @@ source "$INSTALL_BASE/setup.bash" export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-1}" +# Optionally bring up RViz (rendered to Xvfb, streamed over noVNC). Started +# before the bringup so it's up whether we launch or drop to a shell; RViz +# tolerates topics/TF arriving after it starts. +if [ "$HAMS_RVIZ" = "vnc" ] || [ "$HAMS_RVIZ" = "1" ]; then + start_rviz_stack || echo "[launch_ros_mac] RViz stack failed to start (continuing without it)" +fi + if [ "${1:-}" = "bash" ]; then echo "[launch_ros_mac] workspace built; dropping to shell (ROS_DOMAIN_ID=$ROS_DOMAIN_ID)" exec bash diff --git a/docker/scripts/mac_vnc_tunnel.sh b/docker/scripts/mac_vnc_tunnel.sh new file mode 100755 index 0000000..c2ef2a2 --- /dev/null +++ b/docker/scripts/mac_vnc_tunnel.sh @@ -0,0 +1,76 @@ +#!/bin/bash +# Mac-side helper: SSH-tunnel the in-container noVNC/VNC ports to the Mac's +# localhost. Colima does NOT forward host-network container ports to the Mac +# (and this Colima has no reachable VM IP), so a tunnel over Colima's own SSH is +# the way in. Pairs with HAMS_DISPLAY=vnc (MuJoCo viewer) and HAMS_RVIZ=vnc (RViz). +# +# Forwards both viewers' ports (harmless if only one is running): +# MuJoCo viewer : http://localhost:6080/vnc.html (VNC localhost:5900) +# RViz : http://localhost:6081/vnc.html (VNC localhost:5901) +# +# Usage: +# ./mac_vnc_tunnel.sh open the tunnel, print the URLs +# ./mac_vnc_tunnel.sh --open ... and open both noVNC pages in the browser +# ./mac_vnc_tunnel.sh --stop close the tunnel +# +# Env: COLIMA_PROFILE (default: default). +set -euo pipefail + +PROFILE="${COLIMA_PROFILE:-default}" +# Ports to forward: noVNC (browser) + raw VNC, for the MuJoCo viewer and RViz. +PORTS=(6080 5900 6081 5901) +MUJOCO_URL="http://localhost:6080/vnc.html?autoconnect=1&resize=scale" +RVIZ_URL="http://localhost:6081/vnc.html?autoconnect=1&resize=scale" + +# Kill whatever ssh is holding any of our forwarded ports. We match by listening +# port (via lsof), NOT by command line: `ssh -f` daemonizes and truncates its +# argv, so pgrep/pkill on "-L " would miss it. +stop_tunnel() { + local killed=0 pid port + for port in "${PORTS[@]}"; do + for pid in $(lsof -nP -iTCP:"$port" -sTCP:LISTEN -t 2>/dev/null); do + if ps -p "$pid" -o comm= 2>/dev/null | grep -q '^ssh'; then + kill "$pid" 2>/dev/null && killed=1 + fi + done + done + [ "$killed" = 1 ] && echo "tunnel closed" || echo "no tunnel running" +} + +case "${1:-}" in + --stop) stop_tunnel; exit 0 ;; +esac + +command -v colima >/dev/null 2>&1 || { echo "ERROR: colima not found on PATH"; exit 1; } + +# Ask colima for the SSH config to its VM and extract the Host alias from it. +CFG="$(mktemp -t colima-ssh.XXXXXX)" +if ! colima ssh-config --profile "$PROFILE" >"$CFG" 2>/dev/null; then + echo "ERROR: 'colima ssh-config' failed — is colima running? (colima start)"; rm -f "$CFG"; exit 1 +fi +HOST_ALIAS="$(awk '/^Host /{print $2; exit}' "$CFG")" +[ -n "$HOST_ALIAS" ] || { echo "ERROR: could not read Host alias from colima ssh-config"; rm -f "$CFG"; exit 1; } + +# Replace any existing tunnel on these ports (else ExitOnForwardFailure trips). +stop_tunnel >/dev/null +sleep 1 + +# Build -L args for every port (forwarding a port whose remote side isn't +# listening yet is fine — only a LOCAL bind clash would fail, and we just cleared those). +FWD_ARGS=() +for port in "${PORTS[@]}"; do + FWD_ARGS+=(-L "${port}:127.0.0.1:${port}") +done + +ssh -F "$CFG" -f -N -o ExitOnForwardFailure=yes "${FWD_ARGS[@]}" "$HOST_ALIAS" + +echo "Tunnel up (Mac -> Colima VM):" +echo " MuJoCo viewer : $MUJOCO_URL" +echo " RViz : $RVIZ_URL" +echo " (raw VNC: localhost:5900 = MuJoCo, localhost:5901 = RViz)" +echo "Close it with: $0 --stop" + +if [ "${1:-}" = "--open" ]; then + open "$MUJOCO_URL" + open "$RVIZ_URL" +fi diff --git a/docker/scripts/robot_cli.sh b/docker/scripts/robot_cli.sh new file mode 100644 index 0000000..056a419 --- /dev/null +++ b/docker/scripts/robot_cli.sh @@ -0,0 +1,78 @@ +#!/bin/bash +# Convenience helpers for driving the H1 from inside the hams_ros container. +# This dir is live-mounted (docker-compose.mac.yml: ./scripts -> /home/code/h12_sim_scripts), +# so edits on the Mac appear here immediately — no rebuild. +# +# Usage (inside hams_ros): +# source /home/code/h12_sim_scripts/robot_cli.sh +# rob_poses # list named postures +# rob_pose t_pose # move to a named posture (default 3s) +# rob_pose arms_overhead 4 # ... over 4 seconds +# rob_grip right close # close/open a gripper +# rob_gripset left 0.5 # set gripper position (0=open .. 1=closed) +# rob_eepose right # print current right-hand pose (pelvis frame) +# rob_home # arms-down baseline +# (rob_reach / Cartesian reaching is disabled — the FrameTask action crashes the +# controller node; drive the robot in joint space with rob_pose for now.) +# +# Watch it live in your browser (from the Mac): http://localhost:6080/vnc.html + +# --- ROS environment (idempotent) --- +source /opt/ros/humble/setup.bash 2>/dev/null +source /opt/core_ws/install/setup.bash 2>/dev/null +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-1}" +export RMW_IMPLEMENTATION=rmw_fastrtps_cpp + +# Named postures available to rob_pose (from utility/named_config.py): +# home t_pose arms_front arms_front_elbow t_pose_elbow arms_overhead +# arms_asym arms_front_yaw elbow_only +rob_poses() { + echo "home t_pose arms_front arms_front_elbow t_pose_elbow arms_overhead arms_asym arms_front_yaw elbow_only" +} + +# rob_pose [seconds] +rob_pose() { + local cfg="${1:?usage: rob_pose [secs] — see rob_poses}" secs="${2:-3}" + echo "[rob_pose] -> $cfg over ${secs}s" + ros2 action send_goal /named_config custom_ros_messages/action/NamedConfig \ + "{config_name: $cfg, duration: {sec: $secs, nanosec: 0}}" +} + +rob_home() { rob_pose home "${1:-3}"; } + +# rob_grip +rob_grip() { + local side="${1:?usage: rob_grip }" act="${2:?open|close}" + ros2 service call "/${side}/gripper/${act}" std_srvs/srv/Trigger +} + +# rob_gripset <0.0..1.0> (0 = open, 1 = closed) +rob_gripset() { + local side="${1:?usage: rob_gripset <0..1>}" pos="${2:?0..1}" + ros2 service call "/${side}/gripper/set_position" magpie_msgs/srv/SetGripperPosition \ + "{position: $pos}" +} + +# rob_eepose — current hand pose in the pelvis frame +# (timeout-guarded: the CLI echo can hang on a QoS mismatch; if it prints +# nothing, that's the known ros2-CLI QoS artifact, not a missing topic.) +rob_eepose() { + local side="${1:-right}" + timeout 8 ros2 topic echo "/${side}_ee_pose" --once \ + || echo "[rob_eepose] no sample in 8s (QoS-match CLI artifact; the topic is live)" +} + +# rob_reach — DISABLED. The /frame_task (FrameTask) action currently CRASHES the +# frame_task_server: after a goal sets self.frame_names, the node's periodic +# /frame_poses publisher calls get_frame_transformation() on that frame in the +# FULL model, where a wrist-frame id is out of range -> IndexError kills the node +# (frame_task_server.py:124 -> robot_model.py:426). The arm does move first, but +# the controller then dies and all further commands hang on "waiting for action +# server". Until that's fixed, drive the robot in joint space (rob_pose) instead. +rob_reach() { + echo "rob_reach is disabled: the FrameTask action crashes frame_task_server" + echo "(get_frame_transformation IndexError on the wrist frame). Use rob_pose ." + return 1 +} + +echo "robot_cli loaded. Try: rob_poses (lists names) | rob_pose t_pose (MOVES) | rob_grip right close" From 38810337a06f20ca89e63cae139c95ff5816b6f2 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Tue, 7 Jul 2026 18:11:06 -0600 Subject: [PATCH 03/19] :bug: Make h1_bringup resolvable in the Mac slim image MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ros2 launch h1_bringup failed: the package was never colcon-built (colcon-ros refused it — its in-workspace exec_depends estop/livox_ros_driver2/ fast_lio/model_server aren't built on this CPU-only image), and interactive shells sourced the stale host install instead of the real build. - launch_ros_mac.sh: add h1_bringup to the build list, dropping empty package.sh stubs for the 4 unbuilt deps first so colcon installs h1_bringup's launch/ config/rviz files and registers it (stubs carry no ament marker, so they never appear in `ros2 pkg list`). Only h1_sim_bringup_mac.launch.py runs here. - RosDockerfile.slim.arm64: source /opt/core_ws/install (the real INSTALL_BASE, a named volume) in .bashrc instead of the virtiofs /home/code/core_ws/install, so `docker exec … bash` sees the same packages as the running bringup. Co-Authored-By: Claude Opus 4.8 --- docker/RosDockerfile.slim.arm64 | 9 +++++++-- docker/scripts/launch_ros_mac.sh | 23 ++++++++++++++++------- 2 files changed, 23 insertions(+), 9 deletions(-) diff --git a/docker/RosDockerfile.slim.arm64 b/docker/RosDockerfile.slim.arm64 index 389c3aa..25edc46 100644 --- a/docker/RosDockerfile.slim.arm64 +++ b/docker/RosDockerfile.slim.arm64 @@ -111,8 +111,13 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ libgl1-mesa-dri x11-utils \ && rm -rf /var/lib/apt/lists/* -# Overlay core_ws/install if present (created by launch_ros_mac.sh's colcon build). -RUN echo '[ -f /home/code/core_ws/install/setup.bash ] && source /home/code/core_ws/install/setup.bash' >> /root/.bashrc +# Overlay the colcon install in interactive shells so `docker exec … bash` sees +# the same packages as the running bringup (incl. h1_bringup, so +# `ros2 launch h1_bringup ` works). launch_ros_mac.sh builds to +# /opt/core_ws/install (INSTALL_BASE, a named volume on the VM's native ext4 — +# NOT the virtiofs-mounted /home/code/core_ws/install, which may hold a stale +# host-side build). Source that same path here. +RUN echo '[ -f /opt/core_ws/install/setup.bash ] && source /opt/core_ws/install/setup.bash' >> /root/.bashrc WORKDIR /home/code/core_ws diff --git a/docker/scripts/launch_ros_mac.sh b/docker/scripts/launch_ros_mac.sh index b10134e..9d2a20a 100755 --- a/docker/scripts/launch_ros_mac.sh +++ b/docker/scripts/launch_ros_mac.sh @@ -72,13 +72,22 @@ BUILD_BASE=/opt/core_ws/build INSTALL_BASE=/opt/core_ws/install cd "$WS" -# Only the packages the minimal Mac bringup needs. h1_bringup itself is NOT -# built: it declares exec-deps on the heavy fast_lio / model_server / -# livox_ros_driver2 packages (which need the ML stack), and the minimal launch -# file is run by absolute path so the package need not be installed. Everything -# else in core_ws (model_server, h12_skills, FAST_LIO, cl_realsense, -# magpie_control, nav2 ...) is intentionally skipped on this CPU-only image. -PKGS="custom_ros_messages magpie_msgs h12_ros2_model h12_ros2_controller h12_safety_layer" +# The packages the minimal Mac bringup needs. h1_bringup (ament_python, just +# launch/config/rviz files) is included so `ros2 launch h1_bringup ` +# resolves with tab completion — but colcon-ros refuses to build it until its +# in-workspace exec_depends have a package.sh in the install space, and we +# deliberately don't build the heavy ones (estop, livox_ros_driver2, fast_lio, +# model_server — the LIO/ML/driver stack). Empty package.sh stubs (below) satisfy +# that check; they carry no ament index marker so they never show up in +# `ros2 pkg list`. NOTE only h1_sim_bringup_mac.launch.py runs here — the other +# launch files exec-launch the omitted packages (nav2, FAST_LIO, model_server...). +PKGS="custom_ros_messages magpie_msgs h12_ros2_model h12_ros2_controller h12_safety_layer h1_bringup" +H1_BRINGUP_STUB_DEPS="estop livox_ros_driver2 fast_lio model_server" + +for _dep in $H1_BRINGUP_STUB_DEPS; do + mkdir -p "$INSTALL_BASE/$_dep/share/$_dep" + [ -e "$INSTALL_BASE/$_dep/share/$_dep/package.sh" ] || : > "$INSTALL_BASE/$_dep/share/$_dep/package.sh" +done echo "[launch_ros_mac] colcon build --packages-select $PKGS" colcon build --symlink-install \ From e36027f54a28b764754f37e541dd94fe404c3f9f Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Tue, 7 Jul 2026 18:15:39 -0600 Subject: [PATCH 04/19] :sparkles: Show the Livox lidar point cloud in the Mac RViz The MuJoCo bridge already publishes /livox/pointcloud (sensor_msgs/PointCloud2), but its frame_id 'lidar_link' had no TF, so RViz couldn't place it. - h1_sim_bringup_mac.launch.py: static TF livox_link -> lidar_link (identity; they're co-located) so the cloud transforms into the robot/TF tree. - h1_sim.rviz: add a PointCloud2 display on /livox/pointcloud (best-effort QoS, AxisColor by Z). Verified: ~20k pts/scan render around the robot. Co-Authored-By: Claude Opus 4.8 --- .../launch/h1_sim_bringup_mac.launch.py | 13 +++++++++++++ docker/scripts/h1_sim.rviz | 17 +++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py index faf34e9..074b797 100644 --- a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py @@ -7,6 +7,7 @@ Nodes started: * static camera_link -> camera_color_optical_frame TF + * static livox_link -> lidar_link TF (so RViz can place /livox/pointcloud) * joint_state_publisher (h12_ros2_controller) * robot_state_publisher * frame_task_server (h12_ros2_controller, Pink IK) @@ -41,6 +42,18 @@ def generate_launch_description(): parameters=[sim_time_param], output='screen', ), + # The MuJoCo lidar bridge stamps /livox/{lidar,pointcloud} with frame_id + # 'lidar_link', which is co-located with the URDF's 'livox_link' mount but + # is not itself a URDF link. Publish the identity bridge so RViz (and any + # lidar consumer) can transform the cloud into the robot/TF tree. + Node( + package='tf2_ros', + executable='static_transform_publisher', + name='lidar_frame_broadcaster', + arguments=['0', '0', '0', '0', '0', '0', 'livox_link', 'lidar_link'], + parameters=[sim_time_param], + output='screen', + ), Node( package='h12_ros2_controller', executable='joint_state_publisher', diff --git a/docker/scripts/h1_sim.rviz b/docker/scripts/h1_sim.rviz index b4ca87e..d0fee73 100644 --- a/docker/scripts/h1_sim.rviz +++ b/docker/scripts/h1_sim.rviz @@ -44,6 +44,23 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /robot_description + - Class: rviz_default_plugins/PointCloud2 + Name: Lidar + Enabled: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /livox/pointcloud + Style: Points + Size (Pixels): 3 + Decay Time: 0 + Position Transformer: XYZ + Color Transformer: AxisColor + Axis: Z + Autocompute Value Bounds: + Autocompute Bounds: true Tools: - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select From 5b207853449aec62f75e50cb2bc9c8d1d40a84a9 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Tue, 7 Jul 2026 19:30:08 -0600 Subject: [PATCH 05/19] :sparkles: Add optional lower-body (FAME stands free) + fix fluxbox wallpaper popup MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Lower-body controller (HAMS_LOWERBODY, default off): - HAMS_LOWERBODY=fame runs the RMA fame_node — it releases the elastic-band tether (after the IK inits) and BALANCES the robot standing unsupported (verified: no fall). It stands/squats but does not locomote. HAMS_LOWERBODY=walk runs the TorchScript walk policy, which currently falls ~4s after release in the RoboCasa sim (kept for completeness). True forward walking needs a sim-tuned policy — future work. - launch_ros_mac.sh builds unitree_hg + h12_lowerbody_controller; the bringup launches fame_node/walking_node gated on HAMS_LOWERBODY; compose threads it. - RosDockerfile: add ros-humble-rosidl-generator-dds-idl (unitree_hg needs it to generate DDS IDL). torch is already in the image. Wallpaper popup: - fluxbox's default style calls fbsetbg, which pops an xmessage "wallpaper cannot be set" error (no wallpaper setter installed). Both images now replace /usr/bin/fbsetbg with a shim that sets a solid root via fbsetroot (bundled). The fluxbox rootCommand init override does NOT stop it — fbsetbg is invoked from the style, so replacing the binary is what works. Co-Authored-By: Claude Opus 4.8 --- README.md | 19 ++++++++++++++ .../launch/h1_sim_bringup_mac.launch.py | 25 +++++++++++++++++-- docker/RobocasaDockerfile.arm64 | 7 ++++++ docker/RosDockerfile.slim.arm64 | 17 +++++++++++++ docker/docker-compose.mac.yml | 5 ++++ docker/scripts/launch_robocasa_mac.sh | 2 +- docker/scripts/launch_ros_mac.sh | 11 ++++++-- 7 files changed, 81 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index fc41cdf..287ad1f 100644 --- a/README.md +++ b/README.md @@ -249,6 +249,25 @@ rob_home Watch the motion in either viewer. (`rob_pose` takes a name and moves the robot; `rob_poses` only prints the list.) +### Lower-body controller (standing free) + +By default an elastic-band tether holds the robot upright and only the upper body +is controlled. Set `HAMS_LOWERBODY=fame` to run the RMA lower-body policy, which +releases the tether and **balances the robot standing unsupported**: + +```bash +HAMS_DISPLAY=vnc HAMS_RVIZ=vnc HAMS_LOWERBODY=fame \ + docker compose -f docker/docker-compose.mac.yml up +``` + +Caveat: `HAMS_LOWERBODY=fame` **stands** (and squats via `/lowerbody/squat_cmd`) +but does **not** locomote — it holds position. `HAMS_LOWERBODY=walk` runs the +TorchScript walk policy, which currently does **not** stay upright in the RoboCasa +sim (falls a few seconds after the tether releases); true forward walking needs a +locomotion policy tuned for this simulator. (`torch` is already in the ros image; +building `unitree_hg` needs `rosidl-generator-dds-idl`, which the image now +includes.) + ### macOS gotchas - **The host `docker` CLI socket is intermittent** under Colima — `docker …` may diff --git a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py index 074b797..9b198a9 100644 --- a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py @@ -31,7 +31,7 @@ def generate_launch_description(): # MuJoCo publishes /clock with sim time; keep all nodes on it. sim_time_param = {'use_sim_time': True} - return LaunchDescription([ + nodes = [ Node( package='tf2_ros', executable='static_transform_publisher', @@ -84,4 +84,25 @@ def generate_launch_description(): parameters=[sim_time_param], output='screen', ), - ]) + ] + + # Optional lower-body locomotion controller (HAMS_LOWERBODY). Off by default: + # the elastic-band tether then holds the robot upright. The node self-sequences + # its band release (waits for the IK to finish) and publishes leg setpoints on + # /safety/lowcmd_lower_in for the safety_node to merge. + # HAMS_LOWERBODY=fame RMA standing/squatting policy — balances the robot + # standing UNSUPPORTED (verified). Does not locomote. + # HAMS_LOWERBODY=walk TorchScript walk policy — currently does NOT stay up + # in the RoboCasa sim (falls ~4s; here for completeness). + lowerbody = os.environ.get('HAMS_LOWERBODY', '').strip().lower() + _lowerbody_exec = {'fame': 'fame_node', 'walk': 'walking_node'}.get(lowerbody) + if _lowerbody_exec: + nodes.append(Node( + package='h12_lowerbody_controller', + executable=_lowerbody_exec, + name=_lowerbody_exec, + parameters=[sim_time_param], + output='screen', + )) + + return LaunchDescription(nodes) diff --git a/docker/RobocasaDockerfile.arm64 b/docker/RobocasaDockerfile.arm64 index d5a3285..98c9e2a 100644 --- a/docker/RobocasaDockerfile.arm64 +++ b/docker/RobocasaDockerfile.arm64 @@ -146,3 +146,10 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ xvfb x11vnc novnc websockify fluxbox \ libgl1-mesa-dri x11-utils \ && rm -rf /var/lib/apt/lists/* + +# fluxbox's default style calls fbsetbg to set a wallpaper; with no wallpaper +# setter installed it pops an xmessage "wallpaper cannot be set" error on the VNC +# display. Replace fbsetbg with a shim that sets a solid root color via fbsetroot +# (bundled with fluxbox) — clean background, no error dialog. +RUN printf '#!/bin/sh\nexec fbsetroot -solid "#2b2b2b"\n' > /usr/bin/fbsetbg \ + && chmod +x /usr/bin/fbsetbg diff --git a/docker/RosDockerfile.slim.arm64 b/docker/RosDockerfile.slim.arm64 index 25edc46..60ef4e9 100644 --- a/docker/RosDockerfile.slim.arm64 +++ b/docker/RosDockerfile.slim.arm64 @@ -111,6 +111,23 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ libgl1-mesa-dri x11-utils \ && rm -rf /var/lib/apt/lists/* +# fluxbox's default style calls fbsetbg to set a wallpaper; with no wallpaper +# setter installed it pops an xmessage "wallpaper cannot be set" error on the VNC +# display. Replace fbsetbg with a shim that sets a solid root color via fbsetroot +# (bundled with fluxbox) — clean background, no error dialog. +RUN printf '#!/bin/sh\nexec fbsetroot -solid "#2b2b2b"\n' > /usr/bin/fbsetbg \ + && chmod +x /usr/bin/fbsetbg + +# ----------------------------------------------------------------------------- +# rosidl DDS-IDL generator — needed to build unitree_hg (the Unitree message +# package the lower-body walk/FAME controllers depend on). unitree_hg ships from +# unitree_ros2/cyclonedds_ws and its CMake generates DDS IDL, which this +# FastDDS-only slim image otherwise lacks. Only pulled in for the optional +# lower-body stack (HAMS_LOWERBODY); torch is already present in the base image. +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-rosidl-generator-dds-idl \ + && rm -rf /var/lib/apt/lists/* + # Overlay the colcon install in interactive shells so `docker exec … bash` sees # the same packages as the running bringup (incl. h1_bringup, so # `ros2 launch h1_bringup ` works). launch_ros_mac.sh builds to diff --git a/docker/docker-compose.mac.yml b/docker/docker-compose.mac.yml index 0a78c3b..f04223d 100644 --- a/docker/docker-compose.mac.yml +++ b/docker/docker-compose.mac.yml @@ -74,6 +74,11 @@ services: # can run either or both. View via docker/scripts/mac_vnc_tunnel.sh: # HAMS_RVIZ=vnc docker compose -f docker/docker-compose.mac.yml up - HAMS_RVIZ=${HAMS_RVIZ:-0} + # Optional lower-body controller: HAMS_LOWERBODY=fame runs the RMA policy + # that balances the robot standing unsupported (releases the elastic band); + # =walk runs the walk policy (falls in this sim). Default (unset) keeps the + # band tether holding the robot upright with upper-body IK only. + - HAMS_LOWERBODY=${HAMS_LOWERBODY:-} volumes: - ../core_ws:/home/code/core_ws - ../CL_Assets:/home/code/CL_Assets:ro diff --git a/docker/scripts/launch_robocasa_mac.sh b/docker/scripts/launch_robocasa_mac.sh index c651ceb..bf903d0 100755 --- a/docker/scripts/launch_robocasa_mac.sh +++ b/docker/scripts/launch_robocasa_mac.sh @@ -41,7 +41,7 @@ start_vnc_stack() { xdpyinfo -display "$VNC_DISPLAY" >/dev/null 2>&1 \ || { echo "[launch_robocasa_mac] Xvfb failed to start"; cat /tmp/xvfb.log; exit 1; } - fluxbox >/tmp/fluxbox.log 2>&1 & + fluxbox >/tmp/fluxbox.log 2>&1 & # bg set by the fbsetbg shim (Dockerfile) sleep 1 # -localhost: only accept VNC connections from within the VM (the SSH tunnel diff --git a/docker/scripts/launch_ros_mac.sh b/docker/scripts/launch_ros_mac.sh index 9d2a20a..969bfd7 100755 --- a/docker/scripts/launch_ros_mac.sh +++ b/docker/scripts/launch_ros_mac.sh @@ -44,7 +44,7 @@ start_rviz_stack() { xdpyinfo -display "$RVIZ_DISPLAY" >/dev/null 2>&1 \ || { echo "[launch_ros_mac] Xvfb failed to start"; cat /tmp/xvfb_rviz.log; return 1; } - fluxbox >/tmp/fluxbox_rviz.log 2>&1 & + fluxbox >/tmp/fluxbox_rviz.log 2>&1 & # bg set by the fbsetbg shim (Dockerfile) sleep 1 x11vnc -display "$RVIZ_DISPLAY" -rfbport "$RVIZ_VNC_PORT" -localhost \ -forever -shared -nopw -quiet -bg >/tmp/x11vnc_rviz.log 2>&1 @@ -81,7 +81,14 @@ cd "$WS" # that check; they carry no ament index marker so they never show up in # `ros2 pkg list`. NOTE only h1_sim_bringup_mac.launch.py runs here — the other # launch files exec-launch the omitted packages (nav2, FAST_LIO, model_server...). -PKGS="custom_ros_messages magpie_msgs h12_ros2_model h12_ros2_controller h12_safety_layer h1_bringup" +# unitree_hg + h12_lowerbody_controller provide the optional lower-body stack +# (walk/FAME policies). They build cheaply and are only *launched* when +# HAMS_LOWERBODY is set (see the bringup), but building them always keeps +# `ros2 run h12_lowerbody_controller fame_node` available in a shell. unitree_hg +# needs ros-humble-rosidl-generator-dds-idl (added to the image); torch is +# already present. NOTE: FAME balances the robot standing unsupported; the walk +# policy currently does not stay up in the RoboCasa sim (see README). +PKGS="custom_ros_messages magpie_msgs h12_ros2_model h12_ros2_controller h12_safety_layer h1_bringup unitree_hg h12_lowerbody_controller" H1_BRINGUP_STUB_DEPS="estop livox_ros_driver2 fast_lio model_server" for _dep in $H1_BRINGUP_STUB_DEPS; do From 6a72f39c5640cdd6a126a4d71dfbbeb53fc3eff9 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 12:03:59 -0600 Subject: [PATCH 06/19] :sparkles: Walk + SLAM demo: switchable stand/walk controller, sim odom, 2D mapping Makes the H1 walk and build a live occupancy map on the Mac port. - Switchable lower-body controller: HAMS_LOWERBODY=switch runs lowerbody_controller_node (band-held idle -> /lowerbody/start_{fame,walk} with a gated handover). FAME stands/manipulates; walk locomotes. From a stable FAME stance the walk policy stays UP and translates (0 falls), vs. the raw walk node that toppled in ~4s. - Sim ground-truth odometry: mujoco_ros_bridge publishes odom -> pelvis TF + /odom from the pelvis free-joint world pose. This is the SLAM odom source, so no FAST-LIO / Livox-SDK build is needed. - 2D SLAM (HAMS_SLAM=1 in the bringup): pointcloud_to_laserscan flattens the Livox hemisphere cloud into /converted_scan (tuned per h1_navigation), and slam_toolbox builds /map against /odom. Verified: ~126 scan returns/frame, occupancy /map updating in RViz as the robot walks. Packages baked into the ros image. - RViz: add Map + LaserScan displays, fixed frame odom, top-down view following the pelvis. - robot_cli.sh: rob_stand / rob_walk / rob_go / rob_stop / rob_odom helpers. Co-Authored-By: Claude Opus 4.8 --- .../launch/h1_sim_bringup_mac.launch.py | 51 ++++++++++++++-- docker/RosDockerfile.slim.arm64 | 9 +++ docker/scripts/h1_sim.rviz | 38 ++++++++++-- docker/scripts/robot_cli.sh | 29 ++++++++- h1_robocasa/h12_mujoco.py | 12 ++++ h1_robocasa/mujoco_ros_bridge.py | 61 +++++++++++++++++++ 6 files changed, 188 insertions(+), 12 deletions(-) diff --git a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py index 9b198a9..3756685 100644 --- a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py @@ -18,6 +18,7 @@ """ import os +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node @@ -90,12 +91,17 @@ def generate_launch_description(): # the elastic-band tether then holds the robot upright. The node self-sequences # its band release (waits for the IK to finish) and publishes leg setpoints on # /safety/lowcmd_lower_in for the safety_node to merge. - # HAMS_LOWERBODY=fame RMA standing/squatting policy — balances the robot - # standing UNSUPPORTED (verified). Does not locomote. - # HAMS_LOWERBODY=walk TorchScript walk policy — currently does NOT stay up - # in the RoboCasa sim (falls ~4s; here for completeness). + # HAMS_LOWERBODY=fame RMA standing/squatting policy — balances the robot + # standing UNSUPPORTED (verified). Does not locomote. + # HAMS_LOWERBODY=walk TorchScript walk policy on its own (marches in place; + # topples in the cluttered kitchen). + # HAMS_LOWERBODY=switch switchable controller: band-held idle until you call + # /lowerbody/start_{fame,walk}; gated handover between + # stand (FAME) and locomotion (walk). This is the one to + # use for stand<->walk. Starts idle by default. lowerbody = os.environ.get('HAMS_LOWERBODY', '').strip().lower() - _lowerbody_exec = {'fame': 'fame_node', 'walk': 'walking_node'}.get(lowerbody) + _lowerbody_exec = {'fame': 'fame_node', 'walk': 'walking_node', + 'switch': 'lowerbody_controller_node'}.get(lowerbody) if _lowerbody_exec: nodes.append(Node( package='h12_lowerbody_controller', @@ -105,4 +111,39 @@ def generate_launch_description(): output='screen', )) + # Optional 2D SLAM (HAMS_SLAM=1): pointcloud_to_laserscan flattens the Livox + # hemisphere cloud into a horizontal scan, and slam_toolbox builds an occupancy + # /map from it against the sim's ground-truth /odom (odom -> pelvis). No + # FAST-LIO needed. p2l params match h1_navigation.launch.py but are fed from + # the raw /livox/pointcloud instead of FAST-LIO's registered cloud. + if os.environ.get('HAMS_SLAM', '').strip().lower() in ('1', 'true', 'on'): + nodes.append(Node( + package='pointcloud_to_laserscan', + executable='pointcloud_to_laserscan_node', + name='pointcloud_to_laserscan', + parameters=[{ + 'target_frame': 'pelvis', + 'min_height': -0.90, 'max_height': 0.55, + 'angle_min': -3.14159, 'angle_max': 3.14159, + 'angle_increment': 0.0087, + 'range_min': 0.6, 'range_max': 6.0, + 'use_inf': True, 'scan_time': 0.0333, + 'transform_tolerance': 0.3, 'queue_size': 20, + }, sim_time_param], + remappings=[('cloud_in', '/livox/pointcloud'), ('scan', '/converted_scan')], + output='screen', + )) + nodes.append(Node( + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + parameters=[ + os.path.join(get_package_share_directory('h1_bringup'), + 'config', 'slam_toolbox_h1.yaml'), + {'odom_frame': 'odom', 'scan_topic': '/converted_scan'}, + sim_time_param, + ], + output='screen', + )) + return LaunchDescription(nodes) diff --git a/docker/RosDockerfile.slim.arm64 b/docker/RosDockerfile.slim.arm64 index 60ef4e9..14b54d3 100644 --- a/docker/RosDockerfile.slim.arm64 +++ b/docker/RosDockerfile.slim.arm64 @@ -128,6 +128,15 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ ros-humble-rosidl-generator-dds-idl \ && rm -rf /var/lib/apt/lists/* +# 2D SLAM (optional, HAMS_SLAM=1): slam_toolbox builds an occupancy map from a +# laserscan; pointcloud_to_laserscan flattens the Livox cloud into that scan. +# Sim ground-truth /odom (from the mujoco bridge) is the odometry source, so no +# FAST-LIO is needed. +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-slam-toolbox \ + ros-humble-pointcloud-to-laserscan \ + && rm -rf /var/lib/apt/lists/* + # Overlay the colcon install in interactive shells so `docker exec … bash` sees # the same packages as the running bringup (incl. h1_bringup, so # `ros2 launch h1_bringup ` works). launch_ros_mac.sh builds to diff --git a/docker/scripts/h1_sim.rviz b/docker/scripts/h1_sim.rviz index d0fee73..a18a2c6 100644 --- a/docker/scripts/h1_sim.rviz +++ b/docker/scripts/h1_sim.rviz @@ -14,7 +14,7 @@ Visualization Manager: Name: root Global Options: Background Color: 48; 48; 48 - Fixed Frame: pelvis + Fixed Frame: odom Frame Rate: 20 Displays: - Class: rviz_default_plugins/Grid @@ -61,6 +61,32 @@ Visualization Manager: Axis: Z Autocompute Value Bounds: Autocompute Bounds: true + - Class: rviz_default_plugins/Map + Name: Map + Enabled: true + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Color Scheme: map + Draw Behind: true + Alpha: 0.7 + - Class: rviz_default_plugins/LaserScan + Name: Scan + Enabled: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /converted_scan + Style: Points + Size (Pixels): 4 + Color: 255; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 Tools: - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select @@ -70,14 +96,14 @@ Visualization Manager: Current: Class: rviz_default_plugins/Orbit Name: Current View - Distance: 3.5 + Distance: 9.0 Focal Point: X: 0 Y: 0 - Z: 0.6 - Pitch: 0.35 - Yaw: 0.8 - Target Frame: + Z: 0 + Pitch: 1.2 + Yaw: 3.14 + Target Frame: pelvis Window Geometry: Displays: collapsed: false diff --git a/docker/scripts/robot_cli.sh b/docker/scripts/robot_cli.sh index 056a419..3c0cf9d 100644 --- a/docker/scripts/robot_cli.sh +++ b/docker/scripts/robot_cli.sh @@ -75,4 +75,31 @@ rob_reach() { return 1 } -echo "robot_cli loaded. Try: rob_poses (lists names) | rob_pose t_pose (MOVES) | rob_grip right close" +# --- Lower-body locomotion (needs HAMS_LOWERBODY=switch) ------------------------ +# The switchable controller starts band-held idle; rob_stand engages FAME (stand +# free), rob_walk hands over to the walk policy, rob_go drives /cmd_vel. + +rob_stand() { ros2 service call /lowerbody/start_fame std_srvs/srv/Trigger; } # stand (FAME) +rob_walk() { ros2 service call /lowerbody/start_walk std_srvs/srv/Trigger; } # locomotion mode + +# rob_go [vy] [wz] [secs] — drive the walk policy (m/s, m/s, rad/s). +# Publishes /cmd_vel at 20 Hz for `secs` (default 6). vx>0 forward, wz>0 turn left. +rob_go() { + local vx="${1:-0.4}" vy="${2:-0.0}" wz="${3:-0.0}" secs="${4:-6}" + echo "[rob_go] vx=$vx vy=$vy wz=$wz for ${secs}s" + timeout "$secs" ros2 topic pub -r 20 /cmd_vel geometry_msgs/msg/Twist \ + "{linear: {x: $vx, y: $vy}, angular: {z: $wz}}" +} + +# rob_stop — zero velocity and switch back to FAME (stand still). +rob_stop() { + ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "{}" >/dev/null 2>&1 + rob_stand +} + +# rob_odom — current base position in the odom (world) frame. +rob_odom() { timeout 5 ros2 topic echo /odom --field pose.pose.position --once; } + +echo "robot_cli loaded." +echo " postures : rob_pose t_pose | rob_grip right close" +echo " locomote : rob_stand -> rob_walk -> rob_go 0.4 0 0.3 (fwd+turn) -> rob_stop (needs HAMS_LOWERBODY=switch)" diff --git a/h1_robocasa/h12_mujoco.py b/h1_robocasa/h12_mujoco.py index 1027375..d15cca6 100644 --- a/h1_robocasa/h12_mujoco.py +++ b/h1_robocasa/h12_mujoco.py @@ -169,6 +169,17 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None): sim_lock = threading.Lock() pfx = env.robots[0].robot_model.naming_prefix # "robot0_" + # Body carrying the pelvis free joint — its world pose is ground-truth base + # odometry, which the bridge publishes as odom -> pelvis TF + /odom (feeds + # SLAM without needing FAST-LIO). + odom_base_body_id = -1 + try: + _fj = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, + f"{pfx}{h1_2_robosuite.FREE_JOINT_NAME}") + odom_base_body_id = int(model.jnt_bodyid[_fj]) + except Exception as e: + print(f"[h12_mujoco] base odom body resolve skipped: {e}") + # DDS control bridge: publishes rt/lowstate, subscribes rt/lowcmd, drives the # 27 body motors by name via the resolver (grippers handled by the hand # bridges below; gripper ctrl indices are disjoint from the body motors). @@ -199,6 +210,7 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None): imu_quat_sensor=f"{pfx}livox_imu_quat", imu_gyro_sensor=f"{pfx}livox_imu_gyro", imu_acc_sensor=f"{pfx}livox_imu_acc", + base_body_id=odom_base_body_id, # -> odom -> pelvis TF + /odom sim_lock=sim_lock, ) diff --git a/h1_robocasa/mujoco_ros_bridge.py b/h1_robocasa/mujoco_ros_bridge.py index ac9a6f0..4d83438 100644 --- a/h1_robocasa/mujoco_ros_bridge.py +++ b/h1_robocasa/mujoco_ros_bridge.py @@ -32,12 +32,15 @@ import rclpy from builtin_interfaces.msg import Time as TimeMsg from PIL import Image as PILImage +from geometry_msgs.msg import TransformStamped from livox_ros_driver2.msg import CustomMsg, CustomPoint +from nav_msgs.msg import Odometry from rclpy.node import Node from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy, qos_profile_sensor_data from rosgraph_msgs.msg import Clock from sensor_msgs.msg import CameraInfo, CompressedImage, Image, Imu, PointCloud2, PointField from std_srvs.srv import Trigger +from tf2_ros import TransformBroadcaster def _sim_time_to_msg(sim_time: float) -> TimeMsg: @@ -127,6 +130,10 @@ def __init__( imu_acc_sensor: str = "livox_imu_acc", imu_frame: str = "lidar_link", imu_rate_hz: float = 100.0, + base_body_id: int = -1, + odom_frame: str = "odom", + base_frame: str = "pelvis", + odom_rate_hz: float = 50.0, elastic_band=None, sim_lock=None, ): @@ -257,6 +264,18 @@ def __init__( self.imu_gyro_adr, self.imu_gyro_dim = self._sensor_adr(imu_gyro_sensor) self.imu_acc_adr, self.imu_acc_dim = self._sensor_adr(imu_acc_sensor) + # Ground-truth base odometry (odom -> base_frame TF + /odom). The sim knows + # the pelvis world pose exactly, so we publish it directly instead of + # running FAST-LIO. This is the odom source SLAM (slam_toolbox) consumes. + self.base_body_id = int(base_body_id) + self.odom_frame = odom_frame + self.base_frame = base_frame + self.odom_period = 1.0 / odom_rate_hz + self._last_odom_sim_t = 0.0 + if self.base_body_id >= 0: + self._tf_broadcaster = TransformBroadcaster(self) + self.pub_odom = self.create_publisher(Odometry, "/odom", 10) + if elastic_band is not None: self.create_service( Trigger, "/elastic_band/toggle", self._on_elastic_band_toggle @@ -312,6 +331,13 @@ def tick(self) -> None: clock_msg.clock = stamp self.pub_clock.publish(clock_msg) + if self.base_body_id >= 0 and sim_t - self._last_odom_sim_t >= self.odom_period: + self._last_odom_sim_t = sim_t + try: + self._publish_odom(stamp) + except Exception as e: + self.get_logger().warn(f"odom publish failed: {e}") + if sim_t - self._last_imu_sim_t >= self.imu_period: self._last_imu_sim_t = sim_t try: @@ -355,6 +381,41 @@ def _publish_imu(self, stamp: TimeMsg) -> None: msg.linear_acceleration.z = float(a[2]) self.pub_imu.publish(msg) + def _publish_odom(self, stamp: TimeMsg) -> None: + # Ground-truth pelvis pose in the sim world (used as the odom frame). + # MuJoCo quaternions are (w, x, y, z); ROS wants (x, y, z, w). + pos = self.data.xpos[self.base_body_id] + quat = self.data.xquat[self.base_body_id] + px, py, pz = float(pos[0]), float(pos[1]), float(pos[2]) + qw, qx, qy, qz = (float(quat[0]), float(quat[1]), + float(quat[2]), float(quat[3])) + + tf = TransformStamped() + tf.header.stamp = stamp + tf.header.frame_id = self.odom_frame + tf.child_frame_id = self.base_frame + tf.transform.translation.x = px + tf.transform.translation.y = py + tf.transform.translation.z = pz + tf.transform.rotation.x = qx + tf.transform.rotation.y = qy + tf.transform.rotation.z = qz + tf.transform.rotation.w = qw + self._tf_broadcaster.sendTransform(tf) + + odom = Odometry() + odom.header.stamp = stamp + odom.header.frame_id = self.odom_frame + odom.child_frame_id = self.base_frame + odom.pose.pose.position.x = px + odom.pose.pose.position.y = py + odom.pose.pose.position.z = pz + odom.pose.pose.orientation.x = qx + odom.pose.pose.orientation.y = qy + odom.pose.pose.orientation.z = qz + odom.pose.pose.orientation.w = qw + self.pub_odom.publish(odom) + def _publish_camera_frame(self, stamp: TimeMsg) -> None: if self._renderer is None: if self.model.vis.global_.offwidth < self.cam_width: From 3db6cc2cc0f46969f0dbdd1fe6999eda1f60baeb Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 12:37:09 -0600 Subject: [PATCH 07/19] :lipstick: RViz: default the 3D lidar cloud off so the SLAM map reads clearly Co-Authored-By: Claude Opus 4.8 --- docker/scripts/h1_sim.rviz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/scripts/h1_sim.rviz b/docker/scripts/h1_sim.rviz index a18a2c6..4d40d32 100644 --- a/docker/scripts/h1_sim.rviz +++ b/docker/scripts/h1_sim.rviz @@ -46,7 +46,7 @@ Visualization Manager: Value: /robot_description - Class: rviz_default_plugins/PointCloud2 Name: Lidar - Enabled: true + Enabled: false Topic: Depth: 5 Durability Policy: Volatile From 8669d9db7e6a3e752251dea74cc63296e18ba446 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 13:10:36 -0600 Subject: [PATCH 08/19] :bug: Fix SLAM demo: pass HAMS_SLAM to ros, relax p2l band, add HAMS_CAMERAS Three fixes to make the walk+map demo actually build a map: - HAMS_SLAM was never threaded through compose to the ros container, so the bringup never launched pointcloud_to_laserscan / slam_toolbox (earlier "working" runs were manual). Add the passthrough. - pointcloud_to_laserscan's h1_navigation tuning (range_min 0.6, band -0.90..0.55) filtered every return out for close-quarters kitchen spawns (drops the counter the robot is against, keeps only the floor which is below the band). Relax to range_min 0.4, band -0.85..1.2, range_max 8 -> ~128 returns; verified the occupancy /map builds in RViz as the robot walks/turns. - HAMS_CAMERAS=0 drops the 3 RGBD camera renders (the heaviest per-step CPU cost); ~doubles sim RTF (0.1 -> 0.2x) for locomotion/SLAM. Lidar + odom unaffected. Co-Authored-By: Claude Opus 4.8 --- .../launch/h1_sim_bringup_mac.launch.py | 11 ++++++++--- docker/docker-compose.mac.yml | 6 ++++++ h1_robocasa/h12_mujoco.py | 17 ++++++++++++----- h1_robocasa/mujoco_ros_bridge.py | 2 +- 4 files changed, 27 insertions(+), 9 deletions(-) diff --git a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py index 3756685..ff8e9c8 100644 --- a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py @@ -123,12 +123,17 @@ def generate_launch_description(): name='pointcloud_to_laserscan', parameters=[{ 'target_frame': 'pelvis', - 'min_height': -0.90, 'max_height': 0.55, + # Relaxed vs h1_navigation's FAST-LIO tuning: the raw sim lidar in + # the cluttered kitchen needs a lower range_min (catch the counter + # the robot is right up against, while still dropping the body, + # which is <=0.35 m from pelvis) and a taller band (counter tops / + # cabinets). The tight nav config filtered every return out here. + 'min_height': -0.85, 'max_height': 1.2, 'angle_min': -3.14159, 'angle_max': 3.14159, 'angle_increment': 0.0087, - 'range_min': 0.6, 'range_max': 6.0, + 'range_min': 0.4, 'range_max': 8.0, 'use_inf': True, 'scan_time': 0.0333, - 'transform_tolerance': 0.3, 'queue_size': 20, + 'transform_tolerance': 1.0, 'queue_size': 20, }, sim_time_param], remappings=[('cloud_in', '/livox/pointcloud'), ('scan', '/converted_scan')], output='screen', diff --git a/docker/docker-compose.mac.yml b/docker/docker-compose.mac.yml index f04223d..85a397c 100644 --- a/docker/docker-compose.mac.yml +++ b/docker/docker-compose.mac.yml @@ -36,6 +36,9 @@ services: # cannot forward to XQuartz. View it with docker/scripts/mac_vnc_tunnel.sh: # HAMS_DISPLAY=vnc docker compose -f docker/docker-compose.mac.yml up robocasa - HAMS_DISPLAY=${HAMS_DISPLAY:-headless} + # HAMS_CAMERAS=0 drops the 3 RGBD camera renders (the heaviest per-step CPU + # cost) to speed the sim up for locomotion/SLAM. Lidar + odom are unaffected. + - HAMS_CAMERAS=${HAMS_CAMERAS:-1} # rclpy runs on FastDDS so it does not load a 2nd CycloneDDS libddsc # alongside unitree_sdk2py's (arm64 in-process coexistence fix). - RMW_IMPLEMENTATION=rmw_fastrtps_cpp @@ -79,6 +82,9 @@ services: # =walk runs the walk policy (falls in this sim). Default (unset) keeps the # band tether holding the robot upright with upper-body IK only. - HAMS_LOWERBODY=${HAMS_LOWERBODY:-} + # HAMS_SLAM=1 adds pointcloud_to_laserscan + slam_toolbox to the bringup: + # the Livox cloud -> 2D scan -> occupancy /map against the sim's /odom. + - HAMS_SLAM=${HAMS_SLAM:-0} volumes: - ../core_ws:/home/code/core_ws - ../CL_Assets:/home/code/CL_Assets:ro diff --git a/h1_robocasa/h12_mujoco.py b/h1_robocasa/h12_mujoco.py index d15cca6..53c8866 100644 --- a/h1_robocasa/h12_mujoco.py +++ b/h1_robocasa/h12_mujoco.py @@ -1,5 +1,6 @@ import argparse import math +import os import random import threading import time @@ -192,13 +193,19 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None): # unprefixed (ctor defaults). cameras: (mujoco name, /realsense/, frame_id). # Head rides the robot prefix; the eye-in-hand gripper cameras ride the gripper # prefixes (same as the hand bridges below). + # RGBD camera rendering (3x 256x256 offscreen renders per frame) is the + # heaviest per-step cost on CPU. HAMS_CAMERAS=0 drops them to speed the sim up + # for locomotion/SLAM (lidar + odom are unaffected). + _cameras_on = os.environ.get('HAMS_CAMERAS', '1').strip().lower() not in ('0', 'off', 'false', 'no') + _cameras = [ + (f"{pfx}head_cam", "head", "camera_color_optical_frame"), + ("gripper0_left_hand_cam", "left_hand", "left_hand_camera_color_optical_frame"), + ("gripper0_right_hand_cam", "right_hand", "right_hand_camera_color_optical_frame"), + ] if _cameras_on else [] + print(f"[h12_mujoco] RGBD cameras {'ON' if _cameras_on else 'OFF (HAMS_CAMERAS=0)'}") ros_bridge = RosSensorBridge( model, data, - cameras=[ - (f"{pfx}head_cam", "head", "camera_color_optical_frame"), - ("gripper0_left_hand_cam", "left_hand", "left_hand_camera_color_optical_frame"), - ("gripper0_right_hand_cam", "right_hand", "right_hand_camera_color_optical_frame"), - ], + cameras=_cameras, cam_width=256, cam_height=256, # all 3 cameras render at 256x256 (RoboCasa default) # MID-360 fidelity: 360x56 @ 10Hz ~= 201k pts/s (real ~200k), 0.1m near / # 40m far range, per-point offset_time for FAST-LIO deskew. el_rays/rate diff --git a/h1_robocasa/mujoco_ros_bridge.py b/h1_robocasa/mujoco_ros_bridge.py index 4d83438..31307d6 100644 --- a/h1_robocasa/mujoco_ros_bridge.py +++ b/h1_robocasa/mujoco_ros_bridge.py @@ -345,7 +345,7 @@ def tick(self) -> None: except Exception as e: self.get_logger().warn(f"imu publish failed: {e}") - if sim_t - self._last_cam_sim_t >= self.cam_period: + if self._camera_specs and sim_t - self._last_cam_sim_t >= self.cam_period: self._last_cam_sim_t = sim_t try: self._publish_camera_frame(stamp) From a65a01b22d63c16b90d005ecd742472cd253f72e Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 15:10:23 -0600 Subject: [PATCH 09/19] :sparkles: Add nav2 autonomous navigation (HAMS_NAV2=1) on the SLAM map The walk policy is blind (tracks /cmd_vel, no obstacle awareness). nav2 adds the avoidance layer: it plans a collision-free path on the SLAM occupancy map + a lidar obstacle costmap and drives /cmd_vel, which the walk policy follows. - nav2_config_mac.yaml: nav2_config.yaml with the odom frame swapped from FAST-LIO's camera_init to the sim's odom, and inflation_radius trimmed to 0.2. RegulatedPurePursuit controller + Smac hybrid planner, costmap from /map + /converted_scan, robot_base_frame pelvis. - Bringup: HAMS_NAV2=1 includes nav2_bringup/navigation_launch.py (implies SLAM). Verified: the full stack (controller/planner/bt_navigator/costmaps/velocity_ smoother) comes up ACTIVE, and its velocity_smoother publishes /cmd_vel -> the walk policy consumes it directly. Send goals via RViz "2D Nav Goal" or /navigate_to_pose. - ros image: bake ros-humble-nav2-bringup. Note: nav2 rejects a goal when the robot's start cell is occupied ("start in lethal space") -- the robot must be standing in open floor, not pressed against a counter, for planning to succeed. Co-Authored-By: Claude Opus 4.8 --- .../h1_bringup/config/nav2_config_mac.yaml | 200 ++++++++++++++++++ .../launch/h1_sim_bringup_mac.launch.py | 21 ++ docker/RosDockerfile.slim.arm64 | 7 + docker/docker-compose.mac.yml | 4 + 4 files changed, 232 insertions(+) create mode 100644 core_ws/src/h1_bringup/config/nav2_config_mac.yaml diff --git a/core_ws/src/h1_bringup/config/nav2_config_mac.yaml b/core_ws/src/h1_bringup/config/nav2_config_mac.yaml new file mode 100644 index 0000000..1a9da0d --- /dev/null +++ b/core_ws/src/h1_bringup/config/nav2_config_mac.yaml @@ -0,0 +1,200 @@ +--- +/**: + ros__parameters: + robot_base_frame: pelvis + map_frame: map + odom_frame: odom + use_sim_time: ${use_sim_time} + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.01 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" # progress_checker_plugin: "progress_checker" For Humble and older + goal_checker_plugins: ["goal_checker"] + controller_plugins: ["FollowPath"] + + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.10 + yaw_goal_tolerance: 0.10 + stateful: True + FollowPath: + plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 0.25 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 1.8 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: false + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.6 + use_collision_detection: true + max_allowed_time_to_collision_up_to_carrot: 1.0 + use_regulated_linear_velocity_scaling: true + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 0.25 + use_cost_regulated_linear_velocity_scaling: false + cost_scaling_dist: 0.3 + cost_scaling_gain: 1.0 + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: true + allow_reversing: true + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 + max_robot_pose_search_dist: 10.0 + min_distance_to_obstacle: 0.4 + stateful: true +behavior_server: + ros__parameters: + use_sim_time: ${use_sim_time} + local_frame: odom + global_frame: map + robot_base_frame: pelvis + transform_tolerance: 0.1 +planner_server: + ros__parameters: + use_sim_time: ${use_sim_time} + planner_plugins: + - GridBased + GridBased: + plugin: nav2_smac_planner/SmacPlannerHybrid + tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + terminal_checking_interval: 5000 # number of iterations between checking if the goal has been cancelled or planner timed out + max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. + motion_model_for_search: "REEDS_SHEPP" # Allows reverse motion; humanoid can step backwards + cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search) + analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius + analytic_expansion_max_cost: 200 # For Hybrid/Lattice nodes: The maximum single cost for any part of an analytic expansion to contain and be valid (except when necessary on approach to goal) + analytic_expansion_max_cost_override: false # For Hybrid/Lattice nodes: Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required). If expansion is within 2*pi*min_r of the goal, then it will override the max cost if ``false``. + minimum_turning_radius: 0.20 # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle + reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.0 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0 + non_straight_penalty: 1.20 # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + retrospective_penalty: 0.025 # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0 + rotation_penalty: 5.0 # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings. + lookup_table_size: 20.0 # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. + smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes. + debug_visualizations: True # For Hybrid/Lattice nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further + ros__parameters: + global_frame: map + robot_base_frame: pelvis + allow_unknown: false + default_tolerance: 0.1 + transform_tolerance: 0.01 + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: ${use_sim_time} + robot_base_frame: pelvis + global_frame: map + update_frequency: 5.0 + publish_frequency: 5.0 + resolution: 0.05 + track_unknown_space: true + always_send_full_costmap: true + plugins: + - static_layer + - obstacle_layer + - inflation_layer + footprint: "[ [0.20, 0.275], [0.20, -0.275], [-0.20, -0.275], [-0.20, 0.275] ]" + static_layer: + plugin: nav2_costmap_2d::StaticLayer + ros__parameters: + map_subscribe_topic: /map + trinary_map: false + unknown_cost_value: 0 + lethal_cost_threshold: 100 + track_unknown_space: true + obstacle_layer: + plugin: nav2_costmap_2d::ObstacleLayer + ros__parameters: + enabled: true + obstacle_build_latch: false + laser_scan_topic: /converted_scan + max_obstacle_range: 2.5 + observation_sources: laser_scan_sensor + laser_scan_sensor: + topic: /converted_scan + max_obstacle_range: 2.5 + obstacle_min_range: 0.6 # drop near-pelvis self-returns (defence in depth) + inf_is_valid: true + clear_buffer_on_each_layer: true + data_type: LaserScan + marking: true + clearing: true + inflation_layer: + plugin: nav2_costmap_2d::InflationLayer + ros__parameters: + enabled: true + cost_scaling_factor: 3.0 + inflation_radius: 0.2 +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 3.0 + publish_frequency: 3.0 + global_frame: odom + robot_base_frame: pelvis + use_sim_time: ${use_sim_time} + rolling_window: true + width: 6 + height: 6 + resolution: 0.06 + footprint: "[ [0.20, 0.275], [0.20, -0.275], [-0.20, -0.275], [-0.20, 0.275] ]" + plugins: ["static_layer", "voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 1.0 + inflation_radius: 0.2 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 60 # 0.05 * 60 = 3.0 m tall (was 16 -> 0.8 m); contains the ~2 m sensor/obstacle plane in odom + max_obstacle_height: 3.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /converted_scan + max_obstacle_height: 3.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.6 # drop near-pelvis self-returns (defence in depth) + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True diff --git a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py index ff8e9c8..8246111 100644 --- a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py @@ -20,6 +20,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node ASSETS_DIR = '/home/code/CL_Assets' @@ -151,4 +153,23 @@ def generate_launch_description(): output='screen', )) + # Optional autonomous navigation (HAMS_NAV2=1, implies SLAM for the map): the + # nav2 stack plans a collision-free path on the SLAM map + lidar costmap and + # drives it via /cmd_vel, which the walk policy consumes (nav2's + # velocity_smoother publishes /cmd_vel). Send goals with the RViz "2D Nav Goal" + # tool or the /navigate_to_pose action. The robot must be in walk mode + # (/lowerbody/start_walk) and standing in open floor — nav2 rejects a goal if + # the robot's start cell is occupied (e.g. pressed against a counter). + # nav2_config_mac.yaml is nav2_config.yaml with the odom frame swapped from + # FAST-LIO's camera_init to the sim's odom. + if os.environ.get('HAMS_NAV2', '').strip().lower() in ('1', 'true', 'on'): + nav2_launch = os.path.join( + get_package_share_directory('nav2_bringup'), 'launch', 'navigation_launch.py') + nav2_cfg = os.path.join( + get_package_share_directory('h1_bringup'), 'config', 'nav2_config_mac.yaml') + nodes.append(IncludeLaunchDescription( + PythonLaunchDescriptionSource(nav2_launch), + launch_arguments={'params_file': nav2_cfg, 'use_sim_time': 'true'}.items(), + )) + return LaunchDescription(nodes) diff --git a/docker/RosDockerfile.slim.arm64 b/docker/RosDockerfile.slim.arm64 index 14b54d3..760522f 100644 --- a/docker/RosDockerfile.slim.arm64 +++ b/docker/RosDockerfile.slim.arm64 @@ -137,6 +137,13 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ ros-humble-pointcloud-to-laserscan \ && rm -rf /var/lib/apt/lists/* +# Optional autonomous navigation (HAMS_NAV2=1): the nav2 stack plans a +# collision-free path on the SLAM map + lidar costmap and drives /cmd_vel (which +# the walk policy follows). See nav2_config_mac.yaml. +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-nav2-bringup \ + && rm -rf /var/lib/apt/lists/* + # Overlay the colcon install in interactive shells so `docker exec … bash` sees # the same packages as the running bringup (incl. h1_bringup, so # `ros2 launch h1_bringup ` works). launch_ros_mac.sh builds to diff --git a/docker/docker-compose.mac.yml b/docker/docker-compose.mac.yml index 85a397c..1074365 100644 --- a/docker/docker-compose.mac.yml +++ b/docker/docker-compose.mac.yml @@ -85,6 +85,10 @@ services: # HAMS_SLAM=1 adds pointcloud_to_laserscan + slam_toolbox to the bringup: # the Livox cloud -> 2D scan -> occupancy /map against the sim's /odom. - HAMS_SLAM=${HAMS_SLAM:-0} + # HAMS_NAV2=1 adds the nav2 stack (implies HAMS_SLAM for the map): plans a + # collision-free path on the SLAM map and drives /cmd_vel (the walk policy + # follows it). Send goals via RViz "2D Nav Goal" or /navigate_to_pose. + - HAMS_NAV2=${HAMS_NAV2:-0} volumes: - ../core_ws:/home/code/core_ws - ../CL_Assets:/home/code/CL_Assets:ro From c1a688816db87e97300f404e6e938b15d52e5749 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 15:30:25 -0600 Subject: [PATCH 10/19] :sparkles: HAMS_SPAWN_BACKOFF: spawn the robot in open floor (for nav2) place_robot_collision_free gains extra_backoff: after clearing the spawn collision, keep backing the robot off the counter (its -x) up to N metres, stopping before a new collision behind, so it stands in open floor with room in front. HAMS_SPAWN_BACKOFF (metres, default 0 = at the counter for manipulation) drives it from h12_mujoco. Verified: "backed robot 1.00 m further into open floor". Co-Authored-By: Claude Opus 4.8 --- docker/docker-compose.mac.yml | 4 ++++ h1_robocasa/h12_mujoco.py | 5 +++++ h1_robocasa/h1_2_robosuite.py | 22 ++++++++++++++++++++-- 3 files changed, 29 insertions(+), 2 deletions(-) diff --git a/docker/docker-compose.mac.yml b/docker/docker-compose.mac.yml index 1074365..0ebc2fe 100644 --- a/docker/docker-compose.mac.yml +++ b/docker/docker-compose.mac.yml @@ -39,6 +39,10 @@ services: # HAMS_CAMERAS=0 drops the 3 RGBD camera renders (the heaviest per-step CPU # cost) to speed the sim up for locomotion/SLAM. Lidar + odom are unaffected. - HAMS_CAMERAS=${HAMS_CAMERAS:-1} + # HAMS_SPAWN_BACKOFF (metres) backs the robot off the counter into open floor + # at spawn (default 0 = at the counter for manipulation). ~1.0 gives nav2 room + # to plan (it rejects a goal when the robot's start cell is occupied). + - HAMS_SPAWN_BACKOFF=${HAMS_SPAWN_BACKOFF:-0} # rclpy runs on FastDDS so it does not load a 2nd CycloneDDS libddsc # alongside unitree_sdk2py's (arm64 in-process coexistence fix). - RMW_IMPLEMENTATION=rmw_fastrtps_cpp diff --git a/h1_robocasa/h12_mujoco.py b/h1_robocasa/h12_mujoco.py index 53c8866..d13c699 100644 --- a/h1_robocasa/h12_mujoco.py +++ b/h1_robocasa/h12_mujoco.py @@ -129,9 +129,14 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None): init_qpos = _initial_motor_qpos() data.qpos[resolver.motor_qpos] = init_qpos data.qvel[:] = 0.0 + # HAMS_SPAWN_BACKOFF backs the robot further off the counter into open + # floor (default 0 = at the counter for manipulation). Set ~1.0 for nav2, + # which needs the robot's start cell to be free to plan. + _spawn_backoff = float(os.environ.get("HAMS_SPAWN_BACKOFF", "0") or 0) h1_2_robosuite.place_robot_collision_free( env, env.init_robot_base_pos, h1_2_robosuite._euler_to_wxyz(getattr(env, "init_robot_base_ori", None)), + extra_backoff=_spawn_backoff, ) print("[h12_mujoco] initial stance from baked-in sim defaults") except Exception as e: diff --git a/h1_robocasa/h1_2_robosuite.py b/h1_robocasa/h1_2_robosuite.py index 07e27da..e98fdd5 100644 --- a/h1_robocasa/h1_2_robosuite.py +++ b/h1_robocasa/h1_2_robosuite.py @@ -291,7 +291,8 @@ def _robot_env_contacts(model, data, prefixes=("robot0_", "gripper0_")): return out -def place_robot_collision_free(env, base_pos, base_quat, step=0.02, max_iters=25, clearance=0.02): +def place_robot_collision_free(env, base_pos, base_quat, step=0.02, max_iters=25, + clearance=0.02, extra_backoff=0.0): """Place the robot collision-free at spawn by backing it away from whatever it overlaps. @@ -304,7 +305,13 @@ def place_robot_collision_free(env, base_pos, base_quat, step=0.02, max_iters=25 Tracks the least-penetrating position seen (fewest contacts, then least total depth). If still colliding after max_iters, restores that best position and warns (best-effort), so the sim still launches. Self-collisions at the zero - pose are not addressed here — translation can't fix them.""" + pose are not addressed here — translation can't fix them. + + extra_backoff: after clearing the spawn collision, keep backing off up to this + many more metres (stopping before a new collision behind) so the robot stands + in OPEN FLOOR with room in front — needed for nav2, which rejects a goal when + the robot's start cell is occupied. Default 0 keeps the robot at the counter + for manipulation reach.""" back = _backward_dir(base_quat) model = env.sim.model._model data = env.sim.data._data @@ -319,6 +326,17 @@ def place_robot_collision_free(env, base_pos, base_quat, step=0.02, max_iters=25 if not contacts: if i: print(f"[h1_2_robosuite] moved robot back {i * step:.2f} m to clear spawn collision") + if extra_backoff > 0.0: + last_clear, moved = pos.copy(), 0.0 + for _ in range(int(extra_backoff / step)): + pos[:2] += step * back[:2] + place_robot_clear(env, pos, base_quat, clearance) + if _robot_env_contacts(model, data): + break + last_clear, moved = pos.copy(), moved + step + place_robot_clear(env, last_clear, base_quat, clearance) + if moved: + print(f"[h1_2_robosuite] backed robot {moved:.2f} m further into open floor (nav spawn)") return pos[:2] += step * back[:2] place_robot_clear(env, best_pos, base_quat, clearance) # restore least-penetrating try From 7cbbd6d5b2f8e4bb6c5f44d1526afcb07a100438 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 15:33:54 -0600 Subject: [PATCH 11/19] :wrench: Raise nav p2l range_min to 0.65 to drop most robot self-returns The torso lidar returns the robot's own legs/feet at ~0.4m; mapping them puts a phantom obstacle under the robot so nav2 rejects the start. 0.65 drops most of it (the arms/thighs in gait still reach ~0.65m -- a proper self-filter is the full fix). Pairs with HAMS_SPAWN_BACKOFF (robot spawns off the counter, so 0.65 keeps the real walls). Co-Authored-By: Claude Opus 4.8 --- .../launch/h1_sim_bringup_mac.launch.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py index 8246111..8c5e5d8 100644 --- a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py @@ -125,15 +125,18 @@ def generate_launch_description(): name='pointcloud_to_laserscan', parameters=[{ 'target_frame': 'pelvis', - # Relaxed vs h1_navigation's FAST-LIO tuning: the raw sim lidar in - # the cluttered kitchen needs a lower range_min (catch the counter - # the robot is right up against, while still dropping the body, - # which is <=0.35 m from pelvis) and a taller band (counter tops / - # cabinets). The tight nav config filtered every return out here. + # Relaxed vs h1_navigation's FAST-LIO tuning for the raw sim lidar: + # a taller band (counter tops / cabinets). range_min 0.65 drops the + # robot's OWN legs/feet — the torso lidar looks down and returns them + # at ~0.4 m; mapping those puts a phantom obstacle under the robot and + # nav2 rejects the start as "lethal space". 0.65 clears the body while + # still catching the counter (the robot spawns ~1 m off it via + # HAMS_SPAWN_BACKOFF). Lower it only if you also see real obstacles + # missed at close range. 'min_height': -0.85, 'max_height': 1.2, 'angle_min': -3.14159, 'angle_max': 3.14159, 'angle_increment': 0.0087, - 'range_min': 0.4, 'range_max': 8.0, + 'range_min': 0.65, 'range_max': 8.0, 'use_inf': True, 'scan_time': 0.0333, 'transform_tolerance': 1.0, 'queue_size': 20, }, sim_time_param], From 5d7d5d460381434883d692085a9a1bbfbe687379 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 15:54:06 -0600 Subject: [PATCH 12/19] :sparkles: Lidar self-filter: drop the robot's own legs/arms from the scan The torso-mounted lidar looks down and its rays hit the robot's own legs/arms (mj_multiRay's bodyexclude only drops the one torso body). Those self-returns get mapped as a phantom obstacle under the robot, so nav2 rejects the start as "lethal space". Flag every geom belonging to a robot body (name prefix robot0_/gripper0_) and drop rays that hit them in _publish_lidar_scan, using the per-geom mask so real obstacles are kept even when a limb is at the same range. Default on for this sim (HAMS_LIDAR_SELF_FILTER=0 restores the limbs as occluders). Verified: the closest scan return jumps from ~0.4 m (legs) to ~0.7 m (real structure). Co-Authored-By: Claude Opus 4.8 --- h1_robocasa/h12_mujoco.py | 10 ++++++++++ h1_robocasa/mujoco_ros_bridge.py | 24 ++++++++++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/h1_robocasa/h12_mujoco.py b/h1_robocasa/h12_mujoco.py index d13c699..cbcb225 100644 --- a/h1_robocasa/h12_mujoco.py +++ b/h1_robocasa/h12_mujoco.py @@ -223,6 +223,16 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None): imu_gyro_sensor=f"{pfx}livox_imu_gyro", imu_acc_sensor=f"{pfx}livox_imu_acc", base_body_id=odom_base_body_id, # -> odom -> pelvis TF + /odom + # Drop the robot's own legs/arms from the lidar (self-returns otherwise map + # a phantom obstacle under the robot and nav2 rejects the start). Default on + # for this locomotion/SLAM/nav sim; HAMS_LIDAR_SELF_FILTER=0 restores them + # as occluders. + lidar_self_prefixes=( + (pfx, "gripper0_") + if os.environ.get("HAMS_LIDAR_SELF_FILTER", "1").strip().lower() + not in ("0", "off", "false", "no") + else () + ), sim_lock=sim_lock, ) diff --git a/h1_robocasa/mujoco_ros_bridge.py b/h1_robocasa/mujoco_ros_bridge.py index 31307d6..0d38722 100644 --- a/h1_robocasa/mujoco_ros_bridge.py +++ b/h1_robocasa/mujoco_ros_bridge.py @@ -134,6 +134,7 @@ def __init__( odom_frame: str = "odom", base_frame: str = "pelvis", odom_rate_hz: float = 50.0, + lidar_self_prefixes: tuple = (), elastic_band=None, sim_lock=None, ): @@ -164,6 +165,21 @@ def __init__( raise RuntimeError(f"body '{lidar_exclude_body}' not found in MJCF") self.lidar_exclude_body_id = int(torso_id) self.geom_excluded = (model.geom_bodyid == self.lidar_exclude_body_id) + # Optional self-return filter: mj_multiRay's bodyexclude only drops ONE + # body (the torso the lidar sits on), so the downward rays still hit the + # robot's own legs/arms/grippers and map a phantom obstacle under the robot + # (nav2 then rejects the start as "lethal space"). When lidar_self_prefixes + # is given, flag every geom whose body name starts with one of them so + # those hits are dropped in _publish_lidar_scan. Off by default (the limbs + # occlude like real obstacles); enabled for locomotion/SLAM/nav. + self.geom_is_robot = np.zeros(model.ngeom, dtype=bool) + if lidar_self_prefixes: + _pfx = tuple(lidar_self_prefixes) + for g in range(model.ngeom): + bn = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, + int(model.geom_bodyid[g])) or "" + if bn.startswith(_pfx): + self.geom_is_robot[g] = True # mj_multiRay / mj_ray require a (6, 1) column vector here. self.lidar_geomgroup = np.array([1, 1, 1, 1, 1, 1], dtype=np.uint8).reshape(6, 1) @@ -542,6 +558,14 @@ def _publish_lidar_scan(self, stamp: TimeMsg) -> None: ) hit_dists = dists.ravel() + # Drop self-returns: rays that hit the robot's own body (legs/arms/grippers + # — the torso is already excluded at cast time). Uses the per-geom robot + # mask so real obstacles are kept even when a limb is at the same range. + if self.geom_is_robot.any(): + gid = geomids.ravel() + self_hit = (gid >= 0) & self.geom_is_robot[np.where(gid >= 0, gid, 0)] + hit_dists = np.where(self_hit, -1.0, hit_dists) + # Points in lidar-local frame: local_dir * distance. valid = (hit_dists >= self.lidar_min_range) & (hit_dists <= self.lidar_max_range) pts = (self.lidar_local_dirs[valid] * hit_dists[valid, np.newaxis]).astype(np.float32) From 8e35087a39b7d3504effc48483f08c41b1bf907f Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 15:58:47 -0600 Subject: [PATCH 13/19] :sparkles: RViz: nav2 path (/plan) display + 2D Goal Pose tool Adds the green planned-path display and the SetGoal tool (-> /goal_pose) so you can click navigation goals directly in RViz. Completes the autonomous-nav demo view: occupancy map + laser scan + robot + planned path. Co-Authored-By: Claude Opus 4.8 --- docker/scripts/h1_sim.rviz | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/docker/scripts/h1_sim.rviz b/docker/scripts/h1_sim.rviz index 4d40d32..ef21d9d 100644 --- a/docker/scripts/h1_sim.rviz +++ b/docker/scripts/h1_sim.rviz @@ -87,7 +87,20 @@ Visualization Manager: Color: 255; 85; 0 Color Transformer: FlatColor Decay Time: 0 + - Class: rviz_default_plugins/Path + Name: NavPath + Enabled: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Color: 0; 255; 0 + Line Style: Lines Tools: + - Class: rviz_default_plugins/SetGoal + Default Topic: /goal_pose - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select - Class: rviz_default_plugins/FocusCamera From 0419d612d8a277ad4e0dfb73a705671a5ce887bb Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 16:05:46 -0600 Subject: [PATCH 14/19] :sparkles: Basic frontier-based autonomous exploration frontier_explore.py: reads the slam /map, finds frontiers (free cells touching unknown, not against walls), clusters them (scipy.ndimage), and sends the nearest sizeable one as a nav2 goal; loops until no frontiers remain. Nearest-frontier policy with a failure blacklist and per-goal timeout. robot_cli.sh: rob_explore helper (start_walk + run it). Needs HAMS_SLAM=1 HAMS_NAV2=1. Co-Authored-By: Claude Opus 4.8 --- docker/scripts/frontier_explore.py | 206 +++++++++++++++++++++++++++++ docker/scripts/robot_cli.sh | 10 ++ 2 files changed, 216 insertions(+) create mode 100644 docker/scripts/frontier_explore.py diff --git a/docker/scripts/frontier_explore.py b/docker/scripts/frontier_explore.py new file mode 100644 index 0000000..f5f5ed1 --- /dev/null +++ b/docker/scripts/frontier_explore.py @@ -0,0 +1,206 @@ +#!/usr/bin/env python3 +"""Very basic frontier-based autonomous exploration for the H1 Mac sim. + +Loop: + 1. Read the slam_toolbox /map (occupancy grid). + 2. Find frontiers = FREE cells that touch UNKNOWN space (the edge of the map). + 3. Cluster them; send the nearest sizeable cluster's centroid as a nav2 goal. + 4. When the goal finishes (or is unreachable -> blacklisted), pick the next one. + 5. Stop when no frontiers remain (map fully explored). + +nav2 plans a collision-free path to each frontier and drives /cmd_vel, which the +walk policy follows -- so the robot must be in walk mode (rob_stand; rob_walk) +with HAMS_SLAM=1 HAMS_NAV2=1. This is deliberately simple: nearest-frontier with a +failure blacklist, no fancy information-gain scoring. + +Run: python3 /home/code/h12_sim_scripts/frontier_explore.py (or: rob_explore) +""" +import math + +import numpy as np +import rclpy +import tf2_ros +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import PoseStamped +from nav2_msgs.action import NavigateToPose +from nav_msgs.msg import OccupancyGrid +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import (DurabilityPolicy, HistoryPolicy, QoSProfile, + ReliabilityPolicy) +from scipy import ndimage + + +class FrontierExplorer(Node): + def __init__(self): + super().__init__("frontier_explorer") + self.declare_parameter("map_topic", "/map") + self.declare_parameter("base_frame", "pelvis") + self.declare_parameter("min_frontier_cells", 6) # ignore tiny frontiers + self.declare_parameter("blacklist_radius", 0.6) # m; near a failed goal + self.declare_parameter("min_goal_distance", 0.7) # m; skip frontiers underfoot + self.declare_parameter("goal_timeout", 120.0) # s; cancel a stuck goal + + self.base_frame = self.get_parameter("base_frame").value + self.min_cells = int(self.get_parameter("min_frontier_cells").value) + self.bl_radius = float(self.get_parameter("blacklist_radius").value) + self.min_dist = float(self.get_parameter("min_goal_distance").value) + self.goal_timeout = float(self.get_parameter("goal_timeout").value) + + self._map = None + # slam_toolbox latches /map (transient-local, reliable). + map_qos = QoSProfile(reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, depth=1) + self.create_subscription(OccupancyGrid, + self.get_parameter("map_topic").value, + self._on_map, map_qos) + self._nav = ActionClient(self, NavigateToPose, "/navigate_to_pose") + self._tf_buffer = tf2_ros.Buffer() + self._tf_listener = tf2_ros.TransformListener(self._tf_buffer, self) + + self._blacklist = [] # [(x, y), ...] map-frame goals that failed + self._busy = False # a goal is in flight + self._goal_xy = None + self._goal_deadline = None + self._goal_handle = None + self._done = False + + self.create_timer(2.0, self._tick) + self.get_logger().info( + "frontier_explorer up: waiting for /map + nav2. Robot must be in walk " + "mode (rob_walk).") + + # -- inputs -------------------------------------------------------------- + def _on_map(self, msg): + self._map = msg + + def _robot_xy(self): + try: + t = self._tf_buffer.lookup_transform("map", self.base_frame, + rclpy.time.Time()) + return t.transform.translation.x, t.transform.translation.y + except Exception: + return None + + # -- frontier detection -------------------------------------------------- + def _frontiers(self): + """Return [(x, y, n_cells)] frontier-cluster centroids in the map frame.""" + m = self._map + g = np.array(m.data, dtype=np.int16).reshape(m.info.height, m.info.width) + free = (g >= 0) & (g < 25) # confidently free + unknown = g < 0 # -1 == unexplored + occupied = g >= 65 # walls + # A frontier cell is free and 8-adjacent to unknown, but NOT touching a + # wall (those edges are just the far side of an obstacle, not open space). + unknown_adj = ndimage.binary_dilation(unknown, iterations=1) + wall_adj = ndimage.binary_dilation(occupied, iterations=1) + frontier = free & unknown_adj & ~wall_adj + + labels, n = ndimage.label(frontier, structure=np.ones((3, 3))) + res = m.info.resolution + ox, oy = m.info.origin.position.x, m.info.origin.position.y + out = [] + for i in range(1, n + 1): + ys, xs = np.where(labels == i) + if len(xs) < self.min_cells: + continue + cx = ox + (xs.mean() + 0.5) * res + cy = oy + (ys.mean() + 0.5) * res + out.append((cx, cy, int(len(xs)))) + return out + + def _blacklisted(self, x, y): + return any(math.hypot(x - bx, y - by) < self.bl_radius + for bx, by in self._blacklist) + + # -- exploration loop ---------------------------------------------------- + def _tick(self): + if self._done or self._map is None: + return + # Time out a stuck goal. + if self._busy: + if self._goal_deadline is not None and \ + self.get_clock().now().nanoseconds * 1e-9 > self._goal_deadline: + self.get_logger().warn("goal timed out — cancelling + blacklisting") + if self._goal_xy: + self._blacklist.append(self._goal_xy) + if self._goal_handle is not None: + self._goal_handle.cancel_goal_async() + self._busy = False + return + + rp = self._robot_xy() + if rp is None: + return + cand = [(x, y, s) for (x, y, s) in self._frontiers() + if not self._blacklisted(x, y) + and math.hypot(x - rp[0], y - rp[1]) > self.min_dist] + if not cand: + self.get_logger().info("*** No frontiers left — exploration complete. ***") + self._done = True + return + # Nearest sizeable frontier (simplest sensible policy). + gx, gy, sz = min(cand, key=lambda c: math.hypot(c[0] - rp[0], c[1] - rp[1])) + self._send_goal(rp, gx, gy, sz) + + def _send_goal(self, rp, gx, gy, sz): + if not self._nav.wait_for_server(timeout_sec=2.0): + self.get_logger().warn("nav2 /navigate_to_pose not available yet") + return + self._busy = True + self._goal_xy = (gx, gy) + self._goal_deadline = self.get_clock().now().nanoseconds * 1e-9 + self.goal_timeout + yaw = math.atan2(gy - rp[1], gx - rp[0]) # face the frontier + ps = PoseStamped() + ps.header.frame_id = "map" + ps.header.stamp = self.get_clock().now().to_msg() + ps.pose.position.x, ps.pose.position.y = float(gx), float(gy) + ps.pose.orientation.z = math.sin(yaw / 2.0) + ps.pose.orientation.w = math.cos(yaw / 2.0) + goal = NavigateToPose.Goal() + goal.pose = ps + self.get_logger().info( + f"exploring frontier at ({gx:.2f}, {gy:.2f}) [{sz} cells, " + f"{math.hypot(gx - rp[0], gy - rp[1]):.1f} m away]; " + f"{len(self._blacklist)} blacklisted") + self._nav.send_goal_async(goal).add_done_callback(self._on_response) + + def _on_response(self, fut): + gh = fut.result() + if gh is None or not gh.accepted: + self.get_logger().warn("goal rejected — blacklisting") + if self._goal_xy: + self._blacklist.append(self._goal_xy) + self._busy = False + return + self._goal_handle = gh + gh.get_result_async().add_done_callback(self._on_result) + + def _on_result(self, fut): + status = fut.result().status + if status == GoalStatus.STATUS_SUCCEEDED: + self.get_logger().info("frontier reached") + else: + self.get_logger().warn(f"goal ended (status {status}) — blacklisting") + if self._goal_xy: + self._blacklist.append(self._goal_xy) + self._goal_handle = None + self._busy = False + + +def main(): + rclpy.init() + node = FrontierExplorer() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/docker/scripts/robot_cli.sh b/docker/scripts/robot_cli.sh index 3c0cf9d..3c5cd89 100644 --- a/docker/scripts/robot_cli.sh +++ b/docker/scripts/robot_cli.sh @@ -100,6 +100,16 @@ rob_stop() { # rob_odom — current base position in the odom (world) frame. rob_odom() { timeout 5 ros2 topic echo /odom --field pose.pose.position --once; } +# rob_explore — autonomous frontier-based exploration (needs HAMS_SLAM=1 HAMS_NAV2=1). +# Engages walk mode, then repeatedly navigates to the nearest unexplored frontier on +# the SLAM map until it's fully mapped. Best after rob_stand (stable stance). Ctrl-C +# to stop. Watch the map grow in RViz (6081). +rob_explore() { + ros2 service call /lowerbody/start_walk std_srvs/srv/Trigger >/dev/null 2>&1 + python3 /home/code/h12_sim_scripts/frontier_explore.py +} + echo "robot_cli loaded." echo " postures : rob_pose t_pose | rob_grip right close" echo " locomote : rob_stand -> rob_walk -> rob_go 0.4 0 0.3 (fwd+turn) -> rob_stop (needs HAMS_LOWERBODY=switch)" +echo " explore : rob_stand -> rob_explore (autonomous frontier mapping; needs HAMS_SLAM=1 HAMS_NAV2=1)" From c6c1c083008736c1b94c4db3eb17278b8876b80a Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 16:35:51 -0600 Subject: [PATCH 15/19] :bug: Fix nav2 costmap so the planner actually sees obstacles The global costmap's obstacle_layer had its params nested under an extra `ros__parameters:` level, so nav2 silently ignored them and gave the layer zero observation sources. The planner was steering on the static SLAM /map alone -- a thin 2D slice that misses appliances -- so it routed the robot straight into them (looked like "MuJoCo collision is broken"; it wasn't: the robot correctly could not pass through, and nav2's progress-checker aborted while the frontier explorer blacklisted and moved on). Fixes: - Flatten the global static/obstacle/inflation layer params to sit directly under the plugin key (matching the working local_costmap). The obstacle layer now subscribes to laser_scan_sensor + pointcloud_sensor, and the inflation layer uses its configured radius instead of defaults. - Add the full 3D /livox/pointcloud as a marking source to BOTH costmaps, height band 0.15-1.8 m (drops the floor at z~0, keeps counter/appliance height; pelvis stands at ~0.97 m). Self-returns are already stripped by the bridge's lidar_self_filter. - Fix the local voxel layer's z_voxels 60 -> 16 (nav2 hard-caps at 16, so it was silently clipped to 0.8 m tall, below counter height) with z_resolution 0.125 -> 2.0 m tall. Verified: global_costmap now "Subscribed to Topics: laser_scan_sensor pointcloud_sensor" (was empty), voxel_grid 16-z-cap error gone. Co-Authored-By: Claude Opus 4.8 --- .../h1_bringup/config/nav2_config_mac.yaml | 84 ++++++++++++------- 1 file changed, 56 insertions(+), 28 deletions(-) diff --git a/core_ws/src/h1_bringup/config/nav2_config_mac.yaml b/core_ws/src/h1_bringup/config/nav2_config_mac.yaml index 1a9da0d..dea3717 100644 --- a/core_ws/src/h1_bringup/config/nav2_config_mac.yaml +++ b/core_ws/src/h1_bringup/config/nav2_config_mac.yaml @@ -125,37 +125,52 @@ global_costmap: - obstacle_layer - inflation_layer footprint: "[ [0.20, 0.275], [0.20, -0.275], [-0.20, -0.275], [-0.20, 0.275] ]" + # NOTE: costmap-layer params go DIRECTLY under the plugin key. An extra + # `ros__parameters:` level here silently hides them, so nav2 falls back to + # defaults: the obstacle_layer ended up with NO observation sources (planner + # blind to live obstacles, driving into appliances) and the inflation_layer + # ignored its radius. The local_costmap below shows the correct (flat) form. static_layer: plugin: nav2_costmap_2d::StaticLayer - ros__parameters: - map_subscribe_topic: /map - trinary_map: false - unknown_cost_value: 0 - lethal_cost_threshold: 100 - track_unknown_space: true + map_subscribe_topic: /map + trinary_map: false + unknown_cost_value: 0 + lethal_cost_threshold: 100 + track_unknown_space: true obstacle_layer: plugin: nav2_costmap_2d::ObstacleLayer - ros__parameters: - enabled: true - obstacle_build_latch: false - laser_scan_topic: /converted_scan + enabled: true + obstacle_build_latch: false + observation_sources: laser_scan_sensor pointcloud_sensor + laser_scan_sensor: + topic: /converted_scan max_obstacle_range: 2.5 - observation_sources: laser_scan_sensor - laser_scan_sensor: - topic: /converted_scan - max_obstacle_range: 2.5 - obstacle_min_range: 0.6 # drop near-pelvis self-returns (defence in depth) - inf_is_valid: true - clear_buffer_on_each_layer: true - data_type: LaserScan - marking: true - clearing: true + obstacle_min_range: 0.6 # drop near-pelvis self-returns (defence in depth) + inf_is_valid: true + clear_buffer_on_each_layer: true + data_type: LaserScan + marking: true + clearing: true + # Full 3-D Livox cloud so the *planner* sees obstacles the flat scan slice + # misses (appliances, counters, overhangs). Height band is in the map frame: + # the floor is z~0 (pelvis stands at ~0.97 m), so 0.15 m drops floor returns + # and 1.8 m caps at head height. Robot self-returns are already stripped by + # the bridge's lidar_self_filter, so the robot never marks itself. + pointcloud_sensor: + topic: /livox/pointcloud + data_type: PointCloud2 + min_obstacle_height: 0.15 + max_obstacle_height: 1.8 + obstacle_max_range: 5.0 + raytrace_max_range: 6.0 + marking: true + clearing: true + inf_is_valid: false inflation_layer: plugin: nav2_costmap_2d::InflationLayer - ros__parameters: - enabled: true - cost_scaling_factor: 3.0 - inflation_radius: 0.2 + enabled: true + cost_scaling_factor: 3.0 + inflation_radius: 0.2 local_costmap: local_costmap: ros__parameters: @@ -179,11 +194,11 @@ local_costmap: enabled: True publish_voxel_map: True origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 60 # 0.05 * 60 = 3.0 m tall (was 16 -> 0.8 m); contains the ~2 m sensor/obstacle plane in odom - max_obstacle_height: 3.0 + z_resolution: 0.125 # nav2's voxel_grid HARD-CAPS z_voxels at 16; 0.125*16 = 2.0 m + z_voxels: 16 # (60 was silently clamped to 16 -> only 0.8 m tall, missing counter-height obstacles) + max_obstacle_height: 2.0 mark_threshold: 0 - observation_sources: scan + observation_sources: scan pointcloud scan: topic: /converted_scan max_obstacle_height: 3.0 @@ -194,6 +209,19 @@ local_costmap: raytrace_min_range: 0.0 obstacle_max_range: 2.5 obstacle_min_range: 0.6 # drop near-pelvis self-returns (defence in depth) + # 3-D Livox cloud for local avoidance of obstacles the flat scan misses. + # Height band excludes the floor (z~0) and caps at head height (see the + # global obstacle_layer's pointcloud_sensor note). + pointcloud: + topic: /livox/pointcloud + data_type: "PointCloud2" + min_obstacle_height: 0.15 + max_obstacle_height: 1.8 + clearing: True + marking: True + raytrace_max_range: 5.0 + raytrace_min_range: 0.0 + obstacle_max_range: 4.0 static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True From a609dcac549533ae6e42cbf2b8e4b056a4b4d810 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 17:33:56 -0600 Subject: [PATCH 16/19] :bug: Make the sim motor watchdog sim-time based (stop spurious falls) The unitree_interface motor-command watchdog zeroed all motors if no rt/lowcmd arrived within 0.1s of WALL time. On the headless CPU sim running ~0.2x real-time, that 0.1s wall window is only ~20ms of sim time, so the compute-starved control loop (policy inference sharing cores with MuJoCo + nav2 + slam) kept overshooting it -- the watchdog released the motors and the robot dropped ~0.7s after the elastic band let go ("Command timeout! Releasing motors" right before each fall). Measure the watchdog in SIM time (data.time) instead, so the threshold is independent of how fast the sim actually runs, and default it to 0.5s sim (override via HAMS_CMD_TIMEOUT). A paused sim no longer trips it either. Verified: robot now survives the band release and stands freely to sim t=48s with no watchdog fire (was: fell at sim t~20s, 0.67s after release). Co-Authored-By: Claude Opus 4.8 --- h1_robocasa/unitree_interface.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/h1_robocasa/unitree_interface.py b/h1_robocasa/unitree_interface.py index dc4e213..ac92479 100644 --- a/h1_robocasa/unitree_interface.py +++ b/h1_robocasa/unitree_interface.py @@ -74,8 +74,14 @@ def __init__(self, model, data, lock=None, resolver=None): # on the floor — preferable to a stiff pose-hold snapshot, which # tries to freeze whatever chaotic mid-fall pose was sampled at the # moment of timeout and tends to NaN against contact dynamics. - self.last_cmd_time = time.time() - self.timeout = 0.1 + # Measured in SIM time (data.time), NOT wall time. The headless CPU sim + # runs at ~0.2x real-time, so a 0.1s *wall* window was only ~20ms of sim + # time; the compute-starved control loop (policy inference sharing cores + # with MuJoCo + nav2 + slam) kept overshooting it, so the watchdog zeroed + # the motors and dropped the robot ~0.7s after the band released. In sim + # time the threshold is independent of how fast the sim actually runs. + self.last_cmd_time = 0.0 # sim seconds of the last received /lowcmd + self.timeout = float(os.environ.get('HAMS_CMD_TIMEOUT', '0.5')) # sim seconds self.timeout_detected = False self.timeout_thread = RecurrentThread( interval=0.01, target=self.check_cmd_timeout, name='cmd_watchdog' @@ -120,7 +126,7 @@ def low_cmd_handler(self, msg: LowCmd_): return # Latch the command only (no data touch, no lock). write_ctrl turns it into # a torque every sim step against the CURRENT state. - self.last_cmd_time = time.time() + self.last_cmd_time = self.data.time # sim time; watchdog compares in sim time for i in range(self.num_motor): mc = msg.motor_cmd[i] self._cmd_mode[i] = mc.mode @@ -152,7 +158,9 @@ def write_ctrl(self): self.data.ctrl[ci] = 0.0 def check_cmd_timeout(self): - current_time = time.time() + if self.data is None: + return + current_time = self.data.time # sim time (paused sim -> no false timeout) if (current_time - self.last_cmd_time) > self.timeout: if not self.timeout_detected: self.timeout_detected = True From a50585a859ddad55d64765984f2a23435d6e8d9f Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 17:33:56 -0600 Subject: [PATCH 17/19] :bug: Clear stale X locks before Xvfb so container restarts don't crash `docker restart` reuses the container filesystem, so a leftover /tmp/.X99-lock (and its abstract socket) from the previous run made Xvfb abort with "Server is already active for display 99" -- the entrypoint then exit(1)'d and the whole sim container died on restart (no MuJoCo, no /clock, no VNC). Kill any stale Xvfb and rm the lock/socket before starting the VNC stack, in both the robocasa (:99) and ros (:100) launch scripts. Co-Authored-By: Claude Opus 4.8 --- docker/scripts/launch_robocasa_mac.sh | 7 +++++++ docker/scripts/launch_ros_mac.sh | 6 ++++++ 2 files changed, 13 insertions(+) diff --git a/docker/scripts/launch_robocasa_mac.sh b/docker/scripts/launch_robocasa_mac.sh index bf903d0..7fc2089 100755 --- a/docker/scripts/launch_robocasa_mac.sh +++ b/docker/scripts/launch_robocasa_mac.sh @@ -30,6 +30,13 @@ VNC_GEOMETRY="${VNC_GEOMETRY:-1280x800x24}" # framebuffer. All children are killed on exit via the trap below. start_vnc_stack() { echo "[launch_robocasa_mac] starting VNC stack on $VNC_DISPLAY ($VNC_GEOMETRY)" + # `docker restart` reuses the container FS, so a stale /tmp/.X99-lock (and the + # abstract socket) from the previous run makes Xvfb abort with "Server is + # already active for display 99". Clear any leftovers first so restarts work. + dnum="${VNC_DISPLAY#:}" + pkill -f "Xvfb ${VNC_DISPLAY}" 2>/dev/null || true + sleep 0.3 + rm -f "/tmp/.X${dnum}-lock" "/tmp/.X11-unix/X${dnum}" 2>/dev/null || true Xvfb "$VNC_DISPLAY" -screen 0 "$VNC_GEOMETRY" +extension GLX +render -noreset \ >/tmp/xvfb.log 2>&1 & export DISPLAY="$VNC_DISPLAY" diff --git a/docker/scripts/launch_ros_mac.sh b/docker/scripts/launch_ros_mac.sh index 969bfd7..ac92624 100755 --- a/docker/scripts/launch_ros_mac.sh +++ b/docker/scripts/launch_ros_mac.sh @@ -34,6 +34,12 @@ RVIZ_CONFIG="${RVIZ_CONFIG:-/home/code/h12_sim_scripts/h1_sim.rviz}" start_rviz_stack() { echo "[launch_ros_mac] starting RViz VNC stack on $RVIZ_DISPLAY ($RVIZ_GEOMETRY)" + # `docker restart` reuses the container FS: clear any stale X lock/socket from + # a previous run so Xvfb doesn't abort with "Server is already active". + dnum="${RVIZ_DISPLAY#:}" + pkill -f "Xvfb ${RVIZ_DISPLAY}" 2>/dev/null || true + sleep 0.3 + rm -f "/tmp/.X${dnum}-lock" "/tmp/.X11-unix/X${dnum}" 2>/dev/null || true Xvfb "$RVIZ_DISPLAY" -screen 0 "$RVIZ_GEOMETRY" +extension GLX +render -noreset \ >/tmp/xvfb_rviz.log 2>&1 & export DISPLAY="$RVIZ_DISPLAY" From 302f7d8acad301c99254cb499b6819bd7a4c8685 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 18:08:09 -0600 Subject: [PATCH 18/19] :memo: Document the autonomous navigation demo Add docs/NAVIGATION_DEMO.md (SLAM + nav2 + frontier exploration walkthrough: run command, what to watch, the env knobs, pipeline diagram, and nav-specific gotchas), link it from the README, and add a "Navigation demo" subsection. Also correct docs that the fixes made stale: - the walk policy stays upright now (via the switchable controller + the sim-time motor watchdog) -- was documented as "falls in this sim"; - HAMS_LOWERBODY=switch is the stand<->walk mode; - HAMS_SPAWN_BACKOFF is baked at container-create (recreate, don't restart) and 1.5-2.0 keeps the robot off the counters; - restart the two containers coherently (the sim owns /clock). Co-Authored-By: Claude Opus 4.8 --- README.md | 55 ++++++++++++++--- docker/docker-compose.mac.yml | 11 ++-- docs/NAVIGATION_DEMO.md | 113 ++++++++++++++++++++++++++++++++++ 3 files changed, 168 insertions(+), 11 deletions(-) create mode 100644 docs/NAVIGATION_DEMO.md diff --git a/README.md b/README.md index 287ad1f..4c11923 100644 --- a/README.md +++ b/README.md @@ -260,13 +260,42 @@ HAMS_DISPLAY=vnc HAMS_RVIZ=vnc HAMS_LOWERBODY=fame \ docker compose -f docker/docker-compose.mac.yml up ``` -Caveat: `HAMS_LOWERBODY=fame` **stands** (and squats via `/lowerbody/squat_cmd`) -but does **not** locomote — it holds position. `HAMS_LOWERBODY=walk` runs the -TorchScript walk policy, which currently does **not** stay upright in the RoboCasa -sim (falls a few seconds after the tether releases); true forward walking needs a -locomotion policy tuned for this simulator. (`torch` is already in the ros image; -building `unitree_hg` needs `rosidl-generator-dds-idl`, which the image now -includes.) +`HAMS_LOWERBODY=fame` **stands** (and squats via `/lowerbody/squat_cmd`) but holds +position — it does not locomote. For **stand *and* walk**, use the switchable +controller instead: + +```bash +HAMS_DISPLAY=vnc HAMS_RVIZ=vnc HAMS_LOWERBODY=switch \ + docker compose -f docker/docker-compose.mac.yml up +``` + +It starts band-held idle; `rob_stand` engages FAME (stand free), `rob_walk` hands +over to the TorchScript walk policy, and `rob_go ` drives it. The +walk policy **does stay upright** now, handed over from a settled FAME stance — the +earlier "falls a few seconds after the tether releases" was a too-tight motor +watchdog on the slow sim (see gotchas). Launching the raw policy directly +(`HAMS_LOWERBODY=walk`) still just marches in place; use `switch`. (`torch` is +already in the ros image; building `unitree_hg` needs `rosidl-generator-dds-idl`, +which the image now includes.) + +### Navigation demo (SLAM + nav2 + frontier exploration) + +The robot can map the kitchen and drive itself around autonomously — 2D SLAM from +the lidar, nav2 planning collision-free paths on a costmap that includes the full +3D cloud, and a frontier explorer sending goals. Full walkthrough: +**[docs/NAVIGATION_DEMO.md](docs/NAVIGATION_DEMO.md)**. In short: + +```bash +HAMS_DISPLAY=vnc HAMS_RVIZ=vnc HAMS_CAMERAS=0 \ +HAMS_LOWERBODY=switch HAMS_SLAM=1 HAMS_NAV2=1 HAMS_SPAWN_BACKOFF=1.5 \ + docker compose -f docker/docker-compose.mac.yml up -d +# then, inside hams_ros: +# source /home/code/h12_sim_scripts/robot_cli.sh +# rob_stand # FAME stand (wait ~15 s sim time for the tether to release) +# rob_explore # walk handover + autonomous frontier exploration +``` + +Watch the map, costmap, and green plan build in RViz (). ### macOS gotchas @@ -280,3 +309,15 @@ includes.) - **Two viewers use two displays.** RoboCasa renders on X display `:99` and RViz on `:100`; they must differ because the containers share one network namespace (`network_mode: host`). The launchers already handle this. +- **Restart the two containers coherently.** The RoboCasa container owns `/clock`. + Restarting it alone resets sim time to 0 while the ROS side keeps its old clock → + TF extrapolation errors and a frozen `0×0` SLAM map. After restarting/recreating + `robocasa`, restart `hams_ros` too so it resyncs to the fresh clock. +- **`HAMS_SPAWN_BACKOFF` is baked at container-create.** `docker restart` reuses the + old value; use `docker compose up --force-recreate --no-deps robocasa` (with the + env set) to change it. For nav, `1.5`–`2.0` keeps the robot off the counters. +- **The sim motor watchdog is sim-time based.** The low-level interface zeroes the + motors if no `rt/lowcmd` arrives within `HAMS_CMD_TIMEOUT` (default `0.5`) seconds + **of sim time** — measured in sim time on purpose, because a wall-clock timeout is + far too tight on a sim running ~0.2× real-time (it used to drop the robot ~0.7 s + after the tether released). Bump `HAMS_CMD_TIMEOUT` if a heavier scene still trips it. diff --git a/docker/docker-compose.mac.yml b/docker/docker-compose.mac.yml index 0ebc2fe..0ac5831 100644 --- a/docker/docker-compose.mac.yml +++ b/docker/docker-compose.mac.yml @@ -40,8 +40,9 @@ services: # cost) to speed the sim up for locomotion/SLAM. Lidar + odom are unaffected. - HAMS_CAMERAS=${HAMS_CAMERAS:-1} # HAMS_SPAWN_BACKOFF (metres) backs the robot off the counter into open floor - # at spawn (default 0 = at the counter for manipulation). ~1.0 gives nav2 room - # to plan (it rejects a goal when the robot's start cell is occupied). + # at spawn (default 0 = at the counter for manipulation). 1.5-2.0 gives nav2 + # room to plan and keeps FAME from leaning into a nearby counter. NOTE: baked + # at container-create -- change it with `up --force-recreate`, not `restart`. - HAMS_SPAWN_BACKOFF=${HAMS_SPAWN_BACKOFF:-0} # rclpy runs on FastDDS so it does not load a 2nd CycloneDDS libddsc # alongside unitree_sdk2py's (arm64 in-process coexistence fix). @@ -83,8 +84,10 @@ services: - HAMS_RVIZ=${HAMS_RVIZ:-0} # Optional lower-body controller: HAMS_LOWERBODY=fame runs the RMA policy # that balances the robot standing unsupported (releases the elastic band); - # =walk runs the walk policy (falls in this sim). Default (unset) keeps the - # band tether holding the robot upright with upper-body IK only. + # =walk runs the raw walk policy (marches in place); =switch is the switchable + # controller (band-held idle -> /lowerbody/start_{fame,walk}) and is the one to + # use for stand<->walk and the nav demo -- the walk policy stays upright when + # handed over from FAME. Default (unset) keeps the band tether + upper-body IK. - HAMS_LOWERBODY=${HAMS_LOWERBODY:-} # HAMS_SLAM=1 adds pointcloud_to_laserscan + slam_toolbox to the bringup: # the Livox cloud -> 2D scan -> occupancy /map against the sim's /odom. diff --git a/docs/NAVIGATION_DEMO.md b/docs/NAVIGATION_DEMO.md new file mode 100644 index 0000000..2d7b70e --- /dev/null +++ b/docs/NAVIGATION_DEMO.md @@ -0,0 +1,113 @@ +# Navigation demo — autonomous SLAM + nav2 exploration (macOS / Apple-Silicon sim) + +This walks the H1-2 through a RoboCasa kitchen **autonomously**: it builds a map +with SLAM, plans collision-free paths around the furniture with nav2, and drives +itself to unexplored frontiers — all on the headless CPU sim, no GPU. + +It runs on top of the macOS (Apple-Silicon) port; see the **macOS (Apple +Silicon)** section of the top-level [`README.md`](../README.md) for the one-time +Colima/Docker setup, image builds, and the noVNC tunnel. + +## What it demonstrates + +- **2D SLAM** — the Livox lidar cloud is flattened to a laser scan + (`pointcloud_to_laserscan`) and fed to `slam_toolbox`, which builds an + occupancy `/map` against the sim's ground-truth `/odom`. +- **nav2** — a Smac/RegulatedPurePursuit stack plans a collision-free path on + the map **and** a live costmap (2D scan + the full 3D Livox cloud, so it sees + counters and appliances the flat slice misses), then drives `/cmd_vel`. +- **Locomotion** — the switchable lower-body controller follows `/cmd_vel` with + the TorchScript walk policy, handed over from a stable FAME stand. +- **Frontier exploration** — `frontier_explore.py` repeatedly sends the nearest + unexplored frontier as a nav2 goal until the reachable space is mapped. + +## Run it + +Start **both** containers with the nav stack enabled (RoboCasa first so `/clock` +is publishing before the ROS nodes latch onto sim time): + +```bash +# from the repo root +HAMS_DISPLAY=vnc HAMS_RVIZ=vnc HAMS_CAMERAS=0 \ +HAMS_LOWERBODY=switch HAMS_SLAM=1 HAMS_NAV2=1 HAMS_SPAWN_BACKOFF=1.5 \ + docker compose -f docker/docker-compose.mac.yml up -d + +# open the noVNC tunnel (Colima doesn't forward container ports) +./docker/scripts/mac_vnc_tunnel.sh +``` + +Then drive it: + +```bash +docker exec -it hams_ros bash # host docker CLI flaky? use: colima ssh -- docker exec -it hams_ros bash +source /home/code/h12_sim_scripts/robot_cli.sh + +rob_stand # engage FAME; wait ~15 s of sim time for the tether to release and the stance to settle +rob_explore # hand over to walk + start autonomous frontier exploration (Ctrl-C to stop) +``` + +Watch it live: + +- **MuJoCo viewer** (robot in the kitchen) — +- **RViz** (map, costmap, green planned path) — + +You'll see the map fill in, the costmap mark the counters/appliances, a green +plan appear to each frontier, and the robot walk to it. Unreachable frontiers are +blacklisted after a timeout and the explorer moves on; it prints +`No frontiers left — exploration complete` when done. + +## The env knobs + +| Variable | For the nav demo | Notes | +|---|---|---| +| `HAMS_LOWERBODY` | `switch` | Switchable controller (`rob_stand`→`rob_walk`). `fame` stands only; `walk` launches the raw policy (use `switch`). | +| `HAMS_SLAM` | `1` | Adds `pointcloud_to_laserscan` + `slam_toolbox` → `/map`. | +| `HAMS_NAV2` | `1` | Adds the nav2 stack (implies SLAM). Send goals via RViz "2D Nav Goal", `/navigate_to_pose`, or `rob_explore`. | +| `HAMS_SPAWN_BACKOFF` | `~1.5` | Metres to back the robot into open floor at spawn so nav2 has room. **Baked at container create — change it with `--force-recreate`, not `docker restart`** (see gotchas). | +| `HAMS_CAMERAS` | `0` | Drops the 3 RGBD renders — the heaviest per-step CPU cost. Roughly doubles sim rate; the nav demo doesn't need them. | +| `HAMS_DISPLAY` / `HAMS_RVIZ` | `vnc` | MuJoCo viewer on 6080, RViz on 6081. | + +## Manual driving (without exploration) + +```bash +rob_stand # FAME stand +rob_walk # hand over to the walk policy +rob_go 0.3 0 0.2 6 # vx vy wz secs — forward + gentle left turn for 6 s +rob_stop # zero velocity, back to FAME +``` + +Or set a single goal in RViz with the **2D Nav Goal** tool and let nav2 drive. + +## How the pipeline fits together + +``` +MuJoCo (robocasa) ROS (hams_ros) + Livox cloud ─/livox/pointcloud──► pointcloud_to_laserscan ─/converted_scan─► slam_toolbox ─/map─┐ + free-joint pose ─/odom, TF──────► ...............................................................│ + nav2 (global+local costmap: /map + scan + 3D cloud) ◄─────────┘ + └─ plan ─► controller ─/cmd_vel─► lowerbody_controller_node + rt/lowcmd ◄──────────────────────────────────────────────────────────────────────┘ (walk policy) +``` + +## Gotchas specific to the nav demo + +- **`HAMS_SPAWN_BACKOFF` needs a recreate, not a restart.** It's an environment + variable baked into the container at create time. `docker restart` reuses the + old value; to change it, recreate the sim service: + ```bash + HAMS_DISPLAY=vnc HAMS_CAMERAS=0 HAMS_SPAWN_BACKOFF=1.5 \ + docker compose -f docker/docker-compose.mac.yml up -d --force-recreate --no-deps robocasa + ``` + The layout is randomized each launch, so a recreate also re-rolls the kitchen — + handy if the robot spawns cramped against a counter (it'll then lean into it + once FAME releases). `1.5`–`2.0` reliably clears the fixtures. +- **Restart the two containers coherently.** The sim owns `/clock`; if you restart + it, its sim time resets to 0 and the still-running ROS side sees the clock jump + backward (TF extrapolation errors, a frozen `0x0` SLAM map). After recreating or + restarting `robocasa`, restart `hams_ros` too so it resyncs. +- **`rob_stand` before `rob_walk`/`rob_explore`.** The walk policy is stable when + handed over from a settled FAME stance; give it ~15 s of sim time after + `rob_stand` before commanding motion. +- **Some exploration goals time out on the slow sim.** At ~0.2× real-time, nav2 + planning + walking to a 2 m frontier can exceed the 120 s goal timeout; the + explorer blacklists it and picks another. That's expected, not a failure. From e1ac3f54c858ec203a35a39f895665937c8f24c5 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Wed, 8 Jul 2026 18:35:04 -0600 Subject: [PATCH 19/19] :sparkles: In-container ROS debugging MCP server (HAMS_ROS_MCP) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Give Claude Code first-class tools to inspect and drive the sim H1 instead of shelling in and parsing ros2 CLI output. The server runs INSIDE the ros container (where the FastDDS graph is reachable) and exposes one HTTP MCP port; reach it from the Mac over the same SSH tunnel as the noVNC viewers (Colima forwards no ports, so running an MCP server on the Mac + DDS-across-the-boundary is the trap this avoids). A warm rclpy node subscribes once to the hot topics (/odom, global costmap, /plan, /converted_scan, /clock) and caches the latest message, so status calls answer instantly instead of paying node-startup + DDS-discovery per query. Tools: robot_status, costmap_summary, nav_status, scan_status, set_lowerbody, drive, wait_for, plus generic list/echo/hz/call_service/node_list passthroughs. - docker/scripts/ros_mcp_server.py — the server (FastMCP streamable-http, port 6082). Sensor topics (/odom, /converted_scan) are best-effort publishers, so subscribe best-effort or a reliable reader silently gets nothing. - launch_ros_mac.sh — HAMS_ROS_MCP=1 starts it (pip installs `mcp` on first run). - docker-compose.mac.yml — HAMS_ROS_MCP / HAMS_ROS_MCP_PORT passthrough. - mac_vnc_tunnel.sh — forward 6082 alongside the viewer ports. - docs/ROS_MCP_DEBUG.md — setup, `claude mcp add` registration, tool list; linked from the README. Verified end-to-end: launcher auto-starts the server; MCP round-trip returns live robot_status (pose/uprightness/posture), scan_status, wait_for, costmap_summary; tunnel reaches http://localhost:6082/mcp from the Mac. Co-Authored-By: Claude Opus 4.8 --- README.md | 7 + docker/docker-compose.mac.yml | 6 + docker/scripts/launch_ros_mac.sh | 27 +++ docker/scripts/mac_vnc_tunnel.sh | 6 +- docker/scripts/ros_mcp_server.py | 342 +++++++++++++++++++++++++++++++ docs/ROS_MCP_DEBUG.md | 82 ++++++++ 6 files changed, 468 insertions(+), 2 deletions(-) create mode 100644 docker/scripts/ros_mcp_server.py create mode 100644 docs/ROS_MCP_DEBUG.md diff --git a/README.md b/README.md index 4c11923..03fcdaf 100644 --- a/README.md +++ b/README.md @@ -297,6 +297,13 @@ HAMS_LOWERBODY=switch HAMS_SLAM=1 HAMS_NAV2=1 HAMS_SPAWN_BACKOFF=1.5 \ Watch the map, costmap, and green plan build in RViz (). +### ROS debugging MCP server (optional) + +`HAMS_ROS_MCP=1` starts an in-container MCP server that exposes ROS-inspection and +robot-driving tools (`robot_status`, `wait_for`, `costmap_summary`, `drive`, …) to +Claude Code over the same SSH tunnel as the viewers. Setup and tool list: +**[docs/ROS_MCP_DEBUG.md](docs/ROS_MCP_DEBUG.md)**. + ### macOS gotchas - **The host `docker` CLI socket is intermittent** under Colima — `docker …` may diff --git a/docker/docker-compose.mac.yml b/docker/docker-compose.mac.yml index 0ac5831..4d4755b 100644 --- a/docker/docker-compose.mac.yml +++ b/docker/docker-compose.mac.yml @@ -96,6 +96,12 @@ services: # collision-free path on the SLAM map and drives /cmd_vel (the walk policy # follows it). Send goals via RViz "2D Nav Goal" or /navigate_to_pose. - HAMS_NAV2=${HAMS_NAV2:-0} + # HAMS_ROS_MCP=1 starts an in-container ROS debugging MCP server (warm rclpy + # node + robot_status/wait_for/costmap/drive tools) on localhost:6082. Reach + # it from the Mac via mac_vnc_tunnel.sh and register with: + # claude mcp add --transport http ros_debug http://localhost:6082/mcp + - HAMS_ROS_MCP=${HAMS_ROS_MCP:-0} + - HAMS_ROS_MCP_PORT=${HAMS_ROS_MCP_PORT:-6082} volumes: - ../core_ws:/home/code/core_ws - ../CL_Assets:/home/code/CL_Assets:ro diff --git a/docker/scripts/launch_ros_mac.sh b/docker/scripts/launch_ros_mac.sh index ac92624..4a67fe3 100755 --- a/docker/scripts/launch_ros_mac.sh +++ b/docker/scripts/launch_ros_mac.sh @@ -15,6 +15,11 @@ set -e HAMS_RVIZ="${HAMS_RVIZ:-0}" +# HAMS_ROS_MCP=1 starts the ROS debugging MCP server (docker/scripts/ros_mcp_server.py) +# on localhost:6082 inside the VM — reach it from the Mac via the SSH tunnel and +# register with `claude mcp add --transport http ros_debug http://localhost:6082/mcp`. +HAMS_ROS_MCP="${HAMS_ROS_MCP:-0}" +HAMS_ROS_MCP_PORT="${HAMS_ROS_MCP_PORT:-6082}" # VNC/noVNC for RViz (only used when HAMS_RVIZ=vnc). Localhost-bound in the VM; # reachable from the Mac only via the SSH tunnel (mac_vnc_tunnel.sh). Ports are @@ -67,6 +72,22 @@ start_rviz_stack() { echo "[launch_ros_mac] http://localhost:${RVIZ_NOVNC_PORT}/vnc.html?autoconnect=1&resize=scale" } +# Background ROS debugging MCP server (warm rclpy node + HTTP MCP tools). Needs ROS +# sourced (done before this is called). Installs the `mcp` pip package on first run. +start_ros_mcp() { + echo "[launch_ros_mac] starting ROS debug MCP server on 127.0.0.1:${HAMS_ROS_MCP_PORT}" + python3 -c "import mcp" 2>/dev/null || { + echo "[launch_ros_mac] installing the 'mcp' pip package (first run)..." + pip install --quiet --disable-pip-version-check mcp >/tmp/pip_mcp.log 2>&1 \ + || { echo "[launch_ros_mac] pip install mcp FAILED:"; tail -5 /tmp/pip_mcp.log; return 1; } + } + HAMS_ROS_MCP_PORT="$HAMS_ROS_MCP_PORT" \ + python3 /home/code/h12_sim_scripts/ros_mcp_server.py >/tmp/ros_mcp.log 2>&1 & + sleep 1 + echo "[launch_ros_mac] MCP server -> /tmp/ros_mcp.log. Register on the Mac (after mac_vnc_tunnel.sh):" + echo "[launch_ros_mac] claude mcp add --transport http ros_debug http://localhost:${HAMS_ROS_MCP_PORT}/mcp" +} + source /opt/ros/humble/setup.bash WS=/home/code/core_ws @@ -118,6 +139,12 @@ if [ "$HAMS_RVIZ" = "vnc" ] || [ "$HAMS_RVIZ" = "1" ]; then start_rviz_stack || echo "[launch_ros_mac] RViz stack failed to start (continuing without it)" fi +# Optional ROS debugging MCP server (started before bringup so it's up whether we +# launch or drop to a shell; the warm subscribers just fill in as topics appear). +if [ "$HAMS_ROS_MCP" = "1" ] || [ "$HAMS_ROS_MCP" = "vnc" ]; then + start_ros_mcp || echo "[launch_ros_mac] MCP server failed to start (continuing without it)" +fi + if [ "${1:-}" = "bash" ]; then echo "[launch_ros_mac] workspace built; dropping to shell (ROS_DOMAIN_ID=$ROS_DOMAIN_ID)" exec bash diff --git a/docker/scripts/mac_vnc_tunnel.sh b/docker/scripts/mac_vnc_tunnel.sh index c2ef2a2..7f35181 100755 --- a/docker/scripts/mac_vnc_tunnel.sh +++ b/docker/scripts/mac_vnc_tunnel.sh @@ -7,6 +7,7 @@ # Forwards both viewers' ports (harmless if only one is running): # MuJoCo viewer : http://localhost:6080/vnc.html (VNC localhost:5900) # RViz : http://localhost:6081/vnc.html (VNC localhost:5901) +# ROS MCP server: http://localhost:6082/mcp (HAMS_ROS_MCP=1) # # Usage: # ./mac_vnc_tunnel.sh open the tunnel, print the URLs @@ -17,8 +18,9 @@ set -euo pipefail PROFILE="${COLIMA_PROFILE:-default}" -# Ports to forward: noVNC (browser) + raw VNC, for the MuJoCo viewer and RViz. -PORTS=(6080 5900 6081 5901) +# Ports to forward: noVNC (browser) + raw VNC for the MuJoCo viewer and RViz, plus +# 6082 for the ROS debugging MCP server (HAMS_ROS_MCP=1). +PORTS=(6080 5900 6081 5901 6082) MUJOCO_URL="http://localhost:6080/vnc.html?autoconnect=1&resize=scale" RVIZ_URL="http://localhost:6081/vnc.html?autoconnect=1&resize=scale" diff --git a/docker/scripts/ros_mcp_server.py b/docker/scripts/ros_mcp_server.py new file mode 100644 index 0000000..aef3489 --- /dev/null +++ b/docker/scripts/ros_mcp_server.py @@ -0,0 +1,342 @@ +#!/usr/bin/env python3 +"""ROS debugging MCP server for the H1 Mac sim. + +Runs INSIDE the hams_ros container (where the FastDDS graph is reachable) and +exposes an HTTP MCP endpoint. Colima doesn't forward container ports to the Mac, +so it binds localhost inside the VM and you reach it over the same SSH tunnel as +the noVNC viewers (see mac_vnc_tunnel.sh). Register it on the Mac with: + + claude mcp add --transport http ros_debug http://localhost:6082/mcp + +Design: a single warm rclpy node subscribes once to the hot topics (odom, +costmap, plan, scan, clock) and caches the latest message, so status tools answer +instantly instead of paying node-startup + DDS-discovery on every call (the thing +that made `ros2 topic echo --once` slow to poll). Actuation (lower-body mode, +/cmd_vel) uses warm publishers/clients. Generic passthroughs (list/echo/hz/service +call for arbitrary topics) shell out to the ros2 CLI, which is robust for the +rarely-used cold path. rclpy spins in a background thread; tool handlers read the +thread-safe cache. + +Env: HAMS_ROS_MCP_PORT (default 6082), ROS_DOMAIN_ID, RMW_IMPLEMENTATION. +""" +import asyncio +import math +import os +import threading +import time + +import numpy as np +import rclpy +from geometry_msgs.msg import Twist +from nav_msgs.msg import OccupancyGrid, Odometry, Path +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from rclpy.qos import (DurabilityPolicy, HistoryPolicy, QoSProfile, + ReliabilityPolicy) +from rosgraph_msgs.msg import Clock +from sensor_msgs.msg import LaserScan +from std_srvs.srv import Trigger + +from mcp.server.fastmcp import FastMCP + +PORT = int(os.environ.get("HAMS_ROS_MCP_PORT", "6082")) +_LATCHED = QoSProfile(reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, depth=1) +# BEST_EFFORT reader is compatible with BOTH best-effort and reliable writers, so +# it receives sensor/odom topics whatever QoS the publisher chose (the sim's /odom +# and /converted_scan are best-effort; a reliable reader silently gets nothing). +_SENSOR = QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + history=HistoryPolicy.KEEP_LAST, depth=10) + + +class RosDebugNode(Node): + """Warm subscriber + actuator. Caches latest messages under a lock.""" + + def __init__(self): + super().__init__("ros_debug_mcp") + self._lock = threading.Lock() + self._odom = None + self._costmap = None + self._plan = None + self._scan = None + self._clock = None + self.create_subscription(Odometry, "/odom", self._cb("_odom"), _SENSOR) + self.create_subscription(LaserScan, "/converted_scan", self._cb("_scan"), _SENSOR) + self.create_subscription(Clock, "/clock", self._cb("_clock"), _SENSOR) + self.create_subscription(OccupancyGrid, "/global_costmap/costmap", + self._cb("_costmap"), _LATCHED) + self.create_subscription(Path, "/plan", self._cb("_plan"), _SENSOR) + self._cmd_pub = self.create_publisher(Twist, "/cmd_vel", 10) + self._fame_cli = self.create_client(Trigger, "/lowerbody/start_fame") + self._walk_cli = self.create_client(Trigger, "/lowerbody/start_walk") + + def _cb(self, attr): + def f(msg): + with self._lock: + setattr(self, attr, msg) + return f + + def get(self, attr): + with self._lock: + return getattr(self, attr) + + # -- derived state ------------------------------------------------------- + def sim_time(self): + c = self.get("_clock") + return None if c is None else c.clock.sec + c.clock.nanosec * 1e-9 + + def robot_state(self): + o = self.get("_odom") + if o is None: + return None + p, q = o.pose.pose.position, o.pose.pose.orientation + upright = 1.0 - 2.0 * (q.x * q.x + q.y * q.y) # body-up . world-up + return {"x": round(p.x, 3), "y": round(p.y, 3), "z": round(p.z, 3), + "uprightness": round(upright, 3), + "posture": ("standing" if (upright > 0.85 and p.z > 0.7) + else "fallen" if (upright < 0.5 or p.z < 0.4) + else "leaning/other")} + + def costmap_summary(self): + m = self.get("_costmap") + if m is None: + return None + a = np.asarray(m.data, dtype=np.int16) + return {"width": m.info.width, "height": m.info.height, + "resolution": round(m.info.resolution, 3), + "lethal": int((a >= 99).sum()), + "inflated": int(((a >= 50) & (a < 99)).sum()), + "free": int((a == 0).sum()), + "unknown": int((a == -1).sum())} + + def plan_summary(self): + pl = self.get("_plan") + if pl is None or not pl.poses: + return {"has_plan": False, "poses": 0, "length_m": 0.0} + pts = [(ps.pose.position.x, ps.pose.position.y) for ps in pl.poses] + length = sum(math.hypot(pts[i][0] - pts[i - 1][0], pts[i][1] - pts[i - 1][1]) + for i in range(1, len(pts))) + return {"has_plan": True, "poses": len(pts), "length_m": round(length, 2)} + + # -- actuation ----------------------------------------------------------- + def call_trigger(self, which, timeout=8.0): + cli = self._fame_cli if which == "fame" else self._walk_cli + if not cli.wait_for_service(timeout_sec=2.0): + return {"ok": False, "error": f"/lowerbody/start_{which} unavailable"} + fut = cli.call_async(Trigger.Request()) + t0 = time.time() + while not fut.done() and time.time() - t0 < timeout: + time.sleep(0.05) + if not fut.done(): + return {"ok": False, "error": "service call timed out"} + r = fut.result() + return {"ok": bool(r.success), "message": r.message} + + def publish_cmd(self, vx, vy, wz): + t = Twist() + t.linear.x, t.linear.y, t.angular.z = float(vx), float(vy), float(wz) + self._cmd_pub.publish(t) + + +NODE: RosDebugNode = None # set in main() + + +def _sh(*args, timeout=10): + """Run a ros2 CLI command (env already sourced by the launcher) -> stdout.""" + import subprocess + try: + out = subprocess.run(args, capture_output=True, text=True, timeout=timeout) + return (out.stdout or out.stderr or "").strip() + except subprocess.TimeoutExpired: + return f"(timed out after {timeout}s)" + + +mcp = FastMCP("ros_debug", host="127.0.0.1", port=PORT) + + +# ---- hot tools (warm cache, instant) -------------------------------------- +@mcp.tool() +def robot_status() -> dict: + """H1 base state at a glance: position (x,y,z, odom/world frame), uprightness + (1.0 = vertical, <0.5 = fallen), posture, and current sim time. Instant.""" + st = NODE.robot_state() + return {"error": "no /odom yet"} if st is None else { + **st, "sim_time": NODE.sim_time()} + + +@mcp.tool() +def costmap_summary() -> dict: + """nav2 GLOBAL costmap cell counts: lethal / inflated / free / unknown, plus + size and resolution. lethal>0 means the planner is seeing obstacles.""" + s = NODE.costmap_summary() + return {"error": "no /global_costmap/costmap (nav2 running?)"} if s is None else s + + +@mcp.tool() +def nav_status() -> dict: + """Current nav2 global plan: whether one exists, pose count, and path length + in metres (from /plan).""" + return NODE.plan_summary() + + +@mcp.tool() +def scan_status() -> dict: + """Is /converted_scan (the SLAM/costmap laser input) live? Reports the latest + scan's stamp and range count, or that it's absent.""" + s = NODE.get("_scan") + if s is None: + return {"live": False, "note": "no /converted_scan (pointcloud_to_laserscan / SLAM running?)"} + return {"live": True, "stamp_sec": s.header.stamp.sec, + "ranges": len(s.ranges), "range_min": round(s.range_min, 2), + "range_max": round(s.range_max, 2)} + + +# ---- actuation ------------------------------------------------------------ +@mcp.tool() +def set_lowerbody(mode: str) -> dict: + """Switch the lower-body controller. mode='fame' -> stand free (FAME); + mode='walk' -> hand over to the walk policy (call after fame). Needs the sim + launched with HAMS_LOWERBODY=switch.""" + if mode not in ("fame", "walk"): + return {"ok": False, "error": "mode must be 'fame' or 'walk'"} + return NODE.call_trigger(mode) + + +@mcp.tool() +async def drive(vx: float = 0.0, vy: float = 0.0, wz: float = 0.0, + duration: float = 4.0) -> dict: + """Publish /cmd_vel (body frame, m/s and rad/s) at 20 Hz for `duration` + seconds, then stop. vx>0 forward, wz>0 turn left. The robot must be in walk + mode (set_lowerbody('walk')). Reports start/end pose so you can see if it moved.""" + duration = max(0.0, min(float(duration), 30.0)) + p0 = NODE.robot_state() + hz, end = 0.05, time.time() + duration + while time.time() < end: + NODE.publish_cmd(vx, vy, wz) + await asyncio.sleep(hz) + NODE.publish_cmd(0.0, 0.0, 0.0) + p1 = NODE.robot_state() + moved = (round(math.hypot(p1["x"] - p0["x"], p1["y"] - p0["y"]), 3) + if p0 and p1 else None) + return {"ok": True, "commanded": {"vx": vx, "vy": vy, "wz": wz, "duration": duration}, + "start": p0, "end": p1, "moved_m": moved} + + +@mcp.tool() +async def wait_for(condition: str, timeout: float = 60.0) -> dict: + """Block until a condition holds, then return (polls the warm cache ~5 Hz). + Replaces sleep-poll loops. condition is one of: + standing | fallen | stopped | moving | scan_live | nav_has_plan | + sim_time>= (e.g. 'sim_time>=20') + Returns {met: bool, waited_s, ...state}.""" + timeout = max(1.0, min(float(timeout), 600.0)) + t0 = time.time() + last = None + + def check(): + nonlocal last + st = NODE.robot_state() + last = st + if condition == "standing": + return bool(st and st["posture"] == "standing") + if condition == "fallen": + return bool(st and st["posture"] == "fallen") + if condition == "scan_live": + return NODE.get("_scan") is not None + if condition == "nav_has_plan": + return NODE.plan_summary()["has_plan"] + if condition.startswith("sim_time>="): + try: + thr = float(condition.split(">=", 1)[1]) + except ValueError: + return False + t = NODE.sim_time() + return t is not None and t >= thr + if condition in ("stopped", "moving"): + a = NODE.robot_state() + time.sleep(0.4) + b = NODE.robot_state() + if not a or not b: + return False + d = math.hypot(b["x"] - a["x"], b["y"] - a["y"]) + return (d < 0.01) if condition == "stopped" else (d >= 0.02) + return False + + while time.time() - t0 < timeout: + if check(): + return {"met": True, "waited_s": round(time.time() - t0, 1), + "state": last, "sim_time": NODE.sim_time()} + await asyncio.sleep(0.2) + return {"met": False, "waited_s": round(time.time() - t0, 1), + "state": last, "sim_time": NODE.sim_time(), + "note": f"condition '{condition}' not met within {timeout}s"} + + +# ---- generic passthroughs (cold path, ros2 CLI) --------------------------- +@mcp.tool() +def list_topics(filter: str = "") -> list: + """List active topics (optionally substring-filtered), as 'name type'.""" + out = [] + for name, types in sorted(NODE.get_topic_names_and_types()): + if not filter or filter in name: + out.append(f"{name} {','.join(types)}") + return out + + +@mcp.tool() +def echo_topic(topic: str, timeout: float = 6.0) -> str: + """One message from any topic (generic, via `ros2 topic echo --once`). Use the + hot tools (robot_status/costmap_summary/scan_status) for the common ones.""" + return _sh("ros2", "topic", "echo", "--once", topic, timeout=int(timeout) + 2) + + +@mcp.tool() +def topic_hz(topic: str, window: float = 5.0) -> str: + """Measured publish rate of a topic over `window` seconds.""" + import subprocess + try: + p = subprocess.run(["ros2", "topic", "hz", topic], capture_output=True, + text=True, timeout=window + 3) + for line in (p.stdout or "").splitlines(): + if "average rate" in line: + return line.strip() + return "(no messages in window)" + except subprocess.TimeoutExpired as e: + for line in ((e.stdout or b"").decode(errors="ignore")).splitlines(): + if "average rate" in line: + return line.strip() + return "(no messages in window)" + + +@mcp.tool() +def call_service(name: str, type: str, args: str = "{}") -> str: + """Call any service (generic, via `ros2 service call`). type is the full type + e.g. 'std_srvs/srv/Trigger'; args is a YAML/JSON dict string.""" + return _sh("ros2", "service", "call", name, type, args, timeout=15) + + +@mcp.tool() +def node_list() -> list: + """List active ROS nodes.""" + return sorted(f"{ns}/{n}".replace("//", "/") for n, ns in NODE.get_node_names_and_namespaces()) + + +def _spin(executor): + executor.spin() + + +def main(): + rclpy.init() + global NODE + NODE = RosDebugNode() + ex = MultiThreadedExecutor(num_threads=3) + ex.add_node(NODE) + threading.Thread(target=_spin, args=(ex,), daemon=True).start() + NODE.get_logger().info(f"ros_debug MCP server on http://127.0.0.1:{PORT}/mcp") + mcp.run(transport="streamable-http") + + +if __name__ == "__main__": + main() diff --git a/docs/ROS_MCP_DEBUG.md b/docs/ROS_MCP_DEBUG.md new file mode 100644 index 0000000..e3d75c5 --- /dev/null +++ b/docs/ROS_MCP_DEBUG.md @@ -0,0 +1,82 @@ +# ROS debugging MCP server (macOS / Apple-Silicon sim) + +A small MCP server that gives an assistant (Claude Code) first-class tools to +inspect and drive the simulated H1 — `robot_status`, `wait_for`, `costmap_summary`, +`drive`, etc. — instead of shelling into the container and parsing `ros2` CLI output. + +## Why it lives in the container + +The ROS 2 graph (FastDDS, `network_mode: host`) runs inside the Colima VM's +containers, and Colima exposes no reachable VM IP and forwards no ports — the same +wall the noVNC viewers hit. So the server **runs inside the `ros` container** (where +DDS is directly reachable) and exposes a single HTTP port, which you reach from the +Mac over the existing SSH tunnel. Only the MCP protocol crosses the boundary; DDS +never has to. + +A warm `rclpy` node subscribes once to the hot topics (`/odom`, the global costmap, +`/plan`, `/converted_scan`, `/clock`) and caches the latest message, so status calls +answer instantly instead of paying node-startup + DDS-discovery on every query. + +## Enable it + +Add `HAMS_ROS_MCP=1` when you launch (it starts on `localhost:6082` inside the VM): + +```bash +HAMS_DISPLAY=vnc HAMS_RVIZ=vnc HAMS_CAMERAS=0 \ +HAMS_LOWERBODY=switch HAMS_SLAM=1 HAMS_NAV2=1 HAMS_SPAWN_BACKOFF=1.5 \ +HAMS_ROS_MCP=1 \ + docker compose -f docker/docker-compose.mac.yml up -d + +./docker/scripts/mac_vnc_tunnel.sh # forwards 6082 alongside the viewer ports +``` + +The first launch `pip install`s the `mcp` package inside the container (logged to +`/tmp/ros_mcp.log`). `HAMS_ROS_MCP` is baked at container-create — set it on a +`compose up` (recreate), not a bare `docker restart`. + +## Register it with Claude Code (one-time, on the Mac) + +```bash +claude mcp add --transport http ros_debug http://localhost:6082/mcp +claude mcp list # verify it connects +``` + +Optionally allowlist the tools so they run without a prompt (in your settings, e.g. +`.claude/settings.json`): + +```json +{ "permissions": { "allow": ["mcp__ros_debug__.*"] } } +``` + +To remove: `claude mcp remove ros_debug`. + +## Tools + +| Tool | What it does | +|---|---| +| `robot_status()` | base position (x,y,z), uprightness (1.0=vertical, <0.5=fallen), posture, sim time | +| `costmap_summary()` | global-costmap cell counts: lethal / inflated / free / unknown | +| `nav_status()` | current nav2 plan: exists?, pose count, path length (m) | +| `scan_status()` | is `/converted_scan` live? stamp + range count | +| `set_lowerbody(mode)` | `'fame'` stand free, or `'walk'` hand over to the walk policy | +| `drive(vx,vy,wz,duration)` | publish `/cmd_vel` for N seconds, then stop; reports start/end pose | +| `wait_for(condition,timeout)` | block until `standing`/`fallen`/`stopped`/`moving`/`scan_live`/`nav_has_plan`/`sim_time>=N` | +| `list_topics(filter)` / `echo_topic(topic)` / `topic_hz(topic)` | generic topic inspection | +| `call_service(name,type,args)` | call any service | +| `node_list()` | list active nodes | + +The first eight are the warm/fast path; the generic passthroughs shell out to the +`ros2` CLI for the occasional arbitrary topic. + +## Notes & caveats + +- **In-container reach only.** The server sees everything on the ROS graph. It does + **not** reach the *other* container's MuJoCo `:99` viewer or the RoboCasa + fall-logger stdout (a second endpoint or docker-socket access would be needed). +- **Sensor QoS.** `/odom` and `/converted_scan` are best-effort publishers; the + server subscribes best-effort so it receives them regardless of publisher QoS. +- **Headless runs.** An unauthenticated localhost HTTP MCP server works in + non-interactive Claude Code runs as long as the tunnel and server are up; there's + no OAuth to complete. +- **Port.** Override with `HAMS_ROS_MCP_PORT` (compose passes it through); update the + `claude mcp add` URL and `mac_vnc_tunnel.sh` `PORTS` to match.