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/README.md b/README.md index 1b8ac97..03fcdaf 100644 --- a/README.md +++ b/README.md @@ -160,3 +160,171 @@ 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.) + +### 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 +``` + +`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 (). + +### 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 + 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. +- **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/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..dea3717 --- /dev/null +++ b/core_ws/src/h1_bringup/config/nav2_config_mac.yaml @@ -0,0 +1,228 @@ +--- +/**: + 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] ]" + # 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 + 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 + enabled: true + obstacle_build_latch: false + observation_sources: laser_scan_sensor pointcloud_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 + # 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 + 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.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 pointcloud + 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) + # 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 + 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 new file mode 100644 index 0000000..8c5e5d8 --- /dev/null +++ b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py @@ -0,0 +1,178 @@ +"""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 + * 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) + * 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 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' + + +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} + + nodes = [ + 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', + ), + # 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', + 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', + ), + ] + + # 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 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', + 'switch': 'lowerbody_controller_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', + )) + + # 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', + # 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.65, 'range_max': 8.0, + 'use_inf': True, 'scan_time': 0.0333, + 'transform_tolerance': 1.0, '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', + )) + + # 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/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..98c9e2a --- /dev/null +++ b/docker/RobocasaDockerfile.arm64 @@ -0,0 +1,155 @@ +# ============================== +# 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/* + +# ----------------------------------------------------------------------------- +# 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/* + +# 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 new file mode 100644 index 0000000..760522f --- /dev/null +++ b/docker/RosDockerfile.slim.arm64 @@ -0,0 +1,157 @@ +# ============================== +# 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/* + +# ----------------------------------------------------------------------------- +# 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/* + +# 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/* + +# 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/* + +# 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 +# /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 + +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..4d4755b --- /dev/null +++ b/docker/docker-compose.mac.yml @@ -0,0 +1,127 @@ +# ============================== +# 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} + # 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} + # 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.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). + - 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 + # 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} + # Optional lower-body controller: HAMS_LOWERBODY=fame runs the RMA policy + # that balances the robot standing unsupported (releases the elastic band); + # =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. + - 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} + # 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 + # 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/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/h1_sim.rviz b/docker/scripts/h1_sim.rviz new file mode 100644 index 0000000..ef21d9d --- /dev/null +++ b/docker/scripts/h1_sim.rviz @@ -0,0 +1,126 @@ +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: odom + 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 + - Class: rviz_default_plugins/PointCloud2 + Name: Lidar + Enabled: false + 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 + - 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 + - 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 + - Class: rviz_default_plugins/Measure + Views: + Current: + Class: rviz_default_plugins/Orbit + Name: Current View + Distance: 9.0 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Pitch: 1.2 + Yaw: 3.14 + Target Frame: pelvis +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 new file mode 100755 index 0000000..7fc2089 --- /dev/null +++ b/docker/scripts/launch_robocasa_mac.sh @@ -0,0 +1,116 @@ +#!/bin/bash +# Apple-Silicon / CPU launcher for the MuJoCo (RoboCasa) sim. +# 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)" + # `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" + # 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 & # bg set by the fbsetbg shim (Dockerfile) + 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. +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}" + +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] 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 new file mode 100755 index 0000000..4a67fe3 --- /dev/null +++ b/docker/scripts/launch_ros_mac.sh @@ -0,0 +1,154 @@ +#!/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 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}" +# 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 +# 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)" + # `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" + 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 & # 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 + 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" +} + +# 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 +# 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" + +# 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...). +# 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 + 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 \ + --build-base "$BUILD_BASE" --install-base "$INSTALL_BASE" \ + --packages-select $PKGS + +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 + +# 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 +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/docker/scripts/mac_vnc_tunnel.sh b/docker/scripts/mac_vnc_tunnel.sh new file mode 100755 index 0000000..7f35181 --- /dev/null +++ b/docker/scripts/mac_vnc_tunnel.sh @@ -0,0 +1,78 @@ +#!/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) +# ROS MCP server: http://localhost:6082/mcp (HAMS_ROS_MCP=1) +# +# 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, 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" + +# 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..3c5cd89 --- /dev/null +++ b/docker/scripts/robot_cli.sh @@ -0,0 +1,115 @@ +#!/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 +} + +# --- 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; } + +# 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)" 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/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. 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. diff --git a/h1_robocasa/h12_mujoco.py b/h1_robocasa/h12_mujoco.py index 09ab08f..cbcb225 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 @@ -99,7 +100,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 @@ -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: @@ -170,6 +175,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). @@ -182,13 +198,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 @@ -200,6 +222,17 @@ 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 + # 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/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 diff --git a/h1_robocasa/mujoco_ros_bridge.py b/h1_robocasa/mujoco_ros_bridge.py index ac9a6f0..0d38722 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,11 @@ 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, + lidar_self_prefixes: tuple = (), elastic_band=None, sim_lock=None, ): @@ -157,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) @@ -257,6 +280,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 +347,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: @@ -319,7 +361,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) @@ -355,6 +397,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: @@ -481,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) 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