curobo Documentation

@misc{curobo_v2,
      title={cuRoboV2: Dynamics-Aware Motion Generation with Depth-Fused Distance Fields for High-DoF Robots},
      author={Balakumar Sundaralingam and Adithyavairavan Murali and Stan Birchfield},
      year={2026},
      eprint={2603.05493},
      archivePrefix={arXiv},
      primaryClass={cs.RO}
}

@INPROCEEDINGS{curobo_icra23,
author={Sundaralingam, Balakumar and Hari, Siva Kumar Sastry and
        Fishman, Adam and Garrett, Caelan and Van Wyk, Karl and Blukis, Valts and
        Millane, Alexander and Oleynikova, Helen and Handa, Ankur and
        Ramos, Fabio and Ratliff, Nathan and Fox, Dieter},
booktitle={2023 IEEE International Conference on Robotics and Automation (ICRA)},
title={CuRobo: Parallelized Collision-Free Robot Motion Generation},
year={2023},
volume={},
number={},
pages={8112-8119},
doi={10.1109/ICRA48891.2023.10160765}}

PRMGraphPlanner [label="PRMGraphPlanner\n(Main Coordinator)", color="#76b900", fontcolor="white"];

   GraphConstructor [label="GraphConstructor\n(Graph Building)", color="#558c8c", fontcolor="white"];
   NodeManager [label="GraphNodeManager\n(Node Storage)", color="#558c8c", fontcolor="white"];
   NodeSampling [label="NodeSamplingStrategy\n(Configuration Sampling)", color="#558c8c", fontcolor="white"];
   DistanceCalculator [label="DistanceNeighborCalculator\n(C-Space Metrics)", color="#558c8c", fontcolor="white"];
   LinearConnector [label="LinearConnector\n(Edge Creation)", color="#558c8c", fontcolor="white"];
   PathFinder [label="NetworkXPathFinder\n(Graph Search)", color="#558c8c", fontcolor="white"];
   PathPruner [label="PathPruner\n(Path Pruning)", color="#558c8c", fontcolor="white"];

   PRMGraphPlanner -> GraphConstructor;
   PRMGraphPlanner -> NodeManager;
   PRMGraphPlanner -> NodeSampling;
   PRMGraphPlanner -> PathFinder;
   PRMGraphPlanner -> PathPruner;

   GraphConstructor -> NodeManager;
   GraphConstructor -> LinearConnector;
   GraphConstructor -> DistanceCalculator;

   NodeManager -> PathFinder;

   subgraph cluster_components {
       style="rounded";
       color="#558c8c";
       label="Core Components";
       fontsize=12;
       GraphConstructor; NodeManager; NodeSampling;
       LinearConnector; DistanceCalculator;
   }

   subgraph cluster_search {
       style="rounded";
       color="#2B4162";
       label="Path Search & Pruning";
       fontsize=12;
       PathFinder; PathPruner;
   }

The :class:`~curobo._src.graph_planner.graph_planner_prm.PRMGraphPlanner` class serves as the primary entry point and coordinates all planning activities:

- Initializes and manages all subcomponents
- Exposes the main path finding interface
- Handles roadmap extension and maintenance
- Integrates with CuRobo's rollout-based feasibility checking

:class:`~curobo._src.graph_planner.graph.constructor.GraphConstructor`
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

The :class:`~curobo._src.graph_planner.graph.constructor.GraphConstructor` handles building and maintaining the graph structure:

- Manages node addition and edge creation
- Coordinates connection of terminal nodes (start/goal)
- Handles special cases like retraction nodes
- Directs the steering process between configurations

:class:`~curobo._src.graph_planner.graph.node_sampling_strategy.NodeSamplingStrategy`
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

This component specializes in generating feasible robot configurations:

- Provides uniform sampling within joint limits
- Implements informed sampling using ellipsoids (similar to BIT*)
- Supports multiple ellipsoid projection methods (SVD, Householder, approximate)
- Filters out collision configurations
- Uses Halton sequences for efficient low-discrepancy sampling

:class:`~curobo._src.graph_planner.graph.node_manager.GraphNodeManager`
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

The :class:`~curobo._src.graph_planner.graph.node_manager.GraphNodeManager` handles the storage and organization of graph nodes:

- Maintains buffers of valid nodes and their connections
- Tracks node indices and their relationships
- Manages node registration and edge creation
- Provides efficient access to graph structure

:class:`~curobo._src.graph_planner.graph.connector_linear.LinearConnector`
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

The :class:`~curobo._src.graph_planner.graph.connector_linear.LinearConnector` creates edges between nodes by:

- Implementing local steering between configurations
- Checking feasibility of connections
- Generating intermediate configurations along edges
- Enforcing joint limits during steering

:class:`~curobo._src.graph_planner.graph.node_distance.DistanceNeighborCalculator`
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

This component computes distances in configuration space:

- Uses weighted Euclidean distance in joint space
- Finds nearest neighbors efficiently
- Supports custom distance metrics with joint weights

:class:`~curobo._src.graph_planner.search.path_finder_networkx.NetworkXPathFinder`
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

The :class:`~curobo._src.graph_planner.search.path_finder_networkx.NetworkXPathFinder` uses NetworkX for graph search:

- Wraps NetworkX for path finding on the roadmap
- Provides shortest path computation using Dijkstra's algorithm
- Checks path existence between nodes
- Computes path lengths between nodes

:class:`~curobo._src.graph_planner.search.path_pruner.PathPruner`
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

The :class:`~curobo._src.graph_planner.search.path_pruner.PathPruner` optimizes the found paths:

- Simplifies paths by removing unnecessary waypoints
- Shortens paths by attempting direct connections
- Preserves path feasibility during optimization

Planning Workflow
-----------------

The planning process follows these main steps:

1. **Initialization**: Configure components and prepare internal buffers
2. **Graph Construction**:
   - Add start and goal nodes to the roadmap
   - Sample feasible configurations using the sampling strategy
   - Connect configurations using the linear connector
   - Register nodes and edges in the graph
3. **Path Finding**:
   - Search for a path using NetworkX
   - If no path exists, extend the roadmap with more samples
   - Use informed sampling to focus on promising regions
4. **Path Optimization**:
   - Prune unnecessary waypoints
   - Attempt shortcutting to simplify the path
5. **Interpolation**:
   - Generate a smooth, densely sampled trajectory from the path

GPU Acceleration
----------------

The implementation leverages GPU acceleration through:

- TorchScript JIT compilation for performance-critical functions
- CUDA graphs for optimized, repeated execution of the same operations
- Batched operations for efficient parallel computation
- Tensor-based storage for graph nodes and connections

Configuration
-------------

The planner is highly configurable through the :class:`~curobo._src.graph_planner.graph_planner_prm_cfg.PRMGraphPlannerCfg` class:

- Memory limits for node storage
- Sampling parameters
- Search iterations and heuristics
- Path optimization settings
- Buffer sizes for batched operations

Performance Considerations
--------------------------

Several strategies improve performance:

- Profiling decorators track component performance
- Batched collision checking minimizes CPU-GPU transfers
- Optimized nearest neighbor search
- JIT-compiled transformation functions
- Multiple ellipsoid projection methods with different performance characteristics

Key Parameters
--------------

.. list-table::
   :header-rows: 1
   :widths: 25 75

   * - Parameter
     - Description
   * - ``max_nodes``
     - Maximum number of nodes in the graph
   * - ``feasibility_buffer_size``
     - Maximum points to check for feasibility per batch
   * - ``steer_buffer_size``
     - Maximum points allowed between two nodes when steering
   * - ``exploration_radius``
     - Maximum radius to sample around the linear path
   * - ``new_nodes_per_iteration``
     - Number of nodes to sample per iteration
   * - ``max_path_finding_iterations``
     - Maximum iterations to find a path
   * - ``min_finetune_iterations``
     - Minimum iterations for path optimization
   * - ``use_default_position_heuristic``
     - Whether to connect through a retracted configuration
   * - ``neighbors_per_node``
     - Number of nearest neighbors to connect to

Usage Example
-------------

.. code-block:: python

   # Import required modules
   from curobo._src.graph_planner.graph_planner_prm_cfg import PRMGraphPlannerCfg
   from curobo._src.graph_planner.graph_planner_prm import PRMGraphPlanner
   import torch

   # Initialize the graph planner with a robot and world configuration
   robot_file = "franka.yml"
   world_file = "collision_thin_walls.yml"

   base_graph_planner_config = PRMGraphPlannerCfg.create(
       robot=robot_file,
       scene_model=world_file,
   )

   planner = PRMGraphPlanner(base_graph_planner_config)

   # Optionally warm up the planner for better performance
   planner.warmup()

   # Generate feasible start and goal configurations
   samples = planner.sampling_strategy.generate_feasible_action_samples(2)
   x_start = samples[0:1, :]  # Start configuration
   x_goal = samples[1:2, :]   # Goal configuration

   # Find a path between start and goal
   result = planner.find_path(
       x_start=x_start,
       x_goal=x_goal,
   )

   # Extract the trajectory if planning was successful
   if result.success[0]:
       path = result.plan[0]
       # Use the path for robot execution

Integration with CuRobo
-----------------------

The graph planner integrates with the broader CuRobo framework:

- Uses CuRobo's collision checking for feasibility testing
- Leverages the robot model for kinematics
- Relies on rollout-based feasibility evaluation
- Integrates with existing trajectory representations

Implementation Details
----------------------

Memory Management
~~~~~~~~~~~~~~~~~

The implementation carefully manages memory through:

- Pre-allocated buffers for nodes and edges
- Configurable buffer sizes
- Efficient tensor operations to minimize allocations
- Reuse of computation buffers when possible

Informed Sampling
~~~~~~~~~~~~~~~~~

The planner implements informed sampling strategies to focus computation in promising regions:

- Initial uniform sampling to explore the space
- Informed ellipsoidal sampling that focuses around the direct path
- Progressive radius reduction to refine the solution

Path Optimization
~~~~~~~~~~~~~~~~~

Once a path is found, it undergoes several optimization steps:

- Shortcutting to remove unnecessary waypoints
- Collision checking of optimized segments
- Smoothing to improve trajectory quality
- Dense interpolation to ensure smooth execution

Use Cases
---------

The PRM planner is particularly effective for:

- High-dimensional configuration spaces
- Problems requiring a reusable roadmap
- Multi-query planning scenarios
- Applications where preprocessing can amortize online planning costs

---

### Concepts Index (`docs/concepts/index.rst`)

Concepts
========

**Understanding-oriented documentation** that provides context and deepens your knowledge of cuRobo's concepts, architecture, and design decisions.


Architecture & Core Concepts
-----------------------------

.. toctree::
   :maxdepth: 1

   rollout_class

Optimization & Planning
-----------------------

.. toctree::
   :maxdepth: 1

   optimization_solver
   graph_planner

---

### Concepts Optimization Solver (`docs/concepts/optimization_solver.rst`)

.. _optimization_solver_note:

Optimization Solvers
================================

cuRobo implements a family of optimization solvers that operate on any class satisfying the
:py:class:`~curobo._src.rollout.rollout_protocol.Rollout` Protocol (see
:ref:`rollout_class_note`) to minimize costs and constraints. Every solver accepts a rollout
that maps ``[batch, horizon, dof]`` actions to ``[batch, horizon]`` cost, and every solver
itself satisfies the :py:class:`~curobo._src.optim.optimizer_protocol.Optimizer` Protocol.
There is no shared base class -- solvers are standalone classes that compose a small set of
reusable components.

The solvers fall into three broad categories:

1. **Gradient-based solvers** (:py:mod:`curobo._src.optim.gradient`) -- use the rollout's
   gradient to take Newton-ish steps with a parallel line search.
2. **Particle-based solvers** (:py:mod:`curobo._src.optim.particle`) -- sample actions from a
   Gaussian distribution and update the distribution from the cost statistics.
3. **External solvers** (:py:mod:`curobo._src.optim.external`) -- delegate the outer loop to
   :py:class:`~curobo._src.optim.external.scipy_opt.ScipyOpt` (SciPy) or
   :py:class:`~curobo._src.optim.external.torch_opt.TorchOpt` (``torch.optim``) while still
   evaluating costs and gradients on the GPU through the cuRobo rollout.

Multiple solvers can be chained into a single solve via
:py:class:`~curobo._src.optim.multi_stage_optimizer.MultiStageOptimizer`, which itself
implements the ``Optimizer`` Protocol (see :ref:`chaining-optimizers` below).


