update curobo sphere fit - #468
Conversation
…j/update-curobo-sphere-fit
Greptile SummaryThe PR changes cuRobo robot sphere fitting and replaces visual-mesh world export with physical collision-shape representations.
Confidence Score: 4/5The PR is not yet safe to merge because the enabled benchmark suites still construct cuRobo with stale voxel padding and therefore do not exercise the planner's normal collision geometry. When the checked-in suites omit voxel padding, the benchmark adapter supplies 0.1 while normal planner construction uses 0.005, so benchmark collision geometry and planning outcomes can differ from the configuration being evaluated. Files Needing Attention: scripts/benchmark/motion_generation/planners/curobo.py
|
| Filename | Overview |
|---|---|
| embodichain/lab/sim/objects/rigid_object.py | Adds planner-independent snapshots of DexSim physical collision descriptors. |
| embodichain/lab/sim/planners/curobo/curobo_yaml.py | Generates mixed analytic, mesh, and voxel representations for cuRobo scenes. |
| embodichain/lab/sim/planners/curobo/curobo_planner.py | Integrates physical collision scenes, cache handling, dynamic updates, and collision-model visualization. |
| scripts/benchmark/motion_generation/planners/curobo.py | Adapts benchmark configuration to the revised planner, but its previously reported voxel-padding discrepancy remains. |
Flowchart
%%{init: {'theme': 'neutral'}}%%
flowchart LR
A[DexSim physical collision shapes] --> B[Representation policy]
B --> C[Analytic primitives]
B --> D[Triangle meshes]
B --> E[Voxel ESDF]
C --> F[cuRobo Scene]
D --> F
E --> F
F --> G[Motion planner]
Reviews (6): Last reviewed commit: "merge main" | Re-trigger Greptile
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
There was a problem hiding this comment.
Pull request overview
This PR updates EmbodiChain’s cuRobo planner integration to use DexSim’s MorphIt sphere fitting for both robot-link and world obstacle collision spheres, and adds runtime support for cuRobo V2 sphere obstacles by registering an analytic sphere SDF into cuRobo’s Warp-based collision checker. It also adds cached collision-model visualization utilities, updates documentation, and expands the test suite to cover the new behavior (including temporarily disabling cuRobo self-collision checking).
Changes:
- Switch sphere fitting for robot/world YAML generation from cuRobo’s fitter to DexSim MorphIt with fixed convex-hull limits.
- Add analytic sphere obstacle storage + cuRobo runtime hooks so sphere scene obstacles are actually checked at runtime.
- Add visualization helpers for cached robot/world collision spheres and extend tests/docs accordingly.
Reviewed changes
Copilot reviewed 6 out of 6 changed files in this pull request and generated 4 comments.
Show a summary per file
| File | Description |
|---|---|
| tests/sim/planners/test_curobo_planner.py | Adds tests for DexSim MorphIt fitting, runtime sphere registration/validation, self-collision disabling, and visualization. |
| examples/sim/planners/curobo_planner.py | Updates the demo to use sphere obstacle representation and optionally visualize cached collision models. |
| embodichain/lab/sim/planners/curobo/curobo_yaml.py | Switches YAML generation sphere fitting to DexSim + Open3D and adds collision-model visualization utilities. |
| embodichain/lab/sim/planners/curobo/curobo_sphere_data.py | Introduces analytic sphere obstacle storage and Warp SDF helpers for cuRobo’s generic collision checker. |
| embodichain/lab/sim/planners/curobo/curobo_planner.py | Wires in runtime sphere support registration, disables self-collision checks, validates runtime sphere loading, and adds visualization entrypoint. |
| docs/source/overview/sim/planners/curobo_planner.md | Documents DexSim MorphIt usage, runtime sphere support, visualization, and the temporary self-collision disablement. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| inv_pose = torch.zeros( | ||
| (num_envs, max_n, 8), | ||
| dtype=device_cfg.dtype, | ||
| device=device_cfg.device, | ||
| ) | ||
| inv_pose[..., 3] = 1.0 |
| # if robot_type == "w1": | ||
| # Keep the W1-specific IK diagnostic batched so it remains useful when | ||
| # checking solver and cuRobo reachability across multiple environments. | ||
| # import ipdb; ipdb.set_trace() | ||
| init_qpos = torch.tensor( | ||
| robot.cfg.init_qpos, dtype=torch.float32, device=robot.device | ||
| ) | ||
| arm_init_qpos = ( | ||
| init_qpos[robot.get_joint_ids(control_part)] | ||
| .unsqueeze(0) | ||
| .expand(num_envs, -1) | ||
| .clone() | ||
| ) | ||
| is_success, ik_qpos = robot.compute_ik( | ||
| pose=target_xpos, name=control_part, joint_seed=arm_init_qpos | ||
| ) | ||
| print(f"robot target xpos ik success: {is_success}, ik_qpos: {ik_qpos}") |
| ``"sphere"`` (default) fits spheres with DexSim's MorphIt implementation | ||
| (approximate, and requires CUDA + Open3D). cuRobo V2 can parse sphere | ||
| obstacles but omits their collision storage; EmbodiChain registers an |
| world=CuroboWorldCfg( | ||
| rigid_objects=obstacles, | ||
| obstacle_representation="cuboid", | ||
| obstacle_representation=("sphere"), |
…j/update-curobo-sphere-fit
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 10 out of 10 changed files in this pull request and generated no new comments.
Suppressed comments (2)
examples/sim/planners/curobo_planner.py:465
- The comment block says this is a "W1-specific IK diagnostic", but the
compute_ik(...)call now runs unconditionally for all robots, so the comment is misleading. Also, leaving commented-out debugger lines (ipdb.set_trace) in an example reads like leftover debug scaffolding.
# if robot_type == "w1":
# Keep the W1-specific IK diagnostic batched so it remains useful when
# checking solver and cuRobo reachability across multiple environments.
# import ipdb; ipdb.set_trace()
init_qpos = torch.tensor(
embodichain/lab/sim/planners/curobo/curobo_planner.py:1563
torch.load(..., weights_only=True)is used without a compatibility fallback. Elsewhere in the codebase (e.g.embodichain/lab/sim/planners/neural_planner.py:_safe_torch_load) the project falls back toweights_only=Falsewhen running on older PyTorch versions or whenweights_only=Truecannot deserialize the file. Without a fallback, loading an existing world cache will raise on those setups and break planner init.
cache_path = os.path.join(cache_dir, f"world_{cache_key}.pt")
if not auto.force and os.path.exists(cache_path):
logger.log_info(f"cuRobo voxel world cache hit: {cache_path}")
scene_data = torch.load(cache_path, map_location="cpu", weights_only=True)
else:
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 10 out of 10 changed files in this pull request and generated no new comments.
Suppressed comments (5)
examples/sim/planners/curobo_planner.py:467
- Commented-out debug code (
ipdb.set_trace()) and a commented conditional were added to the example. Leaving these in the repo makes the example harder to follow and risks reintroducing debug statements later.
# if robot_type == "w1":
# Keep the W1-specific IK diagnostic batched so it remains useful when
# checking solver and cuRobo reachability across multiple environments.
# import ipdb; ipdb.set_trace()
init_qpos = torch.tensor(
robot.cfg.init_qpos, dtype=torch.float32, device=robot.device
)
embodichain/lab/sim/planners/curobo/curobo_planner.py:1563
torch.load(..., weights_only=True)is used without a compatibility fallback. Because EmbodiChain does not pin a minimum PyTorch version (and older versions don't supportweights_only), this can raiseTypeErrorand break cuRobo world-cache reuse. The codebase already uses a try/fallback pattern (e.g.embodichain/lab/sim/planners/neural_planner.py:_safe_torch_load).
if not auto.force and os.path.exists(cache_path):
logger.log_info(f"cuRobo voxel world cache hit: {cache_path}")
scene_data = torch.load(cache_path, map_location="cpu", weights_only=True)
else:
embodichain/lab/sim/planners/curobo/curobo_planner.py:1581
deepcopy(scene_data)will deep-copy the cached voxel tensors, which can be extremely expensive in CPU memory/time before they are moved to GPU. Since you only need a device-mapped runtime view, prefer a shallow copy of the dict and rebuild the per-voxel entries while movingfeature_tensortoself._curobo_device.
runtime_data = deepcopy(scene_data)
for voxel in runtime_data.get("voxel", {}).values():
voxel["feature_tensor"] = voxel["feature_tensor"].to(
device=self._curobo_device, dtype=torch.float16
)
return self._bindings.Scene.create(runtime_data)
docs/source/overview/sim/planners/curobo_planner.md:242
- The documentation says
planner.visualize_collision_models(...)shows an "Open3D overlay" and instructs readers to close an Open3D window, but the implementation overlays temporary actors in the DexSim scene and blocks on a terminalinput()prompt. This mismatch will confuse users.
For an Open3D overlay of the live robot/obstacle meshes and the exact spheres
read back from those YAML caches, call
`planner.visualize_collision_models(control_part)`. Robot sphere centers are
transformed by the simulator's live link poses. The interactive cuRobo example
calls this once after planner initialization; close the Open3D window to continue.
embodichain/lab/sim/planners/curobo/curobo_yaml.py:179
- The
generate_curobo_robot_yamldocstring lists ImportError only for DexSim/Open3D, but the function also imports cuRobo (UrdfRobotParser) and will raise ImportError if cuRobo is missing. The raised-exception documentation should match actual imports.
Raises:
ImportError: If DexSim or Open3D is not installed.
RuntimeError: If CUDA is unavailable or no spheres could be fitted.
Proposal: add an
|
| DexSim physical shape | Suggested cuRobo representation |
|---|---|
BOX |
cuboid; exact for the physics shape and normally the cheapest query |
PLANE |
a workspace-bounded thin cuboid |
SPHERE / CAPSULE |
preferably an analytic SDF checker; otherwise analytically generated voxel ESDF or a mesh fallback |
CONVEX |
the actual collision convex mesh |
MESH |
mesh for moderate geometry; voxel ESDF when mesh complexity and repeated-query cost justify it |
SDF |
reuse/convert to a cuRobo voxel grid when the grid data and conventions are compatible |
| compound / ACD | preserve the individual sub-shapes and build a mixed scene |
| unsupported/custom | explicit override or a documented conservative fallback |
Shape type should be the first decision key, not the only one. Mesh selection should also consider triangle count, static versus pose-dynamic behavior, expected query reuse, and an estimated voxel budget such as prod(ceil(dims / voxel_size)).
Suggested public API
Keep the normal user path simple:
CuroboWorldCfg(
representation="auto",
overrides={
"room_scan": "voxel",
"precision_fixture": "mesh",
},
)Internally, EmbodiChain could expose a planner-independent snapshot API such as:
RigidObject.get_collision_shapes(env_id=0) -> list[CollisionShapeDesc]This would avoid having the cuRobo adapter access private obj._entities, and it would ensure that planning uses the same collision geometry as DexSim physics. A compound object should map one object UID to multiple stable cuRobo obstacle names, with dynamic object pose updates fanned out through each shape local pose.
DexSim gaps that should be addressed first
ShapeGeometry.local_poseis bound to Python, but the currentRigidBodyX::GetShapeGeometry()implementation constructs geometry descriptors without copyingshape->getLocalPose(). Compound and USD collision-shape offsets would therefore be lost.SDFGeometryis bound, but the Pythonget_shape_geometry()dispatch does not includeSHAPE_SDF, so an SDF can currently be detected through the type bitmask but its geometry cannot be retrieved through this API. Reusing an existing DexSim SDF would additionally require exposing its grid metadata/data or a canonical collision mesh.- The runtime geometry should be treated as already scaled; the adapter must avoid applying
RigidObjectCfg.body_scalea second time. - Batched environments should validate that shape topology is identical across rows before cloning environment 0 geometry.
Recommendation
I suggest keeping forced voxel as an explicit mode, but making auto the long-term default. In auto, simple physics primitives remain primitives, collision meshes remain meshes when practical, and ESDF is selected where it provides a real benefit. This preserves the simplified user experience without making voxel ESDF the universal scene representation, and it avoids planning against visual geometry that does not match the simulator collision model.
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 12 out of 12 changed files in this pull request and generated no new comments.
Suppressed comments (3)
examples/sim/planners/curobo_planner.py:465
- The example currently includes commented-out
ipdb.set_trace()debug lines in a newly added block. These comments are easy to copy/paste into production runs and are noisy in a public example; please remove or replace with a neutral note.
# if robot_type == "w1":
# Keep the W1-specific IK diagnostic batched so it remains useful when
# checking solver and cuRobo reachability across multiple environments.
# import ipdb; ipdb.set_trace()
init_qpos = torch.tensor(
embodichain/lab/sim/planners/base_planner.py:193
- The PR description references
motion_generator.planner.visualize_collision_models(control_part), but the public API added here is namedvisualize_robot_collision_models. This mismatch can confuse users and documentation consumers; consider renaming to match the described API, or update the PR description/docs consistently.
def visualize_robot_collision_models(
self,
control_part: str,
env_id: int = 0,
) -> None:
docs/source/overview/sim/planners/curobo_planner.md:242
- The documentation says this visualization is an "Open3D overlay" and instructs readers to close an Open3D window, but
visualize_robot_collision_modelsuses a DexSim overlay and blocks on terminal input ("Press Enter..."). Align the docs with the actual interaction so users aren't stuck looking for a window to close.
For an Open3D overlay of the live robot/obstacle meshes and the exact spheres
read back from those YAML caches, call
`planner.visualize_robot_collision_models(control_part)`. Robot sphere centers are
transformed by the simulator's live link poses. The interactive cuRobo example
calls this once after planner initialization; close the Open3D window to continue.
| world_values.get("collision_cache", {"cuboid": 8, "mesh": 2}) | ||
| ), | ||
| voxel_size=float(world_values.get("voxel_size", 0.01)), | ||
| voxel_padding=float(world_values.get("voxel_padding", 0.1)), |
There was a problem hiding this comment.
When either checked-in cuRobo benchmark suite omits world.voxel_padding, the adapter supplies 0.1 instead of the planner's new 0.005 default, causing benchmarks to generate different ESDF collision geometry and potentially different planning outcomes than normal planner construction. The associated default-value test also retains the stale 0.1 expectation and fails against the new configuration default.
Prompt To Fix With AI
This is a comment left during a code review.
Path: scripts/benchmark/motion_generation/planners/curobo.py
Line: 78
Comment:
**Benchmark uses stale voxel padding**
When either checked-in cuRobo benchmark suite omits `world.voxel_padding`, the adapter supplies `0.1` instead of the planner's new `0.005` default, causing benchmarks to generate different ESDF collision geometry and potentially different planning outcomes than normal planner construction. The associated default-value test also retains the stale `0.1` expectation and fails against the new configuration default.
---
For each issue above, determine whether it is valid and should be fixed. If so, fix it directly.
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 12 out of 12 changed files in this pull request and generated no new comments.
Suppressed comments (5)
examples/sim/planners/curobo_planner.py:465
- This example currently runs an unconditional IK diagnostic (and includes commented-out
ipdbnotes) on every run, but the result is only printed and never used. That adds overhead/noise and the surrounding comment still implies this is W1-specific even though the guard is commented out.
# if robot_type == "w1":
# Keep the W1-specific IK diagnostic batched so it remains useful when
# checking solver and cuRobo reachability across multiple environments.
# import ipdb; ipdb.set_trace()
init_qpos = torch.tensor(
embodichain/lab/sim/planners/curobo/curobo_planner.py:155
- CuroboWorldCfg.voxel_padding default (0.005) contradicts the updated tests/benchmarks expecting 0.1, which will make the new unit test fail and likely under-pad the ESDF grid for collision queries near the boundary.
voxel_padding: float = 0.005
embodichain/lab/sim/planners/curobo/curobo_planner.py:1514
- _robot_yaml_cache_key no longer includes any generator/schema version. Because the generation logic changed (DexSim sphere fitting, self-collision fields removed), existing cached YAMLs from older versions can be silently reused and the new behavior may never take effect unless users set auto_gen.force or delete caches.
"""Hash the URDF path/content and fit parameters into a stable cache key."""
hasher = hashlib.md5()
hasher.update(urdf_path.encode("utf-8"))
embodichain/lab/sim/planners/curobo/curobo_planner.py:1574
- _world_scene_cache_key also lacks an explicit generator/schema version. If voxel generation/VisACD parameters change in the future (e.g., hull limits, ESDF sampling rules), stale .pt caches could be reused with no easy signal to users.
"""Hash object geometry, initial poses, and voxel settings."""
hasher = hashlib.md5()
hasher.update(str(world_cfg.voxel_size).encode("utf-8"))
hasher.update(str(world_cfg.voxel_padding).encode("utf-8"))
embodichain/lab/sim/planners/base_planner.py:193
- The PR description references
motion_generator.planner.visualize_collision_models(control_part), but the new public API introduced here isvisualize_robot_collision_models(...)(and there is novisualize_collision_modelsmethod anywhere in the repo). This will confuse users following the PR description.
def visualize_robot_collision_models(
self,
control_part: str,
env_id: int = 0,
) -> None:
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 16 out of 16 changed files in this pull request and generated no new comments.
Suppressed comments (5)
examples/sim/planners/curobo_planner.py:465
- The commented-out
ipdb.set_trace()and the now-commentedif robot_type == "w1":block read like leftover debugging and can confuse users of this example. Either keep the W1-only conditional or update the comment to match the now-unconditional IK diagnostic.
# if robot_type == "w1":
# Keep the W1-specific IK diagnostic batched so it remains useful when
# checking solver and cuRobo reachability across multiple environments.
# import ipdb; ipdb.set_trace()
embodichain/lab/sim/planners/base_planner.py:215
- The PR description references
motion_generator.planner.visualize_collision_models(control_part), but the public API added here isvisualize_robot_collision_models. Adding a small wrapper keeps the documented entrypoint available and delegates to backend-specific overrides (e.g., cuRobo).
def visualize_robot_collision_models(
self,
control_part: str,
env_id: int = 0,
) -> None:
embodichain/lab/sim/planners/curobo/curobo_planner.py:152
- The docstring for
CuroboWorldCfg.representationsays onlyauto/forcedvoxel, but the implementation validates and supports additional forced representations (mesh,cuboid,sphere,capsule). This mismatch can mislead config users.
representation: str = "auto"
"""Collision representation policy: ``"auto"`` or forced ``"voxel"``."""
embodichain/lab/sim/planners/curobo/curobo_planner.py:2374
- This plane check relies on the enum member name string (
"PLANE"), which is more brittle than comparing the enum value itself. Using the enum class avoids false negatives if the enum representation changes.
local_pose = shape.local_pose.clone()
if shape.shape_type.name == "PLANE":
local_offset = torch.eye(4, dtype=torch.float32)
embodichain/lab/sim/planners/curobo/curobo_yaml.py:437
- If
trimeshis not installed, this raisesModuleNotFoundErrorwithout context. Since this helper is part of cuRobo scene generation, it’s helpful to raise a clearerImportErrorexplaining whytrimeshis needed (mesh/voxel conversion for analytic shapes).
import trimesh
Description
visualization:
motion_generator.planner.visualize_collision_models(control_part)（only support curobo)TODO:
Type of change
Checklist
black .command to format the code base.