navigation-primitives

Iterate prims with UsdPhysics.CollisionAPI under /World, compute world AABBs with UsdGeom.BBoxCache, and rasterize them into a grid at your chosen resolution. Filter out shells, floor markings, and ceiling objects to avoid thousands of false obstacles.

Erode the navigable grid with a circular kernel sized to the robot's inscribed radius, run heapq-based A* on the eroded grid, then smooth with Catmull-Rom. Validate every smoothed waypoint with an oriented-footprint collision check before accepting the path.

Many robots have their articulation origin above the ground contact point, such as Spot at roughly 0.69 m. Compute the Z-offset from the collider AABB and spawn the robot at z = ground + z_offset.

The inscribed radius is safe for any yaw and used for grid erosion, while the circumscribed radius covers worst-case rotation and sizes conservative buffers. For rectangular robots, an oriented-footprint overlap_box check recovers space lost to circular approximations.

Yes, use HolonomicController with HolonomicRobotUsdSetup to extract wheel positions, orientations, and mecanum angles from the robot USD, then apply commands via WheeledRobot.apply_wheel_actions. Kaya is a validated holonomic example.

When the camera forward vector is parallel to the up vector, the cross product degenerates and produces a broken matrix. Detect abs(fwd dot up) > 0.99 and fall back to an alternate up vector such as (0, 1, 0).