Collision · repose-collision

Planning scenes

A planning scene is the robot's picture of its surroundings: world obstacles plus objects attached to the robot. It is a Copper payload, so it can be published, logged and replayed like any other message.

The type

CuPlanningScene<MAX_OBSTACLES = 32, MAX_ATTACHED = 8>. All storage is preallocated. Nothing grows: exceeding a capacity is an error.

rust
let mut scene = CuPlanningScene::<32, 8>::new();

let table = scene.add_obstacle(table_shape, None)?;          // fresh ID assigned
let fixture = scene.add_obstacle(fixture_shape, Some(CuObjectId::new(100)))?;  // your ID

scene.update_obstacle(fixture, moved_fixture_shape)?;
scene.remove_obstacle(table)?;

Object IDs

CuObjectId is a stable u32 handle. IDs survive removals of other objects and moving an object from obstacle to attached (grasp) and back (release). CuObjectId::NIL (0) is never valid. Pass None to let the scene allocate, or your own IDs when another system (a vision pipeline, a cell database) already names the objects.

Revisions

Every successful change advances scene.revision(), a monotonic CuSceneRevision that never wraps. Planned paths record the revision they were planned against (CuJointPlanPath::scene_revision). If the scene has moved on since, the path was checked against a world that no longer exists: republish the scene and replan.

Transactions

Every single mutation is all-or-nothing: on any error (capacity, unknown or duplicate ID, invalid geometry, revision exhaustion) the scene is unchanged. To make several changes atomic, wrap them:

rust
scene.transaction(|s| {
    s.remove_obstacle(old_part)?;
    s.add_obstacle(new_part_shape, Some(new_part))?;
    s.add_obstacle(new_fixture_shape, None)?;
    Ok(())
})?;  // either all three happened, with one revision bump, or none

Reading the scene

obstacles()Iterator of CuSceneObstacle { id, shape }.
obstacle_shapes() / obstacle_ids()Parallel slices.
attached_shapes()CuAttachedShape { id, frame, shape } slice.
obstacle(id) / attached_shape(id) / contains(id)Lookup by ID.
collision_env()Zero-copy CuCollisionEnv for checks.
world_attached_shapes(&frames, &mut out)Attached shapes in world coordinates at a configuration, for display.

Clearing

clear(), clear_obstacles() and clear_attached() remove everything of that kind and advance the revision only if something was removed.

Errors

CuSceneError names the failure: ObstacleCapacityExceeded, AttachedCapacityExceeded, ObjectNotFound(id), InvalidObjectId(id), DuplicateObjectId(id), InvalidLinkIndex, OutputCapacityExceeded, RevisionExhausted, InvalidGeometry.

Scenes in the planning pipeline

A planning request carries its own scene, CuPlanningScene<16, 8> in CuJointPlanRequest, and the planner captures it at the start of the job. Changing the scene mid-plan doesn't affect the running job. See Requests & results.

API reference