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:

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

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

2. 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:

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:

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

Common Mistakes

Copyright 2026 The MathWorks, Inc.