matlab-plan-robot-motion

Manipulator Motion Planning

Plan collision-free paths for manipulators and convert them to time-optimal trajectories that respect velocity and acceleration limits.

When to Use

• Moving a robot arm between joint configurations or Cartesian poses — regardless of whether the user says "trajectory," "motion," "path," or "plan"
• Generating any trajectory between waypoints (time-optimal, constant-velocity, trapezoidal, minimum-jerk)
• Planning a collision-free path between configurations
• Modeling obstacles as collision environments for planning
• Choosing between motion planners (RRT, CHOMP, dlCHOMP)
• Verifying a trajectory is collision-free
• Any task where the robot moves through space and geometry exists in the workspace — collision safety is implicit even if the user doesn't mention it

When NOT to Use

• Building, loading, or importing the robot model — use /matlab-model-robot-kinematics
• Solving IK/FK or validating solutions — use /matlab-model-robot-kinematics
• Attaching grippers or configuring end-effectors — use /matlab-model-robot-kinematics
• Mobile robot navigation (wheeled platforms, ground vehicles)
• Dynamics simulation (inverseDynamics, forwardDynamics)
• Code generation / MEX deployment of planners

Pre-conditions

Before planning, verify these are satisfied:

1. MANDATORY: Invoke /matlab-model-robot-kinematics first — Do NOT proceed with any step below until the robot model exists in the MATLAB workspace AND was set up by that skill. Never call loadrobot, importrobot, or build a rigidBodyTree inline in a motion-planning workflow — even if it seems trivial. No exceptions. The modeling skill handles collision mesh validation, gripper selection, EE frame alignment, and known pitfalls (e.g., articulated grippers causing self-collisions with planners). Skipping it leads to wasted iterations.

2. Robot has collision meshes — A robot without collision geometry causes planners to report "collision-free" for paths that actually collide. Confirm with:

bodies = robot.Bodies;
hasCollision = any(cellfun(@(b) ~isempty(b.Collisions), bodies));
assert(hasCollision, "Robot has no collision meshes — planning results will be invalid.");

1. DataFormat="row" — All planning functions expect row-vector configurations. Verify:

assert(strcmp(robot.DataFormat, "row"), "Set DataFormat to 'row' before planning.");

Workflow

Step 0: Assess Collision Risk

Before choosing a trajectory tool, determine whether the workspace has geometry that the robot could collide with. Default assumption: if any objects exist near the robot (tables, workpieces, fixtures, tools), collision checking is mandatory — even if the user only asked for a "trajectory" or "interpolation."

Does the workspace contain any geometry (workpiece, table, fixture, tool)?
├── YES → Model it as collision objects (Step 1) and either:
│         ├── Use a planner (Steps 2–3) if configs are far apart or path is non-trivial
│         └── Interpolate directly (Step 4) BUT verify collision-free (Step 5) — if collisions
│             are found, go back and use a planner
└── NO (free space, no objects) → Skip to Step 4 (trajectory generation)

Never assume a straight-line joint interpolation is safe. Even nearby configurations can produce intermediate poses that dip into workpieces, especially for 6-DOF arms operating close to surfaces (welding, machining, assembly). The cost of checking is low; the cost of a collision is high.

Step 1: Model the Environment

Choose a representation based on what you have and which planner you'll use:

Representation	Create With	Works With	Best For
Cell array of collision objects	collisionBox, collisionCylinder, collisionSphere, collisionMesh	manipulatorRRT	Known geometric obstacles (tables, walls, bins)
occupancyMap3D	occupancyMap3D + insertPointCloud	manipulatorRRT (via Map property)	Point cloud or sensor data
meshtsdf	meshtsdf from geom2struct output	manipulatorCHOMP	Mesh-based environments needing smooth cost
SphericalObstacles	N×4 matrix [x y z radius]	manipulatorCHOMP	Simple sphere approximations

Collision objects pattern:

% Table top surface at z = -0.01 (center at z = -0.02 for a 0.02 m thick box)
table = collisionBox(1.2, 0.8, 0.02);
table.Pose = trvec2tform([0.5, 0, -0.02]);

pillar = collisionCylinder(0.05, 0.6);
pillar.Pose = trvec2tform([0.4, 0.0, 0.3]);

env = {table, pillar};

occupancyMap3D pattern:

