CPU-checked input examples

These examples require an installed recursive source checkout and run without CUDA compilation or GPU calls. From the repository root:

python docs/examples/model_inputs.py
python -m pytest docs/test_doc_examples.py -q

Fixed and floating input packing

The two fixtures are fixed-base iiwa14 (NQ=NV=7) and floating-base Go2 (NQ=19, NV=18). The example initializes a valid identity base quaternion and builds the dynamics inputs and the plant state at their public widths. It uses the default Pinocchio convention. Zero joint angles are illustrative, not a guaranteed collision-free or joint-limit-safe posture. This initializer is not general to spherical joints or other conventions; use the model’s coordinate maps for those cases.

def model_inputs(name, batch=2):
    """Neutral inputs for these two scalar-joint fixtures, not arbitrary URDFs."""
    floating = {"iiwa14": False, "go2": True}[name]
    robot = URDFParser().parse(
        str(ROOT / "config/robot_assets" / f"{name}.urdf"),
        floating_base=floating,
    )
    if robot is None:
        raise ValueError("URDF parsing failed")
    nq, nv = robot.get_num_pos(), robot.get_num_vel()
    q = np.zeros((batch, nq), dtype=np.float32)
    if floating:
        q[:, 6] = 1  # Pinocchio base [xyz, qx, qy, qz, qw].
    # Velocities, accelerations and torques are NV wide on every surface.
    v = np.zeros((batch, nv), dtype=np.float32)
    u = np.zeros_like(v)
    # plant_step takes the state x = [q, v] and the NV-wide control.
    x = np.concatenate((q, v), axis=1)
    return robot, q, v, u, x

For a batch of two, Go2’s q is (2, 19); qd and the control u are (2, 18), the tangent width, on every surface (NumPy, JAX, PyTorch and the C ABI); the plant state x is (2, 37). Dynamics vector outputs such as torques and accelerations come back (2, 18) as well. See Input / Output ABI (h_q_qd_u) and Python Wrappers (grid-rbd) for GPU calls and operation selection.

Constructing a regressor parameter vector

Read the parser’s merged body inertias, not the original XML link order. The example extracts mass, first moment and body-origin inertia into GRiD’s [m, hx, hy, hz, Ixx, Ixy, Ixz, Iyy, Iyz, Izz] order:

def inertial_parameters(robot):
    """Stack GRiD parameters from parsed, fixed-joint-merged body inertias."""
    params = []
    for body in range(robot.get_num_bodies()):
        I = np.asarray(robot.get_Imat_by_id(body), dtype=np.float64)
        # Angular-linear spatial inertia: top-right block is skew(m*c).
        params.extend([
            I[3, 3], I[2, 4], I[0, 5], I[1, 3],
            I[0, 0], I[0, 1], I[0, 2], I[1, 1], I[1, 2], I[2, 2],
        ])
    return np.asarray(params)

The executable example checks Y @ pi == tau on a nonzero iiwa14 state using the CPU reference. This verifies the parameter basis and example, not an independent GPU comparison. Do not copy Pinocchio’s dynamic-parameter vector without converting its order. See Bias terms, centroidal quantities and energy.

An integrator diagnostic

Run python docs/examples/integrator_semantics.py to exercise the actual CPU reference integrator with constant acceleration, initially zero position and velocity, and final time 1. The exact final position is 0.5. Euler’s position errors for 10, 20, 40 and 80 steps are respectively 0.05, 0.025, 0.0125 and 0.00625. The updated CPU reference’s full-state RK4, midpoint, Heun (trapezoidal), and constant_acceleration schemes are exact up to roundoff for this particular problem, not for arbitrary dynamics.

This diagnostic isolates the update rule; it is not a robot benchmark, a GPU test or a general convergence certification. Independent oscillator and quaternion-ODE tests in RBDReference/tests/test_integrator_contract.py check order two for midpoint/Heun and order four for RK4 in Euclidean coordinates, and the second-order rotational limit of the base-point retraction scheme. Reference availability does not establish GPU support; see Integrators and the plant layer for the generated API.