Skip to main content

Motion planning

MoveIt 2 + OMPL (RRTConnect) plan collision-checked trajectories. The planning scene is a projection of world knowledge, not the inventory database.

world snapshot (+ obstacle boxes)
        │ planning_scene_projector (lease)
        ▼
/planning_scene diffs under restocker/{object,workcell,obstacle}/
        │
        ▼
move_group (OMPL) ──► MotionPort plan+execute ──► controllers

Projector authority​

Gate motion on /planning_scene_projection/status: expect STATE_APPLIED and an applied revision at least as fresh as the snapshot generation you planned against. Each trajectory also retains the projector epoch and scene_content_generation captured before planning. The motion port checks that identity immediately before execution and between bounded controller goals. A changed generation invalidates the old collision proof even when the projector is already APPLIED again. The next controller goal can proceed only after collision validation against a fresh, complete scene, including attached products, world geometry, padding and collision permissions. Serialized projector-status service reads bracket the scene read and validation; cached topic messages alone cannot authorize submission. Concurrent changes get at most three attempts within the existing settle deadline. Each later slice needs its own proof against the original provenance. This lets a harmless distant lane update progress while refusing a changed obstacle on the route.

Validation checks the measured start, its connection to the first command, and the controller's linear/cubic/quintic spline at 0.0025 m/rad resolution, with bounded work and cancellation checks. This geometric check does not prove servo tracking through an allowed start-position mismatch. Missing scene data, a changed projector epoch or lease, stale evidence, collision or exhausted validation prevents submission. Before motion this is a planning failure; after a completed slice it is an execution failure at the stopped boundary. Scoped-contact grasp escape retains strict generation checks because it uses a private collision matrix.

Ordinary robot motion does not change this identity. A carried product's collision body stays fixed relative to its attachment link; its changing world pose is used for obstacle subtraction. No-diff verification and newer world revisions preserve content generation. Attach/detach and scene-lease release change the identity; behavioral transactions still precede independently planned segments, and a changed scene never silently renews an earlier trajectory's proof. A held lease requires its validated capability throughout planning and execution. Stale depth obstacles are retained rather than silently forgotten, so degradation is visible.

This gate covers projector-managed geometry and authorized scene transactions. Other scene writers must honor that ownership boundary. It is not continuous collision monitoring: changes after a check or during an already submitted controller goal are handled at the next boundary; a bounded goal duration does not establish dynamic-obstacle stopping clearance.

Obstacle boxes come from depth_obstacle_node, which obstacle_perception (default on) composes into the baseline. They are AABBs, not an octomap.

Candidates then MoveIt​

Grasp and placement candidates are generated by deterministic geometry first (Manipulation). MoveIt only validates reachability and collision for those goals. It does not invent grasps.

PreInsert path corridor​

Tray→lane PreInsert stays MotionPathKind::kFreeSpace (OMPL), not Cartesian LIN. After retract, a short Cartesian carry-start moves one held-product diameter plus surveyed side clearance toward the rear lane mouth while retaining retract X/Z/orientation. It removes the unstable OMPL goal that reoriented beside the tray and cannot arc upward. PreInsert then owns reorientation and rail travel under a survey-derived AABB path constraint expanded from its endpoints by held product radius, planner padding, and lane side clearance. MoveItMotionPort applies setPathConstraints for that plan only and never retries unconstrained on failure. Box numbers are logged for later max-Z comparison; claiming a shorter/lower path still needs benchmark traces. just demo drives the sensor acceptance fixture by default (three lanes wanting one product each, one product per class in the tray). The labelled ground-truth opt-in just demo gt:=true uses dense_restock_products.yaml instead: six partially stocked front lanes and three independently ordered, dense-but-not-full back grids. The 25 back-tray products cover the 13 transfers needed by the configured lane targets; the surplus provides slack.

This project​

PiecePath
OMPL configrestocker_moveit_config/config/ompl_planning.yaml
Projectorrestocker_task_executor/src/planning_scene_projector_node.cpp
Motion port…/src/moveit_motion_port.cpp
Transfer AABB…/transfer_path_constraints.{hpp,cpp}

References​