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:

• 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.

• 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.");

• 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:

• [isColl,sepDist,witPts] = checkCollision(robot, startConfig, {}, SkippedSelfCollisions="parent") — if isColl(1)==0, use "parent".
• 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.

----