Plan collision-free paths

A motion plan that ignores the table, the back wall, and the fixture on the bench next to the arm will collide on its way to the target. The motion service can plan around these obstacles, but only if you describe them in a WorldState. This guide walks through building a WorldState for a typical bench setup, running a plan against it, and verifying that the planner actually routes around each obstacle.

Prerequisites

Steps

1. Describe your workspace obstacles

A WorldState is a list of geometries expressed in a reference frame. Each geometry needs a shape (box, capsule, or sphere), a pose relative to the frame, and a label. Model every significant obstacle: surfaces the arm can press into, fixtures it can strike, and vertical obstructions like posts or walls.

from viam.proto.common import (
    Pose, Vector3, RectangularPrism, Capsule,
    Geometry, GeometriesInFrame, WorldState
)

# Table
table = Geometry(
    center=Pose(x=0, y=0, z=-20),
    box=RectangularPrism(dims_mm=Vector3(x=800, y=600, z=40)),
    label="table"
)

# Back wall
wall = Geometry(
    center=Pose(x=0, y=400, z=500),
    box=RectangularPrism(dims_mm=Vector3(x=1200, y=20, z=1000)),
    label="back-wall"
)

# Support post
post = Geometry(
    center=Pose(x=300, y=0, z=200),
    capsule=Capsule(radius_mm=25, length_mm=400),
    label="support-post"
)

obstacles = GeometriesInFrame(
    reference_frame="world",
    geometries=[table, wall, post]
)
world_state = WorldState(obstacles=[obstacles])

obstacles := make([]spatialmath.Geometry, 0)

table, _ := spatialmath.NewBox(
    spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: -20},
        &spatialmath.OrientationVectorDegrees{OX: 0, OY: 0, OZ: 1, Theta: 0}),
    r3.Vector{X: 800, Y: 600, Z: 40}, "table")
obstacles = append(obstacles, table)

wall, _ := spatialmath.NewBox(
    spatialmath.NewPose(r3.Vector{X: 0, Y: 400, Z: 500},
        &spatialmath.OrientationVectorDegrees{OX: 0, OY: 0, OZ: 1, Theta: 0}),
    r3.Vector{X: 1200, Y: 20, Z: 1000}, "back-wall")
obstacles = append(obstacles, wall)

post, _ := spatialmath.NewCapsule(
    spatialmath.NewPose(r3.Vector{X: 300, Y: 0, Z: 200},
        &spatialmath.OrientationVectorDegrees{OX: 0, OY: 0, OZ: 1, Theta: 0}),
    25, 400, "support-post")
obstacles = append(obstacles, post)

obstaclesInFrame := referenceframe.NewGeometriesInFrame(
    referenceframe.World, obstacles)
worldState, _ := referenceframe.NewWorldState(
    []*referenceframe.GeometriesInFrame{obstaclesInFrame}, nil)

2. Move with obstacle avoidance

from viam.services.motion import MotionClient
from viam.proto.common import PoseInFrame, Pose

motion_service = MotionClient.from_robot(machine, "builtin")

destination = PoseInFrame(
    reference_frame="world",
    pose=Pose(x=300, y=200, z=400, o_x=0, o_y=0, o_z=-1, theta=0)
)

await motion_service.move(
    component_name="my-arm",
    destination=destination,
    world_state=world_state
)

_, err = motionService.Move(ctx, motion.MoveReq{
    ComponentName: "my-arm",
    Destination:   destination,
    WorldState:    worldState,
})

3. Verify the planner is actually routing around obstacles

The quickest sanity check is a before-and-after comparison. Run the motion with a box placed directly between the arm’s start pose and the target, and the arm should take an indirect path around it. Remove the box, rerun the same motion, and the arm should take a more direct path. If the paths look the same with and without the obstacle, the WorldState is not reaching the planner.

4. Use frame geometry for permanent obstacles

WorldState is re-sent with every Move call, which is useful when obstacles come and go but wasteful for things that never move. Permanent obstacles (the table the arm is bolted to, the back wall, a mounted tool) belong in the component’s frame configuration instead. The planner reads frame geometry from the config on every plan, so you describe those obstacles once and forget about them.

See Define Obstacles for the full configuration reference.

What’s next