.. graphviz::
    :caption: Optimizer Protocol and the composition of its concrete implementations

    digraph {
        edge [color="#2B4162", fontsize=10];
        node [shape="box", style="rounded, filled", fontsize=12, color="#cccccc"];

        // Interface node (dotted border).
        optimizer [
            label="Optimizer\n(Protocol, interface only)",
            shape="box",
            style="rounded, dashed",
            color="#76b900",
            fontcolor="#2B4162"
        ];

        subgraph cluster_gradient {
            label="Gradient-Based Optimizers";
            style="rounded, dashed";
            color="#558c8c";

            lbfgs   [label="LBFGSOpt",             color="#76b900", style="filled, rounded", fontcolor="white"];
            lsr1    [label="LSR1Opt",              color="#76b900", style="filled, rounded", fontcolor="white"];
            cg      [label="ConjugateGradientOpt", color="#76b900", style="filled, rounded", fontcolor="white"];
            gd      [label="GradientDescentOpt",   color="#76b900", style="filled, rounded", fontcolor="white"];
        }

        subgraph cluster_particle {
            label="Particle-Based Optimizers";
            style="rounded, dashed";
            color="#558c8c";

            mppi    [label="MPPI",                 color="#76b900", style="filled, rounded", fontcolor="white"];
            es      [label="EvolutionStrategies",  color="#76b900", style="filled, rounded", fontcolor="white"];
        }

        subgraph cluster_external {
            label="External Optimizers";
            style="rounded, dashed";
            color="#558c8c";

            scipy_opt [label="ScipyOpt", color="#ff9900", style="filled, rounded", fontcolor="white"];
            torch_opt [label="TorchOpt", color="#ff9900", style="filled, rounded", fontcolor="white"];
        }

        subgraph cluster_meta {
            label="Chaining";
            style="rounded, dashed";
            color="#558c8c";

            multi [label="MultiStageOptimizer", color="#ff9900", style="filled, rounded", fontcolor="white"];
        }

        subgraph cluster_components {
            label="Shared Components (composed, not inherited)";
            style="rounded, dashed";
            color="#934337";

            gradient_core [label="GradientOptCore",    color="#cccccc", fontcolor="black"];
            particle_core [label="ParticleOptCore",    color="#cccccc", fontcolor="black"];
            qn            [label="QuasiNewtonBuffers", color="#cccccc", fontcolor="black"];
            best          [label="BestTracker",        color="#cccccc", fontcolor="black"];
            gauss         [label="GaussianDistribution", color="#cccccc", fontcolor="black"];
            bounds        [label="ActionBounds",       color="#cccccc", fontcolor="black"];
            debug         [label="DebugRecorder",      color="#cccccc", fontcolor="black"];
        }

        // "Implements" edges from each optimizer to the Optimizer protocol.
        {lbfgs, lsr1, cg, gd, mppi, es, scipy_opt, torch_opt, multi}
            -> optimizer [label="implements", style="dashed", fontcolor="#2B4162"];

        // "Composes" edges from each optimizer to the components it owns.
        lbfgs   -> {gradient_core, qn}      [label="composes"];
        lsr1    -> {gradient_core, qn}      [label="composes"];
        cg      -> {gradient_core}          [label="composes"];
        gd      -> {bounds, best, debug}    [label="composes"];

        mppi    -> {particle_core}          [label="composes"];
        es      -> {particle_core}          [label="composes"];
        particle_core -> {gauss, bounds, debug} [label="composes"];

        scipy_opt -> {bounds, debug}        [label="composes"];
        torch_opt -> {bounds, debug}        [label="composes"];

        // MultiStageOptimizer chains other optimizers, it does not compose
        // the lower-level buffer components itself.
        multi -> {lbfgs, mppi} [label="stages", style="dashed"];
    }


Gradient-Based Optimization Solvers
-----------------------------------

The gradient-based solvers that cuRobo implements use a parallel line search strategy on the GPU
to find the minimum instead of a sequential line search strategy (e.g., backtracking line search).
At every iteration of a line-search based optimization solver, cuRobo performs the following steps:

1. Compute the cost and gradient of the current action.
2. Compute a step direction based on the gradient (and possibly other information).
3. Perform a line search to find the best step size along the step direction.
4. Update the action using the step direction and step size.
5. Repeat until convergence or maximum iterations reached.

The specific implementation details vary depending on the optimization algorithm being used (LBFGS, CG, etc.).



Line Search Optimization
^^^^^^^^^^^^^^^^^^^^^^^^

1. Compute the best step magnitude using line search from the current action and step direction (from previous iteration).
2. Store the best action and gradient.
3. Compute the new step direction using the gradient of the best action.


.. digraph:: LineSearchOptimization
   :align: center
   :caption: Line Search Optimization Iteration

   node [shape="box", style="filled", color="#76b900", fontcolor="white", fontname="sans-serif"];
   edge [color= "#aaaaaa" , fontname="sans-serif", fontsize=10, style="dashed"];

   subgraph cluster_input {
      label="Input to Iteration\n OptimizationIterationResult";
      style="rounded, dashed";
      color="#558c8c";

      current_action [label="current_action", shape="box", color="#cccccc", style="filled", fontcolor="black"];
      step_direction [label="step_direction", shape="box", color="#cccccc", style="filled", fontcolor="black"];
   }
   compute_best_step [label="compute_best_step\nusing line search"];
   compute_cost_gradient [label="Rollout.evaluate_action"];
   compute_best_step -> compute_cost_gradient [dir="both"];
   subgraph cluster_output {
      label="Output of Iteration\n OptimizationIterationResult";
      style="rounded, dashed";
      color="#558c8c";

      next_action [label="next_action", shape="box", color="#cccccc", style="filled", fontcolor="black"];
      next_cost [label="next_cost", shape="box", color="#cccccc", style="filled", fontcolor="black"];
      next_gradient [label="next_gradient", shape="box", color="#cccccc", style="filled", fontcolor="black"];
   }
   store_best [label="store_best_action"];
   compute_direction [label="compute_step_direction"];
   new_direction [label="new_step_direction", shape="box", color="#cccccc", style="filled", fontcolor="black"];

   {current_action, step_direction} -> compute_best_step;
   compute_best_step -> {next_action, next_cost, next_gradient};
   {next_action, next_gradient, next_cost} -> store_best;
   {next_action, next_gradient, next_cost} -> compute_direction;
   compute_direction -> new_direction;
   next_action -> current_action [label="Next Iteration", color= "#000000", style="solid"];
   new_direction -> {step_direction} [label="Next Iteration", color= "#000000", style="solid"];


Particle-Based Optimization Solvers
-----------------------------------

Particle-based solvers sample a batch of candidate actions from a Gaussian distribution over the
action sequence, score each sample with the rollout, and update the distribution from the
cost-weighted statistics. cuRobo ships two particle solvers:

- :py:class:`~curobo._src.optim.particle.mppi.MPPI` -- Model-Predictive Path Integral control.
  Computes exponential-utility weights
  (``w ∝ exp(-beta * cost)``) and takes an exponentially-moving-average step on the
  distribution mean and covariance. The ``beta`` temperature is the primary tuning knob for the
  exploration/exploitation trade-off.
- :py:class:`~curobo._src.optim.particle.evolution_strategies.EvolutionStrategies` -- a natural-
  gradient evolution-strategies solver that uses z-score-normalised cost weights and a
  configurable ``learning_rate`` for the mean update.

Both solvers share the same hot loop: they compose a
:py:class:`~curobo._src.optim.components.particle_opt_core.ParticleOptCore`, which owns the
:py:class:`~curobo._src.optim.components.gaussian_distribution.GaussianDistribution`,
:py:class:`~curobo._src.optim.components.action_bounds.ActionBounds`, and
:py:class:`~curobo._src.optim.components.debug_recorder.DebugRecorder`, and drives sampling,
rollout evaluation, and weight computation. The solvers differ only in their
distribution-update function; everything else is reused through composition.


External Optimization Solvers
-----------------------------

External solvers delegate the optimizer loop to a third-party library while still evaluating
costs, constraints, and gradients on the GPU through the cuRobo rollout. Both external solvers
compose :py:class:`~curobo._src.optim.components.action_bounds.ActionBounds` (for projecting and
scaling actions) and :py:class:`~curobo._src.optim.components.debug_recorder.DebugRecorder` (for
optional per-iteration traces).


Scipy Optimization
^^^^^^^^^^^^^^^^^^

The :py:class:`~curobo._src.optim.external.scipy_opt.ScipyOpt` solver wraps
``scipy.optimize.minimize``. On each iteration it:

1. Computes costs, constraints, and gradients on the GPU via the rollout.
2. Transfers the scalar cost and gradient to CPU.
3. Hands control to SciPy (e.g. ``L-BFGS-B`` or ``SLSQP``) to produce the next action.

The method is selected through ``scipy_minimize_method`` on
:py:class:`~curobo._src.optim.external.scipy_opt.ScipyOptCfg`. Constraint-aware methods like
``SLSQP`` automatically receive cuRobo constraints as inequality constraints, and the action is
converted to ``float64`` on CPU for numerical stability.


Torch Optimization
^^^^^^^^^^^^^^^^^^

The :py:class:`~curobo._src.optim.external.torch_opt.TorchOpt` solver wraps any optimizer from
:py:mod:`torch.optim` (e.g. Adam, SGD, LBFGS). It computes the cost via the rollout,
backpropagates through the computation graph, and delegates the parameter update to the wrapped
PyTorch optimizer. The specific optimizer class is selected by
:py:attr:`~curobo._src.optim.external.torch_opt.TorchOptCfg.torch_optim_name` (looked up in
``torch.optim``) or by passing an explicit
:py:attr:`~curobo._src.optim.external.torch_opt.TorchOptCfg.torch_optim_class`. Unlike the
gradient-based solvers above, ``TorchOpt`` does not use cuRobo's parallel line search -- the
wrapped PyTorch optimizer owns the step-size logic.


.. _chaining-optimizers:

Chaining Optimization Solvers
-----------------------------

cuRobo chains solvers through
:py:class:`~curobo._src.optim.multi_stage_optimizer.MultiStageOptimizer`, which itself satisfies
the :py:class:`~curobo._src.optim.optimizer_protocol.Optimizer` Protocol. Each stage runs to
completion and its output is fed as the seed for the next. This is how
:class:`curobo.inverse_kinematics.InverseKinematics` and
:class:`curobo.trajectory_optimizer.TrajectoryOptimizer` are wired: a random seed is first
optimized by :py:class:`~curobo._src.optim.particle.mppi.MPPI` for global exploration, and the
resulting distribution mean is handed to
:py:class:`~curobo._src.optim.gradient.lbfgs.LBFGSOpt` for local refinement.


.. graphviz::

   digraph {
        #rankdir=LR;
        edge [color = "#2B4162"; fontsize=10];
        node [shape="box", style="rounded, filled", fontsize=12, color="#cccccc"]
        seed [label="random seed"]
        opt_sol [label="optimized solution", shape="plain", style="rounded"]
        subgraph cluster_query{
        label="Query";
        start [label="Current State"]
        goal [label="Goal Pose"]

        }


        subgraph cluster_robot{
        label="Robot World Configuration";
        kin_model [label="RobotCfg", color="#76b900",fontcolor="white"]

        world_model [label="SceneCollision", color="#76b900",fontcolor="white"]
        }

        kin_model -> mppi_rollout [style="dashed", fontcolor="#708090", label="reference"];
        world_model -> mppi_rollout [style="dashed", fontcolor="#708090", label="reference"];
        seed -> mppi_solver [color="#76b900"];
        start -> mppi_rollout [style="dashed"];
        goal -> mppi_rollout [style="dashed"];
        start -> lbfgs_rollout [style="dashed"];
        goal -> lbfgs_rollout [style="dashed"];

        kin_model -> lbfgs_rollout [style="dashed", fontcolor="#708090", label="reference"];
        world_model -> lbfgs_rollout [style="dashed", fontcolor="#708090", label="reference"];

        mppi_solver -> mppi_sol [color="#76b900"];
        subgraph cluster_particle {
        label="Particle Optimization";
        mppi_solver[label = "Parallel MPPI"];
        mppi_rollout [label = "Rollout for MPPI"]
        mppi_sol[label="MPPI Solution"]
        mppi_rollout -> mppi_solver;
        }

        subgraph cluster_gradient {
        label="Gradient Optimization";
        lbfgs_solver[label = "LBFGS Solver"];
        lbfgs_rollout [label = "Rollout for L-BFGS"]
        lbfgs_sol [label = "LBFGS Solution"]
        lbfgs_rollout -> lbfgs_solver;

        }
        mppi_sol -> lbfgs_solver [color="#76b900"];
        lbfgs_solver -> lbfgs_sol [color="#76b900"];
        lbfgs_sol -> opt_sol [color="#76b900"];


   }


The steps inside the optimization solver is illustrated for reference. Gray blocks indicate running
rollout for 1 trajectory in the batch and green blocks refer to the cost terms.

.. figure:: ../images/optimization_graph.png
   :width: 500px
   :align: center

---

### Concepts Rollout Class (`docs/concepts/rollout_class.rst`)

.. _rollout_class_note:

Rollout Classes
================================

A rollout in optimal control refers to the computation of costs and constraints from given
optimization variables; these optimization variables are often a sequence of actions that a robot
will take from a given initial state at fixed timesteps ``dt``. The length of the sequence is the
rollout horizon. In some cases, the horizon of the action will be different from the rollout.
For example, if the robot needs to be static at the final timestep, we execute the last action
for a few more timesteps to ensure the robot is static.

.. digraph:: Rollout
   :align: center
   :caption: Rollout Flow

   node [shape="box", style="filled", color="#76b900", fontcolor="white", fontname="sans-serif"];
   edge [color="#2B4162", fontname="sans-serif", fontsize=10];

   subgraph cluster_rollout {
   label="Evaluate Action";
   style="rounded, dashed";
   color="#558c8c";

   action [label="Action Sequence", shape="box", color="#cccccc", style="filled", fontcolor="black"];
   compute_state [label="compute_state_from_action(act_seq)"];
   transition_model [label="Transition Model\n (Optional)", shape="box3d", color="#934337", style="filled", fontcolor="white"];
   state [label="Robot State", shape="box", color="#cccccc", style="filled", fontcolor="black"];

   compute_costs [label="compute_costs_and_constraints(state)"];
   cost_manager [label="CostManager\n (Optional)", shape="box3d", color="#934337", style="filled", fontcolor="white"];
   costs_constraints [label="Costs and Constraints", shape="box", color="#cccccc", style="filled", fontcolor="black"];
   result [label="RolloutResult", shape="box", color="#558c8c", style="filled", fontcolor="white"];
   }
   state_2 [label="Robot State", shape="box", color="#cccccc", style="filled", fontcolor="black"];

   subgraph cluster_metrics {
   label="Compute Metrics";
   style="rounded, dashed";
   color="#558c8c";

   convergence_result [label="RolloutMetrics", shape="box", color="#558c8c", style="filled", fontcolor="white"];
   compute_convergence_metrics [label="compute_convergence_metrics(state)"];
   }

   action -> compute_state;
   compute_state -> state;
   state -> compute_costs;
   compute_costs -> cost_manager[dir="both"];
   compute_state -> transition_model[dir="both"];
   compute_costs -> costs_constraints;
   costs_constraints -> result;
   state_2 -> compute_convergence_metrics [label="Robot State"];
   compute_convergence_metrics -> convergence_result;

In cuRobo, evaluation of action sequences is often done for a batch of action sequences as cuRobo
optimizes over different seeds/variations in parallel to find the best action sequence. For e.g.,
when using sampling based methods like :py:class:`~curobo._src.optim.particle.mppi.MPPI`,
evaluations are done across multiple particles to find the best action sequence. When using
gradient-based methods like :py:class:`~curobo._src.optim.gradient.lbfgs.LBFGSOpt`, evaluations
are done across step sizes (parallel line search). In addition to the batching due to optimization
solvers, we also need to batch across multiple seeds and also across different optimization
targets.

