GPU Whole-Body IK
Experimental API
The GPU WBC API is suitable for evaluation and high-throughput research, but it is not yet the stable production default. Unsupported model shapes and features fail explicitly; GPU mode never silently calls the CPU solver.
EmbodiK's current GPU path combines:
- Newton for model-derived, batched forward kinematics and Jacobians;
- Warp directional SRINV for parallel prioritized velocity IK;
- Torch CUDA tensors for device-resident application state and targets;
- CUDA graphs for fixed-shape warm execution.
The solver derives dimensions, joint limits, velocity limits, frame ancestry,
and specialization metadata from RobotModel. Robot names are diagnostic and
cache labels, not dispatch keys. Panda, AI Worker, RB-Y1, G1, and Spot examples
exercise the same public adapters without a robot-family DoF table.
When to use CPU or GPU
| Workload | Recommended path |
|---|---|
| One interactive robot and minimum latency | CPU KinematicsSolver remains the baseline |
| Hundreds or thousands of independent worlds | GPU warp_srinv with device-resident state |
| One GPU world with supported constraints | GPU is available, but benchmark the complete application loop |
| A CPU-only feature or unsupported joint manifold | Stay on CPU; construction fails rather than falling back |
GPU throughput improves when launch and kinematics work are amortized across many worlds. A fast solve kernel does not make Viser rendering, host tensor copies, target generation, or physics stepping free.
Requirements
- Python 3.10–3.12 and a working EmbodiK source build;
- an NVIDIA GPU supported by the installed Torch, Warp, and Newton versions;
torch,warp-lang>=1.17.0, and a compatible Newton installation;- Viser, yourdfpy, and robot descriptions for the visual examples.
With pip or a virtual environment:
python -m pip install -e ".[examples,gpu-wbc]"
git clone --depth 1 https://github.com/newton-physics/newton.git ../newton
python -m pip install -e ../newton
For a Linux x86-64 repository checkout managed by Pixi, one command creates the CUDA environment, installs EmbodiK, selects a Torch wheel that can execute on the current GPU, and installs Newton:
The script clones ../newton when that checkout is absent. Set NEWTON_DIR to
use a different Newton source tree. It runs check-cuda before and after the
optional sm_120 Torch repair. Newton is installed after that repair, because
replacing the Torch wheel removes packages that were installed into the same
environment. check-cuda verifies both CUDA visibility and execution of a real
kernel for the device architecture. The repair stays inside .pixi/envs/cuda.
The current integration was validated against the Newton 1.6 development line with Warp 1.17.0. Older Warp builds can fail during Newton import before an EmbodiK solve begins. The first run builds and caches Newton/Warp kernels for the selected model and shape; benchmark only after warm-up.
Run the parallel showcase
The same script supports three materially different model and task shapes:
# The interactive viewer defaults to 512 solved and rendered CUDA worlds.
pixi run -e cuda demo-parallel-tracking
pixi run -e cuda python examples/10_parallel_trajectory_tracking.py --robot ai-worker
pixi run -e cuda python examples/10_parallel_trajectory_tracking.py --robot g1
# Reproduce the 1,024-world solve-only reference profile.
pixi run -e cuda demo-parallel-tracking-benchmark
The viewer listens on http://localhost:8080 by default and runs until
Ctrl+C. Pass --port when another process already uses that address.
Worlds rotate through circle, figure-eight, helix, and sweep targets with
independent phases and speeds. The viewer renders all 512 default worlds through
shared per-link mesh instances. Four colored world bands make the motion
families easy to distinguish at field scale. --show controls only browser
visualization; every run still solves the full --worlds batch. When omitted,
--show follows --worlds, so --worlds 1024 solves and renders all 1,024.
To solve 1,024 worlds while publishing only 512 robots to the browser, pass
--worlds 1024 --show 512.
Model-derived API
Opt-in primary SCALE with coupled joint bounds
Fixed-base scalar-joint models can select solver_backend="warp_polytope"
in GpuWbcMultiFrameSolver.from_robot(). This experimental backend maximizes
the common primary task scale, subject to the original joint velocity box
and optional coupled configuration bounds. It uses native double-precision
Warp linear programming; reduced coordinates use a checked full-space fallback
when numerical rank or conditioning prevents a valid reduction.
Configure bounds in compact active-joint order, outside CUDA graph capture:
# -0.5 <= q[0] + q[1] <= 0.5, for a two-active-joint model.
solver.set_linear_position_constraints([[1.0, 1.0]], [-0.5], [0.5])
assert solver.get_linear_position_constraint_rows() == 1
The matrix has shape (rows, active_dofs); lower and upper bounds have shape
(rows,). Values must be finite and ordered. An empty matrix of shape
(0, active_dofs) clears the bounds. Updating bounds invalidates the captured
graph. Invalid updates preserve the previous configuration.
Rows are rebuilt at each inner integration step. A seed already outside a bound can move inward without projection, but cannot worsen its starting violation. A small numerical inset reserves float32 integration error; the actual integrated configuration is independently checked against the original row envelope before publication. Rejected row candidates hold the incoming configuration. This endpoint check does not certify swept nonlinear geometry.
This is primary Cartesian SCALE, with one common scale across stacked frame
rows. It does not implement ELASTIC SCALE, secondary objectives, or general
WBC constraints. Collision, centroidal/CoM, torso, contact, acceleration and
non-additive joint models fail explicitly. The default warp_srinv behavior
is unchanged. Benchmark the complete application: exact-ray optimization adds
work and is not a guarantee of lower latency or nonlinear target recovery.
SRINV bounded completion and refinement
Native warp_srinv and cusolver_srinv bounded completion can release an
unnecessary joint bound when a consistent combined task stalls at zero velocity.
The recovery search runs in one graph-compatible Warp launch. It preserves the
task ray and original velocity box, including boxes that require inward joint
motion. Valid worlds with positive motion bypass the search. An invalid moving
realization can be repaired only while preserving its achieved combined task
velocity exactly; it is not silently rescaled to make it fit. If no checked
repair is found, the original candidate remains subject to the surrounding
solver's publication checks. Recovery does not replace collision, CoM,
nonlinear acceptance or separately prioritized task handling. The recovery flag
reports a repaired velocity, not convergence to the target.
Fixed-base GpuWbcMultiFrameSolver.from_robot() callers can additionally set
refine_partial_scale=True with warp_srinv or cusolver_srinv to seek a
larger feasible primary task scale. The default is disabled. Refinement adds
work and can change the joint trajectory; it does not optimize the complete
nonlinear motion or replace later constraint checks. Unsupported backends
reject this option.
For a slightly off-ray float32 seed, refinement can restart its numerical search
from exact zero velocity when the joint velocity box contains zero. It retains
the existing final equality, box and scale-improvement checks. No intermediate
zero command is published and the robot configuration is not reset. Boxes that
require inward motion retain their existing path. Custom seed admission uses
refinement_ray_tolerance; refine_from_origin=True explicitly requests origin
initialization for admitted seeds. These two advanced options require partial
refinement to be enabled.
Build a solver with from_robot(). That factory selects warp_srinv and
derives the joint layout from the loaded RobotModel. The class constructor
is the legacy artifact path and is not the integration entry point.
Use GpuWbcMultiFrameSolver when robot.is_floating_base is false. Use
GpuWbcFloatingMultiFrameSolver for one standard free root. Passing the other
class raises before any CUDA work. A fixed-base call looks like this:
from pathlib import Path
import numpy as np
import torch
import embodik
from embodik.gpu.wbc import GpuWbcMultiFrameSolver
urdf = Path("robot.urdf")
robot = embodik.RobotModel(str(urdf), floating_base=False)
q0 = np.asarray(robot.neutral_configuration(), dtype=np.float64)
batch_size = 1024
solver = GpuWbcMultiFrameSolver.from_robot(
urdf,
Path("build/gpu-wbc-cache"),
robot=robot,
robot_name="my_robot", # label only
frames=("tool_frame",),
frame_task_dimensions=(6,),
default_configuration=q0,
batch_size=batch_size,
dt=0.01,
)
q_active = np.asarray(q0, dtype=np.float32)[list(solver.active_configuration_indices)]
q_cuda = torch.as_tensor(q_active, dtype=torch.float32, device="cuda").expand(
batch_size, -1
).contiguous()
target_cuda = torch.zeros(
(batch_size, len(solver.frames), 7), dtype=torch.float32, device="cuda"
)
target_cuda[..., 3] = 1.0 # identity quaternion, WXYZ
result = solver.solve_device_batch(q_cuda, target_cuda)
accepted = result.world_status == solver.WORLD_STATUS_SUCCESS
tracked = accepted & result.converged
q_next_cuda = result.q_solution
default_configuration is the full model vector from RobotModel. The tensor
passed to solve_device_batch() is narrower: [batch_size, configuration_dim],
with one column per name in active_joint_names. Gather it with
active_configuration_indices. q_solution has that same width, so scatter it
back with those indices. The factory includes every supported movable joint
unless you pass active_joint_names.
Finite joint positions that fall outside their URDF limits are projected onto
the nearest limit before the solve. The Franka Panda description places joint 4
outside its upper limit at neutral_configuration(), and that home is repaired
automatically. Non-finite values and a zero quaternion still return
WORLD_STATUS_INVALID_INPUT and do not move. get_joint_limits() reports the
limits when an application wants to inspect them.
The fixed-base factory defaults to dt=0.1 and iterations=2. Pass dt
explicitly when the control period is different. The floating-base factory
defaults to dt=0.01 and also requires frame_position_gains,
frame_orientation_gains, and six positive base_velocity_limits. Its q
is the full configuration, shaped [batch_size, robot.nq], not an active-joint
slice. The first solve for a new model or shape compiles Newton and Warp
kernels, so exclude it from latency measurements.
Quaternion layout
These three tensors do not share a quaternion convention:
| Value | Shape | Quaternion |
|---|---|---|
Fixed-base q and q_solution |
[batch, configuration_dim] |
none; scalar joints only |
Floating-base q and q_solution |
[batch, robot.nq] |
root quaternion is XYZW, after the root translation |
target |
[batch, frame_count, 7] |
position, then WXYZ |
evaluate_body_poses_device(q) |
[batch, body_count, 7] |
position, then XYZW |
A body pose cannot be copied into a target without swapping the quaternion.
A floating-base root packed as WXYZ is still finite, so the solve runs and the
base orientation is wrong. current_velocity and previous_velocity, when
supplied, are float32 tensors of shape [batch, velocity_dim] in
active_velocity_indices order.
Per-world status
result.status is always "solved_or_held_needs_verification". The outcome
of each row is result.world_status, an int8 tensor of shape [batch_size].
Both adapters publish the same codes:
| Code | Constant | Meaning |
|---|---|---|
| 0 | WORLD_STATUS_SUCCESS |
The world produced an accepted step |
| 1 | WORLD_STATUS_INVALID_INPUT |
Non-finite input or a zero quaternion. q_solution stays at the caller's configuration |
| 2 | WORLD_STATUS_HELD |
No accepted step. q_solution stays at the projected seed |
| 3 | WORLD_STATUS_NUMERICAL_FAILURE |
The numerical solve failed for that world |
| 4 | WORLD_STATUS_INACTIVE |
valid_mask excluded the world |
Invalid and inactive rows keep the configuration the caller passed in. A held
row keeps the seed after finite joint positions have been projected onto their
limits. WORLD_STATUS_SUCCESS means publication was accepted; it can describe
an intermediate progress step whose target error is still large. To identify
worlds that reached the configured tracking tolerances, also read
result.converged, as shown above. One bad row does not reject the batch.
Result lifetime during graph replay
With CUDA graph capture enabled, result tensors borrow reusable output buffers.
The next call on the same solver overwrites them, including tensors held through
an earlier result object. Consume them before the next solve, or use .clone()
for a state snapshot, retained trajectory sample or before/after comparison:
q_before = q_cuda.clone()
result = solver.solve_device_batch(q_cuda, target_cuda)
q_after = result.q_solution.clone()
delta = q_after - q_before
Passing the latest q_solution directly into the next solve is supported;
retaining that reference does not preserve the previous configuration.
RL and simulator integration
Treat GPU WBC as a device-resident control layer between policy outputs and simulator actuator commands. The scalable path keeps simulator state, targets, solver results, and reset masks on CUDA for the entire control tick:
q_index = torch.tensor(
solver.active_configuration_indices, device=q_sim.device
)
dq_index = torch.tensor(solver.active_velocity_indices, device=dq_sim.device)
q_active = q_sim.index_select(1, q_index)
dq_active = dq_sim.index_select(1, dq_index)
# Shape: [num_envs, num_frames, 7], encoded as xyz + WXYZ quaternion.
result = solver.solve_device_batch(
q_active,
policy_targets,
current_velocity=dq_active,
reset_mask=done_mask,
valid_mask=active_mask,
)
q_command = q_sim.clone()
q_command.index_copy_(1, q_index, result.q_solution)
sim.set_joint_position_targets(q_command)
That gather is the fixed-base shape. A floating-base solver takes the full
[num_envs, robot.nq] configuration and does not expose
active_configuration_indices. Its root quaternion remains XYZW.
reset_mask and valid_mask are boolean tensors of shape [batch_size].
Resetting selected worlds zeros command-acceleration history without rebuilding
the solver. Inactive worlds hold the caller's configuration and do not advance
stored history. One non-finite world sets WORLD_STATUS_INVALID_INPUT on that
row and does not reject the batch. A finite position outside its URDF limit is
projected before the solve and is not invalid input.
Provided tensors for a participating world are contemporaneous this tick.
Omitted previous_velocity retains the accepted command. There is no separate
per-field freshness schedule.
Build one solver at the training batch size, warm it before collecting timing
or rollouts, and reuse it across episode resets. Rebuilding the solver or
changing tensor shapes forces compilation and CUDA-graph setup back onto the
critical path. For velocity-controlled actuators, consume
result.accepted_velocity instead of q_solution. Use
measure_device_batch() when you need host-dispatch versus synchronize time
and a CUDA allocator snapshot. That helper is a solver-path measurement, not a
physics + observation + policy + rendering profile.
The exact transport depends on the simulator:
- A GPU-native simulator can gather active joints, solve, and apply commands without a host synchronization.
- CPU MuJoCo with mjviser requires a host/device boundary. Batch one state upload and one command download per control tick rather than copying each world independently.
- Run WBC at the control cadence, which may be decimated from physics, and hold or interpolate commands between control ticks. Measure policy inference, WBC, transfers, and physics together when setting the environment rate.
- Publish only selected environments at a decimated rate. mjviser is a viewer and MuJoCo integration surface, not the transport for a 512-world training batch; rendering every environment every physics step can dominate runtime.
Example 10 demonstrates the batched control and selective-publication pattern. The Spot locomanipulation example shows a MuJoCo/mjviser application loop; combine the two patterns when bringing GPU WBC into an RL environment.
Model envelope
Supported today:
- fixed-base models with scalar one-DoF joints;
- floating-base models with one standard 7-coordinate/6-velocity free root plus scalar joints;
- link-frame position and pose tasks, including overdetermined layouts;
- model-derived fixed shapes and arbitrary Warp batch sizes;
cusolver_srinvfor batch size one andwarp_srinvfor parallel worlds.
Unsupported joint manifolds, inconsistent frame layouts, insufficient row capacity, and non-CUDA runtime attribution fail during construction or solve. Adding a new robot normally means supplying its URDF, frames, and policy—not editing a kernel with that robot's joint count.
Native bounded-task completion uses a device-resident undamped Warp inverse for rank and reachability checks. These checks participate in the existing spectral-status publication veto and support CUDA graph replay with changed targets. The Torch reference helper remains available for algebraic comparisons; its general SVD path is not a guarantee of CUDA graph compatibility.
Exactly zero native factor matrices return zero inverses/solutions without spectral work, clearing all reused outputs and retaining supplied RHS finiteness checks. Finite bounded-completion candidates that cannot be used feed zero factors to this path. Required candidates and non-finite matrices retain their normal factorization/status checks. No small nonzero matrix is treated as zero; rank, regularization and motion-acceptance thresholds are unchanged.
Constraint and task coverage
Newton CoM/centroidal evaluators use the caller's Torch CUDA stream. Each evaluator owns mutable buffers: serialize calls on one evaluator, or allocate separate evaluators for concurrent workers. CoM stream scopes serialize Warp's shared default-stream save/restore while enqueuing kernels; GPU work remains asynchronous. This protects independent CoM evaluators from interleaved scope restoration and does not make arbitrary shared solver objects thread-safe.
Applications can inspect GPU_WBC_CAPABILITIES before exposing a control. A
false capability must be disabled or rejected.
| Feature | GPU status |
|---|---|
| Pose tasks and two-level primary/secondary priority | Supported |
| Joint position/velocity bounds and contact-frame constraints | Supported |
| Self-collision constraints and lazy collision debug | Supported |
| Posture/nullspace, torso bounds, and torso staging | Supported |
| Adaptive dt and command acceleration-history limits | Supported |
| CoM support polygon | Supported |
| Capture point and velocity ZMP in the world support frame | Supported |
| Centroidal momentum task and hard momentum bounds | Supported |
| Runtime shape-stable constraint enable/disable and tuning | Supported where advertised by capabilities |
| Device-resident multi-world solve | Supported with Warp |
| Per-world reset/validity masks and categorical status | Supported |
| Solver-path dispatch, sync, and memory snapshot | Supported via measure_device_batch() |
| General task axis masks and joint metrics | Planned |
| Exact CPU collision tuning/certification policy | Planned |
Velocity ZMP requires measured generalized velocity. Pass current_velocity
to solve_step() or solve_device_batch() when it is enabled; missing or
non-finite state fails closed. Collision contact and row capacities are fixed at
construction so CUDA graph shapes remain stable. Collision debug publication is
lazy because decoding witnesses requires host-visible data.
Public interactive examples
GPU-enabled examples retain their CPU default. Passing --gpu-wbc initializes
the GPU adapter and exposes the supported backend controls:
python examples/02_collision_aware_IK.py --gpu-wbc
python examples/06_bimanual_whole_body_ik.py \
--gpu-wbc --gpu-wbc-cache-dir build/gpu-wbc-cache
python examples/07_unitree_g1_retargeting_ik.py \
--gpu-wbc --gpu-wbc-cache-dir build/gpu-wbc-cache
python examples/08_spot_full_body_ik_viser.py --gpu-wbc
Use each script's --help for model-specific collision and backend flags.
First use of a newly selected model, task layout, or fixed capacity includes
kernel warm-up and should not be treated as steady-state solve latency.
Measured scale-out profile
The following profile was captured on a CUDA-capable NVIDIA GPU with approximately 24 GB of device memory, 1,024 CUDA-resident worlds, two solver iterations, 20 warm-up steps, and 50 measured steps:
| Model | Active DoF | 6D tasks / world | p50 | p95 | Mean throughput |
|---|---|---|---|---|---|
| Franka Panda | 7 | 1 | 0.906 ms | 0.924 ms | 1.13M worlds/s |
| ROBOTIS AI Worker SG2 | 15 | 2 | 4.854 ms | 4.875 ms | 211k worlds/s |
| Unitree G1 | 29 | 4 | 20.295 ms | 20.408 ms | 50.4k worlds/s |
This is a warm CUDA solve-only profile. Target generation, Viser, collision,
host publication, and physics stepping were disabled or excluded. The table is
evidence of batch scaling, not a claim that every constrained model runs below
one millisecond or that a 1,024-world simulation has the same end-to-end rate.
Use measure_device_batch() to split host dispatch from the blocking
synchronize and to read allocated/reserved CUDA bytes for the same path. Full
RL-loop budgets still have to be measured in the training application.
Acceleration-level slice
GpuAccelerationSolver is a separate API for fixed-base models whose movable
joints each have nq == nv == 1. It consumes an already assembled generalized
acceleration reference and applies the CPU-compatible acceleration, one-step
velocity, position endpoint, continuous-path, and braking-viability state-box
equations. It supports CUDA graph capture and independent per-world failure.
import numpy as np
import torch
from embodik.gpu.wbc import GpuAccelerationSolver
solver = GpuAccelerationSolver(robot, acceleration_limits=np.full(robot.nv, 8.0))
q = torch.zeros((1024, robot.nq), dtype=torch.float64, device="cuda")
dq = torch.zeros((1024, robot.nv), dtype=torch.float64, device="cuda")
reference = torch.zeros_like(dq)
result = solver.solve_device_batch(q, dq, 0.01, reference)
Frame and CoM tasks, multiple acceleration priorities, SCALE, floating bases,
centroidal, contact, effort, and geometric constraints are not yet part of this
acceleration slice. Inspect GPU_ACCELERATION_CAPABILITIES before exposing
them.