--- name: matlab/matlab-plan-robot-motion source: https://app.decimal.ai/s/matlab-matlab-plan-robot-motion@1/SKILL.md source_sha256: f80109da9131 --- # 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: ```matlab 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: ```matlab 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:** ```matlab % 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:** ```matlab omap = occupancyMap3D(20); sensorPose = [0 0 0 1 0 0 0]; insertPointCloud(omap, sensorPose, pointCloudData, 5.0); ``` **meshtsdf pattern (for CHOMP):** ```matlab 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:** ```matlab 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:** ```matlab 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:** ```matlab 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):** ```matlab 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):** ```matlab [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):** ```matlab 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: ```matlab % 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 ```matlab 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: ```matlab 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. ----