.. _rollout_protocol:

The Rollout Protocol
--------------------

cuRobo rollouts are defined structurally rather than through inheritance. Any class whose public
surface matches the :py:class:`~curobo._src.rollout.rollout_protocol.Rollout`
:class:`~typing.Protocol` can be passed wherever the solvers expect a rollout -- no base class is
required, and there is no ``RolloutBase`` to subclass.

A rollout must expose the following **properties** (all read-only):

- ``action_dim`` -- dimensionality of a single action.
- ``action_horizon`` -- number of timesteps in an action sequence.
- ``action_bound_lows`` / ``action_bound_highs`` -- per-dimension action limits, shape
  ``(action_dim,)``.
- ``dt`` -- integration timestep (seconds).
- ``sum_horizon`` -- whether costs are summed across the horizon before being returned.

and the following **methods**:

- ``evaluate_action(act_seq)`` -- the core forward pass used every optimizer iteration; returns a
  :py:class:`~curobo._src.rollout.metrics.RolloutResult`.
- ``compute_metrics_from_state(state)`` / ``compute_metrics_from_action(act_seq)`` -- compute
  full :py:class:`~curobo._src.rollout.metrics.RolloutMetrics` (costs, constraints, convergence)
  after optimization finishes.
- ``update_params`` / ``update_batch_size`` / ``update_dt`` -- lifecycle hooks the solver calls
  between solves to retarget, resize internal buffers, or change the timestep.
- ``reset`` / ``reset_shape`` / ``reset_seed`` -- clear per-problem state, cached goal shapes, and
  the Halton sampler respectively.

Separating ``evaluate_action`` from ``compute_metrics_from_*`` lets the optimizer hot-loop stay
tight while post-optimization analysis can afford to compute extra information. For inverse
kinematics, for example, ``compute_metrics_from_state`` reports position and rotation error
separately even though the optimizer only needed their weighted sum.

Two reference implementations ship with cuRobo:

- :py:class:`~curobo._src.rollout.rollout_rosenbrock.RosenbrockRollout` -- a minimal
  implementation over the Rosenbrock function, intended as a pedagogical example and as a test
  rollout for the solvers. See :ref:`tut_user_rollout_optimization`.
- :py:class:`~curobo._src.rollout.rollout_robot.RobotRollout` -- the realistic rollout used
  throughout cuRobo, composing a transition model, a
  :py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager`, and a scene
  collision checker.


CUDA Graph Acceleration
-----------------------

cuRobo supports `CUDA graphs <https://developer.nvidia.com/blog/cuda-graphs/>`_ to reduce the
overhead of kernel launches during the hot loop. CUDA graphs require the shape of inputs and
outputs to be constant across replays.

CUDA-graph acceleration is a **constructor parameter**, not a subclass or mixin. Pass
``use_cuda_graph=True`` when constructing the rollout:

.. code-block:: python

   from curobo._src.rollout.rollout_rosenbrock import RosenbrockCfg, RosenbrockRollout

   rollout = RosenbrockRollout(config, use_cuda_graph=True)

Internally the rollout wraps the post-optimization hooks
(``compute_metrics_from_state`` and ``compute_metrics_from_action``) in
:py:class:`~curobo._src.util.cuda_graph_util.GraphExecutor` instances. The graph is recorded
lazily on the first call and replayed on every subsequent call. When
``use_cuda_graph=False`` (the default), ``GraphExecutor`` transparently falls back to a direct
function call, so the same rollout class runs with or without graphs.

Two CUDA graphs are captured per rollout when ``use_cuda_graph=True``:

1. One for :py:meth:`~curobo._src.rollout.rollout_protocol.Rollout.compute_metrics_from_state`.
2. One for :py:meth:`~curobo._src.rollout.rollout_protocol.Rollout.compute_metrics_from_action`.

A separate CUDA graph for the inner ``evaluate_action`` loop is captured inside the optimizer
(see :ref:`optimization_solver_note`), since the optimizer's batch size typically differs from
the post-optimization batch size. Because it is also a constructor parameter
(``LBFGSOpt(config, rollout_list, use_cuda_graph=True)``), user rollout code does not need to
opt in; simply satisfying the :py:class:`~curobo._src.rollout.rollout_protocol.Rollout`
Protocol is enough. See :ref:`tut_rosen_rollout_optimization_cuda_graphs` for a worked example.


Cost Managers
--------------

When a rollout has many cost and constraint terms, listing all of them inline in ``evaluate_action``
becomes unwieldy. cuRobo provides a single flat class,
:py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager`, that owns every
cost term used by :py:class:`~curobo._src.rollout.rollout_robot.RobotRollout`. There is no cost
manager hierarchy -- ``RobotCostManager`` is not abstract and has no subclasses.

The manager is **config-driven**:
:py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot_cfg.RobotCostManagerCfg` has one
optional field per cost kind:

- ``self_collision_cfg``
- ``scene_collision_cfg``
- ``cspace_cfg``
- ``start_cspace_dist_cfg``
- ``target_cspace_dist_cfg``
- ``tool_pose_cfg``

Setting any of these to ``None`` disables that cost for the rollout. At construction time the
manager instantiates the corresponding :py:class:`~curobo._src.cost.cost_base.BaseCost` objects,
registers them via ``register_cost(name, component)``, and allocates a dedicated CUDA
stream/event pair for each via
:py:mod:`~curobo._src.util.cuda_stream_util` so the enabled costs can be evaluated in parallel.

A single :py:meth:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager.compute_costs`
call evaluates every enabled cost component inline (no ``super()`` chain) and returns a
:py:class:`~curobo._src.rollout.metrics.CostCollection`. Individual terms can still be toggled at
runtime through ``enable_cost_component(name)`` / ``disable_cost_component(name)``.

.. digraph:: CostManager
   :align: center
   :caption: Cost Manager Architecture

   node [shape="box", style="filled", color="#76b900", fontcolor="white", fontname="sans-serif"];
   edge [color="#2B4162", fontname="sans-serif", fontsize=10];

   state [label="Robot State", shape="box", color="#cccccc", style="filled", fontcolor="black"];
   manager [label="RobotCostManager"];

   subgraph cluster_costs {
      label="Registered Costs (enabled subset)";
      style="rounded, dashed";
      color="#558c8c";

      cost_a [label="self_collision"];
      cost_b [label="scene_collision"];
      cost_c [label="cspace"];
      cost_d [label="start_cspace_dist"];
      cost_e [label="target_cspace_dist"];
      cost_f [label="tool_pose"];
   }

   collection [label="CostCollection", shape="box", color="#cccccc", style="filled", fontcolor="black"];

   state -> manager;
   manager -> cost_a;
   manager -> cost_b;
   manager -> cost_c;
   manager -> cost_d;
   manager -> cost_e;
   manager -> cost_f;
   cost_a -> collection;
   cost_b -> collection;
   cost_c -> collection;
   cost_d -> collection;
   cost_e -> collection;
   cost_f -> collection;

For a step-by-step guide showing how ``RobotCostManager`` is wired into a rollout, see
:ref:`tut_motion_optimization`.


Available Rollout Classes
-------------------------

cuRobo ships two rollout classes, both of which satisfy the
:py:class:`~curobo._src.rollout.rollout_protocol.Rollout` Protocol.

.. list-table::
   :widths: 30 70
   :header-rows: 1
   :align: left

   * - Rollout Class
     - Description
   * - :py:class:`~curobo._src.rollout.rollout_rosenbrock.RosenbrockRollout`
     - Minimal pedagogical rollout over the Rosenbrock function. Useful for testing optimization
       algorithms and as a starting point for custom rollouts.
   * - :py:class:`~curobo._src.rollout.rollout_robot.RobotRollout`
     - Realistic rollout used across cuRobo's IK, trajectory optimization, and MPC pipelines.
       Composes a
       :py:class:`~curobo._src.transition.robot_state_transition.RobotStateTransition` model,
       a :py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager`,
       and a :py:class:`~curobo._src.geom.collision.collision_scene.SceneCollision` checker.

To create your own rollout class and optimize it using cuRobo, see
:ref:`tut_user_rollout_optimization`.

---

### Getting Started Build Robot Model (`docs/getting-started/build_robot_model.rst`)

.. _tutorial_build_robot_model:

Build Robot Model
=================

.. automodule:: curobo.examples.getting_started.build_robot_model
   :no-members:

Once you have run the tutorial, open
:mod:`curobo.examples.getting_started.build_robot_model` in your editor.
The inline comments walk through the key design decisions: how sphere density
affects collision checking fidelity, when to raise or lower the MorphIt loss
weights, and how the self-collision matrix is pruned.

---

### Getting Started Feature Mapping (`docs/getting-started/feature_mapping.rst`)

.. _tutorial_feature_mapping:

Feature Mapping
==================

.. automodule:: curobo.examples.getting_started.feature_mapping
   :no-members:

Once you have run the tutorial, open
:mod:`curobo.examples.getting_started.feature_mapping` in your editor.
The inline comments walk through the key design decisions: why depth is
filtered before integration, how C-RADIO patch features are attached to
``CameraObservation.feature_grid``, how block features are visualized with PCA,
and how optional text matching projects map features into the SigLIP adaptor
space.

---

### Getting Started Forward Kinematics (`docs/getting-started/forward_kinematics.rst`)

.. _tutorial_forward_kinematics:

Forward Kinematics
==================

.. automodule:: curobo.examples.getting_started.forward_kinematics
   :no-members:

---

### Getting Started Humanoid Retargeting (`docs/getting-started/humanoid_retargeting.rst`)

.. _humanoid_retargeting:

Humanoid Motion Retargeting
===========================

.. automodule:: curobo.examples.getting_started.humanoid_retargeting
   :no-members:

---

### Getting Started Index (`docs/getting-started/index.rst`)

Getting Started
=================

**Learning-oriented lessons** that guide you through cuRobo step-by-step.

.. toctree::
   :maxdepth: 1

   installation
   build_robot_model
   forward_kinematics
   inverse_kinematics
   reactive_control
   humanoid_retargeting
   motion_planning
   volumetric_mapping
   feature_mapping

---

### Getting Started Installation (`docs/getting-started/installation.rst`)

.. _tutorial_installation:

Installation
============

This tutorial walks you through installing cuRobo on Ubuntu 22.04 with PyTorch.
Follow these steps exactly for a guaranteed working installation.

System Requirements
-------------------

Before starting, ensure you have:

1. **Ubuntu>=20.04** (other Linux environments may work)
2. **NVIDIA GPU > Turing** and at least 4 GB VRAM
3. **NVIDIA Driver >= 580.65.06** (driver should support at least CUDA 12)
4. **Python>=3.10** (> 3.13 is not validated)

Installation
------------------

We recommend using `uv <https://docs.astral.sh/uv/getting-started/installation/>`_ for
installation.

Step 1: Clone cuRobo
~~~~~~~~~~~~~~~~~~~~~

.. parsed-literal::

   git clone |repo_url| && cd curobo

Step 2: Create a virtual environment
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

.. code-block:: bash

   uv venv --python 3.11
   source .venv/bin/activate

Step 3: Install cuRobo
~~~~~~~~~~~~~~~~~~~~~~~~

Check your driver's CUDA version with ``nvidia-smi | grep CUDA``, then pick the
matching install command:

**CUDA 13.x:**

.. code-block:: bash

   uv pip install .[cu13-torch]       # fresh install (includes PyTorch)
   uv pip install .[cu13]             # if PyTorch is already installed

**CUDA 12.x:**

.. code-block:: bash

   uv pip install .[cu12-torch]       # fresh install (includes PyTorch)
   uv pip install .[cu12]             # if PyTorch is already installed

Step 4: Test your installation
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

.. code-block:: bash

   python -c "import curobo; print(curobo.__version__)"
   pytest --pyargs curobo.tests

Third-Party Software
--------------------

This project will download and install additional third-party open source software projects.
Review the license terms of these open source projects before use.

---

### Getting Started Inverse Kinematics (`docs/getting-started/inverse_kinematics.rst`)

.. _tutorial_inverse_kinematics:

Inverse Kinematics
==================

.. automodule:: curobo.examples.getting_started.inverse_kinematics
   :no-members:

---

### Getting Started Motion Planning (`docs/getting-started/motion_planning.rst`)

.. _tutorial_motion_planning:

Motion Planning
===============

.. automodule:: curobo.examples.getting_started.motion_planning
   :no-members:

---

### Getting Started Reactive Control (`docs/getting-started/reactive_control.rst`)

.. _tutorial_reactive_control:

Reactive Control
================

.. automodule:: curobo.examples.getting_started.reactive_control
   :no-members:

---

### Getting Started Volumetric Mapping (`docs/getting-started/volumetric_mapping.rst`)

.. _tutorial_volumetric_mapping:

Volumetric Mapping
==================

.. automodule:: curobo.examples.getting_started.volumetric_mapping
   :no-members:

Once you have run the tutorial, open
:mod:`curobo.examples.getting_started.volumetric_mapping` in your editor.
The inline comments walk through the key design decisions: why depth is
filtered before integration, how the static geometry channel differs from
depth-fused surfaces, and why the ESDF is generated at a coarser resolution
than the TSDF.

---

### Guides Custom Cost (`docs/guides/custom_cost.rst`)

.. _tut_custom_cost:

Extending RobotCostManager with a Custom Cost
=============================================

:py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager` ships with the
six cost terms exposed through
:py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot_cfg.RobotCostManagerCfg`
(self-collision, scene-collision, c-space, start/target c-space distance, tool-pose). When you
need a term that is not in that list, the refactor removed the old "subclass the manager"
path -- there is no ``ArmBaseCostManager`` to extend. Instead, the manager is a plain Python
object you can register new costs on **without subclassing**.

This guide shows the full extension flow:

1. Subclass :py:class:`~curobo._src.cost.cost_base.BaseCost` to implement your cost math.
2. Pair it with a :py:class:`~curobo._src.cost.cost_base_cfg.BaseCostCfg` subclass for its
   runtime parameters (weight, any tunables).
