# ----------------------------------------------------------------------------
# Copyright (c) 2021-2026 DexForce Technology Co., Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ----------------------------------------------------------------------------
"""cuRobo primary-baseline adapter for an empty collision world."""
from __future__ import annotations
import importlib.util
import torch
from embodichain.lab.sim.motion.motion_generator import (
MotionGenCfg,
MotionGenOptions,
MotionGenerator,
)
from embodichain.lab.sim.motion.planners import (
CuroboAutoGenCfg,
CuroboPlanOptions,
CuroboPlannerCfg,
CuroboWorldCfg,
MoveType,
PlanResult,
PlanState,
)
from ..config import PlannerSpecCfg
from ..models import BenchmarkCase
from ..registry import register_planner_adapter
from .base import PlannerAdapter, PlannerContext
__all__ = ["CuroboAdapter"]
[docs]
class CuroboAdapter(PlannerAdapter):
"""Run cuRobo with a frozen, empty-world operational configuration."""
capabilities = frozenset(
{
"eef_waypoint",
"joint_waypoint",
"batched",
"empty_world",
"atomic_action",
}
)
model_revision = "curobo-v2"
separate_prepare = True
timing_is_solved = True
"""cuRobo optimizes the trajectory's timing under its own dynamic limits."""
[docs]
def __init__(self, spec: PlannerSpecCfg, context: PlannerContext) -> None:
super().__init__(spec, context)
self.motion_generator: MotionGenerator | None = None
[docs]
def availability(self) -> tuple[bool, str | None]:
"""Require both CUDA and the optional cuRobo runtime."""
if not torch.cuda.is_available():
return False, "cuRobo requires CUDA, but CUDA is unavailable."
if importlib.util.find_spec("curobo") is None:
return False, "cuRobo is not installed; install one of the cuRobo extras."
return True, None
def _backend_motion_limits(self) -> dict[str, list[float]]:
"""Map common control-part budgets into cuRobo's active-joint order."""
limits = self.resolve_motion_limits(require_timing=True)
robot = self.context.robot
active_ids = list(robot.active_joint_ids)
controlled_ids = list(robot.get_joint_ids(self.context.control_part))
if any(index not in active_ids for index in controlled_ids):
raise ValueError("cuRobo control parts must contain only active joints.")
from embodichain.lab.sim.motion.planners.curobo.curobo_yaml import (
_curobo_joint_limits,
)
defaults = _curobo_joint_limits(robot)
# Explicit requests are positive; the production exporter intersects
# these with Robot caps again, retaining zero caps on locked joints.
velocity = [value if value > 0 else 0.2 for value in defaults["max_velocity"]]
acceleration = defaults["max_acceleration"]
for column, joint_id in enumerate(controlled_ids):
speed = limits["velocity"][column]
if speed <= 0:
raise ValueError(
"cuRobo cannot plan a joint with a zero velocity limit."
)
index = active_ids.index(joint_id)
velocity[index] = speed
acceleration[index] = limits["acceleration"][column]
for key, actual in (
("max_velocity", velocity),
("max_acceleration", acceleration),
):
if key in self.spec.config:
requested = torch.as_tensor(self.spec.config[key], dtype=torch.float64)
if not torch.allclose(
requested, torch.tensor(actual, dtype=torch.float64)
):
raise ValueError(
f"Planner config.{key} differs from the suite motion budget."
)
return {"max_velocity": velocity, "max_acceleration": acceleration}
[docs]
def build(self) -> None:
"""Construct MotionGenerator without materializing its lazy backend."""
limits = self._backend_motion_limits()
values = self.spec.config
world_values = dict(values.get("world", {}))
auto_values = dict(values.get("auto_gen", {}))
if bool(world_values.get("multi_env", False)):
raise ValueError(
"The current cuRobo benchmark adapter requires one shared empty world "
"with world.multi_env=false."
)
world = CuroboWorldCfg(
rigid_objects=None,
voxel_size=float(world_values.get("voxel_size", 0.01)),
voxel_padding=float(world_values.get("voxel_padding", 0.1)),
dynamic_obstacle_names=[],
multi_env=False,
)
planner_cfg = CuroboPlannerCfg(
robot_uid=self.context.robot.uid,
max_velocity=limits["max_velocity"],
max_acceleration=limits["max_acceleration"],
world=world,
auto_gen=CuroboAutoGenCfg(**auto_values),
collision_activation_distance=float(
values.get("collision_activation_distance", 0.01)
),
max_attempts=int(values.get("max_attempts", 5)),
max_planning_time=values.get("max_planning_time"),
cuda_device=values.get("cuda_device"),
use_cuda_graph=bool(values.get("use_cuda_graph", True)),
cuda_graph_fallback=bool(values.get("cuda_graph_fallback", True)),
interpolation_dt=float(values.get("interpolation_dt", 0.025)),
preserve_plan_samples=bool(values.get("preserve_plan_samples", True)),
warmup_iterations=int(values.get("warmup_iterations", 1)),
)
self.motion_generator = MotionGenerator(MotionGenCfg(planner_cfg=planner_cfg))
@staticmethod
def _case_move_type(case: BenchmarkCase) -> MoveType:
"""Return the planner move type required by a supported case."""
validity = case.case_parameters.get(
"motion_validity", "ordered_cartesian_waypoints"
)
if validity == "ordered_cartesian_waypoints":
return MoveType.EEF_MOVE
if validity == "ordered_joint_waypoints":
return MoveType.JOINT_MOVE
raise ValueError(f"Unsupported motion_validity mode {validity!r}.")
[docs]
def prepare(self, case: BenchmarkCase) -> dict[str, object]:
"""Materialize and warm the backend required by the benchmark case."""
if self.motion_generator is None:
raise RuntimeError("cuRobo adapter must be built before prepare().")
planner = self.motion_generator.planner
return planner.prepare_backend(
control_part=self.context.control_part,
batch_size=case.batch_size,
move_type=self._case_move_type(case),
)
[docs]
def prepare_cases(self, cases: list[BenchmarkCase]) -> dict[str, object]:
"""Prepare every distinct batch-size and planning-mode backend."""
if not cases:
raise ValueError("prepare_cases requires at least one supported case.")
prepared = []
seen = set()
for case in cases:
move_type = self._case_move_type(case)
key = (case.batch_size, move_type)
if key in seen:
continue
seen.add(key)
prepared.append(self.prepare(case))
return {"backends": prepared}
[docs]
def plan(self, case: BenchmarkCase) -> PlanResult:
"""Plan all ordered Cartesian or joint waypoints in one call."""
if self.motion_generator is None:
raise RuntimeError("cuRobo adapter must be built before plan().")
move_type = self._case_move_type(case)
if move_type is MoveType.JOINT_MOVE:
targets = [
PlanState.from_qpos(
case.reference_qpos[:, index], move_type=MoveType.JOINT_MOVE
)
for index in range(case.num_waypoints)
]
else:
targets = [
PlanState.from_xpos(
case.target_waypoints[:, index], move_type=MoveType.EEF_MOVE
)
for index in range(case.num_waypoints)
]
return self.motion_generator.generate(
targets,
MotionGenOptions(
start_qpos=case.start_qpos,
control_part=self.context.control_part,
plan_opts=CuroboPlanOptions(),
),
)
[docs]
def close(self) -> None:
"""Destroy cached cuRobo graph and planner resources."""
self._close_motion_generator()
register_planner_adapter("curobo", CuroboAdapter)