feat(planner): integrate NVIDIA cuRobo V2#377
Open
yuecideng wants to merge 21 commits into
Open
Conversation
Add BasePlanner.preinterpolate_targets / preserve_plan_samples / default_plan_options / with_motion_context so MotionGenerator is capability-driven rather than special-casing NeuralPlanner. Neural and TOPPRA migrate to the new hooks; behavior is preserved. Co-Authored-By: Claude <noreply@anthropic.com>
Add CuroboRobotProfileCfg/CuroboWorldCfg/CuroboPlannerCfg/CuroboPlanOptions and pure helpers (_reorder_by_names, _matrix_to_position_quaternion, _validate_dynamic_obstacles, _require_curobo). The package exports the configs without importing curobo; construction triggers the lazy V2 import. Co-Authored-By: Claude <noreply@anthropic.com>
Implement CuroboPlanner.plan (pose + cspace), backend cache keyed by (control_part, batch_size, multi_env), segment chaining without resampling, failure/over-budget hold, dynamic obstacle updates, and close(). Add fake-binding unit tests (require CUDA) and an optional real V2 integration test skipped without cuRobo/CUDA. Includes the shared cuboid world asset. Co-Authored-By: Claude <noreply@anthropic.com>
ActionCfg validates motion_source/planner_type. TrajectoryBuilder dispatches by capability (preinterpolate_targets / preserve_plan_samples) with strict planner-type matching, result validation, and a new plan_joint_motion path. MoveJoints, PickUp, Place, Press allocate from returned phase lengths; Press returns via plan_joint_motion; PickUp skips the IK prefilter for cuRobo. Coordinated dual-arm primitives reject the cuRobo backend. Co-Authored-By: Claude <noreply@anthropic.com>
Harden batched V2 collision worlds and add the documented atomic-action demo with tests.
yuecideng
added
enhancement
Co-authored-by: matafela <chenjian@dexforce.com>
yuecideng
marked this pull request as ready for review
July 23, 2026 08:58
Comment on lines
+405
to
+411
| raise ImportError( | ||
| "cuRobo V2 is required for the 'curobo' planner but was not found. " | ||
| "Install it using NVIDIA's CUDA-matched extras, e.g. " | ||
| "`pip install .[cu12]` or `pip install .[cu13]` " | ||
| "(also `.[cu12-torch]` / `.[cu13-torch]`). " | ||
| f"See {_CUROBO_INSTALL_URL} for details." | ||
| ) from exc |
| import torch | ||
|
|
||
| from embodichain.lab.sim.planners import MotionGenOptions, PlanState | ||
| from embodichain.lab.sim.planners.curobo_planner import CuroboPlanOptions |
Comment on lines
+208
to
+210
| returned full-DoF trajectory. It disables cuRobo CUDA graph capture by default because graph | ||
| capture can conflict with DexSim GPU physics; pass `--cuda-graph` only after | ||
| validating that the local simulator stream setup supports it. Headless runs |
Comment on lines
+286
to
+289
| sim: SimulationManager | None = None | ||
| # try: | ||
| sim, robot, demo_block = _build_scene(args.headless) | ||
| if not args.headless: |
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 36 out of 37 changed files in this pull request and generated 1 comment.
Comments suppressed due to low confidence (3)
docs/source/overview/sim/planners/curobo_planner.md:210
- The docs mention passing a
--cuda-graphCLI flag, but the example script’s argument parser does not define it. This makes the demo instructions inaccurate; either add the flag or describe enabling CUDA graphs viaCuroboPlannerCfg.use_cuda_graph(or similar) instead.
returned full-DoF trajectory. It disables cuRobo CUDA graph capture by default because graph
capture can conflict with DexSim GPU physics; pass `--cuda-graph` only after
validating that the local simulator stream setup supports it. Headless runs
examples/sim/planners/curobo_planner.py:288
- There is a stray commented-out
# try:line, but the function body is not actually wrapped in a try/finally. This looks like leftover editing and is confusing in an example script.
sim: SimulationManager | None = None
# try:
sim, robot, demo_block = _build_scene(args.headless)
examples/sim/planners/curobo_planner.py:376
- If any exception is raised after the simulation is created (e.g., planning failure),
sim.destroy()andSimulationManager.flush_cleanup_queue()won’t run because cleanup is not in afinallyblock. In CI or batch runs this can leave DexSim resources and recordings open; please wrap the main body intry/finallyso recording is stopped and the simulation is always destroyed.
if sim.is_window_recording():
sim.stop_window_record()
sim.wait_window_record_saves()
sim.destroy()
SimulationManager.flush_cleanup_queue()
Comment on lines
+307
to
+313
| def _is_motion_gen_curobo(self) -> bool: | ||
| """Whether this action is configured to plan through the cuRobo backend.""" | ||
| return ( | ||
| getattr(self.cfg, "motion_source", None) == "motion_gen" | ||
| and getattr(self.cfg, "planner_type", None) == "curobo" | ||
| ) | ||
|
|
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 36 out of 37 changed files in this pull request and generated 3 comments.
Comments suppressed due to low confidence (3)
embodichain/lab/sim/planners/curobo/curobo_planner.py:417
- The actionable ImportError message suggests installing cuRobo with extras
.[cu12]/.[cu13], but this repo defines extras ascurobo-cu12/curobo-cu13(see pyproject.toml). As written, the suggested install commands will fail.
except ModuleNotFoundError as exc:
raise ImportError(
"cuRobo V2 is required for the 'curobo' planner but was not found. "
"Install it using NVIDIA's CUDA-matched extras, e.g. "
"`pip install .[cu12]` or `pip install .[cu13]` "
"(also `.[cu12-torch]` / `.[cu13-torch]`). "
f"See {_CUROBO_INSTALL_URL} for details."
docs/source/overview/sim/planners/curobo_planner.md:210
- The demo docs mention a
--cuda-graphCLI flag and claim CUDA graph capture is disabled by default, butexamples/sim/planners/curobo_planner.pydoes not define--cuda-graph, and the backend design here captures graphs inside the subprocess worker (no user toggle).
returned full-DoF trajectory. It disables cuRobo CUDA graph capture by default because graph
capture can conflict with DexSim GPU physics; pass `--cuda-graph` only after
validating that the local simulator stream setup supports it. Headless runs
examples/sim/planners/curobo_planner.py:293
- The demo has a commented-out
try:and nofinallyaround simulation/recording cleanup. If planning or playback raises, the viewer/recorder and sim resources may be left in a bad state (especially in headless recording).
sim: SimulationManager | None = None
# try:
sim, robot, demo_block = _build_scene(args.headless)
if not args.headless:
sim.open_window()
_start_headless_recording(sim, args)
if args.hold_steps:
sim.update(step=args.hold_steps)
Comment on lines
+79
to
+88
| mg = MotionGenerator( | ||
| MotionGenCfg( | ||
| planner_cfg=CuroboPlannerCfg( | ||
| robot_uid=ROBOT_UID, | ||
| world=CuroboWorldCfg(rigid_objects=[block]), | ||
| warmup=False, | ||
| use_cuda_graph=False, | ||
| ) | ||
| ) | ||
| ) |
Comment on lines
+94
to
+96
| `CuroboPlannerCfg.use_cuda_graph` defaults to `False` for the same DexSim GPU | ||
| stream-safety reason. Enable it explicitly only after validating the local | ||
| simulation stack. |
Comment on lines
159
to
164
| - name: Install test package | ||
| run: | | ||
| pip install -e ".[gensim]" \ | ||
| pip install -e ".[gensim, curobo-cu12]" \ | ||
| --extra-index-url http://pyp.open3dv.site:2345/simple/ \ | ||
| --trusted-host pyp.open3dv.site \ | ||
| --extra-index-url https://download.blender.org/pypi/ |
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 36 out of 37 changed files in this pull request and generated 4 comments.
Comments suppressed due to low confidence (4)
tests/sim/atomic_actions/test_curobo_motion_source_e2e.py:85
- CuroboPlannerCfg does not define
warmuporuse_cuda_graph(it useswarmup_iterationsand always captures CUDA graphs in the subprocess worker). As written, this test will raiseTypeErrorduring config construction.
planner_cfg=CuroboPlannerCfg(
robot_uid=ROBOT_UID,
world=CuroboWorldCfg(rigid_objects=[block]),
warmup=False,
use_cuda_graph=False,
.github/workflows/main.yml:164
- The PR description states cuRobo is an optional dependency, but this workflow now installs the cuRobo extra for every PR test run. Installing cuRobo from git (and its CUDA-matched deps) can significantly increase CI time and may fail on runners without the expected CUDA/PyTorch stack; the cuRobo-dependent tests are already guarded with import/CUDA skips.
- name: Install test package
run: |
pip install -e ".[gensim, curobo-cu12]" \
--extra-index-url http://pyp.open3dv.site:2345/simple/ \
--trusted-host pyp.open3dv.site \
--extra-index-url https://download.blender.org/pypi/
docs/source/overview/sim/planners/curobo_planner.md:96
- The docs reference
CuroboPlannerCfg.use_cuda_graph, butCuroboPlannerCfgdoes not define this option (the subprocess worker always captures CUDA graphs, per the planner config and unit tests). This is misleading for users configuring the backend.
`CuroboPlannerCfg.use_cuda_graph` defaults to `False` for the same DexSim GPU
stream-safety reason. Enable it explicitly only after validating the local
simulation stack.
docs/source/overview/sim/planners/curobo_planner.md:153
- The code sample imports
CuroboPlanOptionsfromembodichain.lab.sim.planners.curobo_planner, but that module path does not exist (the implementation lives underembodichain.lab.sim.planners.curobo.curobo_planner, and the public re-export isembodichain.lab.sim.planners). As written, the snippet will raise ImportError.
from embodichain.lab.sim.planners import MotionGenOptions, PlanState
from embodichain.lab.sim.planners.curobo_planner import CuroboPlanOptions
Comment on lines
+505
to
+507
| if batch_size > 1 and not torch.allclose( | ||
| positions, positions[:1].expand_as(positions) | ||
| ): |
Comment on lines
+66
to
+68
| import numpy as np | ||
| from scipy.spatial.transform import Rotation as R | ||
| from embodichain.lab.sim.robots import FrankaPandaCfg, URRobotCfg |
Comment on lines
+203
to
+206
| hand_urdf_path = get_data_path( | ||
| "BrainCoHandRevo1/BrainCoLeftHand/BrainCoLeftHand.urdf" | ||
| ) | ||
| hand_attach_xpos = np.eye(4) |
Comment on lines
+166
to
+168
| def _build_scene( | ||
| headless: bool, robot_type: str = "franka" | ||
| ) -> tuple[SimulationManager, Robot, RigidObject]: |
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 36 out of 37 changed files in this pull request and generated 1 comment.
Comments suppressed due to low confidence (4)
tests/sim/atomic_actions/test_curobo_motion_source_e2e.py:86
- CuroboPlannerCfg in this test is constructed with unsupported parameters (warmup, use_cuda_graph). The cuRobo planner config uses warmup_iterations and always uses CUDA graphs in the subprocess worker (no toggle), so this will raise a TypeError during test collection/execution.
planner_cfg=CuroboPlannerCfg(
robot_uid=ROBOT_UID,
world=CuroboWorldCfg(rigid_objects=[block]),
warmup=False,
use_cuda_graph=False,
)
docs/source/overview/sim/planners/curobo_planner.md:96
- The docs mention
CuroboPlannerCfg.use_cuda_graph, but the current config has no such field (CUDA graphs are always enabled in the subprocess worker). This section should be updated to reflect the actual configuration surface (e.g., warmup_iterations) and avoid referencing a non-existent option.
`CuroboPlannerCfg.use_cuda_graph` defaults to `False` for the same DexSim GPU
stream-safety reason. Enable it explicitly only after validating the local
simulation stack.
docs/source/overview/sim/planners/curobo_planner.md:153
- The example import
from embodichain.lab.sim.planners.curobo_planner import CuroboPlanOptionspoints to a module path that does not exist in this PR (the implementation lives underembodichain.lab.sim.planners.curobo.*, and is re-exported fromembodichain.lab.sim.planners). This would break the copy/paste example.
from embodichain.lab.sim.planners import MotionGenOptions, PlanState
from embodichain.lab.sim.planners.curobo_planner import CuroboPlanOptions
docs/source/overview/sim/planners/curobo_planner.md:210
- The demo section says CUDA graph capture is disabled by default and references a
--cuda-graphflag, but the example CLI in this PR does not define such a flag, and the planner worker always captures CUDA graphs. This is likely to confuse users.
returned full-DoF trajectory. It disables cuRobo CUDA graph capture by default because graph
capture can conflict with DexSim GPU physics; pass `--cuda-graph` only after
validating that the local simulator stream setup supports it. Headless runs
Comment on lines
+368
to
+369
| if robot is None: | ||
| raise RuntimeError(f"Failed to add robot '{robot.uid}' to the cuRobo demo.") |
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 36 out of 37 changed files in this pull request and generated 1 comment.
Comments suppressed due to low confidence (3)
.github/workflows/main.yml:163
- The workflow now installs
curobo-cu12for all pull-request test runs, which makes the optional cuRobo backend effectively mandatory for CI and can fail/slow down on runners that don't have a CUDA-capable environment. Consider keeping the base PR test job dependency-free and installing the cuRobo extra only right before the dedicated GPU test step (or in a separate GPU-only job).
- name: Install test package
run: |
pip install -e ".[gensim, curobo-cu12]" \
--extra-index-url http://pyp.open3dv.site:2345/simple/ \
--trusted-host pyp.open3dv.site \
docs/source/overview/sim/planners/curobo_planner.md:96
- This page references
CuroboPlannerCfg.use_cuda_graph, but the cuRobo backend config/tests indicate there is nouse_cuda_graphtoggle (the subprocess worker always captures graphs). The docs should match the actual public API to avoid misleading users.
`CuroboPlannerCfg.use_cuda_graph` defaults to `False` for the same DexSim GPU
stream-safety reason. Enable it explicitly only after validating the local
simulation stack.
docs/source/overview/sim/planners/curobo_planner.md:215
- The demo section mentions
--cuda-graph, but the example CLI doesn't define that flag and the backend doesn't expose ause_cuda_graphtoggle. Update the demo instructions to reflect the current behavior (graphs enabled in the isolated worker) or add the flag consistently across code/docs.
returned full-DoF trajectory. It disables cuRobo CUDA graph capture by default because graph
capture can conflict with DexSim GPU physics; pass `--cuda-graph` only after
validating that the local simulator stream setup supports it. Headless runs
Comment on lines
+546
to
+552
| _replay_full_dof_trajectory( | ||
| sim, | ||
| robot, | ||
| trajectory, | ||
| step_repeat=args.step_repeat, | ||
| ) | ||
| input("Press Enter to exit the cuRobo demo...") |
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 36 out of 37 changed files in this pull request and generated 1 comment.
Comments suppressed due to low confidence (4)
examples/sim/planners/curobo_planner.py:369
- The
robot is Noneguard dereferencesrobot.uid, which will raiseAttributeErrorand hide the real failure to add the robot.
if robot is None:
raise RuntimeError(f"Failed to add robot '{robot.uid}' to the cuRobo demo.")
examples/sim/planners/curobo_planner.py:472
main()no longer wraps simulation setup/execution in atry/finally, so exceptions (including failed planning) will skipsim.destroy()andflush_cleanup_queue(), potentially leaking the simulator process/resources in long-running sessions.
sim: SimulationManager | None = None
# try:
sim, robot, demo_block, target_xpos, control_part = _build_scene(
args.headless, args.robot
)
examples/sim/planners/curobo_planner.py:552
- The example always blocks on
input()even in--headlessmode, which prevents non-interactive runs (e.g., CI, remote execution) from completing.
input("Press Enter to exit the cuRobo demo...")
.github/workflows/main.yml:164
- The PR test workflow now unconditionally installs the
curobo-cu12extra. Since this extra is a git-based dependency (NVlabs/curobo) and cuRobo requires a CUDA-capable environment, this can make CPU-only PR CI fail during installation and also undermines the PR’s stated goal of keeping cuRobo optional for the codebase.
- name: Install test package
run: |
pip install -e ".[gensim, curobo-cu12]" \
--extra-index-url http://pyp.open3dv.site:2345/simple/ \
--trusted-host pyp.open3dv.site \
--extra-index-url https://download.blender.org/pypi/
Comment on lines
431
to
435
| success = ( | ||
| result.success | ||
| if isinstance(result.success, torch.Tensor) | ||
| else torch.tensor(result.success, device=self.device) | ||
| ) |
…/EmbodiChain into yueci/curobo-integraion
added 2 commits
July 24, 2026 23:39
…/EmbodiChain into yueci/curobo-integraion
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 37 out of 38 changed files in this pull request and generated no new comments.
Comments suppressed due to low confidence (6)
embodichain/lab/sim/planners/curobo/curobo_planner.py:436
- The missing-cuRobo ImportError message suggests installing
.[cu12]/.[cu13], but those extras are for the cuRobo repo, not for EmbodiChain. This is likely to confuse users because EmbodiChain exposes the dependency viacurobo-cu12/curobo-cu13extras (see pyproject/docs/example).
raise ImportError(
"cuRobo V2 is required for the 'curobo' planner but was not found. "
"Install it using NVIDIA's CUDA-matched extras, e.g. "
"`pip install .[cu12]` or `pip install .[cu13]` "
"(also `.[cu12-torch]` / `.[cu13-torch]`). "
f"See {_CUROBO_INSTALL_URL} for details."
) from exc
docs/source/overview/sim/planners/curobo_planner.md:96
- This doc references
CuroboPlannerCfg.use_cuda_graph, but the implementation runs cuRobo in a subprocess worker withuse_cuda_graph=Trueunconditionally and does not expose ause_cuda_graphconfig field. The docs should reflect the actual behavior/API to avoid misleading configuration guidance.
`CuroboPlannerCfg.use_cuda_graph` defaults to `False` for the same DexSim GPU
stream-safety reason. Enable it explicitly only after validating the local
simulation stack.
docs/source/overview/sim/planners/curobo_planner.md:215
- The demo docs say CUDA graph capture is disabled by default and mention a
--cuda-graphCLI flag, but the example CLI does not define that flag and the backend worker captures CUDA graphs by design. This section should be updated to match the current CLI and backend behavior.
auto-generated), prints the result status and trajectory shape, then replays the
returned full-DoF trajectory. It disables cuRobo CUDA graph capture by default because graph
capture can conflict with DexSim GPU physics; pass `--cuda-graph` only after
validating that the local simulator stream setup supports it. Headless runs
.github/workflows/main.yml:164
- The PR description says cuRobo is optional and adds no mandatory dependency, but the PR CI workflow now installs the
curobo-cu12extra for all pull_request test runs. This makes the optional dependency effectively required in CI and may introduce install-time failures or large slowdowns on runners that don't support cuRobo.
- name: Install test package
run: |
pip install -e ".[gensim, curobo-cu12]" \
--extra-index-url http://pyp.open3dv.site:2345/simple/ \
--trusted-host pyp.open3dv.site \
--extra-index-url https://download.blender.org/pypi/
examples/sim/planners/curobo_planner.py:552
input()will block indefinitely in headless/non-interactive runs (including automated environments), preventing the demo from exiting and writing recordings/cleaning up. Gate the prompt on interactive mode (or at leastnot args.headless).
input("Press Enter to exit the cuRobo demo...")
examples/sim/planners/curobo_planner.py:472
- The demo has a commented-out
try:and nofinallycleanup. If anything raises after the sim is created, the window recorder and simulation may not be stopped/destroyed cleanly. Consider restoring atry/finallyaround the main body so cleanup always runs.
sim: SimulationManager | None = None
# try:
sim, robot, demo_block, target_xpos, control_part = _build_scene(
args.headless, args.robot
)
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Description
This draft PR adds NVIDIA cuRobo V2 as an optional CUDA collision-aware motion-planning backend for EmbodiChain.
It adds:
Motivation: expose GPU-accelerated collision-aware planning through the existing robot, planner, motion-generator, and atomic-action abstractions.
Dependencies: NVIDIA cuRobo V2 is optional and must be installed in a CUDA/PyTorch-compatible environment. This PR adds no mandatory EmbodiChain dependency.
Type of change
Screenshots
The headless Panda demo can generate an MP4 recording when requested.
Draft note
The initial offscreen recorder samples wall time. Because simulation playback is very fast, a recording can appear nearly static even though trajectory generation and simulation execution are verified. Simulation-time recording is a follow-up before the PR is marked ready for review.
Checklist
black .command to format the code base.