3. Register it on a live
   :py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager` via
   :py:meth:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager.register_cost`.
4. Call it from your own ``compute_costs``-like code, or re-run the solver's normal evaluation
   loop and read the cost out of the returned
   :py:class:`~curobo._src.rollout.metrics.CostCollection`.

We'll implement a **joint-velocity energy** cost that penalises large joint velocities along
the rollout horizon.

Prerequisites
-------------

You should be comfortable with the material in
:ref:`tut_motion_optimization`, particularly the "Cost Managers" and "Toggling costs at
runtime" sections. The example below assumes you have already constructed a
:py:class:`~curobo._src.rollout.rollout_robot.RobotRollout` as shown there.

Writing a custom ``BaseCost``
-----------------------------

Every cost inherits a tiny contract from
:py:class:`~curobo._src.cost.cost_base.BaseCost`. The important pieces are:

- ``forward(...)`` -- compute the raw cost tensor of shape
  ``(batch, horizon, n_terms)`` and return it (the ``n_terms`` axis is usually 1).
- ``_weight`` -- a tensor of shape ``(n_terms,)`` the manager multiplies into the cost.
- ``enable_cost`` / ``disable_cost`` -- flip ``_cost_enabled`` on or off so the manager can
  skip this component without removing it.
- ``setup_batch_tensors(batch_size, horizon)`` -- called by the manager when shapes change;
  override if you keep per-batch buffers.
- ``reset(reset_problem_ids=None, **kwargs)`` -- called at the start of each solve.

The cost itself is a ``@dataclass`` extending
:py:class:`~curobo._src.cost.cost_base_cfg.BaseCostCfg`. The base dataclass already holds
``weight``, ``device_cfg``, ``class_type``, ``convert_to_binary``, and ``use_grad_input``, so
you typically only add whatever tunables your cost needs.

.. code-block:: python

   from dataclasses import dataclass
   from typing import Type

   import torch

   from curobo._src.cost.cost_base import BaseCost
   from curobo._src.cost.cost_base_cfg import BaseCostCfg


   @dataclass
   class JointVelocityEnergyCostCfg(BaseCostCfg):
       """Configuration for JointVelocityEnergyCost.

       The ``weight`` field inherited from BaseCostCfg is broadcast against every
       degree of freedom; pass a scalar for uniform weighting or a length-n_dof
       list to weight joints individually.
       """

       #: Optional per-joint scaling that multiplies velocity before squaring.
       joint_scale: float = 1.0

       class_type: Type[BaseCost] = None  # set by JointVelocityEnergyCost below.


   class JointVelocityEnergyCost(BaseCost):
       """Sum-of-squares penalty on joint velocities along the rollout horizon."""

       def __init__(self, config: JointVelocityEnergyCostCfg):
           super().__init__(config)
           self._joint_scale = config.joint_scale

       def forward(self, joint_state) -> torch.Tensor:
           """Compute the velocity-energy cost.

           Args:
               joint_state: JointState with a ``velocity`` tensor of shape
                   ``(batch, horizon, n_dof)``.

           Returns:
               Cost tensor of shape ``(batch, horizon, 1)``.
           """
           velocities = joint_state.velocity * self._joint_scale
           # Sum squared velocities across joints -> per-(batch, horizon) cost.
           per_step_energy = (velocities ** 2).sum(dim=-1, keepdim=True)
           # The manager multiplies by self._weight after the fact; keep the math here
           # weight-free so toggling disable_cost() stays well-defined.
           return per_step_energy


   # Wire the config's class_type to the concrete class (the manager uses
   # ``config.class_type(config)`` to instantiate).
   JointVelocityEnergyCostCfg.class_type = JointVelocityEnergyCost

A couple of design notes:

- Do not apply ``self._weight`` inside ``forward``. The manager multiplies the weight on its
  side; keeping the cost weight-free lets ``disable_cost()`` zero out the weight transparently
  and keeps the cost reusable for other callers that want the raw value (for example
  ``CostCollection.add(unweighted, name, weight=...)``).
- Return shape should be ``(batch, horizon, 1)`` for a scalar cost term. The manager's
  :py:class:`~curobo._src.rollout.metrics.CostCollection` expects the trailing 1-dim so it can
  stack multiple single-scalar costs.
- ``setup_batch_tensors(batch_size, horizon)`` is a no-op for this cost because we don't keep
  per-batch buffers; the base class implementation is fine.

Registering with ``RobotCostManager``
-------------------------------------

Once the cost and its config exist, wire them into a live rollout. The manager is exposed on
the rollout as ``rollout.cost_manager`` (and, for post-optimization metrics, as
``rollout.metrics_cost_manager``).
:py:meth:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager.register_cost`
takes a name and a constructed ``BaseCost`` instance and allocates the cost its own CUDA
stream/event pair so it can run in parallel with the six built-in costs:

.. code-block:: python

   from curobo._src.types.device_cfg import DeviceCfg

   device_cfg = rollout.device_cfg  # already on CUDA

   velocity_cfg = JointVelocityEnergyCostCfg(
       weight=[0.1],             # scalar weight => shape (1,).
       device_cfg=device_cfg,
       joint_scale=1.0,
   )
   velocity_cost = JointVelocityEnergyCost(velocity_cfg)

   # Register on the optimizer manager...
   rollout.cost_manager.register_cost("joint_velocity_energy", velocity_cost)

   # ...and on the metrics manager if you want the cost to show up in
   # compute_metrics_from_action() too.
   metrics_cost = JointVelocityEnergyCost(velocity_cfg)
   rollout.metrics_cost_manager.register_cost("joint_velocity_energy", metrics_cost)

The two-manager setup is deliberate. The optimizer manager (``rollout.cost_manager``) runs on
every optimizer iteration with the optimizer's batch shape; the metrics manager
(``rollout.metrics_cost_manager``) runs once after the solve with the (smaller) evaluation
batch shape. Registering on both keeps the new cost active everywhere; registering on only
``cost_manager`` means the cost drives the optimizer but is not reported by
:py:meth:`~curobo._src.rollout.rollout_robot.RobotRollout.compute_metrics_from_action`.

Feeding state into the cost
---------------------------

The built-in ``RobotCostManager.compute_costs`` knows how to unpack the
:py:class:`~curobo._src.state.state_robot.RobotState` for each registered cost because every
built-in cost has a known signature.  Custom costs need a little more work -- the simplest
approach is to call your cost yourself after ``evaluate_action`` and add the result to the
returned :py:class:`~curobo._src.rollout.metrics.CostCollection`:

.. code-block:: python

   result = rollout.evaluate_action(action_sequence)
   extra = rollout.cost_manager.get_cost("joint_velocity_energy").forward(
       result.state.joint_state
   )
   result.costs_and_constraints.costs.add(
       extra * rollout.cost_manager.get_cost("joint_velocity_energy")._weight,
       "joint_velocity_energy",
   )

If you want the cost to be driven automatically by the optimizer instead, the cleanest path is
to expose it through the same
:py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot_cfg.RobotCostManagerCfg` that
describes the rest of the manager. Define a ``joint_velocity_energy_cfg`` field on your own
subclass of ``RobotCostManagerCfg`` and extend
:py:meth:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager.initialize_from_config`
in a lightweight wrapper class to register the new cost. ``RobotRolloutCfg`` accepts a custom
manager type via its
:py:attr:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg.cost_manager_config_instance_type`
and
:py:attr:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg.transition_model_config_instance_type`
slots, and
:py:meth:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg.create_with_component_types`
takes both as keyword arguments, so the extension is opt-in per rollout. This is more code
than post-hoc ``register_cost`` but keeps the per-iteration call signature inside
``compute_costs``.

Toggling the custom cost at runtime
-----------------------------------

Once registered, the cost uses the exact same lifecycle hooks as the built-in costs:

.. code-block:: python

   # Temporarily disable (e.g., during warm-up).
   rollout.cost_manager.disable_cost_component("joint_velocity_energy")

   # Re-enable later.
   rollout.cost_manager.enable_cost_component("joint_velocity_energy")

   # Inspect which components are live.
   rollout.cost_manager.get_enabled_costs()

Disabling flips ``_cost_enabled`` to False and zeros the stored weight copy, so disabled costs
contribute nothing to
:py:meth:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager.compute_costs`
without having to re-register them when you re-enable.

Conclusion
----------

Custom costs are the one inheritance relationship the refactor preserved, because
:py:class:`~curobo._src.cost.cost_base.BaseCost` is a thin public interface rather than a
framework-y god class. The recipe:

1. Subclass :py:class:`~curobo._src.cost.cost_base.BaseCost`, implement ``forward``, and leave
   weight multiplication to the manager.
2. Pair it with a :py:class:`~curobo._src.cost.cost_base_cfg.BaseCostCfg` subclass for runtime
   parameters.
3. Call
   :py:meth:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager.register_cost`
   on the live manager (and on the metrics manager if you want the cost reported after the
   solve).
4. Toggle with ``enable_cost_component`` / ``disable_cost_component`` exactly as you would for
   a built-in cost.

When the custom cost stabilises and you want it wired through YAML configs the same way the
built-in costs are, promote it to a
:py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot_cfg.RobotCostManagerCfg`
subclass field and plug that subclass into
:py:meth:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg.create_with_component_types`.

See Also
--------

- :ref:`rollout_class_note` -- how the cost manager fits inside the rollout.
- :ref:`tut_motion_optimization` -- the production rollout the manager plugs into.
- :ref:`optimization_solver_note` -- the optimizers that consume the combined cost.

---

### Guides Custom Optimization (`docs/guides/custom_optimization.rst`)

.. _tutorial_03-custom-optimization:

Writing Custom Optimization Problems
====================================

.. automodule:: curobo.examples.guides.custom_optimization
   :members:
   :undoc-members:

Run this example
----------------

.. code-block:: bash

   python -m curobo.examples.guides.custom_optimization

---

### Guides Index (`docs/guides/index.rst`)

Guides
======

**Problem-oriented documentation** for solving specific tasks with cuRobo.
Assumes you know the basics from :doc:`../getting-started/index`.

Writing Optimization Problems
-----------------------------

.. toctree::
   :maxdepth: 1

   optimization_problem
   optimization_motion
   custom_cost
   custom_optimization

---

### Guides Optimization Motion (`docs/guides/optimization_motion.rst`)

.. _tut_motion_optimization:

Writing Motion Optimization Problems
====================================

In the previous tutorial :ref:`tut_user_rollout_optimization`, we implemented a minimal rollout
class for the Rosenbrock function by hand. That was a good introduction to the
:py:class:`~curobo._src.rollout.rollout_protocol.Rollout` Protocol, but realistic motion-planning
problems involve a robot model, dozens of cost terms, self-collision and scene-collision
checks, and a transition model that turns joint actions into joint trajectories.

Rather than asking you to wire all of that up yourself, cuRobo ships a production-quality
rollout -- :py:class:`~curobo._src.rollout.rollout_robot.RobotRollout` -- that handles the full
stack. This tutorial is a guided tour of that rollout: what it composes, how it is configured,
and how to drive it from your own code. By the end you'll know:

1. What :py:class:`~curobo._src.rollout.rollout_robot.RobotRollout` composes and why.
2. How to build a rollout from a robot YAML through the
   :py:func:`~curobo._src.solver.solver_core_cfg.resolve_yaml_configs` /
   :py:func:`~curobo._src.solver.solver_core_cfg.create_solver_core_cfg` factory pipeline.
3. How the lifecycle hooks (goal updates, batching, resets) fit together across a solve.
4. How to toggle individual cost terms at runtime.
5. Where the high-level solvers (IK, trajectory optimization, MPC) sit on top of this rollout.

Prerequisites
-------------

Before proceeding, make sure you understand:

- The :py:class:`~curobo._src.rollout.rollout_protocol.Rollout` Protocol and the Rollout Flow
  diagram from :ref:`rollout_class_note`.
- The solver families described in :ref:`optimization_solver_note` -- in particular that every
  solver composes a rollout list.
- The basics of rollout authoring from :ref:`tut_user_rollout_optimization`.

What ``RobotRollout`` Composes
------------------------------

:py:class:`~curobo._src.rollout.rollout_robot.RobotRollout` is a standalone class (no base
class, no inheritance) that wires together four concerns:

- A :py:class:`~curobo._src.transition.robot_state_transition.RobotStateTransition` model that
  integrates action sequences into joint trajectories.
- One or more
  :py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager` instances
  that evaluate every cost and constraint term used during optimization and metrics
  computation.
- A :py:class:`~curobo._src.geom.collision.collision_scene.SceneCollision` checker for
  world-collision queries
  that is shared across the cost managers.
- A :py:class:`~curobo._src.util.sampling.sample_buffer.SampleBuffer` Halton sampler that
  produces initial action seeds.

All of these are described by a single flat dataclass,
:py:class:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg`. The fields mirror the
components one-to-one:

.. graphviz::
    :caption: RobotRolloutCfg fields and the RobotRollout components they configure

    digraph RobotRolloutComposition {
        edge [color="#2B4162", fontsize=10];
        node [shape="box", style="rounded, filled", fontsize=12, color="#cccccc"];

        subgraph cluster_cfg {
            label="RobotRolloutCfg";
            style="rounded, dashed";
            color="#558c8c";

            transition_model_cfg       [label="transition_model_cfg"];
            cost_cfg                   [label="cost_cfg"];
            constraint_cfg             [label="constraint_cfg"];
            hybrid_cost_constraint_cfg [label="hybrid_cost_constraint_cfg"];
            convergence_cfg            [label="convergence_cfg"];
            scene_collision_cfg        [label="scene_collision_cfg"];
        }

        subgraph cluster_rollout {
            label="RobotRollout";
            style="rounded, dashed";
            color="#558c8c";

            transition         [label="transition_model",         color="#76b900", fontcolor="white"];
            metrics_transition [label="metrics_transition_model", color="#76b900", fontcolor="white"];
            cost_mgr           [label="cost_manager",             color="#76b900", fontcolor="white"];
            constraint_mgr     [label="constraint_manager",       color="#76b900", fontcolor="white"];
            hybrid_mgr         [label="hybrid_cost_constraint_manager", color="#76b900", fontcolor="white"];
            convergence_mgr    [label="metrics_convergence_manager",    color="#76b900", fontcolor="white"];
            collision          [label="scene_collision_checker",  color="#76b900", fontcolor="white"];
        }

        transition_model_cfg       -> transition;
        transition_model_cfg       -> metrics_transition;
        cost_cfg                   -> cost_mgr;
        constraint_cfg             -> constraint_mgr;
        hybrid_cost_constraint_cfg -> hybrid_mgr;
        convergence_cfg            -> convergence_mgr;
        scene_collision_cfg        -> collision;
        collision                  -> cost_mgr            [style="dashed", label="shared"];
        collision                  -> constraint_mgr      [style="dashed", label="shared"];
        collision                  -> hybrid_mgr          [style="dashed", label="shared"];
        collision                  -> convergence_mgr     [style="dashed", label="shared"];
    }

