diff --git a/.gitignore b/.gitignore index bf0a360..5825ec5 100644 --- a/.gitignore +++ b/.gitignore @@ -11,9 +11,7 @@ __pycache__/ # tiptop cache and outputs tiptop/.cache/ tiptop_outputs/ -tiptop_outputs* -tiptop_h5_outputs/ -tiptop_server_outputs/ +tiptop_*_outputs/ # JetBrains .idea/ diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..621fd73 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,65 @@ +# Changelog + +All notable changes to TiPToP are documented in this file. + +The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.1.0/), +and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). + +## [0.2.0] - 2026-06-05 + +### Breaking Changes + +- `viz-tiptop-run` renamed its `--save-dir` flag to `--run-dir` ([#16]). +- `tiptop_cfg()` no longer accepts `force_reload` and no longer merges CLI overrides + from `sys.argv` via `OmegaConf.from_cli`. Load a specific config with the new + `set_tiptop_cfg_from_file()` before the first `tiptop_cfg()` call. The + `tiptop.config.tiptop_config_path` constant is no longer exported ([#16]). + +### Added + +- `tiptop-rerun` CLI: re-runs the pipeline from a saved run directory, reusing the + recorded observation. Task instruction and planning parameters default to the + original run's values and can be overridden via flags ([#16]). +- Pick-only tasks: the pipeline now supports plans that just pick an object, including + a prompt to safely catch the object on the real robot after a pick ([#27]). +- Rerun-disabled mode and a config option to run without applying M2T2 bounds ([#27]). +- `save_dir` is now included in the `tiptop-server` response ([#27]). +- `set_tiptop_cfg_from_file()` and `get_tiptop_cfg_path()` in `tiptop.config` for + loading a config from an explicit path and querying the cached config's source path ([#16]). +- Integration tests for the offline H5 pipeline, runnable via the `test-integration` + pixi task ([#14]). + +### Changed + +- `tiptop-h5` and the offline rerun logic are consolidated into `tiptop/tiptop_offline.py` + (entry points `h5_entrypoint` and `rerun_entrypoint`); `tiptop/tiptop_h5.py` is removed. + The `tiptop-h5` CLI and its flags are unchanged ([#16]). +- Offline runs save the merged config into the run directory so re-runs are reproducible ([#16]). +- Logging setup moved to the entrypoint level ([#16]). +- Updated the Gemini perception model from `gemini-robotics-er-1.5-preview` to + `gemini-robotics-er-1.6-preview` ([#26]). +- Updated cuTAMP to 0.0.4 and added a configurable max number of motion-refinement + attempts ([#19], [#20]). +- `tiptop-server` serializes pipeline runs with an asyncio lock so concurrent requests + no longer interleave ([#23]). +- Point-cloud erosion falls back to no erosion when it would leave too few points ([#27]). + +### Fixed + +- Fixed broken intrinsics imports in `calibrate_wrist_cam` ([#21]). + +## [0.1.0] + +Initial tagged release. + +[0.2.0]: https://github.com/tiptop-robot/tiptop/compare/v0.1.0...v0.2.0 +[0.1.0]: https://github.com/tiptop-robot/tiptop/releases/tag/v0.1.0 + +[#14]: https://github.com/tiptop-robot/tiptop/pull/14 +[#16]: https://github.com/tiptop-robot/tiptop/pull/16 +[#19]: https://github.com/tiptop-robot/tiptop/pull/19 +[#20]: https://github.com/tiptop-robot/tiptop/pull/20 +[#21]: https://github.com/tiptop-robot/tiptop/pull/21 +[#23]: https://github.com/tiptop-robot/tiptop/pull/23 +[#26]: https://github.com/tiptop-robot/tiptop/pull/26 +[#27]: https://github.com/tiptop-robot/tiptop/pull/27 diff --git a/CLAUDE.md b/CLAUDE.md index 6b8f4d8..93fbb62 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -244,6 +244,7 @@ gdown - **Discuss before implementing** — When the approach isn't obvious (e.g., where to put docs, how to structure config), talk through options before writing code - **Comments and docstrings must be accurate** — Don't write comments that describe implementation details irrelevant to the reader or are vaguely wrong. If a comment doesn't add real information, drop it +- **Don't pile iteration rationale into comments/docstrings** — When you revise code, don't tack on explanations of why you chose this shape or cross-module consequences ("path is cached so reruns forward the original", "preserves comments"). Keep comments minimal and code-local; rationale and history belong in the PR description - **Don't add dead code paths** — If every case goes down the same branch, don't add the other branch "just in case." Keep what's tested, remove what isn't ## Documentation Style Guide diff --git a/docs/command-reference.md b/docs/command-reference.md index f5b93ab..e33f2c8 100644 --- a/docs/command-reference.md +++ b/docs/command-reference.md @@ -84,7 +84,7 @@ After warmup and health checks you'll be prompted to enter a natural language ta ### tiptop-h5 -Offline evaluation mode. Loads a pre-recorded RGB-D observation from an H5 file (droid-sim-evals format), runs perception and planning without a real robot, and saves the resulting plan as a JSON file for downstream evaluation. +Offline evaluation mode. Loads a pre-recorded RGB-D observation from an H5 file (droid-sim-evals format) and runs the TiPToP pipeline without a real robot, saving a serialized plan JSON for downstream evaluation. **Prerequisites:** - M2T2 server must be running @@ -118,6 +118,46 @@ tiptop-h5 \ --- +### tiptop-rerun + +Re-runs TiPToP from a saved run directory. Loads the observation (RGB, depth, intrinsics, camera pose, joint positions, and gripper mask) from a previous run. Task instruction and planning parameters default to the original run's values but can be overridden. + +**Prerequisites:** +- M2T2 server must be running +- `GOOGLE_API_KEY` environment variable must be set +- A saved TiPToP run directory (from `tiptop-run`, `tiptop-h5`, or `tiptop-rerun`) + +**Available flags:** + +- `--run-dir STR` - Path to a saved TiPToP run directory (required) +- `--task-instruction STR` - Task instruction override; defaults to the original run's instruction +- `--output-dir STR` - Top-level directory to save results; a timestamped subdirectory will be created (default: "tiptop_rerun_outputs") +- `--max-planning-time FLOAT` - Override max planning time; defaults to the original run's value +- `--opt-steps-per-skeleton INT` - Override optimization steps per skeleton; defaults to the original run's value +- `--num-particles INT` - Override number of particles; defaults to the original run's value +- `--cutamp-visualize / --no-cutamp-visualize` - Enable cuTAMP planning visualization in Rerun, significantly slows down planning (default: False) +- `--rr-spawn / --no-rr-spawn` - Spawn a Rerun viewer; set to False to skip visualization (default: True) + +**Example usage:** + +```bash +# Re-run from a previous run with original parameters +tiptop-rerun --run-dir tiptop_outputs/eval/2026-04-05_13-30-20/ + +# Re-run with a different task instruction +tiptop-rerun \ + --run-dir tiptop_outputs/eval/2026-04-05_13-30-20/ \ + --task-instruction "put the cube on the table" + +# Re-run with more particles and longer planning time +tiptop-rerun \ + --run-dir tiptop_outputs/eval/2026-01-24_15-30-00/ \ + --num-particles 512 \ + --max-planning-time 120.0 +``` + +--- + ### tiptop-server Runs the TiPToP perception and planning pipeline as a WebSocket server. Clients send RGB-D observations and receive serialized trajectory plans, allowing consumers on separate machines or running different Python versions to query TiPToP. A `/health` HTTP endpoint is available for liveness checks. @@ -407,7 +447,7 @@ Replays and visualizes the outputs of a saved TiPToP run in Rerun. Loads percept **Available flags:** -- `--save-dir STR` - Path to the saved run directory (required) +- `--run-dir STR` - Path to a saved TiPToP run directory (required) - `--visualize-grasps / --no-visualize-grasps` - Visualize M2T2 grasp candidates (default: True) - `--visualize-plan / --no-visualize-plan` - Animate the TiPToP plan trajectory with object poses (default: True) - `--num-grasps-per-object INT` - Maximum number of grasp candidates to display per object (default: 30) @@ -417,13 +457,13 @@ Replays and visualizes the outputs of a saved TiPToP run in Rerun. Loads percept ```bash # Visualize a run directory -viz-tiptop-run --save-dir tiptop_outputs/eval/2026-01-24_15-30-00/ +viz-tiptop-run --run-dir tiptop_outputs/eval/2026-01-24_15-30-00/ # Visualize perception only, skip plan animation -viz-tiptop-run --save-dir tiptop_outputs/eval/2026-01-24_15-30-00/ --no-visualize-plan +viz-tiptop-run --run-dir tiptop_outputs/eval/2026-01-24_15-30-00/ --no-visualize-plan # Show fewer grasps -viz-tiptop-run --save-dir tiptop_outputs/eval/2026-01-24_15-30-00/ --num-grasps-per-object 10 +viz-tiptop-run --run-dir tiptop_outputs/eval/2026-01-24_15-30-00/ --num-grasps-per-object 10 ``` A Rerun window will open automatically. Use the `tiptop_execution` timeline to step through the planned trajectory. The `cam` entity shows the camera pose and image at capture time, and `world/` contains all objects and their poses throughout execution. diff --git a/pyproject.toml b/pyproject.toml index 6304b10..0ea6ce7 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -89,8 +89,9 @@ gripper-close = "tiptop.scripts.gripper_actuate:gripper_close_entrypoint" # Main tiptop demo tiptop-run = "tiptop.tiptop_run:entrypoint" -# tiptop h5 run -tiptop-h5 = "tiptop.tiptop_h5:entrypoint" +# Offline tiptop (no real robot) +tiptop-h5 = "tiptop.tiptop_offline:h5_entrypoint" +tiptop-rerun = "tiptop.tiptop_offline:rerun_entrypoint" # tiptop as a service tiptop-server = "tiptop.tiptop_websocket_server:entrypoint" diff --git a/tests/test_tiptop_h5.py b/tests/test_tiptop_h5.py index a320a0b..d937ad6 100644 --- a/tests/test_tiptop_h5.py +++ b/tests/test_tiptop_h5.py @@ -25,7 +25,7 @@ def test_tiptop_h5_pipeline(tmp_path, h5_assets, h5_filename, task_instruction): assert h5_path.exists(), f"Test asset not found: {h5_path}" # Local import to avoid slow transitive imports affecting other tests - from tiptop.tiptop_h5 import run_tiptop_h5 + from tiptop.tiptop_offline import run_tiptop_h5 run_tiptop_h5( h5_path=str(h5_path), diff --git a/tiptop/config/__init__.py b/tiptop/config/__init__.py index a17e0fe..6cc7327 100644 --- a/tiptop/config/__init__.py +++ b/tiptop/config/__init__.py @@ -9,23 +9,36 @@ config_dir = Path(__file__).parent config_assets_dir = config_dir / "assets" -tiptop_config_path = config_dir / "tiptop.yml" calib_info_path = config_assets_dir / "calibration_info.json" -_cached_cfg = None # Cache for lazy loading +_cached_cfg: DictConfig | None = None +_cached_cfg_path: Path | None = None -def tiptop_cfg(force_reload: bool = False) -> DictConfig: - """Load TiPToP config from file.""" - global _cached_cfg - if _cached_cfg is None or force_reload: - _cached_cfg = OmegaConf.load(tiptop_config_path) - # Merge CLI overrides from sys.argv - cli = OmegaConf.from_cli() - _cached_cfg = OmegaConf.merge(_cached_cfg, cli) +def set_tiptop_cfg_from_file(cfg_path: Path) -> DictConfig: + """Load and cache the TiPToP config from a specific file. Call before any tiptop_cfg() usage.""" + global _cached_cfg, _cached_cfg_path + cfg = OmegaConf.load(cfg_path) + _cached_cfg = cfg + _cached_cfg_path = Path(cfg_path) + return cfg + + +def tiptop_cfg() -> DictConfig: + """Return the cached TiPToP config, loading the default config file on first call.""" + if _cached_cfg is None: + return set_tiptop_cfg_from_file(config_dir / "tiptop.yml") return _cached_cfg +def get_tiptop_cfg_path() -> Path: + """Return the source path of the currently-cached config. Loads the default config if not yet cached.""" + if _cached_cfg_path is None: + tiptop_cfg() + assert _cached_cfg_path is not None + return _cached_cfg_path + + def load_calibration_info(): if not os.path.exists(calib_info_path): raise FileNotFoundError(f"{calib_info_path} not found.") diff --git a/tiptop/recording.py b/tiptop/recording.py index 1f13aef..c5601e3 100644 --- a/tiptop/recording.py +++ b/tiptop/recording.py @@ -18,7 +18,7 @@ from PIL import Image import tiptop -from tiptop.config import tiptop_config_path +from tiptop.config import get_tiptop_cfg_path from tiptop.perception.cameras.zed_camera import ZedCamera, convert_svo_to_mp4 from tiptop.perception.utils import get_o3d_pcd from tiptop.perception.visualization import visualize_detections, visualize_masks @@ -213,7 +213,7 @@ def save_run_outputs(save_dir: Path, env, grasps: dict) -> None: _log.info(f"Saved grasps to {perception_dir}/grasps.pt") # tiptop config for reproducibility - shutil.copy2(tiptop_config_path, save_dir / "tiptop.yml") + shutil.copy2(get_tiptop_cfg_path(), save_dir / "tiptop.yml") _log.info(f"Saved tiptop config to {save_dir}/tiptop.yml") diff --git a/tiptop/scripts/viz_tiptop_run.py b/tiptop/scripts/viz_tiptop_run.py index ccd587a..2a15480 100644 --- a/tiptop/scripts/viz_tiptop_run.py +++ b/tiptop/scripts/viz_tiptop_run.py @@ -115,7 +115,7 @@ def viz_tiptop_plan(tiptop_plan: dict, cutamp_env: TAMPEnvironment, robot_rr: Re def viz_tiptop_run( - save_dir: str, + run_dir: str, visualize_grasps: bool = True, visualize_plan: bool = True, num_grasps_per_object: int = 30, @@ -125,21 +125,21 @@ def viz_tiptop_run( Visualize TiPToP outputs (perception and plan) from a saved run directory in Rerun. Args: - save_dir: Path to the saved run directory containing metadata.json, rgb.png, perception/, etc. + run_dir: Path to a saved TiPToP run directory (contains metadata.json, rgb.png, etc.). visualize_grasps: Whether to visualize the M2T2 grasp candidates. visualize_plan: Whether to visualize the TiPToP plan trajectory, including object poses while grasped. num_grasps_per_object: Maximum number of grasp candidates to display per object. log_transform_arrows: Whether to log coordinate frame arrows on object transforms. """ setup_logging() - save_dir = Path(save_dir) - perception_dir = save_dir / "perception" + run_dir = Path(run_dir) + perception_dir = run_dir / "perception" # Load metadata - metadata_path = save_dir / "metadata.json" + metadata_path = run_dir / "metadata.json" if not metadata_path.exists(): - raise FileNotFoundError(f"Could not find metadata.json in {save_dir}") - with open(save_dir / "metadata.json") as f: + raise FileNotFoundError(f"Could not find metadata.json in {run_dir}") + with open(run_dir / "metadata.json") as f: metadata = json.load(f) if metadata["version"] != "1.0.0": raise NotImplementedError(f"Version {metadata['version']} not supported") @@ -149,7 +149,7 @@ def viz_tiptop_run( rr.init(application_id="viz_tiptop_outputs", spawn=True) # Load tiptop config from this run - tiptop_cfg = OmegaConf.load(save_dir / "tiptop.yml") + tiptop_cfg = OmegaConf.load(run_dir / "tiptop.yml") robot_type = tiptop_cfg["robot"]["type"] robot_rr = get_robot_rerun(robot_type=robot_type) robot_rr.set_joint_positions(metadata["observation"]["q_at_capture"]) @@ -161,7 +161,7 @@ def viz_tiptop_run( intrinsics = json.load(f) K = np.array(intrinsics["intrinsics"]) rr.log("cam", rr.Pinhole(image_from_camera=K), static=True) - rgb = Image.open(save_dir / "rgb.png") + rgb = Image.open(run_dir / "rgb.png") rr.log("cam/rgb", rr.Image(rgb), static=True) # Mask out depth where gripper is present @@ -175,14 +175,14 @@ def viz_tiptop_run( rr.log("cam/depth", rr.DepthImage(depth, meter=1000.0), static=True) # Bounding boxes and mask visualization - bboxes_viz = Image.open(save_dir / "bboxes_viz.png") - masks_viz = Image.open(save_dir / "masks_viz.png") + bboxes_viz = Image.open(run_dir / "bboxes_viz.png") + masks_viz = Image.open(run_dir / "masks_viz.png") rr.log("bboxes_viz", rr.Image(bboxes_viz), static=True) rr.log("masks_viz", rr.Image(masks_viz), static=True) # Point cloud pcd = o3d.io.read_point_cloud(perception_dir / "pointcloud.ply") - rr.log("pcd", rr.Points3D(positions=pcd.points, colors=pcd.colors)) + rr.log("pcd", rr.Points3D(positions=pcd.points, colors=pcd.colors), static=True) # Grasps if visualize_grasps: @@ -210,9 +210,9 @@ def viz_tiptop_run( return # Load and visualize the tiptop plan - tiptop_plan_path = save_dir / "tiptop_plan.json" + tiptop_plan_path = run_dir / "tiptop_plan.json" if not tiptop_plan_path.exists(): - _log.warning(f"Could not find tiptop_plan.json in {save_dir}") + _log.warning(f"Could not find tiptop_plan.json in {run_dir}") return tiptop_plan = load_tiptop_plan(tiptop_plan_path) if tiptop_plan["version"] != "1.0.0": diff --git a/tiptop/tiptop_h5.py b/tiptop/tiptop_h5.py deleted file mode 100644 index 6ae13a3..0000000 --- a/tiptop/tiptop_h5.py +++ /dev/null @@ -1,224 +0,0 @@ -"""Offline H5 evaluation mode for TiPToP. - -Loads observations from H5 files (pi-sim-evals format) and runs perception + planning -without a real robot, saving a serialized plan JSON file for downstream evaluation. -""" - -import asyncio -import logging -import os -import time -from datetime import datetime -from pathlib import Path - -import aiohttp -import h5py -import numpy as np -import rerun as rr -import tyro -from scipy.spatial.transform import Rotation - -from tiptop.config import tiptop_cfg -from tiptop.motion_planning import build_curobo_solvers -from tiptop.perception.cameras import Frame -from tiptop.planning import build_tamp_config, run_planning, save_tiptop_plan, serialize_plan -from tiptop.recording import save_run_metadata, save_run_outputs -from tiptop.tiptop_run import Observation, run_perception -from tiptop.utils import ( - add_file_handler, - check_cutamp_version, - get_robot_rerun, - print_tiptop_banner, - remove_file_handler, - setup_logging, -) - -_log = logging.getLogger(__name__) - - -def load_h5_observation(h5_path: Path) -> Observation: - """Load an observation from an H5 file (pi-sim-evals format). - - Expected fields: rgb, depth, intrinsic_matrix, pos_w, quat_w_ros (w,x,y,z), q_init. - """ - h5_path = Path(h5_path) - if not h5_path.exists(): - raise FileNotFoundError(f"H5 file not found: {h5_path}") - - with h5py.File(h5_path, "r") as f: - rgb = f["rgb"][:] - depth = f["depth"][:] - pos_w = f["pos_w"][:] - quat_w_ros = f["quat_w_ros"][:] # stored as [w, x, y, z] in this format - intrinsics = f["intrinsic_matrix"][:] - q_init = np.atleast_1d(f["q_init"][()]) - if q_init.ndim != 1 or q_init.shape[0] < 2: - raise ValueError( - f"q_init in H5 file has unexpected shape {q_init.shape} (value: {q_init}). " - f"Expected a 1D array of joint positions (e.g., 7 for Franka). " - f"The H5 file may have been generated incorrectly — re-run save_h5_obs.py to regenerate it." - ) - - if depth.ndim == 3 and depth.shape[-1] == 1: - depth = depth.squeeze(-1) - - if rgb.dtype != np.uint8: - rgb = (rgb * 255.0).clip(0, 255).astype(np.uint8) if rgb.max() <= 1.0 else rgb.astype(np.uint8) - - # Preprocess depth: remove invalid values and truncate range - depth = np.nan_to_num(depth.astype(np.float32), nan=0.0, posinf=0.0, neginf=0.0) - depth[depth < 0] = 0.0 - depth[depth > tiptop_cfg().perception.depth_trunc_m] = 0.0 - - # Build world_from_cam: quat_w_ros is [w, x, y, z] in this h5 format - quat_xyzw = np.array([quat_w_ros[1], quat_w_ros[2], quat_w_ros[3], quat_w_ros[0]], dtype=np.float32) - R = Rotation.from_quat(quat_xyzw).as_matrix() - world_from_cam = np.eye(4, dtype=np.float32) - world_from_cam[:3, :3] = R - world_from_cam[:3, 3] = pos_w.astype(np.float32) - world_from_cam[:3, 3] -= np.array([0.0, 0.0, -0.015], dtype=np.float32) # calibration offset for this h5 format - - frame = Frame( - serial="static", - timestamp=0.0, - rgb=rgb.astype(np.uint8), - intrinsics=intrinsics.astype(np.float32), - depth=depth, - ) - return Observation(frame=frame, world_from_cam=world_from_cam, q_init=q_init.astype(np.float32)) - - -def run_tiptop_h5( - h5_path: str, - task_instruction: str, - output_dir: str = "tiptop_h5_outputs", - max_planning_time: float = 60.0, - opt_steps_per_skeleton: int = 500, - num_particles: int = 256, - cutamp_visualize: bool = False, - rr_spawn: bool = True, -): - """ - TiPToP offline runner. Loads an H5 observation file and runs perception + planning, - saving a serialized plan JSON file for downstream evaluation. - - Args: - h5_path: Path to H5 observation file. - task_instruction: Task instruction (e.g. 'put the cube in the bowl'). - output_dir: Top-level directory to save outputs; a timestamped subdirectory is created per run. - max_planning_time: Maximum time to spend planning with cuTAMP across all skeletons (approximate). - opt_steps_per_skeleton: Number of optimization steps per skeleton in cuTAMP. - num_particles: Number of particles for cuTAMP; decrease if running out of GPU memory. - cutamp_visualize: Whether to visualize cuTAMP optimization. - rr_spawn: Whether to spawn a Rerun viewer. - """ - assert max_planning_time > 0 - assert opt_steps_per_skeleton > 0 - assert num_particles > 0 - - if not task_instruction: - raise ValueError("--task-instruction is required") - - print_tiptop_banner() - check_cutamp_version() - - cfg = tiptop_cfg() - config = build_tamp_config( - num_particles=num_particles, - max_planning_time=max_planning_time, - opt_steps=opt_steps_per_skeleton, - robot_type=cfg.robot.type, - time_dilation_factor=cfg.robot.time_dilation_factor, - collision_activation_distance=0.0, - enable_visualizer=cutamp_visualize, - ) - - setup_logging(level=logging.INFO) - ik_solver, motion_gen, _ = build_curobo_solvers(num_particles, config.coll_n_spheres, include_workspace=False) - - rr.init("tiptop_h5_run", spawn=rr_spawn) - observation = load_h5_observation(Path(h5_path)) - robot_rr = get_robot_rerun() - robot_rr.set_joint_positions(observation.q_init) - - timestamp = datetime.now() - save_dir = Path(output_dir) / timestamp.strftime("%Y-%m-%d_%H-%M-%S") - save_dir.mkdir(parents=True, exist_ok=True) - file_handler = add_file_handler(save_dir / "tiptop_run.log") - - try: - - async def _run_perception(): - connector = aiohttp.TCPConnector(limit=10, force_close=True) - timeout = aiohttp.ClientTimeout(total=60.0) - async with aiohttp.ClientSession(connector=connector, timeout=timeout) as session: - return await run_perception( - session, - observation, - task_instruction, - save_dir, - depth_estimator=None, - gripper_mask=None, - include_workspace=False, - ) - - _log.info("Running perception pipeline...") - perception_start = time.perf_counter() - env, all_surfaces, processed_scene, grounded_atoms = asyncio.run(_run_perception()) - perception_duration = time.perf_counter() - perception_start - _log.info("Planning with cuTAMP...") - cutamp_plan = None - planning_duration = None - failure_reason = None - try: - cutamp_plan, planning_duration, failure_reason = run_planning( - env, - config, - observation.q_init, - ik_solver, - processed_scene.grasps, - motion_gen, - all_surfaces, - experiment_dir=save_dir / "cutamp", - ) - finally: - save_run_outputs(save_dir, env, processed_scene.grasps) - save_run_metadata( - save_dir=save_dir, - timestamp=timestamp.isoformat(timespec="seconds"), - task_instruction=task_instruction, - q_at_capture=observation.q_init, - world_from_cam=observation.world_from_cam, - perception_duration=perception_duration, - grounded_atoms=grounded_atoms, - planning_success=cutamp_plan is not None, - planning_failure_reason=failure_reason, - planning_duration=planning_duration, - ) - - if cutamp_plan is not None: - plan_path = save_dir / "tiptop_plan.json" - save_tiptop_plan(serialize_plan(cutamp_plan, observation.q_init), plan_path) - _log.info(f"Saved TiPToP plan to {plan_path}") - else: - _log.warning(f"No plan found: {failure_reason}") - - _log.info(f"Saved outputs to {save_dir}") - finally: - remove_file_handler(file_handler) - rr.disconnect() - - -def entrypoint(): - """CLI entrypoint wrapper. Calls run_tiptop_h5 and force-exits to avoid GPU cleanup segfaults.""" - try: - tyro.cli(run_tiptop_h5) - except Exception: - _log.exception("TiPToP run failed") - os._exit(1) - else: - os._exit(0) - - -if __name__ == "__main__": - entrypoint() diff --git a/tiptop/tiptop_offline.py b/tiptop/tiptop_offline.py new file mode 100644 index 0000000..e925e39 --- /dev/null +++ b/tiptop/tiptop_offline.py @@ -0,0 +1,418 @@ +"""Offline TiPToP pipeline (no real robot). + +Provides run_tiptop() for general use, with two CLI entrypoints: +- run_tiptop_h5(): loads from an H5 file (tiptop-h5 command) +- run_tiptop_rerun(): loads from a saved run directory (tiptop-rerun command) +""" + +import asyncio +import json +import logging +import os +import time +from datetime import datetime +from pathlib import Path + +import aiohttp +import cv2 +import h5py +import numpy as np +import rerun as rr +import tyro +from omegaconf import OmegaConf +from scipy.spatial.transform import Rotation + +from tiptop.config import set_tiptop_cfg_from_file, tiptop_cfg +from tiptop.motion_planning import build_curobo_solvers +from tiptop.perception.cameras import Frame +from tiptop.planning import build_tamp_config, run_planning, save_tiptop_plan, serialize_plan +from tiptop.recording import save_run_metadata, save_run_outputs +from tiptop.tiptop_run import Observation, run_perception +from tiptop.utils import ( + add_file_handler, + check_cutamp_version, + get_robot_rerun, + print_tiptop_banner, + remove_file_handler, + setup_logging, +) + +_log = logging.getLogger(__name__) + + +def run_tiptop( + observation: Observation, + task_instruction: str, + output_dir: str, + max_planning_time: float, + opt_steps_per_skeleton: int, + num_particles: int, + gripper_mask: np.ndarray | None = None, + cutamp_visualize: bool = False, + rr_spawn: bool = True, +): + """Run the TiPToP pipeline from an observation. + + Args: + observation: Pre-built observation (from H5, a previous run, etc.). + task_instruction: Task instruction (e.g. 'put the cube in the bowl'). + output_dir: Top-level directory to save outputs; a timestamped subdirectory is created per run. + max_planning_time: Maximum time to spend planning with cuTAMP across all skeletons (approximate). + opt_steps_per_skeleton: Number of optimization steps per skeleton in cuTAMP. + num_particles: Number of particles for cuTAMP; decrease if running out of GPU memory. + gripper_mask: Optional boolean mask for gripper pixels in the image. + cutamp_visualize: Whether to visualize cuTAMP optimization. + rr_spawn: Whether to spawn a Rerun viewer. + """ + if max_planning_time <= 0: + raise ValueError(f"max_planning_time must be > 0, got {max_planning_time}") + if opt_steps_per_skeleton <= 0: + raise ValueError(f"opt_steps_per_skeleton must be > 0, got {opt_steps_per_skeleton}") + if num_particles <= 0: + raise ValueError(f"num_particles must be > 0, got {num_particles}") + + if not task_instruction: + raise ValueError("task_instruction is required") + + check_cutamp_version() + + cfg = tiptop_cfg() + config = build_tamp_config( + num_particles=num_particles, + max_planning_time=max_planning_time, + opt_steps=opt_steps_per_skeleton, + robot_type=cfg.robot.type, + time_dilation_factor=cfg.robot.time_dilation_factor, + collision_activation_distance=0.0, + enable_visualizer=cutamp_visualize, + ) + + ik_solver, motion_gen, _ = build_curobo_solvers(num_particles, config.coll_n_spheres, include_workspace=False) + + rr.init("tiptop_run", spawn=rr_spawn) + robot_rr = get_robot_rerun() + robot_rr.set_joint_positions(observation.q_init) + + timestamp = datetime.now() + save_dir = Path(output_dir) / timestamp.strftime("%Y-%m-%d_%H-%M-%S") + save_dir.mkdir(parents=True, exist_ok=True) + file_handler = add_file_handler(save_dir / "tiptop_run.log") + + try: + + async def _run_perception(): + connector = aiohttp.TCPConnector(limit=10, force_close=True) + timeout = aiohttp.ClientTimeout(total=60.0) + async with aiohttp.ClientSession(connector=connector, timeout=timeout) as session: + return await run_perception( + session, + observation, + task_instruction, + save_dir, + depth_estimator=None, + gripper_mask=gripper_mask, + include_workspace=False, + ) + + _log.info("Running perception pipeline...") + perception_start = time.perf_counter() + env, all_surfaces, processed_scene, grounded_atoms = asyncio.run(_run_perception()) + perception_duration = time.perf_counter() - perception_start + _log.info("Planning with cuTAMP...") + cutamp_plan = None + planning_duration = None + failure_reason = None + try: + cutamp_plan, planning_duration, failure_reason = run_planning( + env, + config, + observation.q_init, + ik_solver, + processed_scene.grasps, + motion_gen, + all_surfaces, + experiment_dir=save_dir / "cutamp", + ) + finally: + save_run_outputs(save_dir, env, processed_scene.grasps) + save_run_metadata( + save_dir=save_dir, + timestamp=timestamp.isoformat(timespec="seconds"), + task_instruction=task_instruction, + q_at_capture=observation.q_init, + world_from_cam=observation.world_from_cam, + perception_duration=perception_duration, + grounded_atoms=grounded_atoms, + planning_success=cutamp_plan is not None, + planning_failure_reason=failure_reason, + planning_duration=planning_duration, + ) + + if cutamp_plan is not None: + plan_path = save_dir / "tiptop_plan.json" + save_tiptop_plan(serialize_plan(cutamp_plan, observation.q_init), plan_path) + _log.info(f"Saved TiPToP plan to {plan_path}") + else: + _log.warning(f"No plan found: {failure_reason}") + + _log.info(f"Saved outputs to {save_dir}") + finally: + remove_file_handler(file_handler) + rr.disconnect() + + +def load_h5_observation(h5_path: Path) -> Observation: + """Load an observation from an H5 file (droid-sim-evals format). + + Expected fields: rgb, depth, intrinsic_matrix, pos_w, quat_w_ros (w,x,y,z), q_init. + """ + h5_path = Path(h5_path) + if not h5_path.exists(): + raise FileNotFoundError(f"H5 file not found: {h5_path}") + + with h5py.File(h5_path, "r") as f: + rgb = f["rgb"][:] + depth = f["depth"][:] + pos_w = f["pos_w"][:] + quat_w_ros = f["quat_w_ros"][:] # stored as [w, x, y, z] in this format + intrinsics = f["intrinsic_matrix"][:] + q_init = np.atleast_1d(f["q_init"][()]) + if q_init.ndim != 1 or q_init.shape[0] < 2: + raise ValueError( + f"q_init in H5 file has unexpected shape {q_init.shape} (value: {q_init}). " + f"Expected a 1D array of joint positions (e.g., 7 for Franka). " + f"The H5 file may have been generated incorrectly — re-run save_h5_obs.py to regenerate it." + ) + + if depth.ndim == 3 and depth.shape[-1] == 1: + depth = depth.squeeze(-1) + + if rgb.dtype != np.uint8: + rgb = (rgb * 255.0).clip(0, 255).astype(np.uint8) if rgb.max() <= 1.0 else rgb.astype(np.uint8) + + # Preprocess depth: remove invalid values and truncate range + depth = np.nan_to_num(depth.astype(np.float32), nan=0.0, posinf=0.0, neginf=0.0) + depth[depth < 0] = 0.0 + depth[depth > tiptop_cfg().perception.depth_trunc_m] = 0.0 + + # Build world_from_cam: quat_w_ros is [w, x, y, z] in this h5 format + quat_xyzw = np.array([quat_w_ros[1], quat_w_ros[2], quat_w_ros[3], quat_w_ros[0]], dtype=np.float32) + R = Rotation.from_quat(quat_xyzw).as_matrix() + world_from_cam = np.eye(4, dtype=np.float32) + world_from_cam[:3, :3] = R + world_from_cam[:3, 3] = pos_w.astype(np.float32) + world_from_cam[:3, 3] -= np.array([0.0, 0.0, -0.015], dtype=np.float32) # calibration offset for this h5 format + + frame = Frame( + serial="static", + timestamp=0.0, + rgb=rgb.astype(np.uint8), + intrinsics=intrinsics.astype(np.float32), + depth=depth, + ) + return Observation(frame=frame, world_from_cam=world_from_cam, q_init=q_init.astype(np.float32)) + + +def run_tiptop_h5( + h5_path: str, + task_instruction: str, + output_dir: str = "tiptop_h5_outputs", + max_planning_time: float = 60.0, + opt_steps_per_skeleton: int = 500, + num_particles: int = 256, + cutamp_visualize: bool = False, + rr_spawn: bool = True, +): + """Load an H5 observation and run the TiPToP pipeline. + + Args: + h5_path: Path to H5 observation file. + task_instruction: Task instruction (e.g. 'put the cube in the bowl'). + output_dir: Top-level directory to save outputs; a timestamped subdirectory is created per run. + max_planning_time: Maximum time to spend planning with cuTAMP across all skeletons (approximate). + opt_steps_per_skeleton: Number of optimization steps per skeleton in cuTAMP. + num_particles: Number of particles for cuTAMP; decrease if running out of GPU memory. + cutamp_visualize: Whether to visualize cuTAMP optimization. + rr_spawn: Whether to spawn a Rerun viewer. + """ + observation = load_h5_observation(Path(h5_path)) + run_tiptop( + observation=observation, + task_instruction=task_instruction, + output_dir=output_dir, + max_planning_time=max_planning_time, + opt_steps_per_skeleton=opt_steps_per_skeleton, + num_particles=num_particles, + cutamp_visualize=cutamp_visualize, + rr_spawn=rr_spawn, + ) + + +def h5_entrypoint(): + """CLI entrypoint for tiptop-h5. Force-exits to avoid GPU cleanup segfaults.""" + setup_logging(level=logging.INFO) + print_tiptop_banner() + try: + tyro.cli(run_tiptop_h5) + except Exception: + _log.exception("TiPToP H5 run failed") + os._exit(1) + else: + os._exit(0) + + +def load_observation_from_run(run_dir: Path) -> tuple[Observation, np.ndarray | None, dict]: + """Load an Observation and optional gripper mask from a saved TiPToP run directory. + + Returns the observation, gripper mask (or None), and the parsed metadata dict. + """ + run_dir = Path(run_dir) + + # Load and validate metadata first — file layout may differ across versions + metadata_path = run_dir / "metadata.json" + if not metadata_path.exists(): + raise FileNotFoundError(f"Could not find metadata.json in {run_dir}") + with open(metadata_path) as f: + metadata = json.load(f) + if metadata["version"] != "1.0.0": + raise ValueError( + f"Unsupported metadata version '{metadata['version']}' (expected '1.0.0'). " + f"The run at {run_dir} may have been saved with an incompatible TiPToP version." + ) + + perception_dir = run_dir / "perception" + + # RGB + rgb_path = run_dir / "rgb.png" + bgr = cv2.imread(str(rgb_path)) + if bgr is None: + raise RuntimeError(f"Failed to read RGB image: {rgb_path}") + rgb = cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB) + + # Depth: saved as uint16 in millimeters, convert back to float32 meters + depth_path = perception_dir / "depth.png" + depth_uint16 = cv2.imread(str(depth_path), cv2.IMREAD_UNCHANGED) + if depth_uint16 is None: + raise RuntimeError(f"Failed to read depth image: {depth_path}") + depth = depth_uint16.astype(np.float32) / 1000.0 + + # Intrinsics + intrinsics_path = perception_dir / "intrinsics.json" + with open(intrinsics_path) as f: + intrinsics = np.array(json.load(f)["intrinsics"], dtype=np.float32) + + # Gripper mask (optional — only present for runs with a hand camera) + gripper_mask = None + gripper_mask_path = perception_dir / "gripper_mask.png" + if gripper_mask_path.exists(): + gripper_mask_img = cv2.imread(str(gripper_mask_path), cv2.IMREAD_GRAYSCALE) + if gripper_mask_img is not None: + gripper_mask = gripper_mask_img > 0 + + # Observation fields from metadata + obs_meta = metadata["observation"] + q_init = np.array(obs_meta["q_at_capture"], dtype=np.float32) + world_from_cam = np.array(obs_meta["world_from_cam"], dtype=np.float32) + + frame = Frame(serial="rerun", timestamp=0.0, rgb=rgb, intrinsics=intrinsics, depth=depth) + observation = Observation(frame=frame, world_from_cam=world_from_cam, q_init=q_init) + return observation, gripper_mask, metadata + + +def run_tiptop_rerun( + run_dir: str, + task_instruction: str | None = None, + output_dir: str = "tiptop_rerun_outputs", + max_planning_time: float | None = None, + opt_steps_per_skeleton: int | None = None, + num_particles: int | None = None, + cutamp_visualize: bool = False, + rr_spawn: bool = True, +): + """Re-run TiPToP from a saved run directory. + + The task instruction and planning parameters default to those from the original run + but can be overridden. + + Args: + run_dir: Path to a saved TiPToP run directory (contains metadata.json, rgb.png, etc.). + task_instruction: Task instruction override. If not provided, uses the original instruction. + output_dir: Top-level directory to save outputs; a timestamped subdirectory is created per run. + max_planning_time: Override max planning time. Defaults to the original run's value. + opt_steps_per_skeleton: Override optimization steps per skeleton. Defaults to the original run's value. + num_particles: Override number of particles. Defaults to the original run's value. + cutamp_visualize: Whether to visualize cuTAMP optimization. + rr_spawn: Whether to spawn a Rerun viewer. + """ + run_dir_path = Path(run_dir) + observation, gripper_mask, metadata = load_observation_from_run(run_dir_path) + + # Restore config from the original run so tiptop_cfg() consumers don't drift + saved_cfg_path = run_dir_path / "tiptop.yml" + if saved_cfg_path.exists(): + set_tiptop_cfg_from_file(saved_cfg_path) + _log.info(f"Loaded tiptop config from original run: {saved_cfg_path}") + else: + _log.warning( + f"No tiptop.yml found in {run_dir_path} — falling back to the current config. " + "Rerun may differ from the original run." + ) + + # Defaults from the original run's cuTAMP config; fall back to tiptop-h5 values if missing + cutamp_config_path = run_dir_path / "cutamp" / "config.yml" + if cutamp_config_path.exists(): + cutamp_config = OmegaConf.to_container(OmegaConf.load(cutamp_config_path)) + default_max_planning_time = cutamp_config["max_loop_dur"] + default_opt_steps = cutamp_config["num_opt_steps"] + default_num_particles = cutamp_config["num_particles"] + defaults_source = "original run" + else: + default_max_planning_time = 60.0 + default_opt_steps = 500 + default_num_particles = 256 + defaults_source = "built-in defaults" + _log.warning( + f"No cuTAMP config at {cutamp_config_path} — falling back to built-in defaults " + f"(max_planning_time={default_max_planning_time}, " + f"opt_steps_per_skeleton={default_opt_steps}, " + f"num_particles={default_num_particles})." + ) + + if task_instruction is None: + task_instruction = metadata["task_instruction"] + _log.info(f"Using task instruction from original run: '{task_instruction}'") + if max_planning_time is None: + max_planning_time = default_max_planning_time + _log.info(f"Using max_planning_time from {defaults_source}: {max_planning_time}") + if opt_steps_per_skeleton is None: + opt_steps_per_skeleton = default_opt_steps + _log.info(f"Using opt_steps_per_skeleton from {defaults_source}: {opt_steps_per_skeleton}") + if num_particles is None: + num_particles = default_num_particles + _log.info(f"Using num_particles from {defaults_source}: {num_particles}") + + run_tiptop( + observation=observation, + task_instruction=task_instruction, + output_dir=output_dir, + max_planning_time=max_planning_time, + opt_steps_per_skeleton=opt_steps_per_skeleton, + num_particles=num_particles, + gripper_mask=gripper_mask, + cutamp_visualize=cutamp_visualize, + rr_spawn=rr_spawn, + ) + + +def rerun_entrypoint(): + """CLI entrypoint for tiptop-rerun. Force-exits to avoid GPU cleanup segfaults.""" + setup_logging(level=logging.INFO) + print_tiptop_banner() + try: + tyro.cli(run_tiptop_rerun) + except Exception: + _log.exception("TiPToP rerun failed") + os._exit(1) + else: + os._exit(0) + diff --git a/tiptop/utils.py b/tiptop/utils.py index ad7bd48..2692aae 100644 --- a/tiptop/utils.py +++ b/tiptop/utils.py @@ -121,6 +121,11 @@ def load_gripper_mask() -> Bool[np.ndarray, "h w"]: def setup_logging(level: int = logging.INFO): + """Configure root logger and stdout console handler. Call once per entrypoint. + + The root logger is set to DEBUG so handlers added later (file handlers, etc.) can + filter at their own level. `level` controls only the stdout console handler. + """ # Ensure stdout and stderr use UTF-8 encoding to handle Unicode characters sys.stdout.reconfigure(encoding="utf-8") sys.stderr.reconfigure(encoding="utf-8") @@ -152,18 +157,18 @@ def format(self, record): log_format = "%(asctime)s - %(name)s - %(levelname)s - %(message)s" formatter = CustomFormatter(log_format, datefmt="%Y-%m-%d %H:%M:%S") - # Configure the root logger (force reconfiguration) + # Root at DEBUG so later-added handlers (e.g. per-run file handler) can capture DEBUG root_logger = logging.getLogger() - root_logger.setLevel(level) + root_logger.setLevel(logging.DEBUG) # Remove only default StreamHandlers (stdout/stderr), keep other handlers (files, etc.) for handler in root_logger.handlers[:]: if isinstance(handler, logging.StreamHandler) and handler.stream in (sys.stdout, sys.stderr): root_logger.removeHandler(handler) - # Add our custom handler handler = logging.StreamHandler(sys.stdout) handler.setFormatter(formatter) + handler.setLevel(level) root_logger.addHandler(handler) # Bamboo can be INFO level @@ -197,22 +202,18 @@ def add_file_handler(log_file: Path, level: int = logging.DEBUG) -> logging.File level: Logging level for the file handler Returns: - The FileHandler instance so it can be removed later + The FileHandler instance so it can be removed later via remove_file_handler(). """ log_file.parent.mkdir(parents=True, exist_ok=True) - # Create file handler with plain formatting (no colors) and UTF-8 encoding + # Plain formatter (no ANSI colors) and UTF-8 encoding so the file stays readable file_handler = logging.FileHandler(log_file, mode="w", encoding="utf-8") file_handler.setLevel(level) - - # Use same format as console but without colors log_format = "%(asctime)s - %(name)s - %(levelname)s - %(message)s" formatter = logging.Formatter(log_format, datefmt="%Y-%m-%d %H:%M:%S") file_handler.setFormatter(formatter) - # Add to root logger - root_logger = logging.getLogger() - root_logger.addHandler(file_handler) + logging.getLogger().addHandler(file_handler) return file_handler @@ -220,10 +221,9 @@ def remove_file_handler(handler: logging.FileHandler): """Remove a file handler from the root logger and close it. Args: - handler: The FileHandler to remove + handler: The FileHandler previously returned by add_file_handler(). """ - root_logger = logging.getLogger() - root_logger.removeHandler(handler) + logging.getLogger().removeHandler(handler) handler.close()