Motion Planning Best Practices
Introduction
Motion planning reliability — the rate at which planners return natural, collision-free paths — depends heavily on how the robot configuration package is set up. A robot whose joint limits allow large wrap-arounds or whose saved waypoints live in different elbow configurations will produce inconsistent, unpredictable, and sometimes scary-looking motions, even when the planner technically succeeds.
This guide collects practical tips for raising planning success rate and producing consistent, human-predictable motions. These recommendations apply to most 6- and 7-DoF serial arms.
Constrain Joint Limits
Prefer a [-π, π] range per joint on revolute joints
In your robot configuration package's joint_limits.yaml, limit every revolute joint to the range [-π, π] (approximately [-3.14159, 3.14159]).
This prevents the planner from finding solutions that wrap around multiple times, which otherwise produce trajectories that slowly "unwind" a fully-rotated joint.
See moveit_params.joint_limits for how to declare these overrides.
After tightening the limits, re-open your waypoints.xml (or waypoints.yaml) and update any saved joint values that fall outside the new range. Waypoints whose joint values are outside the new limits will fail to load or will plan poorly.
If the joint doesn't have position limits, i.e. it can rotate continuously, set its type to 'continuous' in the URDF. Planners handle these joints differently to avoid wrap-around.
Further constrain the elbow
If your application allows it, limit the elbow joint to a single, narrow range — for example [-0.05, 1.57] (approximately 0 to π/2).
This forces solvers to work entirely on one side of the kinematic singularity, preventing the elbow from flipping between positive and negative during a plan.
Why this matters: an elbow flip requires the arm to pass through a fully-stretched configuration at some point during the motion. This normally produces weird-looking, fast, or potentially unsafe motions even when the planner is successfully finding a 'short' path.
If the application requires the full elbow range (both positive and negative), at minimum ensure that all pre-recorded waypoints share the same elbow sign, or you minimize switching between the two elbow 'hemispheres' as much as possible.
Save Waypoints in Natural Configurations
Stay away from limits and self-collision
Prefer "natural" configurations — joints as close to their zero as possible (while not being at a singularity) and links well away from each other.
The further the joints are from their position limits or from link-to-link collision, the easier it is for the planner to find a smooth path in and out of that waypoint.
Keep waypoints consistent with each other
The planner finds a path between two waypoints by effectively interpolating in joint space and then correcting for collisions and constraints. If the saved waypoints already live in the same kinematic "branch" (same elbow sign, same wrist flip, similar J0 orientation), the planner usually finds a short, direct path. If they live in different branches, the planner has to re-solve for a kinematic change in the middle of the motion — which is slow, unreliable, and often produces surprising motions.
When saving a new waypoint, consciously choose a configuration that is consistent with your existing waypoints.
Teleoperation Technique for Saving Waypoints
Avoid using the interactive marker (IMarker) to save new waypoints except for small, local displacements of an existing waypoint. The IMarker drives an IK solver which is free to choose any branch it likes — so two waypoints saved via IMarker from similar-looking end-effector poses can easily end up in very different joint configurations.
The following teleoperation sequence produces more consistent waypoints. See the Teleoperation view pane for how to access the Joint Jog and Pose Jog controls. Note that this assumes a typical 6R shoulder-elbow-wrist robot manipulator:
- Orient the arm with J0 (base). Use the joint slider for the base joint to roughly aim the arm at the working area.
- Rough tip position with J1 / J2 (shoulder and elbow). Use the sliders to bring the tool tip into the right region of the workspace.
- Orient the end-effector with J3–J6 (wrist). Use the remaining joint sliders to set the tool orientation.
- Local adjustments with IMarker or Pose Jog. Only now, once the kinematic branch is fixed by the joint-slider pose, use IMarker or Pose Jog for the final local refinement. Because the starting configuration already constrains the IK solver, it will stay on the same branch instead of flipping.
This workflow trades a few seconds of teleoperation for waypoints that are kinematically consistent with each other — which pays back many times over in planning reliability and motion predictability.
Debug Unnecessarily Long Paths and Joint Flips
Even with well-constrained joint limits and consistent waypoints, you will occasionally see a plan that takes a longer path than expected, or that includes a joint flip or "unwind" mid-motion. These symptoms tend to occur more often in MoveIt Task Constructor (MTC) pipelines, which explore multiple solution branches in search of a feasible task plan. A single unlucky branch can survive the search and produce a path that is feasible but ugly.
When this happens, the most effective approach is to introspect the MTC pipeline result in RViz to see which candidates each stage proposed and why the others were rejected. See Debug Task Constructor Planning Pipelines for how to open the MTC Debugger and step through the stages.
Start with the generator stages
Generator stages (for example, Inverse Kinematics) are where the kinematic branch is chosen, so that is where most "weird" motions originate. For each generator stage, ask:
- Is IK proposing reasonable solutions but they are being rejected? Check the failure reason for each rejected candidate. A common cause is that the IK target, or the robot configuration that would reach it, is in collision with the scene or with the robot itself.
- Is IK not proposing the expected solution at all? The target may simply be unreachable given the current joint limits, or it may only be reachable in a configuration you did not anticipate (e.g., with the opposite elbow sign).
If the IK solution itself is unreasonable — far from the starting configuration, in the wrong kinematic branch, or near a limit — expect the downstream path planner to produce an equally unreasonable path to reach it. Fix the IK result first; the path will usually follow.
Then inspect the path planner
If the IK solution looks reasonable but the plan to reach it is still long, inspect the path planning stage (typically a sampling-based planner such as RRT). Two causes are common:
- Obstacles in the way. Stray voxels in the occupancy map, phantom objects in the planning scene, or an overly conservative collision padding can force the planner to route around a region that looks clear visually. Inspect the planning scene in RViz and clean up any spurious obstacles.
- A joint limit between the start and the goal. Even when the start and goal configurations look close in Cartesian space, a joint may need to "unwind" all the way across its range because it cannot cross its limit to take the direct route. This often looks like an unexpectedly long motion on a single joint.
Visual similarity of the robot pose is not a reliable signal of configuration similarity. Two configurations that look nearly identical in RViz can still be far apart in joint space if a joint is near a limit and has to unwind. Always check the joint values, not just the visualization.
Understanding what each MTC stage is doing — and why it is accepting or rejecting each candidate — is the key to producing robust, predictable, and natural-looking motion.