Each cost-manager slot (``cost_cfg``, ``constraint_cfg``,
``hybrid_cost_constraint_cfg``, ``convergence_cfg``) is optional. Setting one to ``None``
means that manager is not constructed. The usual configuration uses
``cost_cfg`` + ``convergence_cfg``; constraint-heavy solvers additionally populate
``constraint_cfg``; and tasks that want different cost weights for costs-vs-constraints put
them in ``hybrid_cost_constraint_cfg``.

The transition model
--------------------

The transition model advances joint state under an action sequence. ``RobotRollout`` keeps two
instances of the same transition model: one for the optimizer's forward pass
(``transition_model``) and one for the post-optimization metrics path
(``metrics_transition_model``). They are constructed from the same
:py:class:`~curobo._src.transition.robot_state_transition_cfg.RobotStateTransitionCfg` but can
hold different batch shapes -- the metrics model typically serves a single evaluation after a
solve, while the optimizer model serves many parallel particles per iteration.

Swapping in a different transition model (for example, a custom dynamics model) is done by
setting
:py:attr:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg.transition_model_config_instance_type`
when constructing ``RobotRolloutCfg`` -- see
:py:meth:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg.create_with_component_types`.

The cost managers
-----------------

Each cost-manager slot contains a
:py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot_cfg.RobotCostManagerCfg`
dataclass. That dataclass has one optional field per cost *kind*:

- ``self_collision_cfg`` -- self-collision on the robot's sphere approximation.
- ``scene_collision_cfg`` -- world collision against the
  :py:class:`~curobo._src.geom.collision.collision_scene.SceneCollision` checker.
- ``cspace_cfg`` -- joint-space regularization (reaching a target configuration).
- ``start_cspace_dist_cfg`` / ``target_cspace_dist_cfg`` -- c-space distance to the start or
  target state.
- ``tool_pose_cfg`` -- task-space pose error at the tool frame.

Setting any of these to ``None`` disables that cost in the manager. On construction each
populated field is realized as a
:py:class:`~curobo._src.cost.cost_base.BaseCost` instance registered on the manager with a
dedicated CUDA stream/event pair (see :ref:`rollout_class_note` for the broader picture).

At evaluation time, a single call to
:py:meth:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager.compute_costs`
runs every enabled component inline (no ``super()`` chain) and returns a
:py:class:`~curobo._src.rollout.metrics.CostCollection`. Individual terms can still be toggled
at runtime via ``enable_cost_component(name)`` / ``disable_cost_component(name)`` -- the
manager remembers the registration but skips disabled components when collecting costs.

Configuring a rollout from YAML
-------------------------------

cuRobo's built-in task configurations live as YAML files under the bundled robot- and
task-config directories (for example ``franka.yml``, ``trajopt.yml``, ``ik.yml``). Rather than
constructing :py:class:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg` by hand, the
usual path is a two-step factory:

.. code-block:: python

   from curobo._src.solver.solver_core_cfg import (
       create_solver_core_cfg,
       resolve_yaml_configs,
   )
   from curobo._src.types.device_cfg import DeviceCfg
   import torch

   device_cfg = DeviceCfg(device=torch.device("cuda:0"), dtype=torch.float32)

   # 1. Resolve YAML paths to plain dicts and a loaded RobotCfg.
   (
       robot_config,
       optimizer_dicts,
       metrics_rollout_dict,
       transition_model_dict,
       scene_model_dict,
   ) = resolve_yaml_configs(
       robot="franka.yml",
       optimizer_configs=["mppi_cfg.yml", "lbfgs_cfg.yml"],
       metrics_rollout="metrics_rollout_cfg.yml",
       transition_model="transition_model_cfg.yml",
       scene_model=None,
       device_cfg=device_cfg,
   )

   # 2. Build a SolverCoreCfg.  This assembles one RobotRolloutCfg per optimizer stage
   #    plus a metrics RobotRolloutCfg, validates the resulting dataclasses, and wraps
   #    the scene-collision config, optimizer configs, and device settings into a single
   #    SolverCoreCfg.
   core_cfg = create_solver_core_cfg(
       robot_config=robot_config,
       optimizer_dicts=optimizer_dicts,
       metrics_rollout_dict=metrics_rollout_dict,
       transition_model_dict=transition_model_dict,
       scene_model_dict=scene_model_dict,
       device_cfg=device_cfg,
       use_cuda_graph=True,
   )

At this point ``core_cfg`` contains:

- ``core_cfg.optimizer_rollout_configs`` -- a list of
  :py:class:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg`, one per optimizer stage.
- ``core_cfg.metrics_rollout_config`` -- a dedicated
  :py:class:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg` for post-optimization
  metric evaluation.
- ``core_cfg.scene_collision_cfg`` -- the world-collision configuration shared across both.
- ``core_cfg.optimizer_configs`` -- the flat optimizer dataclasses (``MPPICfg``,
  ``LBFGSOptCfg``, etc.) for each stage.

If you want an isolated rollout (no solver), you can also skip ``create_solver_core_cfg`` and
use the lower-level factory
:py:meth:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg.create_with_component_types`,
which builds a single :py:class:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg` from a
dict plus an already-loaded :py:class:`~curobo._src.types.robot.RobotCfg`.

Constructing and using the rollout
----------------------------------

Once you have a :py:class:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg` and a
:py:class:`~curobo._src.geom.collision.collision_scene.SceneCollision` checker, constructing
the rollout is a
two-line operation:

.. code-block:: python

   from curobo._src.geom.collision.collision_scene import create_scene_collision
   from curobo._src.rollout.rollout_robot import RobotRollout

   scene_collision_checker = create_scene_collision(core_cfg.scene_collision_cfg)

   # Pick one of the optimizer rollout configs (or the metrics config).
   rollout_cfg = core_cfg.optimizer_rollout_configs[0]
   rollout = RobotRollout(
       config=rollout_cfg,
       scene_collision_checker=scene_collision_checker,
       use_cuda_graph=True,
   )

The rollout now exposes the full
:py:class:`~curobo._src.rollout.rollout_protocol.Rollout` Protocol. You can inspect action
geometry via the standard properties:

.. code-block:: python

   print(rollout.action_dim)         # Number of actuated joints.
   print(rollout.action_horizon)     # Length of an action sequence.
   print(rollout.action_bound_lows)  # Per-joint lower limits, shape (action_dim,).
   print(rollout.action_bound_highs) # Per-joint upper limits.
   print(rollout.dt)                 # Integration timestep in seconds.

and feed it action sequences of shape ``(batch, action_horizon, action_dim)``:

.. code-block:: python

   init_action = rollout.act_sample_gen.get_samples(
       n=batch_size * rollout.action_horizon, bounded=True
   ).view(batch_size, rollout.action_horizon, rollout.action_dim)

   result = rollout.evaluate_action(init_action)
   print(result.costs_and_constraints.get_sum_cost().shape)   # (batch, horizon)
   print(result.state.position.shape)                         # (batch, horizon, n_dof)

``RobotRollout.evaluate_action`` returns a
:py:class:`~curobo._src.rollout.metrics.RolloutResult` with the integrated state, the per-cost
values packed in a :py:class:`~curobo._src.rollout.metrics.CostsAndConstraints`, and the
original action sequence. After a solve finishes, call
:py:meth:`~curobo._src.rollout.rollout_robot.RobotRollout.compute_metrics_from_action` (or
``...from_state``) to get the richer :py:class:`~curobo._src.rollout.metrics.RolloutMetrics`,
which additionally reports feasibility and convergence tolerances.

Lifecycle: goals, batching, and timesteps
-----------------------------------------

Between solves, the rollout's runtime state is updated through four lifecycle hooks on the
Protocol: ``update_params``, ``update_batch_size``, ``update_dt``, and the ``reset*`` family.

Setting a goal
~~~~~~~~~~~~~~

``RobotRollout`` consumes goals through a
:py:class:`~curobo._src.rollout.goal_registry.GoalRegistry`. The registry holds the current
joint state, any target tool poses or joint configurations, and the per-environment index
buffers used for batched multi-environment solves. The solvers build a
:py:class:`~curobo._src.rollout.goal_registry.GoalRegistry` for you; if you are driving the
rollout directly, construct it with the target data your cost manager expects (for example
``tool_pose_cfg`` needs ``goal_tool_poses``, ``cspace_cfg`` needs ``goal_state``):

.. code-block:: python

   from curobo._src.rollout.goal_registry import GoalRegistry

   goal = GoalRegistry(current_js=start_joint_state, goal_tool_poses=target_pose)
   rollout.update_params(goal, num_particles=rollout_cfg_num_particles)

On the first call,
:py:meth:`~curobo._src.rollout.rollout_robot.RobotRollout.update_params` allocates and stores
both a particle-expanded goal (``_num_particles_goal``) and a metrics-sized goal
(``_metrics_goal``). Subsequent calls overwrite tensor *values* in place so the pre-allocated
buffers are reused -- this is what lets CUDA graphs replay correctly across solves.

Changing the batch size and timestep
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

:py:meth:`~curobo._src.rollout.rollout_robot.RobotRollout.update_batch_size` resizes every
internal tensor that depends on ``batch_size * num_particles``. It is typically called by the
optimizer before each ``evaluate_action``; you only need to call it yourself if you are driving
the rollout outside of a solver.

:py:meth:`~curobo._src.rollout.rollout_robot.RobotRollout.update_dt` propagates a new
integration timestep to the transition model. Scalars affect every problem uniformly;
per-problem tensors let different environments march at different rates.

Resets
~~~~~~

Three reset hooks clear progressively more state:

- ``reset(reset_problem_ids=None)`` clears cost-manager per-problem buffers so the next solve
  starts with no residual state.
- ``reset_shape()`` drops any cached goal tensors so the next ``update_params`` reallocates
  buffers. Use this when the batch shape changes in a way that
  :py:meth:`~curobo._src.rollout.rollout_robot.RobotRollout.update_batch_size` can't handle on
  its own.
- ``reset_seed()`` resets the Halton sampler -- useful for reproducible experiments.

Toggling costs at runtime
-------------------------

The easiest way to experiment with which costs are active during a solve is to toggle them on
the live cost manager. Every optimization rollout exposes its managers as public attributes:

.. code-block:: python

   rollout.cost_manager.disable_cost_component("scene_collision")
   rollout.cost_manager.enable_cost_component("scene_collision")

The manager remembers the registration; a disabled component is simply skipped during
:py:meth:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager.compute_costs`.
You can also disable a whole slot at configuration time by setting the corresponding field
(``self_collision_cfg``, ``tool_pose_cfg``, etc.) to ``None`` on the
:py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot_cfg.RobotCostManagerCfg`.

For adding *new* cost types (for example, a custom energy-penalty cost) without subclassing,
see :ref:`tut_custom_cost`.

Using ``RobotRollout`` via the high-level solvers
-------------------------------------------------

In practice, most users do not construct :py:class:`~curobo._src.rollout.rollout_robot.RobotRollout`
directly. cuRobo provides three public solvers that build
:py:class:`~curobo._src.solver.solver_core.SolverCore` -- and therefore the rollouts behind
it -- from a robot YAML in a few lines:

.. code-block:: python

   from curobo import (
       InverseKinematics, InverseKinematicsCfg,
       TrajectoryOptimizer, TrajectoryOptimizerCfg,
       ModelPredictiveControl, ModelPredictiveControlCfg,
   )

   # Inverse kinematics
   ik = InverseKinematics(InverseKinematicsCfg.create(robot="franka.yml"))
   ik_result = ik.solve_pose(goal_tool_poses=target_poses)

   # Trajectory optimization
   trajopt = TrajectoryOptimizer(TrajectoryOptimizerCfg.create(robot="franka.yml"))
   traj_result = trajopt.solve_pose(goal_tool_poses=target_poses)

   # Model predictive control
   mpc = ModelPredictiveControl(ModelPredictiveControlCfg.create(robot="franka.yml"))
   next_action = mpc.optimize_action_sequence(current_state)

Each of these wraps the same
:py:func:`~curobo._src.solver.solver_core_cfg.resolve_yaml_configs` /
:py:func:`~curobo._src.solver.solver_core_cfg.create_solver_core_cfg` pipeline used above, and
each exposes the underlying rollouts through ``solver.core.optimizer_rollouts`` and
``solver.core.metrics_rollout`` if you need to introspect or modify them between solves. Use
the direct ``RobotRollout`` path only when you need finer-grained control than the high-level
solvers expose.

Conclusion
----------

In this tutorial we:

1. Mapped the fields of :py:class:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg` to
   the components of :py:class:`~curobo._src.rollout.rollout_robot.RobotRollout` -- a
   transition model, a set of
   :py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager` instances,
   and a shared
   :py:class:`~curobo._src.geom.collision.collision_scene.SceneCollision` checker.
