Visualize a motion plan

The 3D SCENE tab renders your configured frame system and live component poses, and the visual checks for a failing plan find most problems by eye. When those checks do not reveal the problem, or you want to see the trajectory itself, publish the plan as custom visuals through a world state store service. The scene then renders the path you can otherwise only read as numbers.

This page shows how to turn a plan into transforms the scene can draw, and how to use the rendered path to debug a plan that failed or moved unexpectedly.

Replay a saved plan in the 3D scene

If the plan already ran, you don’t need to write any code to see it. The motion service can save plans as files, and the 3D SCENE tab’s Replay mode steps through them, with the obstacles each plan was planned against. To replay plans, set the motion service up to save them and sync them to the cloud, then open them in Replay mode.

Save plans for replay

The motion service saves plans only when its configuration tells it to:

  • plan_file_path sets the directory for plan files. It’s required by both settings below.
  • log_planner_errors: true saves plans that fail. A failed plan’s file holds the request only, with no trajectory, but replaying it still shows the scene the planner was working in.
  • log_slow_plan_threshold_ms saves plans that take longer than that many milliseconds. Set it to -1 to save every plan, which is what captures successful plans for replay.

These settings apply only to the motion service you configure them on, and to plans requested through that service. See Motion service configuration.

Then add the plan directory to the data manager’s additional_sync_paths so the files reach the cloud (see Upload external data). Syncing walks the tag= subdirectories the service writes and carries those tags to the cloud, which is how the replayer finds the plans. For example:

{
  "name": "data_manager-1",
  "api": "rdk:service:data_manager",
  "model": "rdk:builtin:builtin",
  "attributes": {
    "additional_sync_paths": ["${environment.HOME}/.viam/plans"]
  }
}

Replay shows a plan the service already computed. To render a plan you are computing yourself, or to keep visuals live while the machine runs, publish the plan as custom visuals instead.

Prerequisites

The custom-visuals path below starts from a plan you already have in Go:

  • A plan from armplanning.PlanMotion, and the fs (*referenceframe.FrameSystem) you assembled into its PlanRequest. See Verify a plan for building both.
  • The goal pose you passed to the planner, to mark the destination.
  • A module that can serve a world state store service, which is how the transforms reach the scene. See Publish visuals from a module.

Why publish the plan as custom visuals

A plan is a sequence of joint configurations. Rendered in the scene, the path shows immediately whether it clips an obstacle, swings wide, or aims at a target outside the arm’s reach. Publishing the plan as world state store transforms puts the trajectory and goals in the same 3D view as the frames and obstacle geometry the planner used, so you can see the path and the world together.

Convert the trajectory into poses

A plan’s Trajectory is a sequence of joint configurations (FrameSystemInputs), one per step. The scene places geometry by pose, so convert each step into end-effector poses with the frame system. ComputePoses takes a configuration and returns the pose of each frame:

var transforms []*commonpb.Transform
for i, step := range plan.Trajectory() {
    poses, err := step.ComputePoses(fs)
    if err != nil {
        return err
    }
    gripperPose := poses["my-gripper"].Pose()
    // stepMarker (next section) turns this pose into a visual.
    transform, err := stepMarker(i, gripperPose)
    if err != nil {
        return err
    }
    transforms = append(transforms, transform)
}

// Mark the destination you passed to the planner (goalMarker, next section).
goalTransform, err := goalMarker(0, goalPose)
if err != nil {
    return err
}
transforms = append(transforms, goalTransform)
// Serve transforms through a world state store service (last section).

Each step is the arm’s configuration at that point in the trajectory, and poses["my-gripper"] is where the gripper sits in that configuration.

Build transforms for the plan

With a pose per step, build the visuals with the draw library: a marker per trajectory step and a marker for each goal. Give each a stable UUID so the scene can update them when you re-plan.

import (
    "fmt"

    "github.com/viam-labs/motion-tools/draw"
    commonpb "go.viam.com/api/common/v1"
    "go.viam.com/rdk/spatialmath"
)

// Distinct colors keep the trajectory and its goals apart in the scene.
var (
    stepColor = draw.NewColor(draw.WithName("blue"))
    goalColor = draw.NewColor(draw.WithName("green"))
)

func stepMarker(i int, pose spatialmath.Pose) (*commonpb.Transform, error) {
    id := fmt.Sprintf("step-%d", i)
    // Build the sphere at the origin; WithPose below places it at the step pose.
    sphere, err := spatialmath.NewSphere(spatialmath.NewZeroPose(), 5, id)
    if err != nil {
        return nil, err
    }
    drawn, err := draw.NewDrawnGeometry(sphere, draw.WithGeometryColor(stepColor))
    if err != nil {
        return nil, err
    }
    // WithID derives a stable UUID from the string, so re-planning updates each
    // marker in place instead of drawing a duplicate.
    return drawn.Draw(id, draw.WithID(id), draw.WithPose(pose))
}

// goalMarker draws a destination pose. Unlike the trajectory poses, a goal pose is
// not a ComputePoses output: it is the destination you passed to the planner.
func goalMarker(i int, goalPose spatialmath.Pose) (*commonpb.Transform, error) {
    id := fmt.Sprintf("goal-%d", i)
    sphere, err := spatialmath.NewSphere(spatialmath.NewZeroPose(), 8, id)
    if err != nil {
        return nil, err
    }
    drawn, err := draw.NewDrawnGeometry(sphere, draw.WithGeometryColor(goalColor))
    if err != nil {
        return nil, err
    }
    return drawn.Draw(id, draw.WithID(id), draw.WithPose(goalPose))
}

The trajectory poses come from ComputePoses, but the goal pose is the destination you passed to the planner. goalMarker draws it with a distinct color, so the target stands out from the trajectory leading to it.

Serve the transforms to the scene

Serve the transforms through a world state store service so the 3D SCENE tab renders them. Return your step and goal markers from the service’s ListUUIDs, GetTransform, and StreamTransformChanges methods, and the plan streams in alongside the frames and obstacle geometry the scene already shows. For those methods, the poll-and-update loop, and how a module pulls data from its dependencies, see Publish visuals from a module.

Diagnose a failed or surprising plan

With the plan rendered, debugging becomes visual. Compare the trajectory against the rest of the scene:

  • Where does the path collide? If a step marker passes through an obstacle geometry, that is where the planner reports a collision. Check whether the obstacle is real or an oversized geometry.
  • Does the goal fall outside the arm’s reach? If a goal marker sits far from any reachable arm configuration, the goal is out of reach. Move the goal or check the frame system.
  • Why the detour? An unexpected route usually means an obstacle is forcing the planner around it. Look for stray geometry between the start and goal.

For checking the obstacle geometry itself, separate from the plan, see Verify obstacles.

When to visualize versus inspect or verify

The 3D scene serves three distinct purposes, and it helps to keep them straight:

  • Visualize a plan (this page): publish the trajectory and goals as custom visuals to see the path in context.
  • Inspect static frames and geometry: use the stock scene to check frame positions and obstacle coverage with no plan involved.
  • Check feasibility: use armplanning.PlanMotion to confirm a goal is reachable and a path exists before you visualize or run anything.

Visualization shows you what the path looks like; static inspection shows you what the world looks like; feasibility checking tells you whether a plan exists at all. Reach for the one that matches the question you are asking.

What’s next