omap = occupancyMap3D(20);
sensorPose = [0 0 0 1 0 0 0];
insertPointCloud(omap, sensorPose, pointCloudData, 5.0);

meshtsdf pattern (for CHOMP):

meshStructs = geom2struct(env);
mTSDF = meshtsdf(meshStructs, Resolution=20, TruncationDistance=0.2);

The TruncationDistance must be larger than the largest robot collision sphere radius — otherwise CHOMP cannot detect proximity correctly.

Step 2: Select a Planner

Primary decision — what is the environment?

What is the environment representation?
├── Simple primitives (boxes, cylinders, spheres) → manipulatorRRT
├── Dense mesh / CAD STLs → manipulatorCHOMP (meshtsdf is its native input)
├── Point cloud / sensor data → manipulatorRRT (via occupancyMap3D)
└── Just a few spheres → manipulatorCHOMP (SphericalObstacles, lightweight)

Secondary decision — path topology:

Does the path require going "around" obstacles (large joint-space excursion)?
├── YES → Favor manipulatorRRT (global search won't get stuck in local minima)
└── NO (small adjustment to avoid nearby obstacle) → Either works; CHOMP may be faster

Default: manipulatorRRT — it handles all environment types, has robust collision checking, and its output feeds cleanly into contopptraj. Note that shorten reduces detours and contopptraj respects velocity/acceleration limits, but neither smooths out sharp direction changes in the path geometry. If smooth joint-space paths matter (e.g., for reduced jerk or mechanical wear), CHOMP is preferable — it jointly optimizes a smoothness cost and collision avoidance cost, producing inherently smoother trajectories.

See references/planner-comparison.md for detailed tuning parameters and performance characteristics.

Step 3: Plan the Path

Before planning with manipulatorRRT: Resolve self-collision skip pairs up front to avoid wasted planning iterations:

1. [isColl,sepDist,witPts] = checkCollision(robot, startConfig, {}, SkippedSelfCollisions="parent") — if isColl(1)==0, use "parent".
2. Otherwise, build the skip list empirically — see references/self-collision-skip-procedure.md.

manipulatorRRT:

rrt = manipulatorRRT(robot, env, SkippedSelfCollisions=skippedSelfCollisions);
rrt.MaxConnectionDistance = 0.3;
rrt.ValidationDistance = 0.05;
rrt.MaxIterations = 5000;
rrt.EnableConnectHeuristic = true;

rng(0, "twister");
path = plan(rrt, startConfig, goalConfig);
shortenedPath = shorten(rrt, path, 20);

Always call shorten after plan — RRT paths are jagged by nature. The second argument is the number of shortening iterations (20 is a good default; increase for complex environments).

manipulatorRRT with occupancyMap3D:

rrt = manipulatorRRT(robot, {}, Map=omap);
rrt.MaxConnectionDistance = 0.2;
rrt.ValidationDistance = 0.02;

Pass an empty cell {} for collision objects when using only the map.

manipulatorCHOMP:

chomp = manipulatorCHOMP(robot, MeshTSDF=mTSDF, ...
    SkippedSelfCollisions="parent");
chomp.SmoothnessOptions.Weight = 100;
chomp.CollisionOptions.CollisionCostWeight = 10;
chomp.SolverOptions.MaxIterations = 200;

trajectory = optimize(chomp, startConfig, goalConfig);

MeshTSDF must be passed at construction — it is read-only after creation.

Self-collision handling: Pass SkippedSelfCollisions at construction time. Setting it after construction can cause type mismatches (especially in codegen contexts).

NEVER use clearCollision to work around self-collision issues. Removing collision meshes silently disables collision checking for those bodies — the planner will report "collision-free" for paths that actually collide.

Strategy: First try SkippedSelfCollisions="parent". If the planner still reports self-collision at a valid configuration, build the skip list empirically using the procedure in references/self-collision-skip-procedure.md.

Key facts about checkCollision (easily misunderstood):

• separationDist body ordering: rows/cols 1:m are robot.Bodies{1:m} in order; index m+1 is robot.BaseName. The base is LAST, not first.
• NaN = pair is in collision. Inf = pair was successfully skipped.
• isColliding returns true if ANY NaN exists in the matrix. The skip list must eliminate all NaN entries — a pair that appears in your skip list but still shows NaN means the skip didn't take effect (check name spelling and char vs string types).
• {" "," "} (space characters in a 1×2 cell) is the dummy skip pair that suppresses the default parent-skip behavior without actually skipping any real pair.
• The always-NaN-across-samples approach catches parent-child overlaps (~100% collision rate), but parallel-linkage grippers (e.g., robotiq2F85) have additional pairs at ~70-90% that still need skipping. After the first pass, re-check home — if still colliding, add the remaining NaN pairs iteratively.

Step 4: Generate Trajectory (Time-Parameterize the Path)

Choose based on application need:

Need	Tool	Key Property
Minimum time respecting joint limits	contopptraj	TOPPRA algorithm — truly time-optimal
Constant velocity in working zone (welding, cutting, dispensing)	trapveltraj	Guaranteed zero-acceleration cruise phase
Smooth motion minimizing jerk (no obstacles)	minjerkpolytraj	Minimizes ∫jerk² cost

contopptraj (time-optimal):

velLimits = [-2*ones(numJoints,1), 2*ones(numJoints,1)];
accelLimits = [-5*ones(numJoints,1), 5*ones(numJoints,1)];
[q, qd, qdd, tSamples] = contopptraj(shortenedPath', velLimits, accelLimits);

trapveltraj (constant-velocity cruise):

[q, qd, qdd, tSamples] = trapveltraj(shortenedPath', 500, AccelTime=0.4);

AccelTime sets the ramp duration — everything between ramps is constant velocity. Use PeakVelocity to set an explicit cruise speed.

minjerkpolytraj (smooth interpolation):

tPoints = linspace(0, 3, size(shortenedPath', 2));
[q, qd, qdd, ~, tSamples] = minjerkpolytraj(shortenedPath', tPoints, 500);

Critical notes:

• All three expect numJoints×numWaypoints — transpose RRT output (shortenedPath')
• All return numJoints×numSamples
• Never use manual trapezoidal code or interp1 + gradient — these tools replace them
• contopptraj does NOT guarantee constant velocity; trapveltraj does NOT minimize time — pick the right one

Step 5: Verify Collision-Free Trajectory

The planner verified waypoints, but contopptraj interpolates between them. Verify the full trajectory:

% q from contopptraj is numJoints×numSamples — transpose for checkCollision (expects row vectors)
qRows = q';
inCollision = false;
for i = 1:size(qRows, 1)
    result = checkCollision(robot, qRows(i,:), env, SkippedSelfCollisions="parent");
    % result is 1×2: [selfCollision, worldCollision] — check BOTH
    if any(result)
        warning("Collision at trajectory sample %d (t=%.3f s), self=%d world=%d", ...
            i, tSamples(i), result(1), result(2));
        inCollision = true;
        break;
    end
end
if ~inCollision
    fprintf("Trajectory is collision-free (%d samples verified).\n", size(qRows,1));
end

If collisions are found in the interpolated trajectory, increase the planner's ValidationDistance (smaller = finer checking during planning) and re-plan.

Step 6: Visualize

figure;
show(robot, q(1,:), Frames="off");
hold on;
for k = 1:numel(env)
    show(env{k});
end
title("Motion Plan — Start Configuration");
axis auto;
view(45, 30);

Animate the trajectory:

figure;
ax = show(robot, q(1,:), Frames="off");
hold on;
for k = 1:numel(env)
    show(env{k});
end
axis auto;
view(45, 30);
title("Motion Plan Animation");

for i = 1:10:size(q,1)
    show(robot, q(i,:), Frames="off", PreservePlot=false, Parent=ax);
    drawnow;
end

Key Functions

Function	Purpose	Toolbox	Available From
manipulatorRRT	Sampling-based collision-free planner	Robotics System Toolbox	R2021b
manipulatorCHOMP	Optimization-based smooth planner	Robotics System Toolbox	R2023a
contopptraj	Time-optimal trajectory (TOPPRA)	Robotics System Toolbox	R2023b
trapveltraj	Trapezoidal velocity profile (constant-velocity cruise)	Robotics System Toolbox	R2019a
minjerkpolytraj	Minimum-jerk polynomial trajectory	Robotics System Toolbox	R2022a
checkCollision	Check robot-environment collision	Robotics System Toolbox	R2020b
collisionBox	Box collision primitive	Robotics System Toolbox	R2019b
collisionCylinder	Cylinder collision primitive	Robotics System Toolbox	R2019b
collisionSphere	Sphere collision primitive	Robotics System Toolbox	R2019b
collisionMesh	Mesh collision primitive	Robotics System Toolbox	R2019b
occupancyMap3D	3D voxel map from point clouds	Navigation Toolbox	R2019b
meshtsdf	Truncated signed distance field from meshes	Robotics System Toolbox	R2023a
geom2struct	Convert collision geometries to mesh structs	Robotics System Toolbox	R2023a
manipulatorStateSpace	Custom state space for planners	Robotics System Toolbox	R2021b
plannerBiRRT	Bidirectional RRT (with state space)	Navigation Toolbox	R2020b

Common Mistakes

Mistake	Why It's Wrong	Correct Approach
Using trapveltraj when time-optimality is needed	Trapezoidal profiles are NOT time-optimal — they over-allocate time	Use contopptraj (TOPPRA). trapveltraj is correct when constant velocity is the goal.
Using contopptraj when constant velocity is needed	TOPPRA varies velocity continuously to minimize time — no steady cruise phase	Use trapveltraj with AccelTime for welding/cutting/dispensing applications
Building manual trapezoidal time computation	Reinvents a solved problem incorrectly; 20+ lines replaced by one call	contopptraj or trapveltraj depending on need
Using interp1 + gradient for trajectory	Numerical differentiation is noisy and doesn't respect limits	Use the appropriate trajectory function — all return q, qd, qdd analytically
Skipping collision checks for "simple" interpolation	Even nearby configurations produce intermediate poses that can collide with workpieces, especially near surfaces	Always verify with checkCollision across all trajectory samples (Step 5)
Using clearCollision to fix self-collision errors	Removes collision checking for those bodies entirely — planner silently ignores real collisions	Discover the correct skip pairs empirically (see Self-collision handling above)
Planning without collision meshes on robot	Planner reports "collision-free" for paths that actually collide	Verify robot.Bodies have collision geometry before planning
Setting SkippedSelfCollisions after construction	Can cause type mismatches; fragile in codegen	Pass as name-value at planner construction
Setting MeshTSDF after CHOMP construction	Property is read-only after creation	Pass MeshTSDF=mTSDF at construction
Not calling shorten after RRT plan	RRT paths are jagged with unnecessary detours	Always call shorten(rrt, path, 20)
Verifying collisions only at planner waypoints	Interpolated trajectory points between waypoints can still collide	Check checkCollision across all contopptraj output samples
Passing RRT path directly to contopptraj without transposing	manipulatorRRT/plan returns N×numJoints (rows=waypoints) but contopptraj expects numJoints×N (columns=waypoints)	Transpose: contopptraj(path', velLimits, accelLimits)
Using contopptraj output q directly with checkCollision	contopptraj returns numJoints×numSamples but checkCollision expects row vectors	Transpose: checkCollision(robot, q(:,i)', env, ...) or work with q'
Using DataFormat="column" with planners	manipulatorRRT/plan expect row vectors	Set DataFormat="row" on the robot before planning
Treating checkCollision output as a scalar	With world objects, checkCollision returns 1×2: [selfCollision, worldCollision]. Using it as if checkCollision(...) only tests selfCollision (element 1) — world collisions are silently ignored	Always capture the result: result = checkCollision(...) then check any(result) or inspect result(2) for world collision specifically
TruncationDistance too small for meshtsdf	CHOMP cannot detect proximity if truncation < robot sphere radius	Set TruncationDistance > max collision sphere radius (typically 0.2+)
Placing a ground/floor collision object with its top surface at z=0	Robot base collision meshes extend up to 5 mm below z=0 (worst case: universalUR5 at 5 mm; most others ~1 mm). Additionally, many loadrobot home configurations extend the arm horizontally, causing distal links to dip below z=0. Both cause immediate world-collision before planning begins.	Place the ground surface top at z ≤ −0.01 (clears all loadrobot base meshes). Only include a ground plane if the task requires one. Always verify start/goal configs are collision-free with checkCollision before calling plan — if they collide, adjust IK initial guesses or raise target positions rather than lowering the floor.

---

Copyright 2026 The MathWorks, Inc.

---