2. Walked through the two-step YAML-to-``SolverCoreCfg`` factory pipeline
   (:py:func:`~curobo._src.solver.solver_core_cfg.resolve_yaml_configs` then
   :py:func:`~curobo._src.solver.solver_core_cfg.create_solver_core_cfg`), and the alternative
   :py:meth:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg.create_with_component_types`
   factory for single-rollout use cases.
3. Constructed a :py:class:`~curobo._src.rollout.rollout_robot.RobotRollout`, evaluated an
   action sequence, and read the resulting
   :py:class:`~curobo._src.rollout.metrics.RolloutResult` and
   :py:class:`~curobo._src.rollout.metrics.RolloutMetrics`.
4. Walked through the ``update_params`` / ``update_batch_size`` / ``update_dt`` / ``reset*``
   lifecycle and how the cost manager lets you toggle components at runtime.

For more information on specific components, refer to the API documentation:

- :py:class:`~curobo._src.rollout.rollout_robot.RobotRollout`
- :py:class:`~curobo._src.rollout.rollout_robot_cfg.RobotRolloutCfg`
- :py:class:`~curobo._src.transition.robot_state_transition.RobotStateTransition`
- :py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager`
- :py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot_cfg.RobotCostManagerCfg`
- :py:class:`~curobo._src.solver.solver_core_cfg.SolverCoreCfg`

Next Steps
----------

- :ref:`tut_custom_cost` -- add a new cost term to
  :py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager` without
  subclassing.
- :doc:`/guides/optimization_problem` -- revisit the Rosenbrock example to see how the Protocol
  fits a minimal, textbook problem.

---

### Guides Optimization Problem (`docs/guides/optimization_problem.rst`)

.. _tut_user_rollout_optimization:

Writing Optimization Problems
==============================
This tutorial will guide you through optimizing the Rosenbrock function using a custom rollout
class and plugging it into cuRobo's optimization pipeline.

cuRobo's optimization pipeline is designed to be modular and extensible. The optimization pipeline
consists of a rollout class and an optimization solver. The rollout class contains information
about the optimization problem, including evaluation of costs and constraints given the optimization
variables. The optimization solver is responsible for finding the optimal values by using the
rollout class for evaluations.

Required Reading
-----------------
- :ref:`rollout_class_note`
- :ref:`optimization_solver_note`






Rosenbrock Function
--------------------

The Rosenbrock function, also known as the banana function or Valley function, is a common test
problem for optimization algorithms. It is defined as:

.. math::

   f(x,y) = (1-x)^2 + 100(y-x^2)^2

The function has a global minimum at (1,1) where f(1,1) = 0. The function's name comes from its
characteristic shape - a parabolic valley. While finding the valley is trivial, converging to the
global minimum is difficult. The valley is very narrow and curved, making it a challenging
optimization problem that tests an algorithm's ability to navigate a narrow curved valley.


.. figure:: ../images/rosenbrock_function.svg
   :width: 690
   :align: center
   :alt: Rosenbrock function


We want to go to the minimum from a random initial point.

.. figure:: ../images/rosenbrock_optimization.svg
   :width: 690
   :align: center
   :alt: Rosenbrock optimization

Optimization solvers can get stuck in local minima. To overcome this, some solvers
randomly perturb when stuck. In cuRobo, we instead parallelize the optimization across multiple
starting points (seeds) and leverage GPU compute to find the best solution.


We will next implement a custom rollout class that can then be plugged into cuRobo's
optimization pipeline. If you are unfamiliar with cuRobo's Rollout class, see
:ref:`rollout_class_note` before continuing.



Implementing a Custom Rollout Class
-----------------------------------

In this section, we'll implement a custom rollout class for the Rosenbrock function. cuRobo
rollouts are defined **structurally** through the
:py:class:`~curobo._src.rollout.rollout_protocol.Rollout` :class:`~typing.Protocol` -- a rollout
is any class that exposes the right set of properties and methods. There is no base class to
inherit from. See :ref:`rollout_class_note` for the full Protocol.

The implementation has three parts:

1. **Configuration** -- a plain ``@dataclass`` holding parameters and dimensions. No
   ``BaseRolloutCfg`` parent; just a regular dataclass.
2. **Implementing the rollout class** -- a standalone class whose public surface matches the
   :py:class:`~curobo._src.rollout.rollout_protocol.Rollout` Protocol.
3. **Computing costs and constraints** -- implementing the Rosenbrock function inside the
   rollout's forward pass.

The full reference implementation lives at
:py:class:`~curobo._src.rollout.rollout_rosenbrock.RosenbrockRollout`; the snippets below mirror
it with short comments.

Let's start with the configuration dataclass:

.. code-block:: python

   from dataclasses import dataclass
   from typing import Dict, List, Optional

   import torch

   from curobo._src.rollout.metrics import (
       CostCollection,
       CostsAndConstraints,
       RolloutMetrics,
       RolloutResult,
   )
   from curobo._src.rollout.rollout_protocol import Rollout
   from curobo._src.state.state_joint import JointState
   from curobo._src.types.device_cfg import DeviceCfg


   @dataclass
   class RosenbrockCfg:
       """Configuration for the Rosenbrock rollout."""

       device_cfg: DeviceCfg
       a: float = 1.0
       b: float = 100.0
       dimensions: int = 2
       time_horizon: int = 1
       time_action_horizon: int = 1
       sum_horizon: bool = False
       sampler_seed: int = 1312

The dataclass holds the Rosenbrock parameters ``a`` and ``b``, the problem dimensionality, and
the usual cuRobo rollout fields (``device_cfg``, horizon length, ``sum_horizon``,
``sampler_seed``). Next, the rollout class itself. ``use_cuda_graph`` is a constructor
parameter -- there is no mixin or subclass needed for CUDA-graph acceleration:

.. code-block:: python

   class RosenbrockRollout:
       """Rollout that evaluates the Rosenbrock cost for optimizer testing.

       f(x, y) = (a - x)^2 + b(y - x^2)^2

       For higher dimensions, uses the generalized form:
       f(x) = sum_{i=1}^{n-1} [ (a - x_i)^2 + b(x_{i+1} - x_i^2)^2 ]
       """

       def __init__(self, config: RosenbrockCfg, use_cuda_graph: bool = False):
           self.a = config.a
           self.b = config.b
           self.dimensions = config.dimensions
           self.time_horizon = config.time_horizon
           self.time_action_horizon = config.time_action_horizon
           self.device_cfg = config.device_cfg
           self.sum_horizon = config.sum_horizon
           self.sampler_seed = config.sampler_seed

           self._use_cuda_graph = use_cuda_graph
           self._action_bound_lows = (
               torch.ones((self.action_horizon,), **self.device_cfg.as_torch_dict()) * -1.5
           )
           self._action_bound_highs = (
               torch.ones((self.action_horizon,), **self.device_cfg.as_torch_dict()) * 2.0
           )
           self._batch_size = 1

Next, the properties the :py:class:`~curobo._src.rollout.rollout_protocol.Rollout` Protocol
requires (``action_dim``, ``action_horizon``, ``action_bound_lows`` / ``action_bound_highs``,
``dt``, ``sum_horizon``):

.. code-block:: python

       @property
       def action_dim(self) -> int:
           return self.dimensions

       @property
       def action_horizon(self) -> int:
           return self.time_action_horizon

       @property
       def action_bound_lows(self) -> torch.Tensor:
           return self._action_bound_lows

       @property
       def action_bound_highs(self) -> torch.Tensor:
           return self._action_bound_highs

       @property
       def dt(self) -> float:
           return 1.0

The Rosenbrock rollout is time-independent, so ``dt`` is just ``1.0``. ``sum_horizon`` is set
from the config in ``__init__`` above.

The forward pass evaluates the Rosenbrock cost. ``evaluate_action`` is the hot loop used by the
optimizer on every iteration. The helper ``_compute_state_from_action_impl`` maps actions to
a ``JointState`` (for Rosenbrock the state is just the action), and
``_compute_costs_and_constraints_impl`` evaluates the generalised Rosenbrock sum:

.. code-block:: python

       def evaluate_action(self, act_seq: torch.Tensor, **kwargs) -> RolloutResult:
           batch_size = act_seq.shape[0]
           if batch_size != self._batch_size:
               self.update_batch_size(batch_size)
           state = self._compute_state_from_action_impl(act_seq)
           costs = self._compute_costs_and_constraints_impl(state, **kwargs)
           return RolloutResult(actions=act_seq, state=state, costs_and_constraints=costs)

       def _compute_state_from_action_impl(self, act_seq: torch.Tensor) -> JointState:
           # For Rosenbrock, the state is just the action.
           return JointState.from_position(act_seq)

       def _compute_costs_and_constraints_impl(
           self, state: JointState, **kwargs
       ) -> CostsAndConstraints:
           x = state.position  # shape: [batch, horizon, dimensions]
           costs_and_constraints = CostsAndConstraints()

           # 2D Rosenbrock term.
           term1 = (self.a - x[:, :, 0]) ** 2
           term2 = self.b * (x[:, :, 1] - x[:, :, 0] ** 2) ** 2
           rosenbrock_cost = term1 + term2

           # Higher-dimensional terms.
           for i in range(1, self.action_dim - 1):
               term1 = (self.a - x[:, :, i]) ** 2
               term2 = self.b * (x[:, :, i + 1] - x[:, :, i] ** 2) ** 2
               rosenbrock_cost += term1 + term2

           # CostCollection uses add(value, name); it is not a list of tensors.
           costs_and_constraints.costs.add(rosenbrock_cost.unsqueeze(-1), "rosenbrock")
           return costs_and_constraints

The Protocol also requires post-optimization metrics hooks and a handful of lifecycle methods.
For the Rosenbrock rollout these are short:

.. code-block:: python

       def compute_metrics_from_state(self, state: JointState, **kwargs) -> RolloutMetrics:
           costs = self._compute_costs_and_constraints_impl(state)
           convergence = costs.get_sum_cost(sum_horizon=False)
           return RolloutMetrics(
               costs_and_constraints=costs,
               feasible=costs.get_feasible(),
               state=state,
               convergence=convergence,
           )

       def compute_metrics_from_action(
           self, act_seq: torch.Tensor, **kwargs
       ) -> RolloutMetrics:
           self.update_batch_size(act_seq.shape[0])
           state = self._compute_state_from_action_impl(act_seq)
           metrics = self.compute_metrics_from_state(state, **kwargs)
           metrics.actions = act_seq
           return metrics

       def update_params(self, a: float = None, b: float = None, **kwargs) -> bool:
           if a is not None:
               self.a = a
           if b is not None:
               self.b = b
           return True

       def update_batch_size(self, batch_size: int) -> None:
           self._batch_size = batch_size

       def update_dt(self, dt, **kwargs) -> bool:
           return True  # dt is constant for Rosenbrock.

       def reset(self, reset_problem_ids=None, **kwargs) -> bool:
           return True

       def reset_shape(self) -> bool:
           return True

       def reset_seed(self) -> None:
           pass  # Stateless sampler; nothing to reset.

That's the entire class. ``RosenbrockRollout`` is a standalone class -- no base class, no
mixins, no ``super()`` -- and it satisfies the
:py:class:`~curobo._src.rollout.rollout_protocol.Rollout` Protocol, so it can be handed to any
cuRobo optimizer. Setting ``use_cuda_graph=True`` at construction time is the only switch
needed to record and replay the metrics hooks through a
:py:class:`~curobo._src.util.cuda_graph_util.GraphExecutor`.

Using the Rosenbrock Rollout in an Optimization Pipeline
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Now that we have a rollout, we can hand it to any of cuRobo's optimization solvers. Read
:ref:`optimization_solver_note` for an overview of the available solvers.

The end-to-end runnable script lives at
`curobo/examples/guides/custom_optimization.py <https://github.com/cuRobo/cuRobo/blob/main/curobo/examples/guides/custom_optimization.py>`_;
this section walks through its key pieces.

**Rollout list idiom.** Every optimizer accepts a ``rollout_list: List[Rollout]`` whose length
matches the optimizer's ``num_rollout_instances`` field, so that a single optimizer can drive
multiple rollouts (e.g. one per line-search branch). Most solvers take a single rollout; L-BFGS
takes two. A small helper keeps the call sites short:

.. code-block:: python

   import torch

   from curobo._src.optim.external.scipy_opt import ScipyOpt, ScipyOptCfg
   from curobo._src.optim.gradient.lbfgs import LBFGSOpt, LBFGSOptCfg
   from curobo._src.optim.multi_stage_optimizer import MultiStageOptimizer
   from curobo._src.optim.particle.mppi import MPPI, MPPICfg
   from curobo._src.types.device_cfg import DeviceCfg

   device_cfg = DeviceCfg(device=torch.device("cuda:0"), dtype=torch.float32)

   rosenbrock_config = RosenbrockCfg(
       a=1.0,
       b=100.0,
       dimensions=2,
       time_horizon=1,
       time_action_horizon=1,
       device_cfg=device_cfg,
   )

   def build_rollout_list(num_instances: int):
       return [
           RosenbrockRollout(rosenbrock_config, use_cuda_graph=True)
           for _ in range(num_instances)
       ]

**MPPI.** The particle-based MPPI solver samples actions from a Gaussian and updates the
distribution from the cost-weighted statistics:

.. code-block:: python

   mppi_config_dict = {
       "num_iters": 10,
       "inner_iters": 5,
       "gamma": 1.0,
       "seed": 0,
       "store_rollouts": False,
       "num_particles": 1000,
       "solver_type": "mppi",
       "store_debug": True,
       "sample_mode": "BEST",
       "init_cov": 0.01,
       "kappa": 0.0001,
       "beta": 0.1,
       "step_size_mean": 0.9,
       "step_size_cov": 0.1,
       "null_act_frac": 0.0,
       "squash_fn": "CLAMP",
       "cov_type": "DIAG_A",
   }
   mppi_config = MPPICfg(**MPPICfg.create_data_dict(mppi_config_dict, device_cfg))
   mppi_rollouts = build_rollout_list(mppi_config.num_rollout_instances)
   mppi = MPPI(mppi_config, mppi_rollouts)

``MPPICfg.create_data_dict`` drops any keys that are not valid
:py:class:`~curobo._src.optim.particle.mppi.MPPICfg` fields, so the dataclass constructor only
sees valid fields.

**L-BFGS.** The gradient-based L-BFGS solver takes a parallel line search step per iteration;
its default config has ``num_rollout_instances = 2``:

.. code-block:: python

   opt_config_dict = {
       "num_iters": 50,
       "line_search_scale": [0.0, 0.01, 0.2, 0.3, 0.5, 0.75, 0.8, 0.9, 1.0, 2.0],
       "cost_convergence": 0.0,
       "cost_delta_threshold": 0.0,
       "fixed_iters": True,
       "store_debug": True,
       "history": 2,
       "epsilon": 0.001,
       "sync_cuda_time": True,
       "debug_info": None,
       "num_problems": 1,
       "use_coo_sparse": True,
       "solver_type": "lbfgs",
       "inner_iters": 1,
       "line_search_type": "wolfe",
       "line_search_wolfe_c_1": 0.001,
       "line_search_wolfe_c_2": 0.98,
       "step_scale": 1.0,
       "cost_relative_threshold": 0.999,
   }
   opt_config = LBFGSOptCfg(**LBFGSOptCfg.create_data_dict(opt_config_dict, device_cfg))
   lbfgs_rollouts = build_rollout_list(opt_config.num_rollout_instances)
   lbfgs_opt = LBFGSOpt(opt_config, lbfgs_rollouts, use_cuda_graph=True)

``use_cuda_graph=True`` on the optimizer is an independent switch from the one on the rollout:
the rollout wraps its metrics hooks, and the optimizer wraps the inner ``evaluate_action``
loop.

**Chaining with MultiStageOptimizer.** To chain MPPI's global exploration with L-BFGS's local
refinement, wrap them in a
:py:class:`~curobo._src.optim.multi_stage_optimizer.MultiStageOptimizer`. The multi-stage
wrapper itself satisfies the
:py:class:`~curobo._src.optim.optimizer_protocol.Optimizer` Protocol, so everything that
consumes an optimizer keeps working:

.. code-block:: python

   optimizer = MultiStageOptimizer(optimizers=[mppi, lbfgs_opt])

**Running the optimizer.** Pick a primary rollout for post-optimization analysis, set the batch
size, get an initial action from the rollout's Halton sampler, and run:

.. code-block:: python

   rosenbrock_rollout = lbfgs_rollouts[0]
   batch_size = 1
   rosenbrock_rollout.batch_size = batch_size
   optimizer.update_num_problems(batch_size)

   init_action = rosenbrock_rollout.get_initial_action()

   for _ in range(2):  # Second run benefits from warmed-up CUDA graphs.
       optimizer.reinitialize(init_action)
       result = optimizer.optimize(init_action)

   optimized_action = result
   metrics = rosenbrock_rollout.compute_metrics_from_action(optimized_action)
   print("Optimized action:", optimized_action.cpu().numpy())
   print("Convergence cost:", metrics.convergence.cpu().numpy())
   print("True minimum: [1.0, 1.0]")

Note that :py:meth:`~curobo._src.optim.multi_stage_optimizer.MultiStageOptimizer.reinitialize`
is the post-refactor replacement for the old ``reset()`` method -- it resets solver state using
the seed action.

**Recording and plotting the trace.** When ``store_debug=True`` is set in the optimizer config,
``optimizer.get_recorded_trace()["debug"]`` returns a list of per-iteration action tensors,
which you can plot over the Rosenbrock contour. See
``visualize_batch_rosenbrock`` in
`custom_optimization.py <https://github.com/cuRobo/cuRobo/blob/main/curobo/examples/guides/custom_optimization.py>`_
for a complete matplotlib-based visualization.

.. _tut_rosen_rollout_optimization_cuda_graphs:

Accelerating with CUDA Graphs
-----------------------------

`CUDA graphs <https://developer.nvidia.com/blog/cuda-graphs/>`_ reduce GPU kernel-launch
overhead by recording a sequence of kernels once and replaying them on every subsequent call.
In cuRobo, CUDA graphs are a **constructor parameter** -- there is no mixin or subclass to
inherit from:

.. code-block:: python

   # Rollout: wraps compute_metrics_from_state / compute_metrics_from_action.
   rollout = RosenbrockRollout(rosenbrock_config, use_cuda_graph=True)

   # Optimizer: wraps the inner evaluate_action loop of the solver.
   lbfgs_opt = LBFGSOpt(opt_config, [rollout, rollout], use_cuda_graph=True)

The two switches are independent -- the rollout captures one graph for each of its
post-optimization metrics hooks, and the optimizer captures its own graph for the hot loop.
Graphs are recorded lazily on the first call and replayed from then on, via
:py:class:`~curobo._src.util.cuda_graph_util.GraphExecutor`. When either switch is left at
``False``, ``GraphExecutor`` transparently falls back to a direct function call, so the same
rollout and optimizer classes run with or without CUDA graphs.

For the Rosenbrock rollout the ``_compute_*_metrics_impl`` helpers happen to match their
``_compute_*_impl`` counterparts (the graph-captured batch shapes don't differ from the
optimizer's batch shapes), so no metrics overrides are needed; more complex rollouts can
override them separately if the post-optimization batch size differs.

Conclusion
----------

In this tutorial we:

1. Defined a plain ``@dataclass`` configuration for the Rosenbrock parameters.
2. Wrote :py:class:`~curobo._src.rollout.rollout_rosenbrock.RosenbrockRollout` as a standalone
   class that satisfies the :py:class:`~curobo._src.rollout.rollout_protocol.Rollout` Protocol
   -- no inheritance, no base class.
3. Drove it with MPPI, L-BFGS, and
   :py:class:`~curobo._src.optim.multi_stage_optimizer.MultiStageOptimizer`, with CUDA-graph
   acceleration enabled via a constructor flag.

The key steps for your own optimization problems are:

1. Define a plain ``@dataclass`` for your rollout parameters, including a ``device_cfg`` field.
2. Implement the methods required by the
   :py:class:`~curobo._src.rollout.rollout_protocol.Rollout` Protocol (``evaluate_action``,
   ``compute_metrics_from_state`` / ``_from_action``, the ``update_*`` / ``reset_*`` lifecycle
   hooks, and the action-bound / dimensionality properties).
3. Put your cost function in ``_compute_costs_and_constraints_impl`` using
   :py:meth:`~curobo._src.rollout.metrics.CostCollection.add` on the returned
   :py:class:`~curobo._src.rollout.metrics.CostsAndConstraints`.
4. Optionally pass ``use_cuda_graph=True`` at construction time for CUDA-graph acceleration.

For rollouts with many cost and constraint terms, writing the math inline in
``_compute_costs_and_constraints_impl`` stops scaling. cuRobo's production rollout,
:py:class:`~curobo._src.rollout.rollout_robot.RobotRollout`, delegates costs to a single flat
:py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot.RobotCostManager` whose
config-driven :py:class:`~curobo._src.rollout.cost_manager.cost_manager_robot_cfg.RobotCostManagerCfg`
has one optional field per cost kind (self-collision, scene collision, c-space, tool pose,
etc.). See :ref:`tut_motion_optimization` for the full walkthrough of that setup.

---

### Reference Benchmarks (`docs/reference/benchmarks.rst`)

Benchmarks & Profiling
=======================

Latest Motion Generation Results
*********************************

Motion Generation on 2600 problems from motion benchmaker and motion policy networks, on a
RTX 6000 Ada:

+---------------------+---------------------------------------------------------------------+
| Metric              | Value                                                               |
+=====================+=====================================================================+
| Success %           | 99.73                                                               |
+---------------------+---------------------------------------------------------------------+
| Plan Time (s)       | mean: 0.038 ± 0.014  median: 0.035  75%: 0.038  98%: 0.081          |
+---------------------+---------------------------------------------------------------------+
| Solve Time (s)      | mean: 0.031 ± 0.013  median: 0.029  75%: 0.031  98%: 0.064          |
+---------------------+---------------------------------------------------------------------+
| Position Error (mm) | mean: 0.041 ± 0.325  median: 0.000  75%: 0.000  98%: 0.373          |
+---------------------+---------------------------------------------------------------------+
| Path Length (rad.)  | mean: 3.126 ± 1.050  median: 3.249  75%: 3.820  98%: 5.078          |
+---------------------+---------------------------------------------------------------------+
| Motion Time(s)      | mean: 1.250 ± 0.357  median: 1.243  75%: 1.486  98%: 2.128          |
+---------------------+---------------------------------------------------------------------+
| Jerk                | mean: 227.365 ± 83.736  median: 213.841  75%: 267.641  98%: 465.294 |
+---------------------+---------------------------------------------------------------------+
| Energy (J)          | mean: 89.270 ± 49.704  median: 78.959  75%: 117.086  98%: 203.041   |
+---------------------+---------------------------------------------------------------------+
| Torque (N·m)        | mean: 71.028 ± 25.216  median: 67.328  75%: 86.430  98%: 132.991    |
+---------------------+---------------------------------------------------------------------+

With torque limits at full payload of 3kg:


+---------------------+---------------------------------------------------------------------+
| Metric              | Value                                                               |
+=====================+=====================================================================+
| Success %           | 99.73                                                               |
+---------------------+---------------------------------------------------------------------+
| Plan Time (s)       | mean: 0.052 ± 0.030  median: 0.043  75%: 0.053  98%: 0.135          |
+---------------------+---------------------------------------------------------------------+
| Solve Time (s)      | mean: 0.042 ± 0.025  median: 0.036  75%: 0.045  98%: 0.113          |
+---------------------+---------------------------------------------------------------------+
| Position Error (mm) | mean: 0.042 ± 0.327  median: 0.000  75%: 0.000  98%: 0.329          |
+---------------------+---------------------------------------------------------------------+
| Path Length (rad.)  | mean: 3.234 ± 1.136  median: 3.317  75%: 3.916  98%: 5.698          |
+---------------------+---------------------------------------------------------------------+
| Motion Time(s)      | mean: 1.336 ± 0.475  median: 1.304  75%: 1.540  98%: 2.738          |
+---------------------+---------------------------------------------------------------------+
| Jerk                | mean: 217.786 ± 82.211  median: 204.219  75%: 254.104  98%: 445.629 |
+---------------------+---------------------------------------------------------------------+
| Energy (J)          | mean: 81.409 ± 42.400  median: 72.707  75%: 105.417  98%: 178.045   |
+---------------------+---------------------------------------------------------------------+
| Torque (N·m)        | mean: 62.270 ± 13.705  median: 65.345  75%: 73.378  98%: 82.450     |
+---------------------+---------------------------------------------------------------------+



Latest Inverse Kinematics Results
*********************************

Reported errors are 90th percentile. You can run this with ``python benchmark/ik_benchmark.py``.

+----+----------------+---------------+--------------+--------------+----------------------+--------------------------+----------------------+---------------------+--------------------------------+------------------------------------+
|    | robot          |   IK-time(ms) |   Batch-Size |   Success-IK |   Position-Error(mm) |   Orientation-Error(deg) |   C-Free-IK-time(ms) |   Success-C-Free-IK |   Position-Error-C-Free-IK(mm) |   Orientation-Error-C-Free-IK(deg) |
+====+================+===============+==============+==============+======================+==========================+======================+=====================+================================+====================================+
|  0 | unitree_g1.yml |      31.3916  |          100 |          100 |          0.000982541 |              5.60822e-05 |            526.871   |                98.4 |                    0.00795739  |                        0.000371568 |
+----+----------------+---------------+--------------+--------------+----------------------+--------------------------+----------------------+---------------------+--------------------------------+------------------------------------+
|  1 | dual_ur10e.yml |       6.05801 |          100 |          100 |          0.000191174 |              1.12155e-05 |             15.6443  |                99.2 |                    0.000170931 |                        1.03467e-05 |
+----+----------------+---------------+--------------+--------------+----------------------+--------------------------+----------------------+---------------------+--------------------------------+------------------------------------+
|  2 | franka.yml     |       2.60138 |          100 |          100 |          0.000127351 |              8.30968e-06 |              2.72574 |               100   |                    0.000130251 |                        8.50011e-06 |
+----+----------------+---------------+--------------+--------------+----------------------+--------------------------+----------------------+---------------------+--------------------------------+------------------------------------+

Running Benchmarks
********************

We use robometrics to run some of the benchmarks and also provide csv export with pandas.
Install the ``benchmark`` extra to pull in all required dependencies
(``tabulate``, ``pandas``, ``robometrics``, ``pin``, ``seaborn``):

.. code-block:: bash

    pip install -e ".[benchmark]"
    # or, with uv:
    uv sync --extra benchmark

USD output (``--save_usd`` for ``motion_plan_benchmark.py``) additionally requires
``usd-core``. Install both extras together when you need to write ``.usd`` files:

.. code-block:: bash

    pip install -e ".[benchmark,usd]"
    # or, with uv:
    uv sync --extra benchmark --extra usd

.. note::

    Do not install ``usd-core`` if you are running cuRobo inside Isaac Sim;
    Isaac Sim provides its own USD runtime.


Kinematics & Collision Checking
********************************
To measure compute time and peak memory for forward kinematics, pose-cost
gradients, self-collision, and world-collision (with and without CUDA graphs)
across all supported robots, run:

``python benchmark/cost_gradient_benchmark.py``

Results are written to ``benchmark/log/`` as four YAML files (one per stage),
with times reported in milliseconds and memory in MiB.

Inverse Kinematics
********************************

To measure success metrics and compute time for inverse kinematics and collision-free inverse kinematics, run

``python benchmark/ik_benchmark.py --save_path=. --file_name=ik``

This will save the results to ``ik.yml``.

Motion Planning
***************************************

To run benchmarks for motion planning:

``python benchmark/motion_plan_benchmark.py``, optionally with ``--use-dynamics`` to
enable torque-limited motion planning.

---

### Reference Index (`docs/reference/index.rst`)

Reference
=========

**Information-oriented documentation** providing technical specifications,
API documentation, and factual information about cuRobo.


Development
-----------

Complete API reference auto-generated from source code.

.. toctree::
   :maxdepth: 1

   api_overview
   styleguide
   runtime_configuration



Performance & Benchmarks
------------------------

.. toctree::
   :maxdepth: 1

   benchmarks

Reference Examples
------------------

.. toctree::
   :maxdepth: 1

   lidar_volumetric_mapping
   live_volumetric_mapping_mpc

Implementation Details
----------------------

.. toctree::
   :maxdepth: 1

   sphere_fitting
   self_collision

---

### Reference Lidar Volumetric Mapping (`docs/reference/lidar_volumetric_mapping.rst`)

.. _reference_lidar_volumetric_mapping:

LiDAR Volumetric Mapping
========================

.. automodule:: curobo.examples.reference.lidar_volumetric_mapping
   :no-members:
   :no-index:

---

### Reference Live Volumetric Mapping Mpc (`docs/reference/live_volumetric_mapping_mpc.rst`)

.. _reference_live_volumetric_mapping_mpc:

Live Volumetric Mapping MPC
===========================

.. automodule:: curobo.examples.reference.live_volumetric_mapping_mpc
   :no-members:
   :no-index:

---

### Reference Runtime Configuration (`docs/reference/runtime_configuration.rst`)

.. _runtime_configuration:

Runtime Configuration
=====================

.. automodule:: curobo._src.runtime
   :members:
   :undoc-members:

---

### Reference Self Collision (`docs/reference/self_collision.rst`)

Robot Self-Collision
====================================

Self-collision avoidance is a required behavior for robot motion generation
(IK, MPC, motion planning). While single end-effector manipulators are usually designed to have
limited self-collisions by limiting joint limits or with specific geometric shapes, multi-arm and
humanoid robots will have a higher likelihood of self-collisions. It is therefore required to incorporate
self-collision avoidance within the optimization process during IK, MPC, and motion planning.

cuRobo represents the robot with spheres. This leads to a larger number of
self-collision checks than if the robot was represented with meshes, as each mesh is represented with
many spheres (20+). During self-collision checks, the distance needs to be computed between every
sphere of each link to every sphere of other links. However, sphere-sphere collision checks are
much cheaper than mesh-mesh collision checks, and we compute them in parallel leveraging
the many cores of a GPU.

When calculating self-collisions, we also need to consider two cases:

1. Some link pairs will always be in collision and are safe to be in collision. E.g., two links attached with a joint (most consecutive links).

2. Some link pairs will never be in collision due to the mechanical design of the robot. We can skip calculating collisions for these pairs.

cuRobo uses a configuration file to specify which link pairs can be ignored for self-collision checks.

1. ``self_collision_ignore``: A dictionary that maps each link to the other links to skip during self-collision checks.


cuRobo uses a single max-reduction kernel for self-collision cost. It returns
the largest penetration distance across all enabled sphere pairs; this is the
scalar minimized during IK, MPC, and motion planning.

The same kernel can optionally write every pair's signed distance to a per-pair
buffer by setting ``store_pair_distance=True`` on
:class:`~curobo._src.cost.cost_self_collision_cfg.SelfCollisionCostCfg`. This is
slower (extra global-memory writes and a larger output buffer) but useful for:

- **Debugging misconfigurations**, by inspecting which specific sphere pairs
  are in collision for a given joint configuration.
- **Generating the** ``self_collision_ignore`` **dictionary automatically**, by
  sampling collision-free joint configurations and recording pairs that are
  never in collision across the sample.

The self-collision parameters (collision pair indices, sphere padding, block
partitioning) are generated and stored in
:class:`~curobo.robot.SelfCollisionKinematicsCfg`.

cuRobo precomputes the index pairs of spheres to check for self-collisions, taking into account the ``self_collision_ignore`` dictionary. This also gives us the total number of sphere pairs to check. We also allow for padding each link's spheres with a radius buffer as we want to be
conservative in our self-collision checks. This is stored per sphere as the self-collision method has no knowledge of which sphere maps to which link.

For larger robots (e.g., bimanual or humanoid, ~1000 spheres), cuRobo splits the
reduction across multiple thread blocks using a two-kernel design: the first
kernel computes a per-block max, and a second kernel performs a warp-level
reduction over the per-block results. Implementation details live in
``curobo/_src/curobolib/kernels/geometry/self_collision/``.


Calculating Self-Collision Skips
--------------------------------

Given robot spheres, cuRobo adds a small padding (``self_collision_buffer``, 2 cm
by default) to each sphere. For a joint configuration that is known to be
collision-free, the max-reduction kernel (run with ``store_pair_distance=True``)
returns every pair's signed distance; any pair reported as in collision is added
to ``self_collision_ignore``.

To skip pairs that are never in collision, cuRobo samples ``n_samples`` joint
configurations uniformly within joint limits and records per-pair distances
across the sample. Pairs that are never in collision across all samples are
added to ``self_collision_ignore``.

---

### Reference Sphere Fitting (`docs/reference/sphere_fitting.rst`)

.. _sphere_fitting_note:

Fitting Spheres to Geometry
==================================

cuRobo represents robots and grasped objects as sets of spheres for collision checking.
This page describes the available sphere fitting techniques.

.. _attach_object_note:

Use Cases
----------

- **Grasped objects**: During pick-and-place, the grasped object must be checked for collisions
  with the world. cuRobo approximates it as spheres and attaches them to the robot's kinematic
  model.
- **Robot links**: Robot geometry is approximated with spheres for self-collision and world
  collision checking. See :ref:`tutorial_build_robot_model` for configuring robot spheres.

Entry Point
-----------

The main function is :func:`curobo.geom.sphere_fit.fit_spheres_to_mesh`:

.. code-block:: python

   from curobo.geom.sphere_fit import fit_spheres_to_mesh, SphereFitType
   import trimesh

   mesh = trimesh.load("my_object.obj")

   # Automatic sphere count with default density
   result = fit_spheres_to_mesh(mesh)

   # Explicit sphere count
   result = fit_spheres_to_mesh(mesh, n_spheres=50)

   # With quality metrics
   result = fit_spheres_to_mesh(mesh, compute_metrics=True)
   print(f"Coverage: {result.metrics.coverage:.2%}, Protrusion: {result.metrics.protrusion:.2%}")

.. list-table:: Parameters
   :header-rows: 1
   :widths: 25 75

   * - Parameter
     - Description
   * - ``n_spheres``
     - Number of spheres to fit. When ``None``, estimated automatically from
       bounding-box volume and ``sphere_density``.
   * - ``sphere_density``
     - Density multiplier for auto sphere count (default ``1.0``).
       ``2.0`` doubles the count, ``0.5`` halves it. Range: ``0.1`` -- ``10.0``.
   * - ``fit_type``
     - Fitting algorithm (default ``MORPHIT``). See below.
   * - ``surface_radius``
     - Radius for surface-sampled spheres. Only affects ``SURFACE`` fit type.
   * - ``iterations``
     - Optimization iterations for ``MORPHIT`` (default ``50``).
   * - ``compute_metrics``
     - When ``True``, populates quality metrics on the result.
   * - ``clip_plane``
     - Half-plane constraint ``((nx, ny, nz), offset)`` in mesh-local coordinates.
       Spheres that cross the plane are penalised during MorphIt optimization and
       hard-clamped afterwards.  Useful for keeping base-link spheres from
       protruding into a mounting surface.


Fit Types
----------

cuRobo provides three methods via :class:`curobo.geom.sphere_fit.SphereFitType`:

.. list-table::
   :header-rows: 1
   :widths: 20 80

   * - Type
     - Description
   * - ``SURFACE``
     - Samples the mesh surface evenly with fixed-radius spheres. Fast fallback
       for thin or degenerate meshes.
   * - ``VOXEL``
     - Voxelizes the bounding box, filters by SDF to keep interior voxels, and
       assigns inscribed radii. Good for convex shapes.
   * - ``MORPHIT``
     - Initialises with ``VOXEL``, then runs Adam optimization to minimise
       coverage gaps and protrusion. Best quality, recommended default.

.. figure:: ../images/sphere_approx.png
   :width: 690
   :align: center

   Comparison of the three fit types on robot link meshes. From left to right:
   MORPHIT (pink), VOXEL (green), SURFACE (blue).

To visually compare the fit types interactively, run the comparison demo:

.. code-block:: bash

   python -m curobo.examples.reference.sphere_fit_comparison

This launches a `Viser <https://viser.studio>`_ viewer showing each fit type as a column of
coloured spheres next to the original mesh. You can customise the robot, links, and methods:

.. code-block:: bash

   python -m curobo.examples.reference.sphere_fit_comparison --robot franka.yml
   python -m curobo.examples.reference.sphere_fit_comparison --links panda_link0 panda_link7
   python -m curobo.examples.reference.sphere_fit_comparison --methods surface voxel morphit

The fitting pipeline automatically handles degenerate cases:

- **Hollow/thin meshes**: Watertight meshes with extremely low fill ratio are replaced
  with their convex hull before fitting.
- **Fallback chain**: If the primary method produces no spheres, falls back to ``VOXEL``,
  then ``SURFACE``.


Result
-------

:class:`curobo.geom.sphere_fit.SphereFitResult` contains:

.. list-table::
   :header-rows: 1
   :widths: 25 75

   * - Field
     - Description
   * - ``centers``
     - Sphere centre positions, shape ``(N, 3)``.
   * - ``radii``
     - Sphere radii, shape ``(N,)``.
   * - ``n_spheres``
     - Number of fitted spheres.
   * - ``fit_time_s``
     - Wall-clock fitting time in seconds.

Quality Metrics
----------------

When ``compute_metrics=True``, the following fields are populated:

.. list-table::
   :header-rows: 1
   :widths: 25 75

   * - Metric
     - Description
   * - ``coverage``
     - Fraction of interior sample points covered by at least one sphere.
   * - ``protrusion``
     - Fraction of sphere-surface sample points outside the mesh.
   * - ``protrusion_dist_mean``
     - Mean distance (m) of protruding points to the mesh surface.
   * - ``protrusion_dist_p95``
     - 95th-percentile protrusion distance (m).
   * - ``surface_gap_mean``
     - Mean gap (m) from mesh surface samples to nearest sphere surface.
   * - ``surface_gap_p95``
     - 95th-percentile surface gap (m).
   * - ``max_uncovered_gap``
     - Maximum gap (m) from mesh surface to nearest sphere.
   * - ``volume_ratio``
     - Total sphere volume divided by mesh volume.

Inspecting Fit Quality
-----------------------

Pass ``--compute-metrics`` to the robot builder script to print a per-link
quality report after sphere fitting:

.. code-block:: bash

   python -m curobo.examples.getting_started.build_robot_model \
       --urdf robot.urdf --asset-path meshes/ --output robot.yml \
       --compute-metrics

The same metrics are available programmatically via
``builder.link_metrics`` (a dict of :class:`SphereFitMetrics`).


Clip Planes
------------

When a robot is mounted on a stand or bolted to the floor, base-link spheres
may protrude into the mounting surface.  Pass ``--clip-link`` to prevent this:

.. code-block:: bash

   python -m curobo.examples.getting_started.build_robot_model \
       --urdf robot.urdf --asset-path meshes/ --output robot.yml \
       --clip-link base_link z 0.0

This adds a half-plane constraint during MorphIt optimization (as a
differentiable loss term) and applies a hard clamp after fitting, so no sphere
on ``base_link`` extends below ``z=0`` in link-local coordinates.  The flag can
be repeated for multiple links.

Programmatically, pass ``clip_links`` to :meth:`RobotBuilder.fit_collision_spheres`:

.. code-block:: python

   builder.fit_collision_spheres(clip_links={"base_link": ("z", 0.0)})


Geometry Helper
----------------

Individual geometry objects also provide a convenience method for sphere fitting:

.. code-block:: python

   from curobo.geom.types import Capsule, WorldCfg

   capsule = Capsule(
      name="capsule",
      radius=0.2,
      base=[0, 0, 0],
      tip=[0, 0, 0.5],
      pose=[0.0, 5, 0.0, 0.043, -0.471, 0.284, 0.834],
   )

   sph = capsule.get_bounding_spheres(n_spheres=500)
   WorldCfg(spheres=sph).save_world_as_mesh("bounding_spheres.obj")

---

### Reference Styleguide (`docs/reference/styleguide.rst`)

.. _styleguide_note:

Style Guide
================================

cuRobo follows the `Google Python Style Guide <https://google.github.io/styleguide/pyguide.html>`_.

In addition, the following principles are also followed:

- Avoid inheriting from configuration dataclasses and instead set MainClass.config during
initialization.
- Avoid instantiaing main classes inside a configuration dataclass.
- Use `@get_torch_jit_decorator` to decorate methods that are used in JIT compiled functions.
- Make jit functions be part of the class using `@staticmethod` and `@get_torch_jit_decorator`, and
  name them as `jit_<function_name>`.





Dataclass as Configuration Class:
-----------------------------------
Many classes in cuRobo use dataclasses to store configuration parameters. Current implementation
moves the parameters to torch tensors in the `__post_init__` method. The alternative is to
initialize the torch tensors as private variables in the dataclass. We are leaving this for a
future PR as we are unsure of the tradeoffs.

---

### Snippets Citation (`docs/snippets/citation.rst`)

.. code:: bibtex

    @misc{curobo_report23,
      title={cuRobo: Parallelized Collision-Free Minimum-Jerk Robot Motion Generation},
      author={Balakumar Sundaralingam and Siva Kumar Sastry Hari and Adam Fishman and Caelan Garrett
              and Karl Van Wyk and Valts Blukis and Alexander Millane and Helen Oleynikova and Ankur Handa
              and Fabio Ramos and Nathan Ratliff and Dieter Fox},
      year={2023},
      eprint={2310.17274},
      archivePrefix={arXiv},
      primaryClass={cs.RO}
    }

---

### Snippets Citation V2 (`docs/snippets/citation_v2.rst`)

.. code:: bibtex

    @misc{curobo_v2,
      title={cuRoboV2: Dynamics-Aware Motion Generation with Depth-Fused Distance Fields for High-DoF Robots},
      author={Balakumar Sundaralingam and Adithyavairavan Murali and Stan Birchfield},
      year={2026},
      eprint={2603.05493},
      archivePrefix={arXiv},
      primaryClass={cs.RO}
    }

---

--- METRICS ---
- Files Extracted: 33
- Estimated Token Budget: ~32394 tokens
- Canonical Reference: https://codewiki.google/github.com/NVlabs/curobo